From a470a733725a4d036976dfa47c78fce0a161611c Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 2 Sep 2022 14:10:40 +0900 Subject: [PATCH 01/28] feat(interpolation): clean code (#1766) Signed-off-by: yutaka Signed-off-by: yutaka --- .../include/interpolation/zero_order_hold.hpp | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/interpolation/include/interpolation/zero_order_hold.hpp index 7de6743ed9d16..e48da814c5740 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/interpolation/include/interpolation/zero_order_hold.hpp @@ -28,23 +28,7 @@ std::vector zero_order_hold( { // throw exception for invalid arguments interpolation_utils::validateKeys(base_keys, query_keys); - - // when vectors are empty - if (base_keys.empty() || base_values.empty()) { - throw std::invalid_argument("Points is empty."); - } - - // when size of vectors are less than 2 - if (base_keys.size() < 2 || base_values.size() < 2) { - throw std::invalid_argument( - "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) + - ", base_values.size() = " + std::to_string(base_values.size())); - } - - // when sizes of indices and values are not same - if (base_keys.size() != base_values.size()) { - throw std::invalid_argument("The size of base_keys and base_values are not the same."); - } + interpolation_utils::validateKeysAndValues(base_keys, base_values); std::vector query_values; size_t closest_segment_idx = 0; From 269b3bde1385506b89f45b7c5dd1f9f5719307e3 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 2 Sep 2022 16:37:36 +0900 Subject: [PATCH 02/28] fix(behavior_velocity): fix insertVelocity (#1768) Signed-off-by: tanaka3 Signed-off-by: tanaka3 --- planning/behavior_velocity_planner/src/utilization/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index f3e9c0591b7e2..1ed17cd4be341 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -212,7 +212,7 @@ void insertVelocity( std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); for (int i = min_idx; i <= max_idx; i++) { if (calcDistance2d(path.points.at(static_cast(i)), path_point) < min_distance) { - path.points.at(i).point.longitudinal_velocity_mps = 0; + path.points.at(i).point.longitudinal_velocity_mps = v; already_has_path_point = true; insert_index = static_cast(i); // set velocity from is going to insert min velocity later From 8c2080c8cd4c0cecd52f75cb8fec2523b5e8cc6f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 2 Sep 2022 16:47:29 +0900 Subject: [PATCH 03/28] fix(behavior_path_planner): fix stop path for pull_over (#1742) * fix(behavior_path_planner): fix stop path for pull_over Signed-off-by: kosuke55 * use previous stop path when is has been generated Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Fumiya Watanabe Signed-off-by: kosuke55 Co-authored-by: Fumiya Watanabe --- .../pull_over/pull_over_module.hpp | 4 +- .../pull_over/pull_over_module.cpp | 54 +++++++++++-------- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index f870f4f337733..ec9689841d8d4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -108,12 +108,14 @@ enum PathType { struct PUllOverStatus { PathWithLaneId path; + std::shared_ptr prev_stop_path = nullptr; lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets pull_over_lanes; lanelet::ConstLanelets lanes; // current + pull_over bool has_decided_path = false; int path_type = PathType::NONE; bool is_safe = false; + bool prev_is_safe = false; bool has_decided_velocity = false; bool has_requested_approval_ = false; }; @@ -187,7 +189,7 @@ class PullOverModule : public SceneModuleInterface std::unique_ptr last_approved_time_; PathWithLaneId getReferencePath() const; - PathWithLaneId getStopPath() const; + PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPullOverLanes() const; std::pair getSafePath(ShiftParkingPath & safe_path) const; Pose getRefinedGoal() const; 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 4630a259b6d5d..49c72ea815057 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 @@ -570,8 +570,18 @@ BehaviorModuleOutput PullOverModule::plan() } BehaviorModuleOutput output; - output.path = status_.is_safe ? std::make_shared(status_.path) - : std::make_shared(getStopPath()); + + // safe: use pull over path + if (status_.is_safe) { + output.path = std::make_shared(status_.path); + } else if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + // safe -> not_safe or no prev_stop_path: generate new stop_path + output.path = std::make_shared(generateStopPath()); + status_.prev_stop_path = output.path; + } else { // not_safe -> not_safe: use previous stop path + output.path = status_.prev_stop_path; + } + status_.prev_is_safe = status_.is_safe; // set hazard and turn signal if (status_.has_decided_path) { @@ -696,7 +706,7 @@ PathWithLaneId PullOverModule::getReferencePath() const // stop pose is behind current pose if (s_forward < s_backward) { - return getStopPath(); + return generateStopPath(); } PathWithLaneId reference_path = @@ -721,45 +731,43 @@ PathWithLaneId PullOverModule::getReferencePath() const return reference_path; } -PathWithLaneId PullOverModule::getStopPath() const +PathWithLaneId PullOverModule::generateStopPath() const { - PathWithLaneId reference_path; - const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; const auto common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + auto stop_path = util::getCenterLinePath( + *route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length, + common_parameters.forward_path_length, common_parameters); + const double current_to_stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - const auto arclength_current_pose = + const double arclength_current_pose = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; - reference_path = util::getCenterLinePath( - *route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); - - for (auto & point : reference_path.points) { - const auto arclength = + for (auto & point : stop_path.points) { + const double arclength = lanelet::utils::getArcCoordinates(status_.current_lanes, point.point.pose).length; - const auto arclength_current_to_point = arclength - arclength_current_pose; - if (0 < arclength_current_to_point) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast(std::max( - 0.0, current_vel * (current_to_stop_distance - arclength_current_to_point) / - current_to_stop_distance))); + const double arclength_current_to_point = arclength - arclength_current_pose; + if (0.0 < arclength_current_to_point) { + point.point.longitudinal_velocity_mps = std::clamp( + static_cast( + current_vel * (current_to_stop_distance - arclength_current_to_point) / + current_to_stop_distance), + 0.0f, point.point.longitudinal_velocity_mps); } else { point.point.longitudinal_velocity_mps = std::min(point.point.longitudinal_velocity_mps, static_cast(current_vel)); } } - reference_path.drivable_area = util::generateDrivableArea( - reference_path, status_.current_lanes, common_parameters.drivable_area_resolution, + stop_path.drivable_area = util::generateDrivableArea( + stop_path, status_.current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, planner_data_); - return reference_path; + return stop_path; } lanelet::ConstLanelets PullOverModule::getPullOverLanes() const From a9b972279562c5d7c6819527b9599a6ba7aeac31 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 2 Sep 2022 19:00:31 +0900 Subject: [PATCH 04/28] feat(behavior_path_planner): update pull out (#1438) * feat(behavior_path_planner): update pull out Signed-off-by: kosuke55 * refactor(behavior_path_planner): rename pull_out params Signed-off-by: kosuke55 * update from review Signed-off-by: kosuke55 * use debug_data Signed-off-by: kosuke55 * enable back Signed-off-by: kosuke55 * move PlannerType Signed-off-by: kosuke55 * fix debug marker Signed-off-by: kosuke55 * add seach priority Signed-off-by: kosuke55 * change before_pull_out_straight_distance to 0.0 Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_out/pull_out.param.yaml | 41 +- planning/behavior_path_planner/CMakeLists.txt | 2 + .../config/pull_out/pull_out.param.yaml | 42 +- .../pull_out/geometric_pull_out.hpp | 41 + .../scene_module/pull_out/pull_out_module.hpp | 111 +-- .../pull_out/pull_out_parameters.hpp | 56 ++ .../scene_module/pull_out/pull_out_path.hpp | 17 +- .../pull_out/pull_out_planner_base.hpp | 71 ++ .../scene_module/pull_out/shift_pull_out.hpp | 56 ++ .../scene_module/pull_out/util.hpp | 47 +- .../scene_module/utils/path_shifter.hpp | 1 + .../behavior_path_planner/utilities.hpp | 32 +- .../src/behavior_path_planner_node.cpp | 46 +- .../avoidance/avoidance_module.cpp | 2 +- .../src/scene_module/lane_change/util.cpp | 6 +- .../pull_out/geometric_pull_out.cpp | 68 ++ .../scene_module/pull_out/pull_out_module.cpp | 888 +++++++----------- .../scene_module/pull_out/shift_pull_out.cpp | 259 +++++ .../src/scene_module/pull_out/util.cpp | 451 +-------- .../src/scene_module/pull_over/util.cpp | 6 +- .../src/scene_module/utils/path_shifter.cpp | 29 +- .../behavior_path_planner/src/utilities.cpp | 98 +- .../include/route_handler/route_handler.hpp | 6 +- planning/route_handler/src/route_handler.cpp | 6 +- 24 files changed, 1172 insertions(+), 1210 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp create mode 100644 planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml index 434545674d283..7a9a63f805d67 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -1,25 +1,30 @@ /**: ros__parameters: pull_out: - min_stop_distance: 2.0 - stop_time: 0.0 - hysteresis_buffer_distance: 1.0 - pull_out_prepare_duration: 4.0 - pull_out_duration: 2.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 pull_out_finish_judge_buffer: 1.0 - minimum_pull_out_velocity: 2.0 - prediction_duration: 30.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - use_dynamic_object: true - enable_blocked_by_obstacle: false - pull_out_search_distance: 30.0 - before_pull_out_straight_distance: 5.0 - after_pull_out_straight_distance: 5.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + before_pull_out_straight_distance: 0.0 + minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 15.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 3e6dc4d0e3325..5036e6ae1649b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -27,6 +27,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/pull_over/util.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp + src/scene_module/pull_out/shift_pull_out.cpp + src/scene_module/pull_out/geometric_pull_out.cpp src/scene_module/utils/geometric_parallel_parking.cpp src/scene_module/utils/occupancy_grid_based_collision_detector.cpp src/scene_module/utils/path_shifter.cpp diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index 872e79340bd3b..7a9a63f805d67 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -1,26 +1,30 @@ /**: ros__parameters: pull_out: - min_stop_distance: 2.0 - stop_time: 0.0 - hysteresis_buffer_distance: 1.0 - pull_out_prepare_duration: 4.0 - pull_out_duration: 2.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 pull_out_finish_judge_buffer: 1.0 - minimum_pull_out_velocity: 2.0 - prediction_duration: 30.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_abort_pull_out: false - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - use_dynamic_object: false - enable_blocked_by_obstacle: false - pull_out_search_distance: 30.0 - before_pull_out_straight_distance: 5.0 - after_pull_out_straight_distance: 5.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + before_pull_out_straight_distance: 0.0 + minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 15.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp new file mode 100644 index 0000000000000..73f3ae83c0f6c --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp @@ -0,0 +1,41 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ + +#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp" +#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" + +#include + +namespace behavior_path_planner +{ +class GeometricPullOut : public PullOutPlannerBase +{ +public: + explicit GeometricPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + const ParallelParkingParameters & parallel_parking_parameters); + + PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; + boost::optional plan(Pose start_pose, Pose goal_pose) override; + + GeometricParallelParking planner_; + ParallelParkingParameters parallel_parking_parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 42004374c1bb6..fd94ef35cd4ef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -15,14 +15,25 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ +#include "behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include +#include +#include #include +#include #include +#include + +#include #include #include #include @@ -30,48 +41,22 @@ namespace behavior_path_planner { -struct PullOutParameters -{ - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; - double pull_out_prepare_duration; - double pull_out_duration; - double pull_out_finish_judge_buffer; - double minimum_pull_out_velocity; - double prediction_duration; - double prediction_time_resolution; - double static_obstacle_velocity_thresh; - double maximum_deceleration; - int pull_out_sampling_num; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - bool use_dynamic_object; - bool enable_blocked_by_obstacle; - double pull_out_search_distance; - double before_pull_out_straight_distance; - double after_pull_out_straight_distance; - double maximum_lateral_jerk; - double minimum_lateral_jerk; - double deceleration_interval; -}; +using geometry_msgs::msg::PoseArray; +using lane_departure_checker::LaneDepartureChecker; struct PullOutStatus { - PathWithLaneId lane_follow_path; PullOutPath pull_out_path; - PullOutPath retreat_path; - PullOutPath straight_back_path; + size_t current_path_idx = 0; + PlannerType planner_type = PlannerType::NONE; + PathWithLaneId backward_path; lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets pull_out_lanes; std::vector lane_follow_lane_ids; std::vector pull_out_lane_ids; - bool is_safe; - double start_distance; - bool back_finished; - bool is_retreat_path_valid; - Pose backed_pose; + bool is_safe = false; + bool back_finished = false; + Pose pull_out_start_pose; }; class PullOutModule : public SceneModuleInterface @@ -91,44 +76,50 @@ class PullOutModule : public SceneModuleInterface void onEntry() override; void onExit() override; - void setParameters(const PullOutParameters & parameters); + void setParameters(const PullOutParameters & parameters) { parameters_ = parameters; } + void resetStatus(); private: PullOutParameters parameters_; + vehicle_info_util::VehicleInfo vehicle_info_; + + std::vector> pull_out_planners_; PullOutStatus status_; - double pull_out_lane_length_ = 200.0; - double check_distance_ = 100.0; + std::vector backed_pose_candidates_; + PoseStamped backed_pose_; + std::deque odometry_buffer_; - PathWithLaneId getReferencePath() const; + std::unique_ptr last_route_received_time_; + std::unique_ptr last_pull_out_start_update_time_; + + std::shared_ptr getCurrentPlanner() const; lanelet::ConstLanelets getCurrentLanes() const; - lanelet::ConstLanelets getPullOutLanes(const lanelet::ConstLanelets & current_lanes) const; - std::pair getSafePath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - PullOutPath & safe_path) const; - std::pair getSafeRetreatPath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - RetreatPath & safe_backed_path, double & back_distance) const; - - bool getBackDistance( - const lanelet::ConstLanelets & pullover_lanes, const double check_distance, - PullOutPath & safe_path, double & back_distance) const; + PathWithLaneId getFullPath() const; + ParallelParkingParameters getGeometricPullOutParameters() const; + std::vector searchBackedPoses(); - // turn signal - TurnSignalInfo calcTurnSignalInfo(const ShiftPoint & shift_point) const; + std::shared_ptr lane_departure_checker_; + // turn signal + TurnSignalInfo calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const; + + void incrementPathIndex(); + PathWithLaneId getCurrentPath() const; + void planWithPriorityOnEfficientPath( + const std::vector & start_pose_candidates, const Pose & goal_pose); + void planWithPriorityOnShortBackDistance( + const std::vector & start_pose_candidates, const Pose & goal_pose); void updatePullOutStatus(); - bool isInLane( + static bool isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const; - bool isLongEnough(const lanelet::ConstLanelets & lanelets) const; - bool isSafe() const; - bool isNearEndOfLane() const; - bool isCurrentSpeedLow() const; + const tier4_autoware_utils::LinearRing2d & vehicle_footprint); bool hasFinishedPullOut() const; - bool hasFinishedBack() const; - vehicle_info_util::VehicleInfo getVehicleInfo( - const BehaviorPathPlannerParameters & parameters) const; + void checkBackFinished(); + bool isStopped(); + bool hasFinishedCurrentPath(); + + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp new file mode 100644 index 0000000000000..a994d57ee1368 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -0,0 +1,56 @@ + +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ + +#include + +namespace behavior_path_planner +{ +struct PullOutParameters +{ + double th_arrived_distance; + double th_stopped_velocity; + double th_stopped_time; + double collision_check_margin; + double pull_out_finish_judge_buffer; + // shift pull out + bool enable_shift_pull_out; + double shift_pull_out_velocity; + int pull_out_sampling_num; + double before_pull_out_straight_distance; + double minimum_shift_pull_out_distance; + double maximum_lateral_jerk; + double minimum_lateral_jerk; + double deceleration_interval; + // geometric pull out + bool enable_geometric_pull_out; + double geometric_pull_out_velocity; + double arc_path_interval; + double lane_departure_margin; + double backward_velocity; + double pull_out_max_steer_angle; + // search start pose backward + std::string search_priority; // "efficient_path" or "short_back_distance" + bool enable_back; + double max_back_distance; + double backward_search_resolution; + double backward_path_update_duration; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp index 5b32ff64babba..3f49dd82b8b29 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp @@ -19,23 +19,16 @@ #include +#include + namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { - PathWithLaneId path; - ShiftedPath shifted_path; - ShiftPoint shift_point; - double acceleration = 0.0; - double preparation_length = 0.0; - double pull_out_length = 0.0; -}; - -struct RetreatPath -{ - behavior_path_planner::PullOutPath pull_out_path; - Pose backed_pose; + std::vector partial_paths; + Pose start_pose; + Pose end_pose; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp new file mode 100644 index 0000000000000..8144373c1e8a6 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp @@ -0,0 +1,71 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/util/create_vehicle_footprint.hpp" + +#include +#include + +#include + +#include +#include + +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LinearRing2d; + +namespace behavior_path_planner +{ +enum class PlannerType { + NONE = 0, + SHIFT = 1, + GEOMETRIC = 2, +}; + +class PullOutPlannerBase +{ +public: + explicit PullOutPlannerBase(rclcpp::Node & node, const PullOutParameters & parameters) + { + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + parameters_ = parameters; + } + virtual ~PullOutPlannerBase() = default; + + void setPlannerData(std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + + virtual PlannerType getPlannerType() = 0; + virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + +protected: + std::shared_ptr planner_data_; + vehicle_info_util::VehicleInfo vehicle_info_; + LinearRing2d vehicle_footprint_; + PullOutParameters parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp new file mode 100644 index 0000000000000..4f43bb080fabd --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp @@ -0,0 +1,56 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__SHIFT_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__SHIFT_PULL_OUT_HPP_ + +#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp" + +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using lane_departure_checker::LaneDepartureChecker; + +class ShiftPullOut : public PullOutPlannerBase +{ +public: + explicit ShiftPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + std::shared_ptr & lane_departure_checker); + + PlannerType getPlannerType() override { return PlannerType::SHIFT; }; + boost::optional plan(Pose start_pose, Pose goal_pose) override; + + std::vector calcPullOutPaths( + const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & common_parameter, + const behavior_path_planner::PullOutParameters & parameter); + + bool hasEnoughDistance( + const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, + const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); + + std::shared_ptr lane_departure_checker_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp index 786686d950ab9..5107d875bcdd1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp @@ -37,49 +37,14 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); -std::vector getPullOutPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::PullOutParameters & parameter, const bool is_retreat_path = false); - -PullOutPath getBackPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::PullOutParameters & parameter, const double back_distance); - -bool isPathInLanelets4pullover( - const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets); - +PathWithLaneId getBackwardPath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const Pose & backed_pose, const double velocity); +lanelet::ConstLanelets getPullOutLanes( + const lanelet::ConstLanelets & current_lanes, + const std::shared_ptr & planner_data); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); - -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose); -bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, - const behavior_path_planner::PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, PullOutPath * selected_path); -bool isPullOutPathSafe( - const behavior_path_planner::PullOutPath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const behavior_path_planner::PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const bool use_buffer = true, - const bool use_dynamic_object = false); -bool hasEnoughDistance( - const PullOutPath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - const lanelet::routing::RoutingGraphContainer & overall_graphs); -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); } // namespace behavior_path_planner::pull_out_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp index 100e2a18072f5..ea489fc0468a6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp @@ -183,6 +183,7 @@ class PathShifter bool is_index_aligned_{false}; rclcpp::Logger logger_{rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; /** * @brief Calculate path index for shift_points and set is_index_aligned_ to true. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 0209990f35514..a3267ba1602b6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -196,10 +196,6 @@ Point lerpByLength(const std::vector & point_array, const double length) bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt); -bool lerpByDistance( - const behavior_path_planner::PullOutPath & path, const double & s, Pose * lerped_pt, - const lanelet::ConstLanelets & road_lanes); - bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon); PredictedPath resamplePredictedPath( @@ -213,16 +209,27 @@ double getDistanceBetweenPredictedPathAndObject( const PredictedObject & object, const PredictedPath & path, const double start_time, const double end_time, const double resolution); -double getDistanceBetweenPredictedPathAndObjectPolygon( - const PredictedObject & object, const PullOutPath & ego_path, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, - const lanelet::ConstLanelets & road_lanes); +/** + * @brief Check collision between ego path footprints and objects. + * @return Has collision or not + */ +bool checkCollisionBetweenPathFootprintsAndObjects( + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const PredictedObjects & dynamic_objects, const double margin); + +/** + * @brief Check collision between ego footprints and objects. + * @return Has collision or not + */ +bool checkCollisionBetweenFootprintAndObjects( + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const PredictedObjects & dynamic_objects, const double margin); /** * @brief Get index of the obstacles inside the lanelets with start and end length * @return Indices corresponding to the obstacle inside the lanelets */ -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets, const double start_arc_length, const double end_arc_length); @@ -230,10 +237,13 @@ std::vector filterObjectsByLanelets( * @brief Get index of the obstacles inside the lanelets * @return Indices corresponding to the obstacle inside the lanelets */ -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +PredictedObjects filterObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -std::vector filterObjectsByPath( +std::vector filterObjectsIndicesByPath( const PredictedObjects & objects, const std::vector & object_indices, const PathWithLaneId & ego_path, const double vehicle_width); 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 82a7fe47f48bb..93e05415595e8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -447,29 +447,34 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() PullOutParameters p; - p.min_stop_distance = dp("min_stop_distance", 5.0); - p.stop_time = dp("stop_time", 2.0); - p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0); - p.pull_out_prepare_duration = dp("pull_out_prepare_duration", 2.0); - p.pull_out_duration = dp("pull_out_duration", 4.0); + p.th_arrived_distance = dp("th_arrived_distance", 1.0); + p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); + p.th_stopped_time = dp("th_stopped_time", 1.0); + p.collision_check_margin = dp("collision_check_margin", 1.0); p.pull_out_finish_judge_buffer = dp("pull_out_finish_judge_buffer", 1.0); - p.minimum_pull_out_velocity = dp("minimum_pull_out_velocity", 8.3); - p.prediction_duration = dp("prediction_duration", 8.0); - p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); - p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1); - p.maximum_deceleration = dp("maximum_deceleration", 1.0); + // shift pull out + p.enable_shift_pull_out = dp("enable_shift_pull_out", true); + p.shift_pull_out_velocity = dp("shift_pull_out_velocity", 8.3); p.pull_out_sampling_num = dp("pull_out_sampling_num", 4); - p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); - p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); - p.use_all_predicted_path = dp("use_all_predicted_path", false); - p.use_dynamic_object = dp("use_dynamic_object", false); - p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false); - p.pull_out_search_distance = dp("pull_out_search_distance", 30.0); - p.after_pull_out_straight_distance = dp("after_pull_out_straight_distance", 3.0); p.before_pull_out_straight_distance = dp("before_pull_out_straight_distance", 3.0); + p.minimum_shift_pull_out_distance = dp("minimum_shift_pull_out_distance", 20.0); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); p.deceleration_interval = dp("deceleration_interval", 10.0); + // geometric pull out + p.enable_geometric_pull_out = dp("enable_geometric_pull_out", true); + p.geometric_pull_out_velocity = dp("geometric_pull_out_velocity", 1.0); + p.arc_path_interval = dp("arc_path_interval", 1.0); + p.lane_departure_margin = dp("lane_departure_margin", 0.2); + p.backward_velocity = dp("backward_velocity", -0.3); + p.pull_out_max_steer_angle = dp("pull_out_max_steer_angle", 0.26); // 15deg + // search start pose backward + p.search_priority = + dp("search_priority", "efficient_path"); // "efficient_path" or "short_back_distance" + p.enable_back = dp("enable_back", true); + p.max_back_distance = dp("max_back_distance", 15.0); + p.backward_search_resolution = dp("backward_search_resolution", 2.0); + p.backward_path_update_duration = dp("backward_path_update_duration", 3.0); // validation of parameters if (p.pull_out_sampling_num < 1) { @@ -479,13 +484,6 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() << "Terminating the program..."); exit(EXIT_FAILURE); } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } return p; } 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 a87e5c267969a..41f9b43c79e1a 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 @@ -182,7 +182,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( current_lanes, parameters_->detection_area_left_expand_dist, parameters_->detection_area_right_expand_dist * (-1.0)); const auto lane_filtered_objects_index = - util::filterObjectsByLanelets(objects_candidate, expanded_lanelets); + util::filterObjectIndicesByLanelets(objects_candidate, expanded_lanelets); DEBUG_PRINT("dynamic_objects size = %lu", planner_data_->dynamic_object->objects.size()); DEBUG_PRINT("object_candidate size = %lu", objects_candidate.objects.size()); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index f767a19788de9..7a8a4d27cd19f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -382,17 +382,17 @@ bool isLaneChangePathSafe( // find obstacle in lane change target lanes // retrieve lanes that are merging target lanes as well const auto target_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, target_lanes); + util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); // find objects in current lane constexpr double check_distance = 100.0; - const auto current_lane_object_indices_lanelet = util::filterObjectsByLanelets( + const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); const double lateral_buffer = (use_buffer) ? 0.5 : 0.0; const auto & vehicle_width = common_parameters.vehicle_width; const auto & vehicle_length = common_parameters.vehicle_length; - const auto current_lane_object_indices = util::filterObjectsByPath( + const auto current_lane_object_indices = util::filterObjectsIndicesByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp new file mode 100644 index 0000000000000..eec8731bbda4e --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -0,0 +1,68 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp" + +#include "behavior_path_planner/scene_module/pull_out/util.hpp" + +using lanelet::utils::getArcCoordinates; +using motion_utils::findNearestIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +namespace behavior_path_planner +{ +using pull_out_utils::combineReferencePath; +using pull_out_utils::getPullOutLanes; + +GeometricPullOut::GeometricPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + const ParallelParkingParameters & parallel_parking_parameters) +: PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parallel_parking_parameters} +{ +} + +boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +{ + PullOutPath output; + + // conbine road lane and shoulder lane + const auto road_lanes = util::getCurrentLanes(planner_data_); + const auto shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); + auto lanes = road_lanes; + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // todo: set params only once? + planner_.setData(planner_data_, parallel_parking_parameters_); + const bool found_valid_path = + planner_.planPullOut(start_pose, goal_pose, road_lanes, shoulder_lanes); + if (!found_valid_path) { + return {}; + } + + // collsion check with objects in shoulder lanes + const auto arc_path = planner_.getArcPath(); + const auto shoulder_lane_objects = + util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { + return {}; + } + + output.partial_paths = planner_.getPaths(); + output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose; + output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; + + return output; +} +} // namespace behavior_path_planner 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 362963fc87d6c..450ba7f5e31db 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 @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2022 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -29,13 +29,32 @@ #include #include +using motion_utils::calcLongitudinalOffsetPose; +using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, const PullOutParameters & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} +: SceneModuleInterface{name, node}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { rtc_interface_ptr_ = std::make_shared(&node, "pull_out"); + lane_departure_checker_ = std::make_shared(); + lane_departure_checker_->setVehicleInfo(vehicle_info_); + + // set enabled planner + if (parameters_.enable_shift_pull_out) { + pull_out_planners_.push_back( + std::make_shared(node, parameters, lane_departure_checker_)); + } + if (parameters_.enable_geometric_pull_out) { + pull_out_planners_.push_back( + std::make_shared(node, parameters, getGeometricPullOutParameters())); + } + if (pull_out_planners_.empty()) { + RCLCPP_DEBUG(getLogger(), "Not found enabled planner"); + } } BehaviorModuleOutput PullOutModule::run() @@ -49,14 +68,31 @@ void PullOutModule::onEntry() { RCLCPP_DEBUG(getLogger(), "PULL_OUT onEntry"); current_state_ = BT::NodeStatus::SUCCESS; - updatePullOutStatus(); - // Get arclength to start lane change - const auto current_pose = planner_data_->self_pose->pose; - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); - status_.back_finished = false; - status_.start_distance = arclength_start.length; + // initialize when receiving new route + if ( + last_route_received_time_ == nullptr || + *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { + RCLCPP_INFO(getLogger(), "Receive new route, so reset status"); + resetStatus(); + updatePullOutStatus(); + } + last_route_received_time_ = + std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); + + // for preventing chattering between back and pull_out + if (!status_.back_finished) { + if (last_pull_out_start_update_time_ == nullptr) { + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + const auto elpased_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); + if (elpased_time < parameters_.backward_path_update_duration) { + return; + } + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + + updatePullOutStatus(); } void PullOutModule::onExit() @@ -73,26 +109,24 @@ bool PullOutModule::isExecutionRequested() const return true; } - const bool car_is_stopping = - (util::l2Norm(planner_data_->self_odometry->twist.twist.linear) <= 1.5) ? true : false; + const bool is_stopped = util::l2Norm(planner_data_->self_odometry->twist.twist.linear) < + parameters_.th_arrived_distance; lanelet::Lanelet closest_shoulder_lanelet; - if ( lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), planner_data_->self_pose->pose, &closest_shoulder_lanelet) && - car_is_stopping) { + is_stopped) { // Create vehicle footprint - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); const auto vehicle_footprint = transformVector( local_vehicle_footprint, tier4_autoware_utils::pose2transform(planner_data_->self_pose->pose)); const auto road_lanes = getCurrentLanes(); // check if goal pose is in shoulder lane and distance is long enough for pull out - if (isInLane(closest_shoulder_lanelet, vehicle_footprint) && isLongEnough(road_lanes)) { + if (isInLane(closest_shoulder_lanelet, vehicle_footprint)) { return true; } } @@ -100,253 +134,252 @@ bool PullOutModule::isExecutionRequested() const return false; } -bool PullOutModule::isExecutionReady() const -{ - if (current_state_ == BT::NodeStatus::RUNNING) { - return true; - } - - // TODO(sugahara) move to utility function - const auto road_lanes = getCurrentLanes(); - const auto shoulder_lanes = getPullOutLanes(road_lanes); - - // Find pull_out path - bool found_valid_path, found_safe_path; - PullOutPath selected_path; - std::tie(found_valid_path, found_safe_path) = - getSafePath(shoulder_lanes, check_distance_, selected_path); - - if (found_valid_path && !found_safe_path) { - double back_distance; - if (getBackDistance(shoulder_lanes, check_distance_, selected_path, back_distance)) { - return true; - } - } - return found_safe_path; -} // namespace behavior_path_planner +bool PullOutModule::isExecutionReady() const { return true; } +// this runs only when RUNNING BT::NodeStatus PullOutModule::updateState() { RCLCPP_DEBUG(getLogger(), "PULL_OUT updateState"); - // finish after lane change - if (status_.back_finished && hasFinishedPullOut()) { + if (hasFinishedPullOut()) { current_state_ = BT::NodeStatus::SUCCESS; return current_state_; } - if (status_.is_retreat_path_valid) { - if (hasFinishedBack()) { - status_.back_finished = true; - } - } - current_state_ = BT::NodeStatus::RUNNING; + + checkBackFinished(); + return current_state_; } BehaviorModuleOutput PullOutModule::plan() { - constexpr double RESAMPLE_INTERVAL = 1.0; + BehaviorModuleOutput output; + if (!status_.is_safe) { + return output; + } PathWithLaneId path; - if (status_.is_retreat_path_valid && !status_.is_safe) { - path = util::resamplePathWithSpline(status_.straight_back_path.path, RESAMPLE_INTERVAL); + if (status_.back_finished) { + if (hasFinishedCurrentPath()) { + RCLCPP_INFO(getLogger(), "Increment path index"); + incrementPathIndex(); + } + path = getCurrentPath(); } else { - path = util::resamplePathWithSpline(status_.pull_out_path.path, RESAMPLE_INTERVAL); - status_.back_finished = true; - } - - if (status_.is_retreat_path_valid && status_.back_finished) { - path = util::resamplePathWithSpline(status_.retreat_path.path, RESAMPLE_INTERVAL); + path = status_.backward_path; } - path.drivable_area = status_.pull_out_path.path.drivable_area; - - BehaviorModuleOutput output; output.path = std::make_shared(path); - output.turn_signal_info = calcTurnSignalInfo(status_.pull_out_path.shift_point); + output.turn_signal_info = + calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + + setDebugData(); return output; } -CandidateOutput PullOutModule::planCandidate() const +CandidateOutput PullOutModule::planCandidate() const { return CandidateOutput{}; } + +std::shared_ptr PullOutModule::getCurrentPlanner() const { - // Get lane change lanes - const auto current_lanes = getCurrentLanes(); - const auto shoulder_lanes = getPullOutLanes(current_lanes); + for (const auto & planner : pull_out_planners_) { + if (status_.planner_type == planner->getPlannerType()) { + return planner; + } + } + return nullptr; +} - const auto current_pose = planner_data_->self_pose->pose; +PathWithLaneId PullOutModule::getFullPath() const +{ + const auto pull_out_planner = getCurrentPlanner(); + if (pull_out_planner == nullptr) { + return PathWithLaneId{}; + } - // Find pull out path - bool found_valid_path, found_safe_path; - PullOutPath selected_path; - std::tie(found_valid_path, found_safe_path) = - getSafePath(shoulder_lanes, check_distance_, selected_path); - - if (found_valid_path && !found_safe_path) { - double back_distance; - if (getBackDistance(shoulder_lanes, check_distance_, selected_path, back_distance)) { - bool found_valid_retreat_path, found_safe_retreat_path; - RetreatPath selected_retreat_path; - std::tie(found_valid_retreat_path, found_safe_retreat_path) = - getSafeRetreatPath(shoulder_lanes, check_distance_, selected_retreat_path, back_distance); - // ROS_ERROR("found safe retreat path in plan candidate :%d", found_safe_retreat_path); - if (found_safe_retreat_path == true) { - selected_retreat_path.pull_out_path.path.header = - planner_data_->route_handler->getRouteHeader(); - CandidateOutput output_retreat(selected_retreat_path.pull_out_path.path); - output_retreat.distance_to_path_change = motion_utils::calcSignedArcLength( - selected_retreat_path.pull_out_path.path.points, current_pose.position, - selected_retreat_path.backed_pose.position); - return output_retreat; - } - } + // combine partial pull out path + PathWithLaneId pull_out_path; + for (const auto & partial_path : status_.pull_out_path.partial_paths) { + pull_out_path.points.insert( + pull_out_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - // ROS_ERROR("found safe path in plan candidate :%d", found_safe_path); - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - CandidateOutput output(selected_path.path); - output.distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, current_pose.position, selected_path.shift_point.start.position); - return output; + if (status_.back_finished) { + // not need backward path or finish it + return pull_out_path; + } + + // concat back_path and pull_out_path and + auto full_path = status_.backward_path; + full_path.points.insert( + full_path.points.end(), pull_out_path.points.begin(), pull_out_path.points.end()); + return full_path; } BehaviorModuleOutput PullOutModule::planWaitingApproval() { - BehaviorModuleOutput out; - const auto common_parameters = planner_data_->parameters; + BehaviorModuleOutput output; const auto current_lanes = getCurrentLanes(); - const auto shoulder_lanes = getPullOutLanes(current_lanes); + const auto shoulder_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); - PathWithLaneId candidate_path; - // Generate drivable area - { - const auto candidate = planCandidate(); - candidate_path = candidate.path_candidate; - lanelet::ConstLanelets lanes; - lanes.insert(lanes.end(), current_lanes.begin(), current_lanes.end()); - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - const double resolution = common_parameters.drivable_area_resolution; - candidate_path.drivable_area = util::generateDrivableArea( - candidate_path, lanes, resolution, common_parameters.vehicle_length, planner_data_); - - updateRTCStatus(candidate.distance_to_path_change); - } - for (size_t i = 1; i < candidate_path.points.size(); i++) { - candidate_path.points.at(i).point.longitudinal_velocity_mps = 0.0; + const auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto stop_path = candidate_path; + for (auto & p : stop_path.points) { + p.point.longitudinal_velocity_mps = 0.0; } - out.path = std::make_shared(candidate_path); - out.path_candidate = std::make_shared(candidate_path); + output.path = std::make_shared(stop_path); + output.turn_signal_info = + calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + output.path_candidate = std::make_shared(candidate_path); waitApproval(); + // requset approval when stopped at the corresponding point, so distance is 0 + updateRTCStatus(0.0); - return out; + setDebugData(); + return output; } -void PullOutModule::setParameters(const PullOutParameters & parameters) +void PullOutModule::resetStatus() { - parameters_ = parameters; + PullOutStatus initial_status; + status_ = initial_status; } -void PullOutModule::updatePullOutStatus() +ParallelParkingParameters PullOutModule::getGeometricPullOutParameters() const { - const auto & route_handler = planner_data_->route_handler; - const auto common_parameters = planner_data_->parameters; + ParallelParkingParameters params{}; - const auto current_lanes = getCurrentLanes(); - status_.current_lanes = current_lanes; + params.th_arrived_distance = parameters_.th_arrived_distance; + params.th_stopped_velocity = parameters_.th_stopped_velocity; + params.arc_path_interval = parameters_.arc_path_interval; + params.departing_velocity = parameters_.geometric_pull_out_velocity; + params.departing_lane_departure_margin = parameters_.lane_departure_margin; + params.max_steer_angle = parameters_.pull_out_max_steer_angle; - // Get pull_out lanes - const auto pull_out_lanes = getPullOutLanes(current_lanes); - status_.pull_out_lanes = pull_out_lanes; + return params; +} - const auto current_pose = planner_data_->self_pose->pose; - // const auto current_twist = planner_data_->self_odometry->twist.twist; - // const auto common_parameters = planner_data_->parameters; - - // Find pull_out path - bool found_valid_path, found_safe_path; - PullOutPath selected_path; - std::tie(found_valid_path, found_safe_path) = - getSafePath(pull_out_lanes, check_distance_, selected_path); - - if (found_valid_path && !found_safe_path) { - double back_distance; - if (getBackDistance(pull_out_lanes, check_distance_, selected_path, back_distance)) { - bool found_valid_retreat_path, found_safe_retreat_path; - RetreatPath selected_retreat_path; - std::tie(found_valid_retreat_path, found_safe_retreat_path) = - getSafeRetreatPath(pull_out_lanes, check_distance_, selected_retreat_path, back_distance); - if (found_valid_retreat_path && found_safe_retreat_path) { - status_.is_retreat_path_valid = true; - status_.backed_pose = selected_retreat_path.backed_pose; - status_.retreat_path = selected_retreat_path.pull_out_path; - status_.retreat_path.path.header = planner_data_->route_handler->getRouteHeader(); - status_.straight_back_path = pull_out_utils::getBackPaths( - *route_handler, pull_out_lanes, current_pose, common_parameters, parameters_, - back_distance); +void PullOutModule::incrementPathIndex() +{ + status_.current_path_idx = + std::min(status_.current_path_idx + 1, status_.pull_out_path.partial_paths.size() - 1); +} + +PathWithLaneId PullOutModule::getCurrentPath() const +{ + return status_.pull_out_path.partial_paths.at(status_.current_path_idx); +} + +void PullOutModule::planWithPriorityOnEfficientPath( + const std::vector & start_pose_candidates, const Pose & goal_pose) +{ + status_.is_safe = false; + + for (const auto & planner : pull_out_planners_) { + for (const auto & pull_out_start_pose : start_pose_candidates) { + // plan with each planner + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + if (pull_out_path) { // found safe path + status_.is_safe = true; + status_.pull_out_path = *pull_out_path; + status_.pull_out_start_pose = pull_out_start_pose; + status_.planner_type = planner->getPlannerType(); + break; } + // pull out start pose is not current_pose(index > 0), so need back. + status_.back_finished = false; + } + if (status_.is_safe) { + break; } } +} - // Update status - status_.is_safe = found_safe_path; - status_.pull_out_path = selected_path; - - status_.lane_follow_lane_ids = util::getIds(current_lanes); - status_.pull_out_lane_ids = util::getIds(pull_out_lanes); - - // Generate drivable area - { - lanelet::ConstLanelets lanes; - lanes.insert(lanes.end(), current_lanes.begin(), current_lanes.end()); - lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); - - const double resolution = common_parameters.drivable_area_resolution; - status_.pull_out_path.path.drivable_area = util::generateDrivableArea( - status_.pull_out_path.path, lanes, resolution, common_parameters.vehicle_length, - planner_data_); +void PullOutModule::planWithPriorityOnShortBackDistance( + const std::vector & start_pose_candidates, const Pose & goal_pose) +{ + status_.is_safe = false; + + for (const auto & pull_out_start_pose : start_pose_candidates) { + // plan with each planner + for (const auto & planner : pull_out_planners_) { + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + if (pull_out_path) { // found safe path + status_.is_safe = true; + status_.pull_out_path = *pull_out_path; + status_.pull_out_start_pose = pull_out_start_pose; + status_.planner_type = planner->getPlannerType(); + break; + } + } + if (status_.is_safe) { + break; + } + // pull out start pose is not current_pose(index > 0), so need back. + status_.back_finished = false; } - - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); - status_.start_distance = arclength_start.length; - - status_.pull_out_path.path.header = planner_data_->route_handler->getRouteHeader(); } -PathWithLaneId PullOutModule::getReferencePath() const +void PullOutModule::updatePullOutStatus() { - PathWithLaneId reference_path; - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto goal_pose = planner_data_->route_handler->getGoalPose(); - const auto common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = route_handler->getRouteHeader(); + const auto & common_parameters = planner_data_->parameters; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & goal_pose = planner_data_->route_handler->getGoalPose(); const auto current_lanes = getCurrentLanes(); - const auto pull_out_lanes = getPullOutLanes(current_lanes); + status_.current_lanes = current_lanes; + + // Get pull_out lanes + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); + status_.pull_out_lanes = pull_out_lanes; - if (current_lanes.empty()) { - return reference_path; + // search pull out start candidates backward + std::vector start_pose_candidates; + { + if (parameters_.enable_back) { + // the first element is current_pose + start_pose_candidates = searchBackedPoses(); + } else { + // pull_out_start candidate is only current pose + start_pose_candidates.push_back(current_pose); + } } - reference_path = util::getCenterLinePath( - *route_handler, pull_out_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); + if (parameters_.search_priority == "efficient_path") { + planWithPriorityOnEfficientPath(start_pose_candidates, goal_pose); + } else if (parameters_.search_priority == "short_back_distance") { + planWithPriorityOnShortBackDistance(start_pose_candidates, goal_pose); + } else { + RCLCPP_ERROR( + getLogger(), + "search_priority should be efficient_path or short_back_distance, but %s is given.", + parameters_.search_priority.c_str()); + } + + if (!status_.is_safe) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Not found safe pull out path"); + status_.is_safe = false; + return; + } - reference_path = util::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_.after_pull_out_straight_distance, - common_parameters.minimum_pull_out_length, parameters_.before_pull_out_straight_distance, - parameters_.deceleration_interval, goal_pose); + checkBackFinished(); + if (!status_.back_finished) { + status_.backward_path = pull_out_utils::getBackwardPath( + *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose, + parameters_.backward_velocity); + status_.backward_path.drivable_area = util::generateDrivableArea( + status_.backward_path, pull_out_lanes, common_parameters.drivable_area_resolution, + common_parameters.vehicle_length, planner_data_); + } - reference_path.drivable_area = util::generateDrivableArea( - reference_path, pull_out_lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); - return reference_path; + // Update status + status_.lane_follow_lane_ids = util::getIds(current_lanes); + status_.pull_out_lane_ids = util::getIds(pull_out_lanes); } lanelet::ConstLanelets PullOutModule::getCurrentLanes() const @@ -368,245 +401,51 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const common_parameters.forward_path_length); } -// getShoulderLanesOnCurrentPose? -lanelet::ConstLanelets PullOutModule::getPullOutLanes( - const lanelet::ConstLanelets & current_lanes) const +// make this class? +std::vector PullOutModule::searchBackedPoses() { - lanelet::ConstLanelets shoulder_lanes; - const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; - lanelet::ConstLanelet shoulder_lane; - - if (current_lanes.empty()) { - return shoulder_lanes; - } - - // Get pull out lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet( - current_lanes, planner_data_->self_pose->pose, ¤t_lane); - - if (route_handler->getPullOutStart( - route_handler->getShoulderLanelets(), &shoulder_lane, current_pose, - planner_data_->parameters.vehicle_width)) { - shoulder_lanes = route_handler->getShoulderLaneletSequence( - shoulder_lane, current_pose, pull_out_lane_length_, pull_out_lane_length_); - - } else { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "getPullOverTarget didn't work"); - shoulder_lanes.clear(); - } - - return shoulder_lanes; -} - -std::pair PullOutModule::getSafePath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - PullOutPath & safe_path) const -{ - std::vector valid_paths; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - const auto road_lanes = getCurrentLanes(); - - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); - - if (!pull_out_lanes.empty()) { - // find candidate paths - const auto pull_out_paths = pull_out_utils::getPullOutPaths( - *route_handler, road_lanes, pull_out_lanes, current_pose, common_parameters, parameters_); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_out_paths.empty()) { - const auto & longest_path = pull_out_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.pull_out_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, pull_out_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_out_utils::selectValidPaths( - 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()) { - RCLCPP_DEBUG(getLogger(), "valid path is empty"); - return std::make_pair(false, false); - } - // select safe path - bool found_safe_path = pull_out_utils::selectSafePath( - valid_paths, road_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, local_vehicle_footprint, - &safe_path); - - return std::make_pair(true, found_safe_path); - } - return std::make_pair(false, false); -} - -std::pair PullOutModule::getSafeRetreatPath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - RetreatPath & safe_retreat_path, double & back_distance) const -{ - std::vector valid_paths; - PullOutPath safe_path; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - - const auto road_lanes = getCurrentLanes(); - - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); + const auto current_lanes = getCurrentLanes(); + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); - lanelet::ConstLanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - pull_out_lanes, current_pose, &closest_shoulder_lanelet)) { - // RCLCPP_ERROR_THROTTLE(getLogger(), clock, 1000, "Failed to find closest lane!"); - } + // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); - - const auto shoulder_line_path = route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, - arc_position_pose.length + pull_out_lane_length_); - const auto idx = motion_utils::findNearestIndex(shoulder_line_path.points, current_pose); - const auto yaw_shoulder_lane = - tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); - - const auto backed_pose = - pull_out_utils::getBackedPose(current_pose, yaw_shoulder_lane, back_distance); - - if (!pull_out_lanes.empty()) { - // find candidate paths - const auto pull_out_paths = pull_out_utils::getPullOutPaths( - *route_handler, road_lanes, pull_out_lanes, backed_pose, common_parameters, parameters_, - true); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_out_paths.empty()) { - const auto & longest_path = pull_out_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.pull_out_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, pull_out_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_out_utils::selectValidPaths( - 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()) { - return std::make_pair(false, false); - } - // select safe path - bool found_safe_path = pull_out_utils::selectSafePath( - valid_paths, road_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, local_vehicle_footprint, - &safe_path); - safe_retreat_path.pull_out_path = safe_path; - safe_retreat_path.backed_pose = backed_pose; - - return std::make_pair(true, found_safe_path); + const double check_distance = parameters_.max_back_distance + 30.0; // buffer + auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + pull_out_lanes, arc_position_pose.length - check_distance, + arc_position_pose.length + check_distance); + + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : backward_shoulder_path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); } - return std::make_pair(false, false); -} - -bool PullOutModule::getBackDistance( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - PullOutPath & safe_path, double & back_distance) const -{ - std::vector valid_paths; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - const double back_distance_search_resolution = 1; - const double maximum_back_distance = 15; - - const auto road_lanes = getCurrentLanes(); - - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); - - double yaw_shoulder_lane; - { - lanelet::ConstLanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - pull_out_lanes, current_pose, &closest_shoulder_lanelet)) { - return false; + // check collision between footprint and onject at the backed pose + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); + std::vector backed_poses; + for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance; + back_distance += parameters_.backward_search_resolution) { + const auto backed_pose = calcLongitudinalOffsetPose( + backward_shoulder_path.points, current_pose.position, -back_distance); + if (!backed_pose) { + continue; } - const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); - - const auto shoulder_line_path = route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, - arc_position_pose.length + pull_out_lane_length_); - const auto idx = motion_utils::findNearestIndex(shoulder_line_path.points, current_pose); - yaw_shoulder_lane = tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); - } - - for (double current_back_distance = back_distance_search_resolution; - current_back_distance <= maximum_back_distance; - current_back_distance += back_distance_search_resolution) { - if (!pull_out_lanes.empty()) { - const auto backed_pose = - pull_out_utils::getBackedPose(current_pose, yaw_shoulder_lane, current_back_distance); - - // find candidate paths - const auto pull_out_paths = pull_out_utils::getPullOutPaths( - *route_handler, road_lanes, pull_out_lanes, backed_pose, common_parameters, parameters_, - true); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_out_paths.empty()) { - const auto & longest_path = pull_out_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.pull_out_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, pull_out_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_out_utils::selectValidPaths( - 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()) { - continue; - } - // select safe path - bool found_safe_path = pull_out_utils::selectSafePath( - valid_paths, road_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, local_vehicle_footprint, - &safe_path); - if (found_safe_path) { - back_distance = current_back_distance; - return found_safe_path; - } + if (util::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, *(planner_data_->dynamic_object), + parameters_.collision_check_margin)) { + break; // poses behind this has a collision, so break. } + + backed_poses.push_back(*backed_pose); } - return false; + return backed_poses; } bool PullOutModule::isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const + const tier4_autoware_utils::LinearRing2d & vehicle_footprint) { for (const auto & point : vehicle_footprint) { if (boost::geometry::within(point, candidate_lanelet.polygon2d().basicPolygon())) { @@ -617,150 +456,127 @@ bool PullOutModule::isInLane( return false; } -bool PullOutModule::isLongEnough(const lanelet::ConstLanelets & lanelets) const +bool PullOutModule::hasFinishedPullOut() const { - PathShifter path_shifter; - const double maximum_jerk = parameters_.maximum_lateral_jerk; - const double pull_out_velocity = parameters_.minimum_pull_out_velocity; + if (!status_.back_finished) { + return false; + } + + // check ego car is close enough to goal pose const auto current_pose = planner_data_->self_pose->pose; - const auto goal_pose = planner_data_->route_handler->getGoalPose(); - const double distance_before_pull_out = parameters_.before_pull_out_straight_distance; - const double distance_after_pull_out = parameters_.after_pull_out_straight_distance; - const double distance_to_road_center = - lanelet::utils::getArcCoordinates(lanelets, planner_data_->self_pose->pose).distance; - - // calculate minimum pull_out distance at pull_out velocity, - // maximum jerk and calculated side offset - const double pull_out_distance_min = path_shifter.calcLongitudinalDistFromJerk( - abs(distance_to_road_center), maximum_jerk, pull_out_velocity); - const double pull_out_total_distance_min = - distance_before_pull_out + pull_out_distance_min + distance_after_pull_out; - const double distance_to_goal_on_road_lane = - util::getSignedDistance(current_pose, goal_pose, lanelets); - - return distance_to_goal_on_road_lane > pull_out_total_distance_min; -} + const auto arclength_current = + lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); + const auto arclength_pull_out_end = + lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); -bool PullOutModule::isSafe() const { return status_.is_safe; } + // has passed pull out end point + return arclength_current.length - arclength_pull_out_end.length > + parameters_.pull_out_finish_judge_buffer; +} -bool PullOutModule::isNearEndOfLane() const +void PullOutModule::checkBackFinished() { + // check ego car is close enough to pull out start pose const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; - const double threshold = 5 + common_parameters.minimum_pull_over_length; + const auto distance = + tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); - return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < - threshold; -} + const bool is_near = distance < parameters_.th_arrived_distance; + const double ego_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); + const bool is_stopped = ego_vel < parameters_.th_stopped_velocity; -bool PullOutModule::isCurrentSpeedLow() const -{ - const auto current_twist = planner_data_->self_odometry->twist.twist; - const double threshold_kmph = 10; - return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600; + if (!status_.back_finished && is_near && is_stopped) { + RCLCPP_INFO(getLogger(), "back finished"); + status_.back_finished = true; + + // requst pull_out approval + waitApproval(); + removeRTCStatus(); + uuid_ = generateUUID(); + // requset approval when stopped at the corresponding point, so distance is 0 + updateRTCStatus(0.0); + current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop + } } -bool PullOutModule::hasFinishedPullOut() const +bool PullOutModule::isStopped() { - // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_pose->pose; - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); - const auto arclength_shift_end = - lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.shift_point.end); - const bool car_is_on_goal = (arclength_shift_end.length - arclength_current.length < - parameters_.pull_out_finish_judge_buffer) - ? true - : false; - - return car_is_on_goal; + odometry_buffer_.push_back(planner_data_->self_odometry); + // Delete old data in buffer + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - + rclcpp::Time(odometry_buffer_.front()->header.stamp); + if (time_diff.seconds() < parameters_.th_stopped_time) { + break; + } + odometry_buffer_.pop_front(); + } + bool is_stopped = true; + for (const auto & odometry : odometry_buffer_) { + const double ego_vel = util::l2Norm(odometry->twist.twist.linear); + if (ego_vel > parameters_.th_stopped_velocity) { + is_stopped = false; + break; + } + } + return is_stopped; } -bool PullOutModule::hasFinishedBack() const +bool PullOutModule::hasFinishedCurrentPath() { - // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_pose->pose; - const auto backed_pose = status_.backed_pose; - const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, backed_pose); + const auto current_path = getCurrentPath(); + const auto current_path_end = current_path.points.back(); + const auto self_pose = planner_data_->self_pose->pose; + const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + parameters_.th_arrived_distance; - return distance < 1; + return is_near_target && isStopped(); } -TurnSignalInfo PullOutModule::calcTurnSignalInfo(const ShiftPoint & shift_point) const +TurnSignalInfo PullOutModule::calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const { TurnSignalInfo turn_signal; - if (status_.is_retreat_path_valid && !status_.back_finished) { + // turn hazard light when backward driving + if (!status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; turn_signal.signal_distance = - tier4_autoware_utils::calcDistance2d(status_.backed_pose, planner_data_->self_pose->pose); + tier4_autoware_utils::calcDistance2d(start_pose, planner_data_->self_pose->pose); return turn_signal; } - const auto current_lanes = getCurrentLanes(); - const auto pull_out_lanes = getPullOutLanes(current_lanes); - const double turn_signal_on_threshold = 30; - const double turn_signal_off_threshold = -3; - const double turn_hazard_on_threshold = 3; - - // calculate distance to pull_out start on current lanes - double distance_to_pull_out_start; - { - const auto pull_out_start = shift_point.start; - const auto arc_position_pull_out_start = - lanelet::utils::getArcCoordinates(current_lanes, pull_out_start); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose); - distance_to_pull_out_start = - arc_position_pull_out_start.length - arc_position_current_pose.length; - } - // calculate distance to pull_out end on target lanes - double distance_to_pull_out_end; - { - const auto pull_out_end = shift_point.end; - const auto arc_position_pull_out_end = - lanelet::utils::getArcCoordinates(pull_out_lanes, pull_out_end); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(pull_out_lanes, planner_data_->self_pose->pose); - distance_to_pull_out_end = arc_position_pull_out_end.length - arc_position_current_pose.length; - } - - // calculate distance to pull_out start on target lanes - double distance_to_target_pose; - { - const auto arc_position_target_pose = lanelet::utils::getArcCoordinates( - pull_out_lanes, planner_data_->route_handler->getGoalPose()); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(pull_out_lanes, planner_data_->self_pose->pose); - distance_to_target_pose = arc_position_target_pose.length - arc_position_current_pose.length; - } - - if (distance_to_pull_out_start < turn_signal_on_threshold) { + const auto current_lanes = getCurrentLanes(); + const auto arc_position_current_pose = + lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose); + const auto arc_position_pull_out_end = lanelet::utils::getArcCoordinates(current_lanes, end_pose); + const double distance_from_pull_out_end = + arc_position_current_pose.length - arc_position_pull_out_end.length; + + // turn on right signal until passing pull_out end point + const double turn_signal_off_buffer = std::min(parameters_.pull_out_finish_judge_buffer, 3.0); + if (distance_from_pull_out_end < turn_signal_off_buffer) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - if (distance_to_pull_out_end < turn_signal_off_threshold) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; - if (distance_to_target_pose < turn_hazard_on_threshold) { - turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - } - } + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; } - turn_signal.signal_distance = distance_to_pull_out_end; + + turn_signal.signal_distance = -distance_from_pull_out_end + turn_signal_off_buffer; return turn_signal; } -vehicle_info_util::VehicleInfo PullOutModule::getVehicleInfo( - const BehaviorPathPlannerParameters & parameters) const +void PullOutModule::setDebugData() const { - vehicle_info_util::VehicleInfo vehicle_info; - vehicle_info.front_overhang_m = parameters.front_overhang; - vehicle_info.wheel_base_m = parameters.wheel_base; - vehicle_info.rear_overhang_m = parameters.rear_overhang; - vehicle_info.wheel_tread_m = parameters.wheel_tread; - vehicle_info.left_overhang_m = parameters.left_over_hang; - vehicle_info.right_overhang_m = parameters.right_over_hang; - return vehicle_info; -} + using marker_utils::createPathMarkerArray; + using marker_utils::createPoseMarkerArray; + + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; + debug_marker_.markers.clear(); + add(createPoseMarkerArray(status_.pull_out_start_pose, "pull_out_start_pose", 0, 0.9, 0.3, 0.3)); + add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp new file mode 100644 index 0000000000000..7371ad99fb80d --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -0,0 +1,259 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/pull_out/util.hpp" + +#include +#include + +using lanelet::utils::getArcCoordinates; +using motion_utils::findNearestIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +namespace behavior_path_planner +{ +using pull_out_utils::combineReferencePath; +using pull_out_utils::getPullOutLanes; + +ShiftPullOut::ShiftPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + std::shared_ptr & lane_departure_checker) +: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} +{ +} + +boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const auto & common_parameters = planner_data_->parameters; + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & road_lanes = util::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_pose->pose; + const auto & shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); + if (shoulder_lanes.empty()) { + return boost::none; + } + + lanelet::ConstLanelets lanes; + lanes.insert(lanes.end(), road_lanes.begin(), road_lanes.end()); + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // find candidate paths + auto pull_out_paths = calcPullOutPaths( + *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters, + parameters_); + if (pull_out_paths.empty()) { + return boost::none; + } + + // extract objects in shoulder lane for collision check + const auto shoulder_lane_objects = + util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); + + // get safe path + for (auto & pull_out_path : pull_out_paths) { + auto & shift_path = + pull_out_path.partial_paths.front(); // shift path is not separate but only one. + + // check lane_depature and collsion with path between current to pull_out_end + PathWithLaneId path_current_to_shift_end; + { + const auto current_idx = findNearestIndex(shift_path.points, current_pose); + const auto pull_out_end_idx = findNearestIndex(shift_path.points, pull_out_path.end_pose); + path_current_to_shift_end.points.insert( + path_current_to_shift_end.points.begin(), shift_path.points.begin() + *current_idx, + shift_path.points.begin() + *pull_out_end_idx + 1); + } + + // check lane departure + if (lane_departure_checker_->checkPathWillLeaveLane(lanes, path_current_to_shift_end)) { + continue; + } + + // check collision + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, path_current_to_shift_end, shoulder_lane_objects, + parameters_.collision_check_margin)) { + continue; + } + + // Generate drivable area + const double resolution = common_parameters.drivable_area_resolution; + shift_path.drivable_area = util::generateDrivableArea( + shift_path, lanes, resolution, common_parameters.vehicle_length, planner_data_); + + shift_path.header = planner_data_->route_handler->getRouteHeader(); + + return pull_out_path; + } + + return boost::none; +} + +std::vector ShiftPullOut::calcPullOutPaths( + const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter) +{ + std::vector candidate_paths; + + if (road_lanes.empty() || shoulder_lanes.empty()) { + return candidate_paths; + } + + // rename parameter + const double backward_path_length = common_parameter.backward_path_length; + const double shift_pull_out_velocity = parameter.shift_pull_out_velocity; + const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; + const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; + const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; + const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; + const int pull_out_sampling_num = parameter.pull_out_sampling_num; + const double jerk_resolution = + std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_out_sampling_num; + + for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; + lateral_jerk += jerk_resolution) { + PathShifter path_shifter; + const double distance_to_road_center = getArcCoordinates(road_lanes, start_pose).distance; + const double pull_out_distance = std::max( + path_shifter.calcLongitudinalDistFromJerk( + abs(distance_to_road_center), lateral_jerk, shift_pull_out_velocity), + minimum_shift_pull_out_distance); + + // check has enough distance + const double pull_out_total_distance = before_pull_out_straight_distance + pull_out_distance; + const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); + if (!hasEnoughDistance( + pull_out_total_distance, road_lanes, start_pose, is_in_goal_route_section, goal_pose)) { + continue; + } + + PathWithLaneId shoulder_reference_path; + { + const auto arc_position = getArcCoordinates(shoulder_lanes, start_pose); + const double s_start = arc_position.length - backward_path_length; + double s_end = arc_position.length + before_pull_out_straight_distance; + s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); + shoulder_reference_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end); + + // lateral shift to start_pose + for (auto & p : shoulder_reference_path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, arc_position.distance, 0); + } + } + + PathWithLaneId road_lane_reference_path; + { + const lanelet::ArcCoordinates arc_position_shift = + getArcCoordinates(road_lanes, shoulder_reference_path.points.back().point.pose); + const lanelet::ArcCoordinates arc_position_goal = getArcCoordinates(road_lanes, goal_pose); + + double s_start = arc_position_shift.length; + double s_end = arc_position_goal.length; + road_lane_reference_path = route_handler.getCenterLinePath(road_lanes, s_start, s_end); + } + + // get shift point start/end + const auto shift_point_start = shoulder_reference_path.points.back(); + const auto shift_point_end = [&]() { + const auto arc_position_shift_start = + lanelet::utils::getArcCoordinates(road_lanes, shift_point_start.point.pose); + const double s_start = arc_position_shift_start.length + pull_out_distance; + const double s_end = s_start + std::numeric_limits::epsilon(); + const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); + return path.points.front(); + }(); + + ShiftPoint shift_point; + { + shift_point.start = shift_point_start.point.pose; + shift_point.end = shift_point_end.point.pose; + shift_point.length = distance_to_road_center; + } + path_shifter.addShiftPoint(shift_point); + path_shifter.setPath(road_lane_reference_path); + + // offset front side + ShiftedPath shifted_path; + const bool offset_back = false; + if (!path_shifter.generate(&shifted_path, offset_back)) { + continue; + } + + const auto pull_out_end_idx = + findNearestIndex(shifted_path.path.points, shift_point_end.point.pose); + const auto goal_idx = findNearestIndex(shifted_path.path.points, goal_pose); + + if (pull_out_end_idx && goal_idx) { + // set velocity + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *pull_out_end_idx) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); + continue; + } else if (i > *goal_idx) { + point.point.longitudinal_velocity_mps = 0.0; + continue; + } + } + + PullOutPath candidate_path; + const auto combined_path = combineReferencePath(shoulder_reference_path, shifted_path.path); + candidate_path.partial_paths.push_back(util::resamplePathWithSpline(combined_path, 1.0)); + candidate_path.start_pose = shift_point.start; + candidate_path.end_pose = shift_point.end; + + // add goal pose because resampling removes it + // but this point will be removed by SmoothGoalConnection again + PathPointWithLaneId goal_path_point = shifted_path.path.points.back(); + goal_path_point.point.pose = goal_pose; + goal_path_point.point.longitudinal_velocity_mps = 0.0; + candidate_path.partial_paths.front().points.push_back(goal_path_point); + candidate_paths.push_back(candidate_path); + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), + "lane change end idx not found on target path."); + continue; + } + } + + return candidate_paths; +} + +bool ShiftPullOut::hasEnoughDistance( + const double pull_out_total_distance, const lanelet::ConstLanelets & road_lanes, + const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose) +{ + // the goal is far so current_lanes do not include goal's lane + if (pull_out_total_distance > util::getDistanceToEndOfLane(current_pose, road_lanes)) { + return false; + } + + // current_lanes include goal's lane + if ( + is_in_goal_route_section && + pull_out_total_distance > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { + return false; + } + + return true; +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp index 835f83f811506..7699a58e02ff1 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp @@ -44,261 +44,42 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa // skip overlapping point path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - return path; + return util::removeOverlappingPoints(path); } -bool isPathInLanelets4pullover( - const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets) +PathWithLaneId getBackwardPath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & current_pose, const Pose & backed_pose, const double velocity) { - for (const auto & pt : path.points) { - bool is_in_lanelet = false; - const lanelet::BasicPoint2d p(pt.point.pose.position.x, pt.point.pose.position.y); - for (const auto & llt : original_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 1)) { - is_in_lanelet = true; - } - } - for (const auto & llt : target_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 1)) { - is_in_lanelet = true; - } - } - if (!is_in_lanelet) { - return false; - } - } - return true; -} - -std::vector getPullOutPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanelets, - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter, - const bool is_retreat_path) -{ - std::vector candidate_paths; - - if (road_lanelets.empty() || shoulder_lanelets.empty()) { - return candidate_paths; - } - // rename parameter - const double backward_path_length = common_parameter.backward_path_length; - const double forward_path_length = common_parameter.forward_path_length; - const double minimum_pull_out_velocity = parameter.minimum_pull_out_velocity; - const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; - const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; - const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; - const int pull_out_sampling_num = parameter.pull_out_sampling_num; - const double jerk_resolution = - std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_out_sampling_num; - // maximum lateral jerk is set on retreat path - double initial_lateral_jerk = is_retreat_path ? maximum_lateral_jerk : minimum_lateral_jerk; - - for (double lateral_jerk = initial_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; - lateral_jerk += jerk_resolution) { - PathWithLaneId reference_path; - PathShifter path_shifter; - ShiftedPath shifted_path; - const double v1 = minimum_pull_out_velocity; - - const double distance_to_road_center = - lanelet::utils::getArcCoordinates(road_lanelets, pose).distance; - - const double distance_to_shoulder_center = - lanelet::utils::getArcCoordinates(shoulder_lanelets, pose).distance; - - double pull_out_distance = - path_shifter.calcLongitudinalDistFromJerk(abs(distance_to_road_center), lateral_jerk, v1); - - PathWithLaneId reference_path1; - { - const auto arc_position = lanelet::utils::getArcCoordinates(shoulder_lanelets, pose); - const double s_start = arc_position.length - backward_path_length; - double s_end = arc_position.length + before_pull_out_straight_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - reference_path1 = route_handler.getCenterLinePath(shoulder_lanelets, s_start, s_end); - } - for (auto & point : reference_path1.points) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(minimum_pull_out_velocity)); - } - - // Apply shifting before shift - for (size_t i = 0; i < reference_path1.points.size(); ++i) { - { - if (fabs(distance_to_shoulder_center) < 1.0e-8) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "no offset from current lane center."); - } - - auto & p = reference_path1.points.at(i).point.pose; - double yaw = tf2::getYaw(p.orientation); - p.position.x -= std::sin(yaw) * distance_to_shoulder_center; - p.position.y += std::cos(yaw) * distance_to_shoulder_center; - } - } - - PathWithLaneId reference_path2; - { - const auto arc_position_goal = - lanelet::utils::getArcCoordinates(shoulder_lanelets, route_handler.getGoalPose()); - const auto arc_position_ref1_back = - lanelet::utils::getArcCoordinates(road_lanelets, reference_path1.points.back().point.pose); - double s_start = arc_position_ref1_back.length + pull_out_distance; - double s_end = arc_position_goal.length; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - reference_path2 = route_handler.getCenterLinePath(road_lanelets, s_start, s_end); - } - - if (reference_path1.points.empty() || reference_path2.points.empty()) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "reference path is empty!! something wrong..."); - continue; - } - - PullOutPath candidate_path; - // candidate_path.acceleration = acceleration; - candidate_path.preparation_length = before_pull_out_straight_distance; - candidate_path.pull_out_length = pull_out_distance; - PathWithLaneId target_lane_reference_path; - { - const lanelet::ArcCoordinates pull_out_start_arc_position = - lanelet::utils::getArcCoordinates(road_lanelets, reference_path1.points.back().point.pose); - double s_start = pull_out_start_arc_position.length; - double s_end = s_start + pull_out_distance + forward_path_length; - target_lane_reference_path = route_handler.getCenterLinePath(road_lanelets, s_start, s_end); - } - - ShiftPoint shift_point; - { - const Pose pull_out_start_on_shoulder_lane = reference_path1.points.back().point.pose; - const Pose pull_out_end_on_road_lane = reference_path2.points.front().point.pose; - shift_point.start = pull_out_start_on_shoulder_lane; - shift_point.end = pull_out_end_on_road_lane; - - shift_point.length = distance_to_road_center; - } - path_shifter.addShiftPoint(shift_point); - path_shifter.setPath(target_lane_reference_path); - - // offset front side - bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) { - // ROS_ERROR("failed to generate shifted path."); - continue; - } - - const auto pull_out_end_idx = motion_utils::findNearestIndex( - shifted_path.path.points, reference_path2.points.front().point.pose); - - const auto goal_idx = - motion_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); - - if (pull_out_end_idx && goal_idx) { - const auto distance_pull_out_end_to_goal = tier4_autoware_utils::calcDistance2d( - shifted_path.path.points.at(*pull_out_end_idx).point.pose, - shifted_path.path.points.at(*goal_idx).point.pose); - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *pull_out_end_idx) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - reference_path1.points.back().point.longitudinal_velocity_mps); - continue; - } else if (i > *goal_idx) { - point.point.longitudinal_velocity_mps = 0.0; - continue; - } - - auto distance_to_goal = tier4_autoware_utils::calcDistance2d( - point.point.pose, shifted_path.path.points.at(*goal_idx).point.pose); - point.point.longitudinal_velocity_mps = std::min( - minimum_pull_out_velocity, - std::max( - 0.0, (distance_to_goal / distance_pull_out_end_to_goal * minimum_pull_out_velocity))); - point.lane_ids = reference_path2.points.front().lane_ids; - } - candidate_path.path = combineReferencePath(reference_path1, shifted_path.path); - candidate_path.shifted_path = shifted_path; - candidate_path.shift_point = shift_point; - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "lane change end idx not found on target path."); - continue; - } - - // check candidate path is in lanelet - if (!isPathInLanelets4pullover(candidate_path.path, road_lanelets, shoulder_lanelets)) { - continue; - } - - // ROS_ERROR("candidate path is push backed"); - candidate_paths.push_back(candidate_path); - } - - return candidate_paths; -} - -PullOutPath getBackPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanelets, - const Pose & pose, const BehaviorPathPlannerParameters & common_parameter, - [[maybe_unused]] const PullOutParameters & parameter, [[maybe_unused]] const double back_distance) -{ - PathShifter path_shifter; - ShiftedPath shifted_path; + const auto current_pose_arc_coords = + lanelet::utils::getArcCoordinates(shoulder_lanes, current_pose); + const auto backed_pose_arc_coords = + lanelet::utils::getArcCoordinates(shoulder_lanes, backed_pose); - // rename parameter - const double backward_path_length = common_parameter.backward_path_length; + const double s_start = backed_pose_arc_coords.length; + const double s_end = current_pose_arc_coords.length; - const double distance_to_shoulder_center = - lanelet::utils::getArcCoordinates(shoulder_lanelets, pose).distance; - - PathWithLaneId reference_path1; + PathWithLaneId backward_path; { - const auto arc_position = lanelet::utils::getArcCoordinates(shoulder_lanelets, pose); - const double s_start = arc_position.length + backward_path_length; - double s_end = s_start - 50; - reference_path1 = route_handler.getCenterLinePath(shoulder_lanelets, s_end, s_start); - // ROS_ERROR("ref1 s_start:%f s_end%f", s_start, s_end); - for (auto & point : reference_path1.points) { - // auto arc_length = - // lanelet::utils::getArcCoordinates(shoulder_lanelets, point.point.pose).length; - point.point.longitudinal_velocity_mps = -5; - // ROS_ERROR("back_distance:%f", back_distance); - // if (arc_position.length - arc_length > back_distance) { - // point.point.longitudinal_velocity_mps = 0; - // } - } - // std::reverse(reference_path1.points.begin(), reference_path1.points.end()); - // reference_path1.points.front().point.longitudinal_velocity_mps = 0; - } + // forward center line path + backward_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end, true); - // Apply shifting before shift - for (size_t i = 0; i < reference_path1.points.size(); ++i) { - { - if (fabs(distance_to_shoulder_center) < 1.0e-8) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "no offset from current lane center."); - continue; - } + // backward center line path + std::reverse(backward_path.points.begin(), backward_path.points.end()); + for (auto & p : backward_path.points) { + p.point.longitudinal_velocity_mps = velocity; + } + backward_path.points.back().point.longitudinal_velocity_mps = 0.0; - auto & p = reference_path1.points.at(i).point.pose; - double yaw = tf2::getYaw(p.orientation); - p.position.x -= std::sin(yaw) * distance_to_shoulder_center; - p.position.y += std::cos(yaw) * distance_to_shoulder_center; + // lateral shift to current_pose + const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; + for (size_t i = 0; i < backward_path.points.size(); ++i) { + auto & p = backward_path.points.at(i).point.pose; + p = tier4_autoware_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); } } - PullOutPath candidate_path; - - candidate_path.path = reference_path1; - - return candidate_path; + return backward_path; } Pose getBackedPose( @@ -312,172 +93,34 @@ Pose getBackedPose( return backed_pose; } -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose) -{ - std::vector available_paths; - - for (const auto & path : paths) { - if (hasEnoughDistance( - path, road_lanes, shoulder_lanes, current_pose, isInGoalRouteSection, goal_pose, - overall_graphs)) { - available_paths.push_back(path); - } - } - - return available_paths; -} - -bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, - [[maybe_unused]] const Pose & current_pose, [[maybe_unused]] const Twist & current_twist, - [[maybe_unused]] const double vehicle_width, const PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, PullOutPath * selected_path) -{ - const bool use_dynamic_object = ros_parameters.use_dynamic_object; - for (const auto & path : paths) { - if (isPullOutPathSafe( - path, road_lanes, shoulder_lanes, dynamic_objects, ros_parameters, - local_vehicle_footprint, true, use_dynamic_object)) { - *selected_path = path; - return true; - } - } - - // set first path for force pullover if no valid path found - if (!paths.empty()) { - *selected_path = paths.front(); - return false; - } - - return false; -} - -bool hasEnoughDistance( - const PullOutPath & path, const lanelet::ConstLanelets & road_lanes, - [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - [[maybe_unused]] const lanelet::routing::RoutingGraphContainer & overall_graphs) +// getShoulderLanesOnCurrentPose +lanelet::ConstLanelets getPullOutLanes( + const lanelet::ConstLanelets & current_lanes, + const std::shared_ptr & planner_data) { - const double pull_out_prepare_distance = path.preparation_length; - const double pull_out_distance = path.pull_out_length; - const double pull_out_total_distance = pull_out_prepare_distance + pull_out_distance; - - if (pull_out_total_distance > util::getDistanceToEndOfLane(current_pose, road_lanes)) { - return false; - } - - // if (pull_out_total_distance > - // util::getDistanceToNextIntersection(current_pose, current_lanes)) { - // return false; - // } - - if ( - isInGoalRouteSection && - pull_out_total_distance > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { - return false; - } - - // if ( - // pull_out_total_distance > - // util::getDistanceToCrosswalk(current_pose, current_lanes, overall_graphs)) { - // return false; - // } - return true; -} - -bool isPullOutPathSafe( - const behavior_path_planner::PullOutPath & path, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const bool use_buffer, - const bool use_dynamic_object) -{ - // TODO(sugahara) check road lanes safety and output road lanes safety - // and shoulder lanes safety respectively - if (path.path.points.empty()) { - return false; - } - if (shoulder_lanes.empty() || road_lanes.empty()) { - return false; - } - if (dynamic_objects == nullptr) { - return true; + const double pull_out_lane_length = 200.0; + lanelet::ConstLanelets shoulder_lanes; + const auto & route_handler = planner_data->route_handler; + const auto current_pose = planner_data->self_pose->pose; + lanelet::ConstLanelet shoulder_lane; + + if (current_lanes.empty()) { + return shoulder_lanes; } - const double min_thresh = ros_parameters.min_stop_distance; + // Get pull out lanes + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet( + current_lanes, planner_data->self_pose->pose, ¤t_lane); - double buffer; - // double lateral_buffer; - if (use_buffer) { - buffer = ros_parameters.hysteresis_buffer_distance; - // lateral_buffer = 0.5; - } else { - buffer = 0.0; - // lateral_buffer = 0.0; + if (route_handler->getPullOutStartLane( + route_handler->getShoulderLanelets(), current_pose, planner_data->parameters.vehicle_width, + &shoulder_lane)) { + shoulder_lanes = route_handler->getShoulderLaneletSequence( + shoulder_lane, current_pose, pull_out_lane_length, pull_out_lane_length); } - // find obstacle in shoulder lanes - const auto shoulder_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); - - // Collision check for objects in shoulder lane - if (use_dynamic_object) { - for (const auto & i : shoulder_lane_object_indices) { - const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - - bool is_object_in_shoulder = false; - if (ros_parameters.use_predicted_path_outside_lanelet) { - is_object_in_shoulder = true; - } else { - for (const auto & llt : shoulder_lanes) { - if (lanelet::utils::isInLanelet( - obj.kinematics.initial_pose_with_covariance.pose, llt, 0.1)) { - is_object_in_shoulder = true; - } - } - } - // TODO(sugahara) static object judge - if (is_object_in_shoulder) { - const double distance = util::getDistanceBetweenPredictedPathAndObjectPolygon( - obj, path, local_vehicle_footprint, 1, road_lanes); - - double thresh = min_thresh + buffer; - if (distance < thresh) { - return false; - } - } else { - const double distance = util::getDistanceBetweenPredictedPathAndObjectPolygon( - obj, path, local_vehicle_footprint, 1, road_lanes); - double thresh = min_thresh + buffer; - if (distance < thresh) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "object is not in shoulder but close to the path."); - // return false; - } - } - } - } - - return true; -} - -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) -{ - tf2::Transform tf_map2ego, tf_map2obj; - Pose obj_from_ego; - tf2::fromMsg(ego_pose, tf_map2ego); - tf2::fromMsg(obj_pose, tf_map2obj); - tf2::toMsg(tf_map2ego.inverse() * tf_map2obj, obj_from_ego); - - return obj_from_ego.position.x > 0; + return shoulder_lanes; } } // namespace behavior_path_planner::pull_out_utils diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index 964e2e0ebcd2c..f13f753164256 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -414,12 +414,12 @@ bool isPullOverPathSafe( // find obstacle in pull_over target lanes // retrieve lanes that are merging target lanes as well const auto target_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, target_lanes); + util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); // find objects in current lane - const auto current_lane_object_indices_lanelet = util::filterObjectsByLanelets( + const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); - const auto current_lane_object_indices = util::filterObjectsByPath( + const auto current_lane_object_indices = util::filterObjectsIndicesByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index 273f0e4b3a9f1..ffaf8312bcf91 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -63,7 +63,7 @@ void PathShifter::setShiftPoints(const std::vector & points) bool PathShifter::generate( ShiftedPath * shifted_path, const bool offset_back, const SHIFT_TYPE type) { - RCLCPP_DEBUG_STREAM(logger_, "PathShifter::generate start!"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "PathShifter::generate start!"); // Guard if (reference_path_.points.empty()) { @@ -75,7 +75,8 @@ bool PathShifter::generate( shifted_path->shift_length.resize(reference_path_.points.size(), 0.0); if (shift_points_.empty()) { - RCLCPP_DEBUG_STREAM(logger_, "shift_points_ is empty. Return reference with base offset."); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, clock_, 3000, "shift_points_ is empty. Return reference with base offset."); shiftBaseLength(shifted_path, base_offset_); return true; } @@ -85,8 +86,8 @@ bool PathShifter::generate( for (const auto & shift_point : shift_points_) { int idx_gap = shift_point.end_idx - shift_point.start_idx; if (idx_gap <= 1) { - RCLCPP_WARN_STREAM( - logger_, + RCLCPP_WARN_STREAM_THROTTLE( + logger_, clock_, 3000, "shift start point and end point can't be adjoining " "Maybe shift length is too short?"); return false; @@ -96,15 +97,15 @@ bool PathShifter::generate( // Sort shift points since applyShift function only supports sorted points if (!sortShiftPointsAlongPath(reference_path_)) { - RCLCPP_ERROR_STREAM(logger_, "has duplicated points. Failed!"); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, clock_, 3000, "has duplicated points. Failed!"); return false; } if (shift_points_.front().start_idx == 0) { // if offset is applied on front side, shifting from first point is no problem if (offset_back) { - RCLCPP_WARN_STREAM( - logger_, + RCLCPP_WARN_STREAM_THROTTLE( + logger_, clock_, 3000, "shift start point is at the edge of path. It could cause undesired result." " Maybe path is too short for backward?"); } @@ -118,8 +119,9 @@ bool PathShifter::generate( motion_utils::insertOrientation(shifted_path->path.points, is_driving_forward); // DEBUG - RCLCPP_DEBUG_STREAM( - logger_, "PathShifter::generate end. shift_points_.size = " << shift_points_.size()); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, clock_, 3000, + "PathShifter::generate end. shift_points_.size = " << shift_points_.size()); return true; } @@ -282,7 +284,7 @@ bool PathShifter::checkShiftPointsAlignment(const ShiftPointArray & shift_points bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId & path) { if (shift_points_.empty()) { - RCLCPP_DEBUG_STREAM(logger_, "shift_points_ is empty. do nothing."); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "shift_points_ is empty. do nothing."); return true; } @@ -312,13 +314,14 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId // Debug for (const auto & p : unsorted_shift_points) { - RCLCPP_DEBUG_STREAM(logger_, "unsorted_shift_points: " << toStr(p)); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "unsorted_shift_points: " << toStr(p)); } for (const auto & i : sorted_indices) { - RCLCPP_DEBUG_STREAM(logger_, "sorted_indices i = " << i); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "sorted_indices i = " << i); } for (const auto & p : sorted_shift_points) { - RCLCPP_DEBUG_STREAM(logger_, "sorted_shift_points: in order: " << toStr(p)); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, clock_, 3000, "sorted_shift_points: in order: " << toStr(p)); } RCLCPP_DEBUG(logger_, "PathShifter::sortShiftPointsAlongPath end."); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index ecc963316c2e1..1e6d796c0d953 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -499,34 +499,6 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t_query, Pose * le return false; } -bool lerpByDistance( - const behavior_path_planner::PullOutPath & path, const double & s, Pose * lerped_pt, - const lanelet::ConstLanelets & road_lanes) -{ - if (lerped_pt == nullptr) { - // ROS_WARN_STREAM_THROTTLE(1, "failed to lerp by distance due to nullptr pt"); - return false; - } - - for (size_t i = 1; i < path.path.points.size(); i++) { - const auto & pt = path.path.points.at(i).point.pose; - const auto & prev_pt = path.path.points.at(i - 1).point.pose; - if ( - (s >= lanelet::utils::getArcCoordinates(road_lanes, prev_pt).length) && - (s < lanelet::utils::getArcCoordinates(road_lanes, pt).length)) { - const double distance = lanelet::utils::getArcCoordinates(road_lanes, pt).length - - lanelet::utils::getArcCoordinates(road_lanes, prev_pt).length; - const auto offset = s - lanelet::utils::getArcCoordinates(road_lanes, prev_pt).length; - const auto ratio = offset / distance; - *lerped_pt = lerpByPose(prev_pt, pt, ratio); - return true; - } - } - - // ROS_ERROR_STREAM("Something failed in function: " << __func__); - return false; -} - double getDistanceBetweenPredictedPaths( const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, const double end_time, const double resolution) @@ -578,43 +550,40 @@ double getDistanceBetweenPredictedPathAndObject( return min_distance; } -double getDistanceBetweenPredictedPathAndObjectPolygon( - const PredictedObject & object, const PullOutPath & ego_path, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, - const lanelet::ConstLanelets & road_lanes) +bool checkCollisionBetweenPathFootprintsAndObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) { - double min_distance = std::numeric_limits::max(); - - const auto ego_path_point_array = convertToGeometryPointArray(ego_path.path); - Polygon2d obj_polygon; - if (!calcObjectPolygon(object, &obj_polygon)) { - // ROS_ERROR("calcObjectPolygon failed"); - return min_distance; + for (const auto & p : ego_path.points) { + if (checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, p.point.pose, dynamic_objects, margin)) { + return true; + } } - const auto s_start = - lanelet::utils::getArcCoordinates(road_lanes, ego_path.path.points.front().point.pose).length; - const auto s_end = lanelet::utils::getArcCoordinates(road_lanes, ego_path.shift_point.end).length; + return false; +} - for (auto s = s_start + distance_resolution; s < s_end; s += distance_resolution) { - Pose ego_pose; - if (!lerpByDistance(ego_path, s, &ego_pose, road_lanes)) { - // ROS_ERROR("lerp failed"); +bool checkCollisionBetweenFootprintAndObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const PredictedObjects & dynamic_objects, const double margin) +{ + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + + for (const auto & object : dynamic_objects.objects) { + Polygon2d obj_polygon; + if (!calcObjectPolygon(object, &obj_polygon)) { continue; } - const auto vehicle_footprint_on_path = - transformVector(vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); - Point2d ego_point{ego_pose.position.x, ego_pose.position.y}; - for (const auto & vehicle_footprint : vehicle_footprint_on_path) { - double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); - if (distance < min_distance) { - min_distance = distance; - } - } + + const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); + if (distance < margin) return true; } - return min_distance; + return false; } + // only works with consecutive lanes -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, const double start_arc_length, const double end_arc_length) { @@ -655,7 +624,7 @@ std::vector filterObjectsByLanelets( } // works with random lanelets -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { std::vector indices; @@ -699,6 +668,17 @@ std::vector filterObjectsByLanelets( return indices; } +PredictedObjects filterObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects filtered_objects; + const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets); + for (const size_t i : indices) { + filtered_objects.objects.push_back(objects.objects.at(i)); + } + return filtered_objects; +} + bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon) { if (object.shape.type == Shape::BOUNDING_BOX) { @@ -744,7 +724,7 @@ std::vector calcObjectsDistanceToPath( return distance_array; } -std::vector filterObjectsByPath( +std::vector filterObjectsIndicesByPath( const PredictedObjects & objects, const std::vector & object_indices, const PathWithLaneId & ego_path, const double vehicle_width) { diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 85aaf9c303ea3..9b9a034b27511 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -250,9 +250,9 @@ class RouteHandler const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; bool getPullOverTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; - bool getPullOutStart( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, - const Pose & pose, const double vehicle_width) const; + bool getPullOutStartLane( + const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, + lanelet::ConstLanelet * target_lanelet) const; double getLaneChangeableDistance( const Pose & current_pose, const LaneChangeDirection & direction) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 607e3c7b590aa..2a9306238db09 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1142,9 +1142,9 @@ bool RouteHandler::getPullOverTarget( return false; } -bool RouteHandler::getPullOutStart( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, - const Pose & pose, const double vehicle_width) const +bool RouteHandler::getPullOutStartLane( + const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, + lanelet::ConstLanelet * target_lanelet) const { for (const auto & shoulder_lanelet : lanelets) { if (lanelet::utils::isInLanelet(pose, shoulder_lanelet, vehicle_width / 2.0)) { From 81ce496b3a2de0d80ebec51a81763435168d9f85 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 2 Sep 2022 20:38:12 +0900 Subject: [PATCH 05/28] fix(interpolation): use slerp for spherical_linear_interpolation (#1769) * fix(interpolation): use slerp for spherical_linear_interpolation Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- common/interpolation/README.md | 2 +- .../spherical_linear_interpolation.hpp | 4 +- .../interpolation/spline_interpolation.hpp | 4 +- .../spline_interpolation_points_2d.hpp | 6 +-- .../src/spherical_linear_interpolation.cpp | 6 +-- .../src/spline_interpolation.cpp | 4 +- .../src/spline_interpolation_points_2d.cpp | 16 +++---- .../test_spherical_linear_interpolation.cpp | 16 +++---- .../test/src/test_spline_interpolation.cpp | 48 +++++++++---------- .../include/motion_common/motion_common.hpp | 10 ++-- .../src/motion_common/motion_common.cpp | 6 +-- common/motion_common/test/interpolation.cpp | 8 ++-- common/motion_utils/src/resample/resample.cpp | 10 ++-- .../src/predicted_path_utils.cpp | 4 +- control/trajectory_follower/src/mpc_utils.cpp | 12 ++--- .../src/path_generator.cpp | 20 ++++---- .../src/scene_module/utils/path_shifter.cpp | 2 +- .../src/mpt_optimizer.cpp | 6 +-- .../obstacle_avoidance_planner/src/node.cpp | 2 +- .../obstacle_avoidance_planner/src/utils.cpp | 16 +++---- 20 files changed, 101 insertions(+), 101 deletions(-) diff --git a/common/interpolation/README.md b/common/interpolation/README.md index a56af1e864e32..5801c06dcb717 100644 --- a/common/interpolation/README.md +++ b/common/interpolation/README.md @@ -12,7 +12,7 @@ Then it calculates interpolated values on y-axis for `query_keys` on x-axis. ## Spline Interpolation -`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. Then it calculates interpolated values on y-axis for `query_keys` on x-axis. ### Evaluation of calculation cost diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 58cb65a2f198f..516b3ab09e8b7 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -31,11 +31,11 @@ namespace interpolation { -geometry_msgs::msg::Quaternion spherical_linear_interpolation( +geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, const double ratio); -std::vector spherical_linear_interpolation( +std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index c56498249e91f..9640bcd6c5120 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -46,10 +46,10 @@ struct MultiSplineCoef }; // static spline interpolation functions -std::vector slerp( +std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); -std::vector slerpByAkima( +std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); } // namespace interpolation diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index c1f08a6d937ae..e1a60c86f036a 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -22,7 +22,7 @@ namespace interpolation { template -std::vector slerpYawFromPoints(const std::vector & points); +std::vector splineYawFromPoints(const std::vector & points); } // namespace interpolation // non-static points spline interpolation @@ -66,8 +66,8 @@ class SplineInterpolationPoints2d private: void calcSplineCoefficientsInner(const std::vector & points); - SplineInterpolation slerp_x_; - SplineInterpolation slerp_y_; + SplineInterpolation spline_x_; + SplineInterpolation spline_y_; std::vector base_s_vec_; }; diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index 1fc562032a990..014e9011e2a61 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -16,7 +16,7 @@ namespace interpolation { -geometry_msgs::msg::Quaternion spherical_linear_interpolation( +geometry_msgs::msg::Quaternion slerp( const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, const double ratio) { @@ -28,7 +28,7 @@ geometry_msgs::msg::Quaternion spherical_linear_interpolation( return tf2::toMsg(interpolated_quat); } -std::vector spherical_linear_interpolation( +std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) @@ -50,7 +50,7 @@ std::vector spherical_linear_interpolation( const double ratio = (query_key - base_keys.at(key_index)) / (base_keys.at(key_index + 1) - base_keys.at(key_index)); - const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio); + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); query_values.push_back(interpolated_quat); } diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index ebf7c1ddb18bd..cf00452f1d850 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -81,7 +81,7 @@ inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma namespace interpolation { -std::vector slerp( +std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { @@ -94,7 +94,7 @@ std::vector slerp( return interpolator.getSplineInterpolatedValues(query_keys); } -std::vector slerpByAkima( +std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 2a34e53fc884e..2570e01790487 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -69,7 +69,7 @@ std::array, 3> getBaseValues( namespace interpolation { template -std::vector slerpYawFromPoints(const std::vector & points) +std::vector splineYawFromPoints(const std::vector & points) { SplineInterpolationPoints2d interpolator; @@ -84,7 +84,7 @@ std::vector slerpYawFromPoints(const std::vector & points) } return yaw_vec; } -template std::vector slerpYawFromPoints( +template std::vector splineYawFromPoints( const std::vector & points); } // namespace interpolation @@ -103,8 +103,8 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin whole_s = base_s_vec_.back(); } - const double x = slerp_x_.getSplineInterpolatedValues({whole_s}).at(0); - const double y = slerp_y_.getSplineInterpolatedValues({whole_s}).at(0); + const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0); + const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0); geometry_msgs::msg::Point geom_point; geom_point.x = x; @@ -126,8 +126,8 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c whole_s = base_s_vec_.back(); } - const double diff_x = slerp_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); - const double diff_y = slerp_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); return std::atan2(diff_y, diff_x); } @@ -150,6 +150,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( const auto & base_y_vec = base.at(2); // calculate spline coefficients - slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); + spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); + spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); } diff --git a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/interpolation/test/src/test_spherical_linear_interpolation.cpp index cfa881fb76648..0bb9ba8ef2ce8 100644 --- a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp +++ b/common/interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -32,9 +32,9 @@ inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( } } // namespace -TEST(spherical_linear_interpolation, slerp_scalar) +TEST(slerp, spline_scalar) { - using interpolation::spherical_linear_interpolation; + using interpolation::slerp; // Same value { @@ -46,7 +46,7 @@ TEST(spherical_linear_interpolation, slerp_scalar) const auto ans_quat = createQuaternionFromRPY(0.0, 0.0, 0.0); for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { - const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio); + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); @@ -62,7 +62,7 @@ TEST(spherical_linear_interpolation, slerp_scalar) const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw); for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { - const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio); + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); const double ans_yaw = M_PI * ratio; tf2::Quaternion ans; @@ -77,9 +77,9 @@ TEST(spherical_linear_interpolation, slerp_scalar) } } -TEST(spherical_linear_interpolation, slerp_vector) +TEST(slerp, spline_vector) { - using interpolation::spherical_linear_interpolation; + using interpolation::slerp; // query keys are same as base keys { @@ -92,7 +92,7 @@ TEST(spherical_linear_interpolation, slerp_vector) const std::vector query_keys = base_keys; const auto ans = base_values; - const auto results = spherical_linear_interpolation(base_keys, base_values, query_keys); + const auto results = slerp(base_keys, base_values, query_keys); for (size_t i = 0; i < results.size(); ++i) { const auto interpolated_quat = results.at(i); @@ -122,7 +122,7 @@ TEST(spherical_linear_interpolation, slerp_vector) ans.at(4) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0 + 3.0 * M_PI / 5.0); ans.at(5) = createQuaternionFromRPY(0.0, 0.0, 0.8 * M_PI / 5.0 + 3.0 * M_PI / 5.0); - const auto results = spherical_linear_interpolation(base_keys, base_values, query_keys); + const auto results = slerp(base_keys, base_values, query_keys); for (size_t i = 0; i < results.size(); ++i) { const auto interpolated_quat = results.at(i); diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index 132b0100ff3b5..c2bb8eb177ba1 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -23,7 +23,7 @@ constexpr double epsilon = 1e-6; -TEST(spline_interpolation, slerp) +TEST(spline_interpolation, spline) { { // straight: query_keys is same as base_keys const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; @@ -31,7 +31,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -43,7 +43,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -55,7 +55,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -67,7 +67,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -79,7 +79,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -91,7 +91,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -103,7 +103,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.808769, -0.077539, 1.035096}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -115,14 +115,14 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } } -TEST(spline_interpolation, slerpByAkima) +TEST(spline_interpolation, splineByAkima) { { // straight: query_keys is same as base_keys const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; @@ -130,7 +130,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -142,7 +142,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -154,7 +154,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -166,7 +166,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.0801, 1.110749, 1.4864}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -178,7 +178,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -190,7 +190,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -202,7 +202,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.8378, -0.0801, 0.927031}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -214,14 +214,14 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } } -TEST(spline_interpolation, slerpYawFromPoints) +TEST(spline_interpolation, splineYawFromPoints) { using tier4_autoware_utils::createPoint; @@ -235,7 +235,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -251,7 +251,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -261,7 +261,7 @@ TEST(spline_interpolation, slerpYawFromPoints) std::vector points; points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(interpolation::slerpYawFromPoints(points), std::logic_error); + EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) @@ -271,7 +271,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -285,7 +285,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } diff --git a/common/motion_common/include/motion_common/motion_common.hpp b/common/motion_common/include/motion_common/motion_common.hpp index e73cff3ed6e3f..2742b1c073bda 100644 --- a/common/motion_common/include/motion_common/motion_common.hpp +++ b/common/motion_common/include/motion_common/motion_common.hpp @@ -159,11 +159,11 @@ T interpolate(T a, T b, RealT t) } /// Spherical linear interpolation -MOTION_COMMON_PUBLIC Orientation slerp(const Orientation & a, const Orientation & b, const Real t); +MOTION_COMMON_PUBLIC Orientation spline(const Orientation & a, const Orientation & b, const Real t); /// Trajectory point interpolation template -Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) +Point interpolate(Point a, Point b, Real t, SlerpF spline_fn) { Point ret{rosidl_runtime_cpp::MessageInitialization::ALL}; { @@ -173,7 +173,7 @@ Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) } ret.pose.position.x = interpolate(a.pose.position.x, b.pose.position.x, t); ret.pose.position.y = interpolate(a.pose.position.y, b.pose.position.y, t); - ret.pose.orientation = slerp_fn(a.pose.orientation, b.pose.orientation, t); + ret.pose.orientation = spline_fn(a.pose.orientation, b.pose.orientation, t); ret.longitudinal_velocity_mps = interpolate(a.longitudinal_velocity_mps, b.longitudinal_velocity_mps, t); ret.lateral_velocity_mps = interpolate(a.lateral_velocity_mps, b.lateral_velocity_mps, t); @@ -191,7 +191,7 @@ MOTION_COMMON_PUBLIC Point interpolate(Point a, Point b, Real t); /// Sample a trajectory using interpolation; does not extrapolate template void sample( - const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF slerp_fn) + const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF spline_fn) { out.points.clear(); out.header = in.header; @@ -230,7 +230,7 @@ void sample( const auto dt_ = std::chrono::duration_cast>( ref_duration - from_message(curr_pt.time_from_start)); const auto t = dt_.count() / dt.count(); - out.points.push_back(interpolate(curr_pt, next_pt, t, slerp_fn)); + out.points.push_back(interpolate(curr_pt, next_pt, t, spline_fn)); } ref_duration += period; } diff --git a/common/motion_common/src/motion_common/motion_common.cpp b/common/motion_common/src/motion_common/motion_common.cpp index aaedabfd83b9a..7ad71096c53f8 100644 --- a/common/motion_common/src/motion_common/motion_common.cpp +++ b/common/motion_common/src/motion_common/motion_common.cpp @@ -153,7 +153,7 @@ Heading nlerp(Heading a, Heading b, Real t) } //////////////////////////////////////////////////////////////////////////////// -Orientation slerp(const Orientation & a, const Orientation & b, const Real t) +Orientation spline(const Orientation & a, const Orientation & b, const Real t) { tf2::Quaternion quat_a; tf2::Quaternion quat_b; @@ -163,12 +163,12 @@ Orientation slerp(const Orientation & a, const Orientation & b, const Real t) } //////////////////////////////////////////////////////////////////////////////// -Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, slerp); } +Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, spline); } //////////////////////////////////////////////////////////////////////////////// void sample(const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period) { - sample(in, out, period, slerp); + sample(in, out, period, spline); } //////////////////////////////////////////////////////////////////////////////// diff --git a/common/motion_common/test/interpolation.cpp b/common/motion_common/test/interpolation.cpp index e59d9e0af9346..ae57e5ea76ba6 100644 --- a/common/motion_common/test/interpolation.cpp +++ b/common/motion_common/test/interpolation.cpp @@ -109,8 +109,8 @@ TEST(Interpolation, Slerp2d) }; // Plain check const auto test_case = [=](Orientation a, Orientation b, Real t, Orientation res, Real tol) { - using motion::motion_common::slerp; - const auto ret = slerp(a, b, t); + using motion::motion_common::spline; + const auto ret = spline(a, b, t); EXPECT_LT(std::fabs(to_angle(ret) - to_angle(res)), tol) << to_angle(ret) << ", " << to_angle(res); }; @@ -123,10 +123,10 @@ TEST(Interpolation, Slerp2d) const auto t_ = motion::motion_common::clamp(t, 0.0F, 1.0F); const auto th_t = th_a + (t_ * ab); const auto res_th = from_angle(th_t); - using motion::motion_common::slerp; + using motion::motion_common::spline; test_case(a, b, t, res_th, tol); if (HasFailure()) { - const auto ret = slerp(a, b, t); + const auto ret = spline(a, b, t); std::cout << "Angles: " << th_a << ", " << th_b << "; " << th_t << ", " << to_angle(ret) << "\n"; } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index affe832f6b221..7f050a4454ffe 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -66,13 +66,13 @@ std::vector resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; - const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_arclength, input, resampled_arclength); + const auto spline = [&](const auto & input) { + return interpolation::spline(input_arclength, input, resampled_arclength); }; - const auto interpolated_x = use_lerp_for_xy ? lerp(x) : slerp(x); - const auto interpolated_y = use_lerp_for_xy ? lerp(y) : slerp(y); - const auto interpolated_z = use_lerp_for_z ? lerp(z) : slerp(z); + const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x); + const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); + const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); std::vector resampled_points; resampled_points.resize(interpolated_x.size()); diff --git a/common/perception_utils/src/predicted_path_utils.cpp b/common/perception_utils/src/predicted_path_utils.cpp index 0eeed0a71bba2..96ca74d2cb234 100644 --- a/common/perception_utils/src/predicted_path_utils.cpp +++ b/common/perception_utils/src/predicted_path_utils.cpp @@ -66,10 +66,10 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return interpolation::lerp(input_time, input, resampled_time); }; const auto spline = [&](const auto & input) { - return interpolation::slerp(input_time, input, resampled_time); + return interpolation::spline(input_time, input, resampled_time); }; const auto slerp = [&](const auto & input) { - return interpolation::spherical_linear_interpolation(input_time, input, resampled_time); + return interpolation::slerp(input_time, input, resampled_time); }; const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index a56dc36a007de..6fce85d88827c 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -100,13 +100,13 @@ bool8_t resampleMPCTrajectoryByDistance( std::vector input_yaw = input.yaw; convertEulerAngleToMonotonic(&input_yaw); - output->x = interpolation::slerp(input_arclength, input.x, output_arclength); - output->y = interpolation::slerp(input_arclength, input.y, output_arclength); - output->z = interpolation::slerp(input_arclength, input.z, output_arclength); - output->yaw = interpolation::slerp(input_arclength, input.yaw, output_arclength); + output->x = interpolation::spline(input_arclength, input.x, output_arclength); + output->y = interpolation::spline(input_arclength, input.y, output_arclength); + output->z = interpolation::spline(input_arclength, input.z, output_arclength); + output->yaw = interpolation::spline(input_arclength, input.yaw, output_arclength); output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength); - output->k = interpolation::slerp(input_arclength, input.k, output_arclength); - output->smooth_k = interpolation::slerp(input_arclength, input.smooth_k, output_arclength); + output->k = interpolation::spline(input_arclength, input.k, output_arclength); + output->smooth_k = interpolation::spline(input_arclength, input.smooth_k, output_arclength); output->relative_time = interpolation::lerp(input_arclength, input.relative_time, output_arclength); diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4d594f46c8af8..093bc5c76f3b8 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -309,28 +309,28 @@ PosePath PathGenerator::interpolateReferencePath( } // Spline Interpolation - std::vector slerp_ref_path_x = - interpolation::slerp(base_path_s, base_path_x, resampled_s); - std::vector slerp_ref_path_y = - interpolation::slerp(base_path_s, base_path_y, resampled_s); - std::vector slerp_ref_path_z = - interpolation::slerp(base_path_s, base_path_z, resampled_s); + std::vector spline_ref_path_x = + interpolation::spline(base_path_s, base_path_x, resampled_s); + std::vector spline_ref_path_y = + interpolation::spline(base_path_s, base_path_y, resampled_s); + std::vector spline_ref_path_z = + interpolation::spline(base_path_s, base_path_z, resampled_s); interpolated_path.resize(interpolate_num); for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - tier4_autoware_utils::createPoint(slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), 0.0); + tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); const auto next_point = tier4_autoware_utils::createPoint( - slerp_ref_path_x.at(i + 1), slerp_ref_path_y.at(i + 1), 0.0); + spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); interpolated_pose.position = tier4_autoware_utils::createPoint( - slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), slerp_ref_path_z.at(i)); + spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } interpolated_path.back().position = tier4_autoware_utils::createPoint( - slerp_ref_path_x.back(), slerp_ref_path_y.back(), slerp_ref_path_z.back()); + spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; return interpolated_path; diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index ffaf8312bcf91..d89cf8ade1e9d 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -203,7 +203,7 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs query_distance.push_back(dist_from_start); } if (!query_distance.empty()) { - query_length = interpolation::slerp(base_distance, base_length, query_distance); + query_length = interpolation::spline(base_distance, base_length, query_distance); } // Apply shifting. diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 3cb66bb7a5087..1dea0206252eb 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -178,7 +178,7 @@ Eigen::Vector2d getState( return kinematics; } -std::vector slerpYawFromReferencePoints(const std::vector & ref_points) +std::vector splineYawFromReferencePoints(const std::vector & ref_points) { if (ref_points.size() == 1) { return {ref_points.front().yaw}; @@ -188,7 +188,7 @@ std::vector slerpYawFromReferencePoints(const std::vector MPTOptimizer::get void MPTOptimizer::calcOrientation(std::vector & ref_points) const { - const auto yaw_angles = slerpYawFromReferencePoints(ref_points); + const auto yaw_angles = splineYawFromReferencePoints(ref_points); for (size_t i = 0; i < ref_points.size(); ++i) { if (ref_points.at(i).fix_kinematic_state) { continue; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 3802dde0e067a..0bd3bf97dd541 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -181,7 +181,7 @@ std::tuple, std::vector> calcVehicleCirclesInfo( for (const auto & traj_point : traj_points) { points.push_back(traj_point.pose.position); } - const auto yaw_vec = interpolation::slerpYawFromPoints(points); + const auto yaw_vec = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < traj_points.size(); ++i) { traj_points.at(i).pose.orientation = diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 90e9daccb0a89..c45d7c808586d 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -110,7 +110,7 @@ std::array, 2> validatePoints( } // only two points is supported -std::vector slerpTwoPoints( +std::vector splineTwoPoints( std::vector base_s, std::vector base_x, const double begin_diff, const double end_diff, std::vector new_s) { @@ -259,8 +259,8 @@ std::vector interpolate2DPoints( } // spline interpolation - const std::vector interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const std::vector interpolated_x = interpolation::spline(base_s, base_x, new_s); + const std::vector interpolated_y = interpolation::spline(base_s, base_y, new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { return std::vector{}; @@ -296,9 +296,9 @@ std::vector interpolateConnec // spline interpolation const auto interpolated_x = - slerpTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); + splineTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); const auto interpolated_y = - slerpTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); + splineTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); for (size_t i = 0; i < interpolated_x.size(); i++) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { @@ -343,9 +343,9 @@ std::vector interpolate2DTraj const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); // spline interpolation - const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); + const auto interpolated_x = interpolation::spline(base_s, base_x, new_s); + const auto interpolated_y = interpolation::spline(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::spline(base_s, monotonic_base_yaw, new_s); for (size_t i = 0; i < interpolated_x.size(); i++) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { From d61f62214d8f22d6e7ec43541a3b01693334f64a Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Sat, 3 Sep 2022 00:46:44 +0300 Subject: [PATCH 06/28] refactor(motion_velocity_smoother): change curvature calculation (#1638) * refactor(motion_velocity_smoother): change curvature calculation Signed-off-by: Berkay Karaman * ci(pre-commit): autofix * add error handler for too close points Signed-off-by: Berkay Karaman * ci(pre-commit): autofix * take copy for first and last points Signed-off-by: Berkay Karaman * ci(pre-commit): autofix Signed-off-by: Berkay Karaman Co-authored-by: Berkay Karaman Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/trajectory_utils.cpp | 35 ++++++++++++------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 0647600ba3e73..629ba1b15040a 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -186,23 +186,32 @@ std::vector calcTrajectoryCurvatureFrom3Points( } // calculate curvature by circle fitting from three points - std::vector k_arr; - for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) { + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); try { - const auto p0 = getPoint(trajectory.at(i - idx_dist)); - const auto p1 = getPoint(trajectory.at(i)); - const auto p2 = getPoint(trajectory.at(i + idx_dist)); - k_arr.push_back(calcCurvature(p0, p1, p2)); - } catch (...) { - k_arr.push_back(0.0); // points are too close. No curvature. + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", + e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } } + k_arr.at(i) = curvature; } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); - // first and last curvature is copied from next value - for (size_t i = 0; i < idx_dist; ++i) { - k_arr.insert(k_arr.begin(), k_arr.front()); - k_arr.push_back(k_arr.back()); - } return k_arr; } From 0c902931e912a3a714a1bc675a61f5d4b01d1a1e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 3 Sep 2022 14:07:50 +0900 Subject: [PATCH 07/28] chore(behavior_path_planner): fix pull_over typo (#1771) --- .../scene_module/pull_over/pull_over_module.hpp | 2 +- .../src/scene_module/pull_over/pull_over_module.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index ec9689841d8d4..0159c1a5956fe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -194,7 +194,7 @@ class PullOverModule : public SceneModuleInterface std::pair getSafePath(ShiftParkingPath & safe_path) const; Pose getRefinedGoal() const; Pose getParkingStartPose() const; - ParallelParkingParameters getGeometricPullOutParameters() const; + ParallelParkingParameters getGeometricPullOverParameters() const; bool isLongEnoughToParkingStart( const PathWithLaneId & path, const Pose & parking_start_pose) const; bool isLongEnough( 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 49c72ea815057..91f127a536e2c 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 @@ -85,7 +85,7 @@ BehaviorModuleOutput PullOverModule::run() return plan(); } -ParallelParkingParameters PullOverModule::getGeometricPullOutParameters() const +ParallelParkingParameters PullOverModule::getGeometricPullOverParameters() const { ParallelParkingParameters params; @@ -130,7 +130,7 @@ void PullOverModule::onEntry() last_received_time_.get() == nullptr || *last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status - parallel_parking_parameters_ = getGeometricPullOutParameters(); + parallel_parking_parameters_ = getGeometricPullOverParameters(); resetStatus(); } From 728875e4a0220f1a921efef3615e23880b96c023 Mon Sep 17 00:00:00 2001 From: v-nakayama7440-esol <97144416+v-nakayama7440-esol@users.noreply.github.com> Date: Mon, 5 Sep 2022 06:05:11 +0900 Subject: [PATCH 08/28] feat: add CRC error monitoring to net_monitor (#638) * feat: add CRC error monitoring to net_monitor Signed-off-by: noriyuki.h Signed-off-by: v-nakayama7440-esol * add CRC error monitoring information to README.md Signed-off-by: v-nakayama7440-esol * ci(pre-commit): autofix Signed-off-by: noriyuki.h Signed-off-by: v-nakayama7440-esol Co-authored-by: noriyuki.h Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../system_monitor/net_monitor.param.yaml | 2 + .../diagnostic_aggregator/system.param.yaml | 6 + system/system_monitor/README.md | 45 +-- .../config/net_monitor.param.yaml | 2 + system/system_monitor/docs/ros_parameters.md | 10 +- .../system_monitor/docs/topics_net_monitor.md | 18 ++ .../net_monitor/net_monitor.hpp | 110 +++++++- .../src/net_monitor/net_monitor.cpp | 264 +++++++++++++----- 8 files changed, 350 insertions(+), 107 deletions(-) diff --git a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml index f8dc832d7b2f2..e91aa4ca3fbb3 100644 --- a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml @@ -3,3 +3,5 @@ devices: ["*"] traffic_reader_port: 7636 monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index af6b9ab8a64c2..290a272eb2605 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -122,6 +122,12 @@ contains: [": Network Traffic"] timeout: 3.0 + netowork_crc_error: + type: diagnostic_aggregator/GenericAnalyzer + path: network_crc_error + contains: [": Network CRC Error"] + timeout: 3.0 + storage: type: diagnostic_aggregator/AnalyzerGroup path: storage diff --git a/system/system_monitor/README.md b/system/system_monitor/README.md index 79f390a80d7e7..6a776a51bb6d0 100644 --- a/system/system_monitor/README.md +++ b/system/system_monitor/README.md @@ -53,28 +53,29 @@ Every topic is published in 1 minute interval. [Usage] ✓：Supported, -：Not supported -| Node | Message | Intel | arm64(tegra) | arm64(raspi) | Notes | -| --------------- | ---------------------- | :---: | :----------: | :----------: | ------------------------------------------------------------- | -| CPU Monitor | CPU Temperature | ✓ | ✓ | ✓ | | -| | CPU Usage | ✓ | ✓ | ✓ | | -| | CPU Load Average | ✓ | ✓ | ✓ | | -| | CPU Thermal Throttling | ✓ | - | ✓ | | -| | CPU Frequency | ✓ | ✓ | ✓ | Notification of frequency only, normally error not generated. | -| HDD Monitor | HDD Temperature | ✓ | ✓ | ✓ | | -| | HDD PowerOnHours | ✓ | ✓ | ✓ | | -| | HDD TotalDataWritten | ✓ | ✓ | ✓ | | -| | HDD Usage | ✓ | ✓ | ✓ | | -| Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | -| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | -| NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | -| Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | -| | High-load Proc[0-9] | ✓ | ✓ | ✓ | | -| | High-mem Proc[0-9] | ✓ | ✓ | ✓ | | -| GPU Monitor | GPU Temperature | ✓ | ✓ | - | | -| | GPU Usage | ✓ | ✓ | - | | -| | GPU Memory Usage | ✓ | - | - | | -| | GPU Thermal Throttling | ✓ | - | - | | -| | GPU Frequency | - | ✓ | - | | +| Node | Message | Intel | arm64(tegra) | arm64(raspi) | Notes | +| --------------- | ---------------------- | :---: | :----------: | :----------: | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| CPU Monitor | CPU Temperature | ✓ | ✓ | ✓ | | +| | CPU Usage | ✓ | ✓ | ✓ | | +| | CPU Load Average | ✓ | ✓ | ✓ | | +| | CPU Thermal Throttling | ✓ | - | ✓ | | +| | CPU Frequency | ✓ | ✓ | ✓ | Notification of frequency only, normally error not generated. | +| HDD Monitor | HDD Temperature | ✓ | ✓ | ✓ | | +| | HDD PowerOnHours | ✓ | ✓ | ✓ | | +| | HDD TotalDataWritten | ✓ | ✓ | ✓ | | +| | HDD Usage | ✓ | ✓ | ✓ | | +| Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | +| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | +| | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | +| NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | +| Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | +| | High-load Proc[0-9] | ✓ | ✓ | ✓ | | +| | High-mem Proc[0-9] | ✓ | ✓ | ✓ | | +| GPU Monitor | GPU Temperature | ✓ | ✓ | - | | +| | GPU Usage | ✓ | ✓ | - | | +| | GPU Memory Usage | ✓ | - | - | | +| | GPU Thermal Throttling | ✓ | - | - | | +| | GPU Frequency | - | ✓ | - | | ## ROS parameters diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/system_monitor/config/net_monitor.param.yaml index 953d32d788ccf..686ee349b0765 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/system_monitor/config/net_monitor.param.yaml @@ -3,3 +3,5 @@ devices: ["*"] traffic_reader_port: 7636 monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 diff --git a/system/system_monitor/docs/ros_parameters.md b/system/system_monitor/docs/ros_parameters.md index 779297492e00a..044c1eb10a5d0 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/system_monitor/docs/ros_parameters.md @@ -53,10 +53,12 @@ mem_monitor: net_monitor: -| Name | Type | Unit | Default | Notes | -| :--------- | :----------: | :-----: | :-----: | :----------------------------------------------------------------------------------- | -| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | -| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | +| Name | Type | Unit | Default | Notes | +| :------------------------ | :----------: | :-----: | :-----: | :-------------------------------------------------------------------------------------------------------------- | +| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | +| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | +| crc_error_check_duration | int | sec | 1 | CRC error check duration. | +| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/system_monitor/docs/topics_net_monitor.md index 36fdeea890e6a..261cede53de21 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/system_monitor/docs/topics_net_monitor.md @@ -63,3 +63,21 @@ | key | value (example) | | ----- | ----------------------------------------------------- | | error | [nethogs -t] execve failed: No such file or directory | + +## Network CRC Error + +/diagnostics/net_monitor: Network CRC Error + +[summary] + +| level | message | +| ----- | ------- | +| OK | OK | + +[values] + +| key | value (example) | +| ------------------------------------------ | --------------- | +| Network [0-9]: interface name | wlp82s0 | +| Network [0-9]: total rx_crc_errors | 0 | +| Network [0-9]: rx_crc_errors per unit time | 0 | diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 8aafb4b08b083..53f3e1250c9f0 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -55,11 +56,6 @@ class NetMonitor : public rclcpp::Node */ ~NetMonitor(); - /** - * @brief Update the diagnostic state. - */ - void update(); - /** * @brief Shutdown nl80211 object */ @@ -86,6 +82,15 @@ class NetMonitor : public rclcpp::Node void monitorTraffic( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief check CRC error + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkCrcError( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** * @brief get wireless speed * @param [in] ifa_name interface name @@ -93,17 +98,104 @@ class NetMonitor : public rclcpp::Node */ float getWirelessSpeed(const char * ifa_name); + /** + * @brief timer callback + */ + void onTimer(); + + /** + * @brief update Network information list + */ + void updateNetworkInfoList(); + + /** + * @brief check NetMonitor General Infomation + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @return check result + */ + bool checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** + * @brief Network information + */ + struct NetworkInfo + { + int mtu_errno; //!< @brief errno set by ioctl() with SIOCGIFMTU + int ethtool_errno; //!< @brief errno set by ioctl() with SIOCETHTOOL + bool is_running; //!< @brief resource allocated flag + std::string interface_name; //!< @brief interface name + float speed; //!< @brief network capacity + int mtu; //!< @brief MTU + float rx_traffic; //!< @brief traffic received + float tx_traffic; //!< @brief traffic transmitted + float rx_usage; //!< @brief network capacity usage rate received + float tx_usage; //!< @brief network capacity usage rate transmitted + unsigned int rx_bytes; //!< @brief total bytes received + unsigned int rx_errors; //!< @brief bad packets received + unsigned int tx_bytes; //!< @brief total bytes transmitted + unsigned int tx_errors; //!< @brief packet transmit problems + unsigned int collisions; //!< @brief number of collisions during packet transmissions + + NetworkInfo() + : mtu_errno(0), + ethtool_errno(0), + is_running(false), + interface_name(""), + speed(0.0), + mtu(0), + rx_traffic(0.0), + tx_traffic(0.0), + rx_usage(0.0), + tx_usage(0.0), + rx_bytes(0), + rx_errors(0), + tx_bytes(0), + tx_errors(0), + collisions(0) + { + } + }; + + /** + * @brief determine if it is a supported network + * @param [in] net_info network infomation + * @param [in] index index of network infomation index + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @param [out] error_str error string + * @return result of determining whether it is a supported network + */ + bool isSupportedNetwork( + const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, + std::string & error_str); + diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics + rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name std::map bytes_; //!< @brief list of bytes rclcpp::Time last_update_time_; //!< @brief last update time std::vector device_params_; //!< @brief list of devices - NL80211 nl80211_; // !< @brief 802.11 netlink-based interface + NL80211 nl80211_; //!< @brief 802.11 netlink-based interface + int getifaddrs_errno_; //!< @brief errno set by getifaddrs() + std::vector net_info_list_; //!< @brief list of Network information - std::string monitor_program_; //!< @brief nethogs monitor program name - bool nethogs_all_; //!< @brief nethogs result all mode - int traffic_reader_port_; //!< @brief port number to connect to traffic_reader + /** + * @brief CRC errors information + */ + typedef struct crc_errors + { + std::deque errors_queue; //!< @brief queue that holds count of CRC errors + unsigned int last_rx_crc_errors; //!< @brief rx_crc_error at the time of the last monitoring + + crc_errors() : last_rx_crc_errors(0) {} + } crc_errors; + std::map crc_errors_; //!< @brief list of CRC errors + + std::string monitor_program_; //!< @brief nethogs monitor program name + bool nethogs_all_; //!< @brief nethogs result all mode + int traffic_reader_port_; //!< @brief port number to connect to traffic_reader + unsigned int crc_error_check_duration_; //!< @brief CRC error check duration + unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold /** * @brief Network usage status messages diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index c1c06ee6f1d4d..983af5668a082 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -47,8 +47,12 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) device_params_( declare_parameter>("devices", std::vector())), monitor_program_(declare_parameter("monitor_program", "greengrass")), - traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)) + traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)), + crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), + crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)) { + using namespace std::literals::chrono_literals; + if (monitor_program_.empty()) { monitor_program_ = GET_ALL_STR; nethogs_all_ = true; @@ -60,23 +64,27 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("Network Usage", this, &NetMonitor::checkUsage); updater_.add("Network Traffic", this, &NetMonitor::monitorTraffic); + updater_.add("Network CRC Error", this, &NetMonitor::checkCrcError); nl80211_.init(); + + // get Network information for the first time + updateNetworkInfoList(); + + timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::onTimer, this)); } NetMonitor::~NetMonitor() { shutdown_nl80211(); } -void NetMonitor::update() { updater_.force_update(); } - void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } -void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::onTimer() { updateNetworkInfoList(); } + +void NetMonitor::updateNetworkInfoList() { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); + net_info_list_.clear(); if (device_params_.empty()) { - stat.summary(DiagStatus::ERROR, "invalid device parameter"); return; } @@ -86,22 +94,12 @@ void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) rclcpp::Duration duration = this->now() - last_update_time_; // Get network interfaces + getifaddrs_errno_ = 0; if (getifaddrs(&ifas) < 0) { - stat.summary(DiagStatus::ERROR, "getifaddrs error"); - stat.add("getifaddrs", strerror(errno)); + getifaddrs_errno_ = errno; return; } - int level = DiagStatus::OK; - int whole_level = DiagStatus::OK; - int index = 0; - std::string error_str; - float rx_traffic{0.0}; - float tx_traffic{0.0}; - float rx_usage{0.0}; - float tx_usage{0.0}; - std::vector interface_names; - for (ifa = ifas; ifa; ifa = ifa->ifa_next) { // Skip no addr if (!ifa->ifa_addr) { @@ -127,90 +125,117 @@ void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) struct ifreq ifrc; struct ethtool_cmd edata; + net_info_list_.emplace_back(); + auto & net_info = net_info_list_.back(); + + net_info.interface_name = std::string(ifa->ifa_name); + // Get MTU information fd = socket(AF_INET, SOCK_DGRAM, 0); strncpy(ifrm.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); if (ioctl(fd, SIOCGIFMTU, &ifrm) < 0) { - if (errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), ifa->ifa_name); - stat.add("ioctl(SIOCGIFMTU)", strerror(errno)); - - ++index; + net_info.mtu_errno = errno; close(fd); - interface_names.push_back(ifa->ifa_name); continue; } // Get network capacity - float speed = 0.0; strncpy(ifrc.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); ifrc.ifr_data = (caddr_t)&edata; edata.cmd = ETHTOOL_GSET; if (ioctl(fd, SIOCETHTOOL, &ifrc) < 0) { // possibly wireless connection, get bitrate(MBit/s) - speed = nl80211_.getBitrate(ifa->ifa_name); - if (speed <= 0) { - if (errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), ifa->ifa_name); - stat.add("ioctl(SIOCETHTOOL)", strerror(errno)); - - ++index; + net_info.speed = nl80211_.getBitrate(ifa->ifa_name); + if (net_info.speed <= 0) { + net_info.ethtool_errno = errno; close(fd); - interface_names.push_back(ifa->ifa_name); continue; } } else { - speed = edata.speed; + net_info.speed = edata.speed; } - level = (ifa->ifa_flags & IFF_RUNNING) ? DiagStatus::OK : DiagStatus::ERROR; + net_info.is_running = (ifa->ifa_flags & IFF_RUNNING); auto * stats = (struct rtnl_link_stats *)ifa->ifa_data; - if (bytes_.find(ifa->ifa_name) != bytes_.end()) { - rx_traffic = toMbit(stats->rx_bytes - bytes_[ifa->ifa_name].rx_bytes) / duration.seconds(); - tx_traffic = toMbit(stats->tx_bytes - bytes_[ifa->ifa_name].tx_bytes) / duration.seconds(); - rx_usage = rx_traffic / speed; - tx_usage = tx_traffic / speed; + if (bytes_.find(net_info.interface_name) != bytes_.end()) { + net_info.rx_traffic = + toMbit(stats->rx_bytes - bytes_[net_info.interface_name].rx_bytes) / duration.seconds(); + net_info.tx_traffic = + toMbit(stats->tx_bytes - bytes_[net_info.interface_name].tx_bytes) / duration.seconds(); + net_info.rx_usage = net_info.rx_traffic / net_info.speed; + net_info.tx_usage = net_info.tx_traffic / net_info.speed; } - stat.add(fmt::format("Network {}: status", index), usage_dict_.at(level)); - stat.add(fmt::format("Network {}: interface name", index), ifa->ifa_name); - stat.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", rx_usage * 1e+2); - stat.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", tx_usage * 1e+2); - stat.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", rx_traffic); - stat.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", tx_traffic); - stat.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", speed); - stat.add(fmt::format("Network {}: mtu", index), ifrm.ifr_mtu); - stat.add(fmt::format("Network {}: rx_bytes", index), stats->rx_bytes); - stat.add(fmt::format("Network {}: rx_errors", index), stats->rx_errors); - stat.add(fmt::format("Network {}: tx_bytes", index), stats->tx_bytes); - stat.add(fmt::format("Network {}: tx_errors", index), stats->tx_errors); - stat.add(fmt::format("Network {}: collisions", index), stats->collisions); + net_info.mtu = ifrm.ifr_mtu; + net_info.rx_bytes = stats->rx_bytes; + net_info.rx_errors = stats->rx_errors; + net_info.tx_bytes = stats->tx_bytes; + net_info.tx_errors = stats->tx_errors; + net_info.collisions = stats->collisions; close(fd); - bytes_[ifa->ifa_name].rx_bytes = stats->rx_bytes; - bytes_[ifa->ifa_name].tx_bytes = stats->tx_bytes; - - ++index; + bytes_[net_info.interface_name].rx_bytes = stats->rx_bytes; + bytes_[net_info.interface_name].tx_bytes = stats->tx_bytes; - interface_names.push_back(ifa->ifa_name); + // Get the count of CRC errors + crc_errors & crc_ers = crc_errors_[net_info.interface_name]; + crc_ers.errors_queue.push_back(stats->rx_crc_errors - crc_ers.last_rx_crc_errors); + while (crc_ers.errors_queue.size() > crc_error_check_duration_) { + crc_ers.errors_queue.pop_front(); + } + crc_ers.last_rx_crc_errors = stats->rx_crc_errors; } freeifaddrs(ifas); + last_update_time_ = this->now(); +} + +void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + if (!checkGeneralInfo(stat)) { + return; + } + + int level = DiagStatus::OK; + int whole_level = DiagStatus::OK; + int index = 0; + std::string error_str; + std::vector interface_names; + + for (const auto & net_info : net_info_list_) { + if (!isSupportedNetwork(net_info, index, stat, error_str)) { + ++index; + interface_names.push_back(net_info.interface_name); + continue; + } + + level = net_info.is_running ? DiagStatus::OK : DiagStatus::ERROR; + + stat.add(fmt::format("Network {}: status", index), usage_dict_.at(level)); + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", net_info.rx_usage * 1e+2); + stat.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", net_info.tx_usage * 1e+2); + stat.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", net_info.rx_traffic); + stat.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", net_info.tx_traffic); + stat.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", net_info.speed); + stat.add(fmt::format("Network {}: mtu", index), net_info.mtu); + stat.add(fmt::format("Network {}: rx_bytes", index), net_info.rx_bytes); + stat.add(fmt::format("Network {}: rx_errors", index), net_info.rx_errors); + stat.add(fmt::format("Network {}: tx_bytes", index), net_info.tx_bytes); + stat.add(fmt::format("Network {}: tx_errors", index), net_info.tx_errors); + stat.add(fmt::format("Network {}: collisions", index), net_info.collisions); + + ++index; + + interface_names.push_back(net_info.interface_name); + } + // Check if specified device exists for (const auto & device : device_params_) { // Skip if all devices specified @@ -234,12 +259,107 @@ void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) stat.summary(whole_level, usage_dict_.at(whole_level)); } - last_update_time_ = this->now(); + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, stat); +} + +void NetMonitor::checkCrcError(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + if (!checkGeneralInfo(stat)) { + return; + } + + int whole_level = DiagStatus::OK; + int index = 0; + std::string error_str; + + for (const auto & net_info : net_info_list_) { + if (!isSupportedNetwork(net_info, index, stat, error_str)) { + ++index; + continue; + } + + crc_errors & crc_ers = crc_errors_[net_info.interface_name]; + unsigned int unit_rx_crc_errors = 0; + + for (auto errors : crc_ers.errors_queue) { + unit_rx_crc_errors += errors; + } + + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.add(fmt::format("Network {}: total rx_crc_errors", index), crc_ers.last_rx_crc_errors); + stat.add(fmt::format("Network {}: rx_crc_errors per unit time", index), unit_rx_crc_errors); + + if (unit_rx_crc_errors >= crc_error_count_threshold_) { + whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); + error_str = "CRC error"; + } + + ++index; + } + + if (!error_str.empty()) { + stat.summary(whole_level, error_str); + } else { + stat.summary(whole_level, "OK"); + } // Measure elapsed time since start time and report SystemMonitorUtility::stopMeasurement(t_start, stat); } +bool NetMonitor::checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (device_params_.empty()) { + stat.summary(DiagStatus::ERROR, "invalid device parameter"); + return false; + } + + if (getifaddrs_errno_ != 0) { + stat.summary(DiagStatus::ERROR, "getifaddrs error"); + stat.add("getifaddrs", strerror(getifaddrs_errno_)); + return false; + } + return true; +} + +bool NetMonitor::isSupportedNetwork( + const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, + std::string & error_str) +{ + // MTU information + if (net_info.mtu_errno != 0) { + if (net_info.mtu_errno == ENOTSUP) { + stat.add(fmt::format("Network {}: status", index), "Not Supported"); + } else { + stat.add(fmt::format("Network {}: status", index), "Error"); + error_str = "ioctl error"; + } + + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.add("ioctl(SIOCGIFMTU)", strerror(net_info.mtu_errno)); + return false; + } + + // network capacity + if (net_info.speed <= 0) { + if (net_info.ethtool_errno == ENOTSUP) { + stat.add(fmt::format("Network {}: status", index), "Not Supported"); + } else { + stat.add(fmt::format("Network {}: status", index), "Error"); + error_str = "ioctl error"; + } + + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.add("ioctl(SIOCETHTOOL)", strerror(net_info.ethtool_errno)); + return false; + } + return true; +} + #include // workaround for build errors void NetMonitor::monitorTraffic(diagnostic_updater::DiagnosticStatusWrapper & stat) From fd9677c7a3397d16f65679201a3d3f418571ab4e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 5 Sep 2022 10:25:03 +0900 Subject: [PATCH 09/28] feat(behavior_velocity_planner): replace Polygon2d with common's one (#1767) * feat(behavior_velocity_planner): replace Polygon2d with common's one Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../geometry/boost_polygon_utils.hpp | 2 +- .../src/geometry/boost_polygon_utils.cpp | 7 ++ .../intersection/scene_intersection.hpp | 18 --- .../include/scene_module/run_out/scene.hpp | 10 +- .../utilization/boost_geometry_helper.hpp | 118 ++---------------- .../include/utilization/util.hpp | 2 - .../intersection/scene_intersection.cpp | 38 ++---- .../scene_no_stopping_area.cpp | 8 +- .../occlusion_spot/grid_utils.cpp | 4 + .../occlusion_spot/occlusion_spot_utils.cpp | 8 +- .../src/scene_module/run_out/scene.cpp | 10 +- .../src/scene_module/run_out/utils.cpp | 1 + .../src/utilization/util.cpp | 15 +-- .../test/src/test_grid_utils.cpp | 3 + .../test/src/test_utilization.cpp | 8 -- 15 files changed, 56 insertions(+), 196 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 835f0c37d3031..8bbfe681fa3ae 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -18,7 +18,6 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -// #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -35,6 +34,7 @@ geometry_msgs::msg::Polygon rotatePolygon( const geometry_msgs::msg::Polygon & polygon, const double & angle); Polygon2d toPolygon2d( const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index f9ba0f537f07f..b54f888ad0ae1 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -167,6 +167,13 @@ Polygon2d toPolygon2d( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } +tier4_autoware_utils::Polygon2d toPolygon2d( + const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + return tier4_autoware_utils::toPolygon2d( + object.kinematics.initial_pose_with_covariance.pose, object.shape); +} + double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) { if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index bffd25f1f5002..3cb169c981e0f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -239,24 +239,6 @@ class IntersectionModule : public SceneModuleInterface bool isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - /** - * @brief convert object to footprint polygon - * @param object detected object - * @return 2d polygon of the object footprint - */ - Polygon2d toFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - - /** - * @brief convert predicted object to footprint polygon - * @param object detected object - * @param predicted_pose predicted object pose - * @return 2d polygon of the object footprint - */ - Polygon2d toPredictedFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const geometry_msgs::msg::Pose & predicted_pose) const; - /** * @brief Whether target tier4_api_msgs::Intersection::status is valid or not * @param target_status target tier4_api_msgs::Intersection::status diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp index a6e99c6d503c3..bb6fc7c237418 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp @@ -90,15 +90,15 @@ class RunOutModule : public SceneModuleInterface const float velocity_mps) const; bool checkCollisionWithShape( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const Shape & shape, std::vector & collision_points) const; + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + std::vector & collision_points) const; bool checkCollisionWithCylinder( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const float radius, std::vector & collision_points) const; + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius, + std::vector & collision_points) const; bool checkCollisionWithBoundingBox( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const geometry_msgs::msg::Vector3 & dimension, std::vector & collision_points) const; diff --git a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp index ffbab904aaf85..51296737ea768 100644 --- a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp @@ -15,6 +15,8 @@ #ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#include + #include #include #include @@ -33,7 +35,6 @@ #include #include #include -#include #include #include @@ -60,16 +61,16 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( namespace behavior_velocity_planner { -using Point2d = boost::geometry::model::d2::point_xy; -using Segment2d = boost::geometry::model::segment; -using LineString2d = boost::geometry::model::linestring; -using Polygon2d = - boost::geometry::model::polygon; // counter-clockwise, open +namespace bg = boost::geometry; + +using Point2d = tier4_autoware_utils::Point2d; +using LineString2d = tier4_autoware_utils::LineString2d; +using Polygon2d = tier4_autoware_utils::Polygon2d; template Point2d to_bg2d(const T & p) { - return Point2d(boost::geometry::get<0>(p), boost::geometry::get<1>(p)); + return Point2d(bg::get<0>(p), bg::get<1>(p)); } template @@ -82,17 +83,6 @@ LineString2d to_bg2d(const std::vector & vec) return ps; } -inline Polygon2d linestring2polygon(const LineString2d & line_string) -{ - Polygon2d polygon; - - for (const auto & p : line_string) { - polygon.outer().push_back(p); - } - - return polygon; -} - inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) { Polygon2d polygon; @@ -107,78 +97,11 @@ inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2 polygon.outer().push_back(*itr); } + bg::correct(polygon); return polygon; } -inline Polygon2d obj2polygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & shape) -{ - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double h = shape.x; - const double w = shape.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( - -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); - - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} - -inline double calcOverlapAreaRate(const Polygon2d & target, const Polygon2d & base) -{ - /* OverlapAreaRate: common area(target && base) / target area */ - - if (boost::geometry::within(target, base)) { - // target is within base, common area = target area - return 1.0; - } - - if (!boost::geometry::intersects(target, base)) { - // target and base has not intersect area - return 0.0; - } - - // calculate intersect polygon - std::vector intersects; - boost::geometry::intersection(target, base, intersects); - - // calculate area of polygon - double intersect_area = 0.0; - for (const auto & intersect : intersects) { - intersect_area += boost::geometry::area(intersect); - } - const double target_area = boost::geometry::area(target); - // specification of boost1.65 - // common area is not intersect area - const double common_area = target_area - intersect_area; - - return common_area / target_area; -} - -inline std::vector makeSegments(const LineString2d & ls) -{ - std::vector segments; - for (size_t i = 0; i < ls.size(); ++i) { - segments.emplace_back(ls.at(i), ls.at(i + 1)); - } - return segments; -} - -inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon) +inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) { geometry_msgs::msg::Polygon polygon_msg; geometry_msgs::msg::Point32 point_msg; @@ -189,27 +112,6 @@ inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon) } return polygon_msg; } - -inline Polygon2d toBoostPoly(const geometry_msgs::msg::Polygon & polygon) -{ - Polygon2d boost_poly; - for (const auto & point : polygon.points) { - const Point2d point2d(point.x, point.y); - boost_poly.outer().push_back(point2d); - } - return boost_poly; -} - -inline Polygon2d toBoostPoly(const lanelet::BasicPolygon2d & polygon) -{ - Polygon2d boost_poly; - for (const auto & vec : polygon) { - const Point2d point2d(vec.x(), vec.y()); - boost_poly.outer().push_back(point2d); - } - - return boost_poly; -} } // namespace behavior_velocity_planner #endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index c05a8819e9267..22c75ddeaab50 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -79,7 +79,6 @@ struct PointWithSearchRangeIndex using geometry_msgs::msg::Pose; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using Point2d = boost::geometry::model::d2::point_xy; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::Shape; @@ -162,7 +161,6 @@ geometry_msgs::msg::Pose transformAbsCoordinate2D( const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id); -Polygon2d toFootprintPolygon(const PredictedObject & object); bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, 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 fee2eb22b3fef..1e326f4975cd0 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 @@ -293,7 +293,7 @@ bool IntersectionModule::checkCollision( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point), &closest_lanelet); - debug_data_.ego_lane_polygon = toGeomMsg(ego_poly); + debug_data_.ego_lane_polygon = toGeomPoly(ego_poly); /* extract target objects */ autoware_auto_perception_msgs::msg::PredictedObjects target_objects; @@ -403,12 +403,14 @@ bool IntersectionModule::checkCollision( polygon.outer().emplace_back(polygon.outer().front()); - debug_data_.candidate_collision_ego_lane_polygon = toGeomMsg(polygon); + bg::correct(polygon); + + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = toPredictedFootprintPolygon(object, *itr); + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); debug_data_.candidate_collision_object_polygons.emplace_back( - toGeomMsg(footprint_polygon)); + toGeomPoly(footprint_polygon)); if (bg::intersects(polygon, footprint_polygon)) { collision_detected = true; break; @@ -464,6 +466,7 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( } polygon.outer().emplace_back(polygon.outer().front()); + bg::correct(polygon); return polygon; } @@ -538,7 +541,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area) const { - debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); for (const auto & object : objects_ptr->objects) { if (!isTargetStuckVehicleType(object)) { @@ -550,7 +553,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = toFootprintPolygon(object); + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); @@ -561,27 +564,6 @@ bool IntersectionModule::checkStuckVehicleInIntersection( return false; } -Polygon2d IntersectionModule::toFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const -{ - Polygon2d obj_footprint; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - obj_footprint = toBoostPoly(object.shape.footprint); - } else { - // cylinder type is treated as square-polygon - obj_footprint = - obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); - } - return obj_footprint; -} - -Polygon2d IntersectionModule::toPredictedFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const geometry_msgs::msg::Pose & predicted_pose) const -{ - return obj2polygon(predicted_pose, object.shape.dimensions); -} - bool IntersectionModule::isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const { @@ -794,7 +776,7 @@ bool IntersectionModule::checkFrontVehicleDeceleration( predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - Polygon2d predicted_obj_footprint = toFootprintPolygon(predicted_object); + auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); debug_data_.predicted_obj_pose.position = stopping_point; debug_data_.predicted_obj_pose.orientation = diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index b753fd36b8b5d..0472cc4aae720 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -162,8 +162,8 @@ bool NoStoppingAreaModule::modifyPathVelocity( setSafe(true); return true; } - debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area); - debug_data_.stop_line_detect_area = toGeomMsg(stop_line_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area); // Find stuck vehicle in no stopping area const bool is_entry_prohibited_by_stuck_vehicle = checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr); @@ -230,11 +230,11 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( continue; // not stop vehicle } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); + const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); - for (const auto p : obj_footprint.outer()) { + for (const auto & p : obj_footprint.outer()) { geometry_msgs::msg::Point point; point.x = p.x(); point.y = p.y(); 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 fa02d33fa0198..66d82418a0520 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 @@ -36,6 +36,8 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle)); // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; + + bg::correct(line_poly); return line_poly; } @@ -173,6 +175,8 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, 0, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); + + bg::correct(poly); return poly; } 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 58cc2d2c6f813..3000fe6d1e042 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 @@ -209,9 +209,9 @@ void categorizeVehicles( stuck_vehicle_foot_prints.clear(); for (const auto & vehicle : vehicles) { if (isMovingVehicle(vehicle, stuck_vehicle_vel)) { - moving_vehicle_foot_prints.emplace_back(planning_utils::toFootprintPolygon(vehicle)); + moving_vehicle_foot_prints.emplace_back(tier4_autoware_utils::toPolygon2d(vehicle)); } else if (isStuckVehicle(vehicle, stuck_vehicle_vel)) { - stuck_vehicle_foot_prints.emplace_back(planning_utils::toFootprintPolygon(vehicle)); + stuck_vehicle_foot_prints.emplace_back(tier4_autoware_utils::toPolygon2d(vehicle)); } } return; @@ -219,7 +219,7 @@ void categorizeVehicles( ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) { - Polygon2d poly = planning_utils::toFootprintPolygon(obj); + const auto poly = tier4_autoware_utils::toPolygon2d(obj); std::deque arcs; for (const auto & p : poly.outer()) { lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; @@ -342,7 +342,7 @@ std::vector filterVehiclesByDetectionArea( // stuck points by predicted objects for (const auto & object : objs) { // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); for (const auto & p : polys) { if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index cf8c78d9e8930..9f629927036f8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -412,8 +412,8 @@ boost::optional RunOutModule::calcPredictedObstaclePos } bool RunOutModule::checkCollisionWithShape( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const Shape & shape, std::vector & collision_points) const + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + std::vector & collision_points) const { bool collision_detected = false; switch (shape.type) { @@ -439,8 +439,8 @@ bool RunOutModule::checkCollisionWithShape( } bool RunOutModule::checkCollisionWithCylinder( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const float radius, std::vector & collision_points) const + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius, + std::vector & collision_points) const { // create bounding box for min and max velocity point const auto bounding_box_for_points = @@ -504,7 +504,7 @@ std::vector RunOutModule::createBoundingBoxForRangedP } bool RunOutModule::checkCollisionWithBoundingBox( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const geometry_msgs::msg::Vector3 & dimension, std::vector & collision_points) const { diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp index 2c035705afd05..0f6921691025f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -195,6 +195,7 @@ Polygon2d createBoostPolyFromMsg(const std::vector & const auto & first_point = input_poly.front(); bg_poly.outer().emplace_back(bg::make(first_point.x, first_point.y)); + bg::correct(bg_poly); return bg_poly; } diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 1ed17cd4be341..9179a53ab854e 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -227,19 +227,6 @@ void insertVelocity( setVelocityFromIndex(insert_index, v, &path); } -Polygon2d toFootprintPolygon(const PredictedObject & object) -{ - Polygon2d obj_footprint; - if (object.shape.type == Shape::POLYGON) { - obj_footprint = toBoostPoly(object.shape.footprint); - } else { - // cylinder type is treated as square-polygon - obj_footprint = - obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); - } - return obj_footprint; -} - bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) { geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); @@ -296,6 +283,8 @@ Polygon2d generatePathPolygon( const double y = path.points.at(i).point.pose.position.y + width * std::cos(yaw); ego_area.outer().push_back(Point2d(x, y)); } + + bg::correct(ego_area); return ego_area; } diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp index 441d6b94bbc03..19e9b5f37b028 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -43,6 +43,7 @@ using behavior_velocity_planner::Point2d; using behavior_velocity_planner::Polygon2d; using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; +namespace bg = boost::geometry; Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) { @@ -57,6 +58,8 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle)); // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; + + bg::correct(line_poly); return line_poly; } diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/test/src/test_utilization.cpp index 1a9d4cdbf4aed..742cccecb9773 100644 --- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/test/src/test_utilization.cpp @@ -33,14 +33,6 @@ std::cerr << ss.str() << std::endl; \ } -TEST(to_footprint_polygon, nominal) -{ - using behavior_velocity_planner::planning_utils::toFootprintPolygon; - autoware_auto_perception_msgs::msg::PredictedObject obj = test::generatePredictedObject(0.0); - auto poly = toFootprintPolygon(obj); - EXPECT_TRUE(true); -} - TEST(is_ahead_of, nominal) { using behavior_velocity_planner::planning_utils::isAheadOf; From 2f379a65e3a9a3f833b7eabf47ef19795dcf3326 Mon Sep 17 00:00:00 2001 From: Xinyu Wang <93699235+angry-crab@users.noreply.github.com> Date: Mon, 5 Sep 2022 10:48:11 +0900 Subject: [PATCH 10/28] fix(obstacle_avoidance_planner, interpolation): improve slerp and fix yaw (#1687) * merge slerp of x y yaw Signed-off-by: Xinyu Wang * ci(pre-commit): autofix * move and rename new slerp Signed-off-by: Xinyu Wang * add include file Signed-off-by: Xinyu Wang Signed-off-by: Xinyu Wang Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../spline_interpolation_points_2d.hpp | 4 ++ .../src/spline_interpolation_points_2d.cpp | 24 ++++++++++ .../obstacle_avoidance_planner/utils.hpp | 4 ++ .../obstacle_avoidance_planner/src/utils.cpp | 44 ++++++++++++++++++- 4 files changed, 75 insertions(+), 1 deletion(-) diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index e1a60c86f036a..d44d16d88dd04 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -21,6 +21,10 @@ namespace interpolation { +std::array, 3> slerp2dFromXY( + const std::vector & base_keys, const std::vector & base_x_values, + const std::vector & base_y_values, const std::vector & query_keys); + template std::vector splineYawFromPoints(const std::vector & points); } // namespace interpolation diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 2570e01790487..71e2629044f11 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -68,6 +68,29 @@ std::array, 3> getBaseValues( namespace interpolation { + +std::array, 3> slerp2dFromXY( + const std::vector & base_keys, const std::vector & base_x_values, + const std::vector & base_y_values, const std::vector & query_keys) +{ + SplineInterpolation interpolator_x, interpolator_y; + std::vector yaw_vec; + + // calculate spline coefficients + interpolator_x.calcSplineCoefficients(base_keys, base_x_values); + interpolator_y.calcSplineCoefficients(base_keys, base_y_values); + const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); + const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); + for (size_t i = 0; i < diff_x.size(); i++) { + double yaw = std::atan2(diff_y[i], diff_x[i]); + yaw_vec.push_back(yaw); + } + // interpolate base_keys at query_keys + return { + interpolator_x.getSplineInterpolatedValues(query_keys), + interpolator_y.getSplineInterpolatedValues(query_keys), yaw_vec}; +} + template std::vector splineYawFromPoints(const std::vector & points) { @@ -86,6 +109,7 @@ std::vector splineYawFromPoints(const std::vector & points) } template std::vector splineYawFromPoints( const std::vector & points); + } // namespace interpolation geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index a3b229ba55276..b21254ef20297 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -18,6 +18,7 @@ #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" @@ -115,6 +116,9 @@ std::vector interpolate2DTraj const std::vector & base_x, const std::vector & base_y, const std::vector & base_yaw, const double resolution); +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution); + template std::vector getInterpolatedPoints( const T & points, const double delta_arc_length, const double offset = 0) diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index c45d7c808586d..f5f030dcd4ac8 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -366,13 +366,55 @@ std::vector interpolate2DTraj return interpolated_points; } +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution) +{ + if (base_x.empty() || base_y.empty()) { + return std::vector{}; + } + std::vector base_s = calcEuclidDist(base_x, base_y); + if (base_s.empty() || base_s.size() == 1) { + return std::vector{}; + } + std::vector new_s; + for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { + new_s.push_back(i); + } + + // spline interpolation + // x = interpolated[0], y = interpolated[1], yaw = interpolated[2] + std::array, 3> interpolated = + interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s); + const auto & interpolated_x = interpolated[0]; + const auto & interpolated_y = interpolated[1]; + const auto & interpolated_yaw = interpolated[2]; + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } + } + + std::vector interpolated_points; + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); + interpolated_points.push_back(point); + } + + return interpolated_points; +} + std::vector getInterpolatedTrajectoryPoints( const std::vector & points, const double delta_arc_length) { std::array, 3> validated_pose = validateTrajectoryPoints(points); return interpolation_utils::interpolate2DTrajectoryPoints( - validated_pose.at(0), validated_pose.at(1), validated_pose.at(2), delta_arc_length); + validated_pose.at(0), validated_pose.at(1), delta_arc_length); } std::vector getConnectedInterpolatedPoints( From ce644b6785db665ab96c1a48a81d00f74760a41c Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Mon, 5 Sep 2022 15:14:03 +0900 Subject: [PATCH 11/28] feat(bag_time_manager_rviz_plugin): add bag time manager rviz plugin (#1776) * feat(bag_timemanager): add bag time manager rviz plugin Signed-off-by: tanaka3 * fix: fix change Signed-off-by: tanaka3 Signed-off-by: tanaka3 --- .../CMakeLists.txt | 28 +++++ common/bag_time_manager_rviz_plugin/README.md | 26 ++++ .../icons/classes/BagTimeManagerPanel.png | Bin 0 -> 561 bytes .../images/add_bag_time_manager_panel.png | Bin 0 -> 82418 bytes .../images/bag_time_manager_panel.png | Bin 0 -> 11149 bytes .../images/select_panels.png | Bin 0 -> 67237 bytes .../bag_time_manager_rviz_plugin/package.xml | 28 +++++ .../plugins/plugin_description.xml | 9 ++ .../src/bag_time_manager_panel.cpp | 117 ++++++++++++++++++ .../src/bag_time_manager_panel.hpp | 72 +++++++++++ 10 files changed, 280 insertions(+) create mode 100644 common/bag_time_manager_rviz_plugin/CMakeLists.txt create mode 100644 common/bag_time_manager_rviz_plugin/README.md create mode 100644 common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png create mode 100644 common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png create mode 100644 common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png create mode 100644 common/bag_time_manager_rviz_plugin/images/select_panels.png create mode 100644 common/bag_time_manager_rviz_plugin/package.xml create mode 100644 common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml create mode 100644 common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp create mode 100644 common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp diff --git a/common/bag_time_manager_rviz_plugin/CMakeLists.txt b/common/bag_time_manager_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..ccf9961fc55cc --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(bag_time_manager_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/bag_time_manager_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/common/bag_time_manager_rviz_plugin/README.md b/common/bag_time_manager_rviz_plugin/README.md new file mode 100644 index 0000000000000..2fad6b2204bb9 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/README.md @@ -0,0 +1,26 @@ +# bag_time_manager_rviz_plugin + +## Purpose + +This plugin allows publishing and controlling the ros bag time. + +## Output + +tbd. + +## HowToUse + +1. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +2. Select BagTimeManagerPanel and press OK. + + ![select_manager_plugin](./images/add_bag_time_manager_panel.png) + +3. See bag_time_manager_rviz_plugin/BagTimeManagerPanel is added. + + ![manager_plugin](./images/bag_time_manager_panel.png) + +- Pause/Resume: pause/resume the clock. +- ApplyRate: apply rate of the clock. diff --git a/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png b/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png new file mode 100644 index 0000000000000000000000000000000000000000..9431c2d4223632e7cf0c3d2c6a6203d8ffbf0af1 GIT binary patch literal 561 zcmV-10?z%3P)Q!bb9uh;93Knc(e6K`;VPnKo9Hw@!?n_y%9 z3Y_}=ezMst>2#V%Bm%&6I;Gd^VVWkkZ3FzUEbFyl7}tJ)P32Qv*D06FM5EEYvawi< zSS&^+lc83tu~;l#>biagyi=Q=e+B{ptx~D%nUzwA$X>F^WJ0xC#kOq+c&2Rzre(9) zqsliCjYi32G5|iHpaD66R4R3=*(04!dpFE!z(Zg=u)E-CMdXp+3m*3Bp_JM?ALv8_ z#sKsA{KzaKM+;y!n|aB{8gL2F?RM|nf~OUcOU-rN3&1f=lgVUqt9d*g({8r`9H8Or z_xpd6$>amqb)OFg0}_b@;c)oB=31@BYPAA5uh;8ed;sxy{3;j>zF4od}o~d-WlVL zJKm4SpQfs-w^Z%D)|_+A3R6~;LPj7!fP#WTmXQ`$g@XEU4Fv_=`WY5-rpIYR9`f-Gvv?b^N&!-F}{n0mW!Ibxr@88lNl7i&feCH(b?3=%*@W&!rtZLBS;8x5xb?j zmW!B^nX!wdz1=_SE&LtFZ*6+F<*)z zJFQ`9U4IO?sAzCk#xCWONFt9D#}Hj01_@()3Zn>=e)ha~^cNO<@SXr?XUskOfeySo z4f}g~dXS8ajelij5%XoC5E2rWmzOJ-sDCC4MBCmo;vkTbm!}{j`<0l8wY|MP1*N5{ zYiep55E23>35=VYQ^&x-xNhqdgS!uUn&e>$fvW?*D2 z;Ma#7(EL%iYTyH%eED~25yfN*JY;k*sZ=2Pe`j8BTP!1+t>vNCI7*x2pIiiYF?JyX z>OxLMdQWbx=UP^3IE*^Xn)x9k&&X3(gE_Rjt(H#Xq8gG#~(|IK?^t@!Y#BqG9rJW&}o}Yl5tuG7p2aUsTnm(%*}tFru%@c;-lZDmA`xuBQb`0L2^X zbp$;&;r(?X%%a;#bz-;Rz&aMj->JY;eG@>)5Aw);O9(R*%t~|y3A;r0=zJ}+GZVa2 zcv=bcST1RYKi>>#h;}UC+a;4Oa0O1oiyU< z$@uA&e&rq{yZW|nVdaW_(@pGi!4X`6NoaZ}jk)SzM`iDl(VOULzJ@XZ7~lFVAH>gY zJN&JbN4-#o7MZ`tOaYe6saMy!k1WCNDv&V(ie}EA4QhW53*jzDek>Aw?G5Z7lhmY^ z<}iy_=R=|JTQ4*vs}Qe27e;NKK=on(BO||eA@*A&Sv0-(+ZWzyD#h<*YO=3{)Z-KC z6$a3pk`t%vQu!@QVj*msVf)vc{y>#{@xRP0!n^xH+-SOUbszdfUY;M}AXf7SrY@#GL5W zt|+7+?A7pgu(my&rGmIk_V@}YM_@_v^A*o1E=RLCWS-E2@>W(W<92Gdc8w%5J>|5D zf6F8siVZ?Wyrcy?l5DkutA0CAJ^Rd*P~f=7usSc{o`(A=YkV)J`UuJptwLDnd*zBk zr8*N>2&4tEd?3H*rqS1`A;=!Rz9nL#DLQkS=)70Imv4RuYJ*9}1#5JSQ@`D=gzU z-;D}d5T7gje%hXm&f|5YU zC&=`YB0}&s20~An92j7=-MY)`NZn3IX3qc28yF~H zK{fZ_%60W{ium?9u0=W^9n#dT(-2FWE)`k!ytng?Z3{T?dFM-6&VNFcK5EFPl_f#X z;6|ylcHmAKUYpIROoSdJ#ClBZ_bsDYs~&jZ&$e7OmbVm4PS;Ql1wjHXuF*K7UArM4rM2Bj4v*}s%1i?jB#O2h;UF|yMrxJGYtdOwW*$(rv&ZAK zpoxCI`dyH^C0k11(Rw){o&6OT5}OfN;GH0$GY=}^%m0@{1*h6_H5y1CSn9GiHsxcd z|DraQ@_+Se!!z?z38D!m_X z&t2k6Wf)|JaUduB-7p&x_3WO?2_9Zvm7(znjJ=PR^8n~F#Z9Ws)xpA(F}tH+AkcK5 z8Q>p$o^%VD_SHr1u)ZT^6A&nUeL_BBU@XD>ry?mM)gcNw>ZE zbUdp4jtWU_y2XQa7SIh|EHZ&t($inND!~)YAw|K=+0f(8dk-q|`p^`_KV*x7)gKaI2D^vmiufl zkhcuHmr~#}E!w{yXU}@1tdr76yY224RHW*COO#xpGoeJv*>^iC-Rf5#^$BWhZACDA zv2~oXb0XxW-i>i~^P0j(4;Pjx;>#+7zmtMvF{V7{yo}pLR4eHi{1zNfrFy|V{X*;0W`oam|r0U)0(_ z1)wzN-$LM5T1K_&F=B}|X;+x*-vpZ-^Z2!#$P~pQSg5F#yAg&Lq#zYr>@V(BIUi1a zm!J2Y;VR9f)ol`T4KMw3U)KE6kT8Xv!5cpl%eNw}p`b4>G|3Q9Kp zJ7T+bIt8-|Xg2DnOf8%bzPZ{b?sK^iC!-7kKKIHnKI8nFKv?^ryrs<<2%ge4O;C%#{~ zUlP+4T6NOl^asWjt1r#Y)%mL`l^DXK;ZATE0KrK;JFu~&KQ)G{TCngdG0{ZL`y!q! zhwV33*Znpc6juu^vC(j#REv3O&#);#y2ULXMF^1;iw$2|McYYm0_l9DZtg$ht0_Wo zvh9BxdoM*MN!D0u^Vr(6%JTcd&Yg^$zcUEqs#67wC1As_7_(*kT7xUvo{1!36e>K)D>#)~ z8iTJ=7OHv-oIaHS-JDEg5m`O&l+47bcVe9iOqgY0a9q;G#%v>!1wN(>?Ew1c9v@Q% zOeXj`vtAMGLEFDJ9D+~6T6h9S0v2BR*eYfhUA$+<{!VaSEl{I-p3^Dj>ccx}<~q3y z4A(~}jG{~(R_J)>?s1J(l&sdVOAz`ifk39TYg?Ek6OJFZoCjIOzM~3??W%RX)nW$g&Hbh36T8id zl|AdVtG%M%D=U4K&^nKsZID5`?lX!e@bz#i7X!6v5W8IAK%1=;TYdI+q5))APFQX}l?gL+Hh}utZo?)N72n65E!k)$I9h z+o}G-Yq#xh-O)pJFQ}3F8wMLdLKP<#B=XcI#LQOj!Lc_n6CbG72J3>x{|11QZ}slI zs2-mFWy9ZF!Z^DnM9;Z!wg7zz!*ZOgPVKIc57$#o|GqpCUi(in#Dd@|hW~dqNd!2M^cLtrCkaFcPD|&C236PAjx_pI1_;p6A?WJr-g*g!JJ}zr zLe`q70DawZeWI$39w$2%5AqO_3u|735te`gci~VAo z%8NKT_tt;8_hpa6KF6v$!=(tacF-o3?3^?tEB^fplM2BbqMucMLfFdx@3~O~X@Sfj^=!3C4dmx5Plb7Dq%-gB&y^{&9uaZp_Gq@RuMg_)>8X4go7rb1q`uKn`W$OWa^E(Kg=`pP zH^@|F?_SP-$i)I6fwbD?NcH}7+4()hak9aBvDxj6fZy%(kKKA}z5OOhl%$X(Wbj}y zj6-zTfM52V;NUvIWZtwzv3?sDr`25Z=F{6jj{oCX)8Qg_dPSE@VcI)guH)h1Vexr%;Bz~bzS{1usju&wnTf}sl%1+El?p~BjSdL-2$;w; z24C+@dMxnUknV;!FF!0NBvPk92CxmI+hj+4vQXi@b&&LB^6&9{8KgXzltRvJhVuF5^Uy)p6dhAQ>jJ|iLmeV^}R za8+BTj>{?@2h*W2HE|va4t$t}boj$pbiUQ}mE|E999Nib#tMKaAY-0fhKzvJDy+&7 zL;`8@W`Epp2wWhM-Cd$q1XXO%;T8Q*q5_14DpJas$P0wY#y&yA!V*_j)-T0v5-V^k z(98N$Z|iEUI?_FqyXANIB86rL#p39wLQ0^{6xP;3f>?o4gmHJseFrz-R;8qks>|RS zo%s29GG17XBy$9B_?)>7efJ_PYXhZfqyqnD6dOtD!SP@E832;`O+MY77;25`>FnaK zwR>WI(Tqu2r}w_Ns&6+Y-+xr>$)mgKpIKIKh&(tvHEtKS_(I9=H&&b@xfOrQ&mMwu zFfQ~qw=xq#Uit$i##*wjM8``pb2N~RD*EkGNQ~d5PsoQmz_(!1;4|5^&S)~D<#b-Z z*kmH$jtG21E|MK4MatWDF#Wu%ois`xu5Sq)aDwsYS640fqi6p&20~E(i2{+z=e);) z`SNEATsX{U<-S`)Lj^&ZhM37q*Gcl?pPF=OyzMpEa*LUOzbu9nBSC1}n^LU{><&&& z&_gbNbp}FS*o990?~cEJvCT_8ka47mOsR+~9>SZMDDN3x$Np}`P}4MyH}O7E^GI=z z#1xgJ7@}AtcCZxxzRu$CnsXxXeh~=Sd)EQ=L+qCi4+oI@MI_)6yWJmgV3895hYGD+ zD4%wq&DjMlhsZ(@wRQqGrDdT4Nj)FT+0Fa2ac13aJoO`xo@p(JBmVeKG(ezjoN&HG zwuq6@7rLbI4(|u2y^$oFQNvE3wCZXWbvo={zkV_Q3D%xXESKiZ_!ONLUJ#l?>i1yS zndmTkV7(661e;8wQjyquEsB!}GvxlEDJi(F$8*MumHP4-TxT_aVfI!ZFxzaQLaz~v za+QIgfS{nEgDt5s?uhqAGH@fmvVJMW9-IQY<3EwqoZf?ItO3v3_>=6hkv++gvujiv-5adela#S#zTUAd3mvb z^zNrgIHk6epkU)fFdJQXA&7Aa#OJ8yygw)pX$};pZy7p>EY#NE_7DR``Tv3FWCtPQ zPTch8PqsL0`6Qch*(}A{;{OXvp7CEMd&0kudejYOlxsCG%>LP^HvUMhM7nd<^i*v! zU+Yu61&q|wdlSlfV*Z;aUk+hV@Aw$I+IaVGpZzj#zFyo;2ox)23k~phJhR<|93?WU zk&#+G(qJy$R~da@k~kN54+}J)F&(3AtXd!K55dvkb~g3I4R4(@=^?G%ZbjP$$^8}x z@LoT9ctbKWGfS}K{i!Wn801lBK3*$Sd_=d&T|MSSV&H20bS^*q9O0>c>?f)2(4|@- zs!~!@?bOp|L(GTt940o+?2NfFR)O66+1D2VL@E3dUEs{>^D4VzS@xr|3?c%;4LR@P zL*^Ly;T5*VWRHzWPhhg^Eg&pB@{f9$Dh1VsL{-yvPteW3OB!ug)bhlKPf<&qa_2R5 zuVOeM%!=V$ZW3(ZQ}2e=@Q+z(_TyG_rSbkcBy_(zefjA8oG8*lIM^_&o5s0Cd~ElW zQ1Lu?Q+jt(CApzeT_v2%cz4rA@{bDs+eE8eXK7DoSOg&)ds!P7@u?oTavdnTgoK5D z{FD!5C_n!%(Nu&C+(RJQP$Fu~PpmOPS+f30e=lxsLrd7)YAcXs{q(nZ(b%w_k5~vC z1ah8F)vRXHNz>u9R%sJa{hefpsBX5GEKcY6=t2N)= z4AxhDC(tAmjP5tcz1i&$U#Gtgyh*4JkLEg}hv#)M^JUAoISz`bshFvl?0b4erP{X zGk*O_FjR-*XQf)z^wIO(Scy?@*60E2laAny5`?y2r+fl`l})4>u6~dbsJFc-G<-Z* zF{9Hg?&*9r_GtFr-cb{XSxDt{Ixm{kCc^Ex{WyELl?&DV3A|C@)j%t+^Gq_zmtSr3 z;xeI0Fg~3FyHy5Duuz0?K>OVN2?xnq`aO?VAZs|ZJ685Mi-w7KeXQwX@}N}h^SHH6 zA+EAozJ{FW@yf;~O5Hej~ZO}$dxYA(=- z6noBIdK7n*7$FHY}CfYdii3Jl+RQ z&LxKPA)TpyE7l#`l-3>k5j|iYcV8PQBvriq%}LTf5@4F ze=vORiBT;TrzIjLErJZeSqV z>^>(%g=W9IRlGNtRjDZ@C9u_orn3TDKQO(uZa#53x-KeCl+r0TymYW=bp7Z_G&`p| z6c|x%uri~2Oc~P-1@?5vVj~dtF4sIP1dO};nGIHj@o1LIPcx`aIWR@Oq9-R-C}T7f zA|;JlHoM&tC0_PUUUZK(rLxDYUr%(+$i@s1DcQVWb}H}QJ|<_+9de|5!bP*SSwf$H zwEK=2ys#kp;+I&+9CKd}F892Vms>ArV1e+6*3@=u?auG7-T_-U4B1L`RO1jrno<4g z4e!%1!Jhd&%5ZYaCH0L@Y!zjIr53wCA)3YemSW-&^>3SoA`OBI-afK-T}{V^?*KF7s8;P9Xdw*gQ8q)ZC+Ehn?INH&Apdq?^c)Yh>#v__cS@kZ&B(Io5_f zwy3+nG9)ZkW>6z}H%T|(Vyjqpp703N5>B?sl>f*N1v5CHw%q82msB-j=Fo=rJHyR* zDQ2|=?P0glyWyhE=Rty=fvZz0LXz?5W**@!)T16hYyC<=JZp92l+3nRm+@y8PVdsm zJ&LgJCZ;{O>KlaU(n4TfOX~*$bo5_9;8y~L`zY|`rGu!bXm+a;bGp*uCs_AN5mA#B zk3;P;KR|c$qGy9@&EQDz*=lc#Gk{iqh7ES@;4Z~S00W=rF7J;wU*;REl=pb-liAz?&Yn|GsGfwf?Oy|9f02;Tbq-CKhU z#qwc{vI|11ce&xOkwmSZ(RH4CUn+wkSTD{LW0zb>iz8b>UDa<90{Y&(cg+j`Ctdes zV$g*h&&V0Su!68AvbMrT+T0waD7M)Ko1!j-ljJ=|x zTgKMEGz>%OfT*#vdzuEJi6=zswZ8%uNeoo_J&a)$)&woDb||x5E3hGz*F%aS=k6H{ z0&b0)w`VD*&V>I!HgT22F~j4Y+x0Y5LnL|4)b%!c_Pi5zzdfd&MrMFcN-gueDbm0w z;?e9jqaPpmlEXDTiICFAm-gH|X@~OrKdx|W_T=66EtN|lSDlu=GwsdeQRheFZOBNF zhM{f0@WQ9m=Gtav!2|EB=@pf^1?X@}18nD74;d1Vj`}ymTH} zSdKRjP!&=dS1Jo?`XJp|9jszos=TBalH@F7<^CQn825l;wLOxKYGDB6ICtZ#Pj>U* ze$Ey~R(W|bHZ28vx$I;7`fo45mWA1`fu3mcDi^>^jN!Fnf~?2t$<#9LR?;~;ua`eD z0cHxW3ox$I4gx_+q6$P&KEe$&^(^jbnS0&L9c4o$n?oyuy; zmce4&bNYt#E7m92w(LIl2#i^_irY_4Mv{S${D(>M@{i1%i|gQmLr?|g=FZLpsq)*J zp3YKQC~s#%H6iw+&z5}YAtu;+6)V_(E!w?ZskXHYuF!e&YJW9i3MI9B|GuC)w4)2# zcN)psURTw>$F38;0aksSP$P(4-@XYiDYxyKedK5wLgII^ko8C}p z_s2t(cB&Ae(x0%{OyBufoy<|&vl{wdSyRvhHVcc~Iq&_os4BusMe8#ff?}+!L~8Pt z9c`aftx^cq`E~=+SNR$l8b|(qK~T@83}r2uuK=X_ z;%q-JZ{cGuyKg^XKVMT?0eFSh(FU?A&#EIkCy;@HcL4*YGZ7ktUTHPY zH+Z8mU$>oc27_2$yEwc&Xuh$Cbh?)q*din>bPm=o&W_Nb>{%?8`>l9>kjMQ{&7ct= zd3S6iLJS@6Ub}k-`h0v1J1$rJN5pKKi*ajw{khq=@@EmW*E`zEa}g;}5@ws5IdAaI z!A^}zP>+)bhhDD53Exgv0Bcur;x$n}i+DD?RIhmIOW>0bU9l$a;*D-`+amy_fTQ3o z0Y@Cd=~Gf&DgfQf_~*U?<~QCf)@&&;6si|91-Ke;&M)ZZsGkdTXqhvKGihe5aQyKT zq~%BQ?y8tXkK*}Mf%a$sXxlDbBx@^kgA=FrmA$WTO%Di4*%F*#z>sKDrp=#Un&38- z8D37*7TM!AQM0JH%YZswo;$J}anld2%@vN$b-hHmV&(8(l12#!XId0_!&~^a|LR1O z@h7lSQ^#lSphOo{)CI?P_N%P7i`)X~^AI4s8=?%sSX)C-*Ed%zN<2GmaZZy@e5<(g z@ZK2;VmecRg19Nih$WoHkOss05w|m&D2uJP;{LXRja4-f>s$%eG?oto&T2<w&iV-`;=1 zjY89D&wZ^rE}ypQw&|gmtcL$rNWC6gYaiy`JO&?TT{k=Zx*`f76T2kgKJTCJ_DK5* zlG<9j6_l*6lG}6IE5=Yp2w8jU!?F&aKN~)vN4Z(;gJK%b8b}{1-&T+cDmB}BL~+~n zP^S9dxXv(pBZ4(kV2m_)#D#P|V}O%RxzfaS#M_eeQ|$(VpnGV9|IF+ebK?X=ps0-l z?T|E>A;Oc9WfJlyONR&%N%M7e&Hak3CjMoeNFT=($6QEaWk2$?>g>G3K~+(k=jf>0 z{K(zpJk7xM+7YL%8&9iNL(^DD_~J$Z4g7AFd8ggrJpXt{q!@Bk#a@4Ag!ENcMd0Xj zos`mc|^qvlJ;Obyz}LS3XJ9`#l*UYgHMvR1Rb{G?jW;NO;;yn zrQ?Cj^@U_aPfJA|L@)4`i3M<2VxW`ye7k+vR4Y&-g%Wkm4LlZYv1}f`Xx%h4GCQ?k z^@Pts!W$UN?v9jUF||C2%&WpWUt#tO*=RXv`!d-(%E7>k@w(7Hq#llwz;53!! zI`@Mpe>UQfP7sW@ylu|39l2Yqjm(-^v8UtuJ@PQu{^$0pn3y_0e6bZ5{}de^ZNahZ z5BC`rm70QLw}k$oCy9A-nAMbKwb9mZQ5>Qk$yxnxR8q%`l=|OV6gfn|mwJYXV9X@i$+G7iAvcB%!@ zBtrrMdS7i-R8$~pe-KbA#)L&cNU%f_n5$YKi$uWj85cK2KH4i{USD>~Pl|rw9sDt> zg4A!($CQatuCu@Y1IQ`A>K|~JZq^>I&w~IVCvk`*e7fG2veDrUk@~ZrS0Os5sE3C~ zAp}p=T?B@Nu$zq}GDKTR0Vfz8!*oV_Dw;?<{j&Cu)!Lh!RFV_H;b>D->Oj^K@R-#+#V=?XikldNpw<_Z1}e?c?#%rrFMA zw2#wV%GT(;lf#8hC+Rf-w*H>xH;7Av4XpbTG^N@gX4!A z)wv%hquCnAF0AmQp_QNgz+XCJg|m3p4vBrUsyk|4ZaJa2z^d1aM{jxj98vO1(x&^_ z;HlB5u=m4d3c9r5Sab(`qO}$&>K;=Z`c<(&8M-QnFR2U`W2FV&te*Kc;2#Vgq!_AG zZgaCWA5Ra`$f(O7t;}py$K5C|GPzjm&)+*&!C~MDwr=BwA9vA)NJQ^`;fel!6mv5F zwDO|5ys_$!`D-mB^IGg04Pur?zQ1kxzp~&f)%hOS68KH!AQUy$u+Kzp?Q*$^Cl*>J zs1_%Q))TGeIQh3Led||#@^47StQlDIXMwCc13HN6T;0?J*d23qZ52m{LzS?0MD@pS93q45>2AuPt4z4BD{au zngQK-XQ?t7A0TYMH_%^U)1sF!WmjQ&>KvNWByoLE81}TstG?bu@X*Gv&nTYn7S7#oQ zklJ8MoPbnzcd-hPWOaXgyQkT4=X(DW&WHBdhwq)Qe~Dq!$}Kwb5hdzg=n4vLCUfgk zH=M1H{?mE3o6|?uA+M0 zqQu#Fd8IvBX>Fz-IwFWnt#TGPY?gj!lQM60?RlKX_%C+%PiI&*E<#By&>rK6h|W$qbny+T~vzB-|OxpF)?rM|MM@YLBiOebasUQj>**j9+J&}R|GnQr2dr`s1m}|iWJe&&|-nUNU$Fvl(f064WO&>AFJvA zLE8TxmL`oqIRZK@PLz<6oS#jbMp=RL!2}kQ9uKJ%<-fjoVEBfVGI>N2s1(jo&$u&Y zv-2Y&B24E?zt`I=U;AnQtE?8}Ic)V5tIwcc<>5z<7hW_YayAl&0&4?bjoD#ba{_3N#4wFM-9XdQe`v9YWv-pk_iR)hNab~Zl zFai81G%yymr%!XN8UgQ*m%S)Ml6iJc%f-0dMXKW2oc-*-{~-4{F69kIBs;v@sLO)G zgar>`5vHW2g@=?&1OxY(_Vk6#a!8@EFkHFxlt=n3zxRP zucXZMXI78ZN$wFx``|~G>HK42H^+Y_smf0Lhqd`|dl#!XvKwLQ```8aK#&TE8%(RU zAY*%!PZNI-|1f?Ai={tex(qZ)`MDWa85A))3RB-`e=_He&hB~OvFfcJn&c#)5-doY znRUm<2oee@{N`Wag-^4zNIIJQ1lErM#2$bQMMK|z`$WV*ifIew+6hPW3=9lXgOFRB zpPwHaYhy0@9_+*V<&2Z;^{)S?q|Y>H>vgg3gJo6C1*|h8A6XmF*~#X6efaM@E#;kv zIy?njFr9M9iEf6B46|7@B0wVtjSk1LYh(raeIP*Fi|chX08GU{&_Fr0i3ryLgTr+` z_i~D6NenXIeuysIp?3GQzx#B2>;(Qmm1(pUYp#AS(n8VD=HH(^*>xUZ+K{FY$upXs&d`+NN8rz`lodf4VR-4hvy00&R~&_ zb)F`B%dHItCUerNEy2?xGV(q5s9Oo8dJftFJOu}C5=u&L{+?CPJ~S1alZwjd!wjrl z9CKsToXE8nAmDoRu;aHk4JR0;{^$Cs!G}4L&Gaedm7Z&O)(BD z7*cNQM4L9I`9vRYR{EzTk*MEEZKzgnp3R|GN`*~`TfKygk1x-L#pbTrlt9?&iYUtd z=*XgAu4o|PqVt#;i?D+2Z$d|k)+9;f)US(EpcA$cpSl^u-zD3ieBcK)1r?1jlX=#JqA zgwA2kl)?5T_qWgJxr|x8ihhvMaKDp&Ut??WyAe(^j5)$^=HvRC^)(5P&6v0QoO}7^ zjOK|;pm+Ua2L9iSQA2iT^Bi4X171VY)t1#|zXj>94Ej(GW zm2p1NidD-^MT{6fg#!b*nv1n)gykp&IdH^BT@=%GZ>BPxX3;VD1wX-LgFD4o2cxeS z$r=aeUNe+|QGZls?PV;v!Y#|Jw{ykojOB4V*B_|E_Pv$_HDcsmTF+QC>?Z+ch`7z3 zkwH0;yA^JYkFTu12ym3XBa`)kxDe~U!>btTMd1?pMTE|sPfE{CDT;(e~!M&;Be7y2xiV$20Zqd z?4Ca4)bTI{9>En(_lkY^4S(GscW0%RT7NH?++mmD=6dfpXS?fygNvhL(Q zsH!UDgfb89M9c$9cOeIY}G32vJNp^00G7mra?A zBk&Dq)e-+Mmt)MPS&0Zs)?A#=l_U0nC{gQKt;gyY>6>bj6=ld=<6;SgV!-Cr4}h(}hk zE3ueEP`Q#>{*gpNFSr&SJ+JUZa4G=I+a;0rQ;fz1Vq&evY{Lxcr9Y1W5&f`aUg;SFm( zI*q&0EmQ&O#hhHB?+`(>Ig;4hewvr#Rq-OraQVube4;yvwv_61^cU?b;`}UJsco-8 z`}urq&)|0>X;hI1m$&} zM~ob63amfa70MVP70Bt$cXG8hjzXCt^q1;qW(-os!Yk9R<1?SliFExP?z(KdUW$97 z1=BW5kNBK*@6oYLA&^^H%45&8JaU^1n+#fv4`zH~yU<`UwAb$a@?KxXzLNg>SZ=Kn z9rYgDubI8~R!Q(q;#*v#*z?vPRDU-_9;b#{bAM2Vm$$(Bcm6t&Ip?jEFEq;C*@*50l`GVy}7K*e&e{hKIs7bz@fOWTSVabMcfEv_isv&U5=FFQj7j3%LwfNa` zDKR=hX&)>RjX)gjDAETdziqxdP^2Mx+)a$^|xpu#ys(g;CN?cl6(vwj9 zFCmDxbrF|@L=mDat7$UvueZAP&Q?)*S1xTGAI0yyp0CN|iIWM=hWwcB!+N*9LbNz~ z!x@R;)|%x7n6Bl7g9+FPl8y2g*(xV&743>JAl0ZCh@Eq1XD7g*YElL4eex@l&o%t# z&!0qRrYv}&QBnQt>w>Gb5QN;}vuDL1Xoi>yKDdq|x;Xezw6ngznPb==I3zwoI-2^{ zhVve8z+<`1`#P!_xq2cWf&J?TCx@MdHFIA^bv@slej-ULZ z57XkJYyTFECjCXe(_deIDJMsOp7)~z*2%K4)CglpM#CPHHqcIPEpvJ=yn=y-4qazc zbZ-_rX=<@WCheP^Cd_y@7}0Z6TaBXm~dQT$@cRm!cE|-{Lx1CC5Y;E~!k zUQ6?fFW;SEmqc2l-hWKG05a3i91uyA2Cz^RqKqT7y#spId>*hel5PIR6yxT&45X^r z7`CNE@TP_`Ny&5b*1GYoPV^O^I2#rWG(MFa&S}Jaj)Ac^i6JdH#jQQKS_1q zAX?+fYS=4oriFw~CMa__Tlcbv8s_xa4|_TaKrHGH@n8JK2hiTBsZc6(vhHV!A&w$B z?053SE5}K*)C`$swhm>WyVJG(!`rHw-3_Y9 z%-1|vzAE?SxcC5P0ji*{nBJ^UKM5}?U!$VbLS|kYaOyoO2tN*vn{S0w6^<*LM{g}& zc&Synkd?b|d1%;l!^urLyjL$T2K!MBQ z*1|A@Mbca6%AVl#b(lM;X4c6<$$9D7q+NcoAZh886ZV&X)+umonBLJk#pYAm7=7hO zl-ZI$(ThQl$gHWY?kZKrhqWDdg7tzpu*m*tM!4J4P3LQ<4i!3(7ps09X27d6>?ois zQAZPjA`yb}m?p4p?zYB%ZJIm(z?|kbQI>T9Z2zq#dOJpJT$A;uzeDYO_F?2&2mO#X zoY3pPug)qb(oD&4-Yh(ro3%8AR%N<6Y3$W%0B0u=a@YB7NL#Hv zlG-;sM+WesRM|{i6(_#Fa2$MUE&Pf<`z66lcNW3XM^#QV_`IuTl|$pbJy0oNC+%6% zx0Sgy-OZM_qRos2)1Q9=;`!wDGZGNNF>0^R_}+$5%xTV=;bvogrFo$|s!c{_^C>FB zf8uK9BiL~rcE8gw4AHQO%45phsWJWMf4R6GK04kBn;+fVj-6?y8cH?nSHLXJ9B2Jk zHPiA9xva_V;GiYpP)~$P6I*nbISq{}fE08znDG&$aH0W1j~KSumz=YWSDX30wmgxZ z!F};ZrKGxg&f#9?Q_93!|F(8O9dB)1YB8RJ3s55mz5cn99rFN%Iv<4UctXv7Q}sH4}0o4WNJhB6^)Y)4djU~cpu`N!FHrpUwT!#kQn>a_{|sERciTm zMUpFcC&d7Pa{tQKrYuo&VOM@X&s{V_QR@7rmg&2=xR4o+t#Z z!so1Hq}P87g0dm?eKCkmpU8aHKSLVAwwsVcb9y{9%)S^JuK0zFEG=F_pLY95e0v?M zoPm)9;c9zhd92r5@vpfJf|WO%4RHqBt-c|OAA3@Dpm5sF9>J}?P`Bd*@~w4%$8UhtJ0t4>r=WoG)7T^d1v{M@)0pI=n?r!WzY;leTa@HT6Ke^$?M{&)P_N)2(A2?8m5LP*GD>_K;Qa+I2_&doZxVJg~-9ckxbjo;KNaS zODpXqBPAcg+@A7fjFH@Ez4#HoxE|T~wlQ9;Gm}Qaa$?{h{`^^tfzDz~CmADXL6YYU z^IMOiliw{KNi@Q3ja+TF+(*_4(JAE=HCWFjyT=!%l^a=GB2B1l>B;R!E@|+en=jQa zuqZGvFhTyr1yTl|z#~4liOiPDZRCwyI=@Fejesh34=zY4lZM+T{a!a(a& zw@xxYEy5QWX|_KoK&!xX!a*PzP2$uPzm)V(!kBUVjNiY1_k7K}9g5gG&-T1+`Lu8* z)U$V!qV?R52mP`po0FP3T!92`B!^?<+{oB7f%doZN@1PX77i|($*UJ)yq#C)l^)^J;N=9vC(Xezu4Y23M-rjp-_-s(4Q-O`3n z8cxI3--LELHK*;C3Dp_*Fac^U1Y7NcMHhY|Mi<|lm0*OZo`Kylkyp`CZHF`d~S3$DF|<&7IzMO zW~th-X}yn4o2nApvvCgPCyekeTC~_NtM|ml!pc@mY?}3@Sz><&KciH^FjH2r>Pm|D5U8Be@C z9r4?lO%GCd*I?wx#NL)M{+-g);9iT5-1U+LTQ5>vNa}E5(llKmwK2DBzMM~O*lgkM z{wk~bOs8}PA&bfsD0l26^u8~_)o?Ph1K+(b;&n@Xfr~Y{dmLp5$@j|FsnwW!+z{~c zQ)jvwW_^bY$RAyYT)NQwiYs_JsxLG=R_%K@W3j59gBbV0;1Fcq*m)UMM+TC)B z@9k}dGTf8r+#bch!6WDb@c02W93Gur%DbeJLh&S1nViB=SggNLhppHW($mA6n_a{7 zQRh7FA=d*=>F^U;wSu8N+0-+nNAMmaJCmf&i6e^?=$Z#lpv@J8CAxU}cw#@mt(hv}aO-_GUw5qKkgs-iPJPv`z9+j%a7Wh^w2$ea?Z0CtuE#Z*~4vG9nM~l<{}NN@D<}g>2Sq z$_I$3s48VF6DG_6g_jBKwAKD}X496()GXHt%q@S;{`76WBe-U~`4EOblzLQrzEuKO$vMC`4fhdRojYA~c$*z>o%45o3&%H!8v5l%#37(a}Ki!BwSk}!H zerrXQ&{oIG<&EAoImU0g`;nP`@)2A!?slyGVT*rb@fqH-ZZUh7b^Ehavc0su6)g4( z75CAVhUTanZMI$z6_^)y)|}-;XI$xdF$l+_W;5>JUJO47dmIrK7EWWeL>nC)4Vr3i zZDsWNcu_2h;}?TtqC$+;`_-(A0R2fFs7Z=&VjLo$KdB@dwA{Vh&fYIi`1>3;G<>B` zkcN@wY$@J83^(<4)g`;Rv!^*hZ01=M8#sC@znCm)pWkMeK1RW-HJ4SlFm4|;5;_){ z4M36O>f_s*4y6ilD^ex}kCjws&g6r!nYpj0rKvWMTUvxs7qTLh>$I?lG(At@y+sso zx)~M)nr)`5meHCr<2MZ)*nV}I;CO?DI@(zJalrl5#;!{zZqiV&3HSb>Mb- z+eG>qj{|qB|8Fc(rqgk@;qddj!t-~06n&C#Mq0EtD!CAI#!ig`Irp^IU*tUCiH}A{ z_|ikJIVx>1cId)9>!A&PS*Bhh5R)(6*&N**qxpK%%Xpm#Ec)Abdhr_UhWKl4H=#R9 zpH%*=A5n5~ukSIiP(B>w`)LZuX0u1gk(Xq(c81nofh0{|*Qk7}_`=Z}7{l#-QtWW| z=YqtS{95DGf>=!X?2MKh6m5S4INn9EaqxaY|FenIa?)|_?d{Bf2O>1aqZbDFoI(}# z!DugtARh)yr7}t!PgWmDGYUy+;qU`HhJZ>`WTUjOeJCahg75It`zw;`fzwv3><^c+ zo?Ud#d8OfIT~~8~W4)X`bpk*S7w41v9gNZE_Ji#k6Dzz)wcNVD&mx;z_-hELsGhts z)OdXL1(L9wygd=SwMS@%l)XYS@6R!oi`evydF)(ni+U>zQ(-wVu8?QFb}@X5uvfUL zFxETnPPMt}h-Rg=tu}sFBpCtW6xHHFUj7O=>%>IKAN$-X?Yo z`r*}io2ZKJIhB>SWXzPrr9R?_2l%9aNSD0s|NYZ*L<=>ah|^)VpbSYFlNv9fcpDQ2 za3h6E-S!eT?~>fVjv;!vRwvhp*)eEC{YU~Iao@x|OOrkxDC6y796F-zWxB7x;iufg}$I zdz#$$!g27l{~R0Q5;qe-TG(uj9g2nj(Z2uOj;C$dc(C%DhkGiV|iIVQ3N4%V3Nj;Te*|^91vIUurV@TJiqy zPs533*bn^bU90Qb6YIy3Q}aUf8AXRN`)F;HQhs}A?{qF8CLjImpb19NK_#_Nl0LqK zep5xtcMoAJDPlEBdow=2l+X10$Fk~BJ$&KCAKsxfcjk$b+l+sD^)OCreF*7a`SKAd zj~Nui9qg^OF@57Y#4-|>XOL$+J$8*&htn{SQ5I|D)kt{McgjEC!;;Dnb1T^XEkGX+ zYWRnK90^pxEM#3cj6Eia=~1$ofod4=?I~&M(&8WG2q&W<*ZR~|iTperA~og4lg$b{ z2J6ne$g<{iMq%zWdC~w$c)Zh|Iiv@^$?6P3fJa-ZN^Gd;p`3f^f~s zwqV{_v_qT3l5Hqvgz;3N@aW(@MyMB~Bd$Fnc>^7Cd{gddM7#8Yf^(t`Qt27Uo8CS& ztmRD7wy`%mZKsJY?+fgWn)yMd=DJa+xBB+Tw+3e|(5jhbcHEdr+Ak(=<7Fy@LpzWu z+d5B%403X7VtVwHF1<`>QT$L5$@avqiZ>P%9wU$c&(Vb+Wv(0%RS=1UpQA7C4F>zg53GRE4cAB)+qvw8(oe{**2^Ml3+*4sN-!R^A4H6HQMxUF6<#a z$6w=GbWqb)(RhZK%0W5s8C!b4)%Hd$rPt|^vd!F>uRV$OILauMe7<4qVEwr9Cm)%~ z^GzS}TewJ%Y`Dqdx}L*Xv7cJflJwWE@Q`XfoVS@p_hXyEwpx|zr zh=h!H&0xiU1t*CXkvBKH{r{Loc|p`+UEc3|*qRxg%Q&|Wj}*=w6*M4%R~~3lHmVN4 zgG^H%s!vXM5q2X)*d9G1BRUSSX>9NYM7o`|^;WYXR*?h%mtQqJp2`9R2hU$lv01b3 z;*VuEosusQi&7k9%$O`JF5bJm)G=5wZ4=jRJ-e^qHN?G->P%NZbE}Hx3U;dcd;wK@ z@r}A2;6lTlN6_tsS99hvZT20B^tN$&slpqSu~+`L(X@Hy!@VX#*)i`6gI-E%MVOmy zKn>whd95{BVk0Gp{)vFUc^`xI2W;XPi%6iklo2o>bN3U*L1+PeUUj+Ar;qDHe~12brxkzrVxc zG8j!;vsE6f5wpe=C})aLuxojI>TEMf$!brag8kQ`?vloCUOuvUxE~7v9{P90=le4t zqQWigcr2)+*;ZYtqd|8lr$tpa#6m?aN?rQKHoN_cA$cnPk%QCJr7!ZvSGtjt<)J7q z|MD#-DC*N}kYd~#2(=UwoIfJfV@9>c8>(8MoCP>={QDBbXT_d(OZdNz7C8Z`a!nVP zYz>tvyL!nIg8xPy!&u$20Yf@%M{}Kj+}OoO{}2JudY{t zi2!=qk*}y-L5qdnf>0)&zeZyDCQkY53wo07Vp-NBqY$qbQAf&m_5)&u0Rg69x@P?G zkLa{KPOs9wx1-cqnDx8A;-q%N#rKBBs$SO05JeFcD*JnxL$N9$7){r=4|wD?|Je#T zaM3vQ-U`-k<}xiARj*r2TH?)8JgclP{`~R#3H-Uly6kA6NbgoQ1%<(k?Q7A!H}>m# zu1VyWbL!M)y07jd zd-|bzs{yC2OO@dM@tDGvsKG=k#7g{-lRqi`b@4`qrN-KPbPoSprcyJ6sIj72lX%H> zU$e&RF->NU5^~lyf(zy~iQ&dO+M%+lYi1jpUOWki0}}*4uE_pxS1aeI7crNetj6OJ zaE6?nP6^toYAYgsb548AO6#oCTEcsmFZ+m$0#~m%)>5WqC74r;tB3^IfTgXFG3(*F zQ-`f1Y6s|c6Q?5G7W^p>T1`(|3%U1st}aW--_sD@<|tPY1zU*SWm&J=PnJuT#RV5@ z8G^sR!DxI+;Iq~m^5&`gQ=e8rH926OX}7%(fAmb8o-WQp0{iC{9cA7xZM}XTjEX-H zRLJ65EL%%N$bP!HK0bG75eMP(Z}%8W?wzT5htceZSBgbcibKkTRP8yr_%v=Bt+%N1 z;O~V*WT8bxf$$}*GrH`HLT@nDP5X?e!m(0Zn}k$)f?@r9%QS5MQkvudBtC`%^G70f zI@QjZ0i5=_`g#oj15@#hwJU8Q?oiLCHxLT>M7|!U9-IxZ`iv9ltmO-*CRZPkhqW@8 zw_?1S@c1GttHv~|89ELF?K2A))AYVs2M41GI64nRbpCM(*RF+!sX8qWIwM2Y+f0ob zT-2WIyNj&LaSOXF))(SSd7mFiCGU0cSG&_2(s1}ZL66FCzIYtqtm$(-k^&Ev)OPMX52T%tol}Bm%PgFELs#;|1_~R;#^JGE!-Clo+V3IBB!dF7*bXIQ8** zE|tfdWxn@<&v-N(wm+QRg{c09ewveNebIGoJSWGzkAwJyLswVtzl-J*P88$Q;Q|Pt zG|3$)H6*hoa`=EEf?gLSxpLnew3df7?8EJMaUpQg)u~;#K%~yqSLK~10&C$P1L<$cy5>4Med|7^ zvpP^*3c;a|=U0s-An-pzwcDKR+BkUh%%j)mDaLMJ4Uya>M<#Kfi-9#6bT>;Y&;m93 zBZF!Mr|O2dF>8-m-yH&vX4qcI4!KvLEAp7%Ny9_Lp}!CjUGdJ44o9E4sSFu27_l8C z+wG%4wq=cv|t0_?0WFd#n<3t$m< zUalSi6pK3Ur?^ST$w6xL`@#7>AC!(JGt|x=3L`9E3`)3pbuCREf$836KOdj|`hNI7 zF5_8~EP3t9MHjcrSDQa_XE%egW3iTpV>qT7nlm*$uFox8p>Vql6KhP>Il6*RZexli zF>|kE5g77y2I$=OHkj)mqQ*M<;O@6AkX1xNOwo0*q&mYU5JL2OSM(mi48Krs_G~Hi zuAKdg>YV6sHR8<3=-A+(Wy_9fXOa3N4SAX@)5jGto`oEx*HF!vr<9OlXR7pp$K5mvg zVMA>BxJd?mqZJ{-vUarJnvi2<&Ub>C0K1y zYsf0bvQjX$j*Z14Um~ZRw+wm?iU(+{ zwMFt1X=3EHI};W0GKMBz9=$a=&1#*Wimfo=t?t>(n?>kzO_x`Vo>GVRp3irxrA*J) zt=+G?Q1iqJz$NQ|nBqat#)x0^Wqij_t+-%Z+&n35{*w~@DH^vOV!uBGpt8#!G+qHJ za`*^^QK-AZ3B3+XlAkG+dSDHX1|YI98!W(lyLeM9{=qa)_GCk#)0W}Vzii9JW#x!F z{f<=C{!TeYmtJgpM{XpuR#vebqC8!3g`=>9N|^otHPv*s{qxl5Orx3I+nWAqcBPWG zqnb^#Q4@-KTSBpI%&4+RK#p@fA{zIGTryKqx+qhjmuebzgzX$8L3F{)=|z@QSIs`q zgrZ`IypTDB3hJWUA`rgNGIBRcNjh3EtP0<(I1e+)M6Z$EG2;(&Sgul%zS((~CWpTp z*h)1|_A_6_5AwiaP%gl(03~QVfp)JI;pLGd1;^i9qc~KvL4BPlp5(Jxl@aiAd=OKF zcL24vwgNU%2pX*>&~H;y@$D{esl(GtAEPVn!8hz(*vsu;3&fyL`=GA1Y)Q**{lV<# zzbfU(mmSIVMk|nGQPJXg%jtpONbM7iO&#v^qdpZFIQy7tPr4)B4k1 zL<=AP*g=u^P4|tF&a3aQM=_wIQu>k;Ye0a757++h<2QpVTWMeGpT%uae|2ZEJt>3k zxzQNEL&I+*oo|s?|Atd-N05#bz!hp15;J#Y3{Hq4{k%kK%`X$ZMk5_jf?hT|iyaGu z&d^}>{l?&;IU8?@1{v`x;b!$>G1BH0{T1Z%Pa^0T7!|sRD}o57lbjD1gS2U#wS^~$&| z98ZFXiz@?QNyU>eI-SgTA}1-Wbe%ycQbiRit#nqK>TFgy-F#&_Q~Mam-cJGWBmD8d z@M$yNP|ZfXbbNmnEEG{~QT_bns^MP_o_|RDRAbXd!hy13h`&fZmh!nUuRmTh`0SoH zWMoDt1-(6}1B&B2q1AzKwC1|o>9;F9qVX37 z0aNqgjfSKoj38d#mdOXGGB+_eFF2azrrt%-3F1Ml$?A>OtT58J6`|HYektSEMOM)Q zUsKDOqqI_e(5ia*WHp;am)zGGMN)BR@_L6|?3p%tB?8ZOqf@(~-HUoGyNDokoN(zAVMF)~+_ch~cb5^rDHi_+;&~ya?*UYNU~)Q;Tz9?Gfd~AM zlam9ey5BrJ8~~!gmDN>5WMpA=bqq;KNr`w7J3EHuYQ1BCEMA3X~`~FM0-mH3ce`$#9u&1c=%JFJ+KG_FaDebHo9zIFj+}DoA{F2P)}* zu&EoC-2V=mq6pXiud8DorDb0<0S>aMY+lc4cYxwnwQeUsD$N&Ik@7z<)`uq``1@EW zRcf%=#FC7MmU)(wlG=a2?k+JLj&ANhlKXyseilcOVh|Z==IC(1+b-8Nu(%F2ZI4I! zS|5S>VDW}M*A}mLI&mDUuK+}7vHt*?e}scc5)u*we0(`=Z5(!+jh^2OKsCkyT_W49 z3BY-}3IMOl)c}&2PnV?RG~~6<5v^Xe&s;G_QwO`XBsu!W@b4|^X2^_b1CQ16Qw4DK zay%nf2GDY-M+MTJy#YNw84^Alo^`({ae%hJ2>&M+;8UNMd#dO&V7;4aI?umvet>yX zpqa`6*`yv*GQLKgrrGnz#>NJ4idtD)E0ijS=1Tx32L%Nsr+J}B;AnZSzSfmzEUM@| zhTbCe9~i3a)>%Heo3|bk?gcCa0cR(ClGy`0=~!NH_jET+^#CmbtbVnx#52+e^^Vfq zyLAKusQ`A0C2j^5mv^?EM6igEei6zR2lM)lFi4nZ$Q>{>gXYp_^Ia zze}0#g{3VOh?H^WlEPsLlYde`WxU?jfifmTlI~F!9N%TKI)Sx4P{+Mlr*K+86nFr!=dVD>J z2L-GTb84{)_0DBs^BCenDm%K@)Pq`v{OIrx>?|0;sNh@c$XbCuF*;k%n3OZnee>#J z(pvj_f4- z|29n7I?SXnX{?qhsk^GS0X&ZN~j+6U*- zZmTBOX&VW7baDd}B@&}eP5g~kt=T$W4|xJJ$)P56B%$+W4lGs~-#<0;9^&!Go-v)O z9zc7KJglqC!EOtNX-W6(J|yX~DiGeb#^rqNt(&@;T*q~rnnuKq`uT+SLxjyIDrr_V z?Ux5!*4R?+dSq7Oj)^WH7lgm_{|^RJtAIHdIz3)dG@JkyQ(17_Pl<7JHKMKx4|z40 zK%naVQxk3Y(h|V0>jA=uy|rJdz-wRa5)6N47H4atZl7?ViEj z#XFz^qf9R|xz1vE)%Du26p-tXSMhWOgfvR-itK#Pc~qez=A7XaO^-dkPP~;b6!V<= za7e-fJFL!R!c+Ydu4IZnfRcKw@ES*E+-6(JX5qaeOD{Uo8(=|8nL&8J_ul!so^3Zr zF*9S{qE70NUp(xIabOuMyj@8QA+C!y3%_qyQ<)$c^_cY+?B0^9Cvy_YbHxSG0`5V&os)Fve$_5u4Srd$)a;>1ERcKP4Z$53)8Zy6sf|POmmQYkGt)osM z8oK+c_8dLgMim#3T^*k?#5((F7D$WrnnRM~)JzFSj>RSRVK$$K=C^3n{gJt_bueOFruT9n)~2$dHbAcu(%EUPsMkq38x zqcxFneX(QKA+o*GemLd{hg{BZe@?1Ae{5$wEFS4wjcuFAIXT3~C;Ej3NMC(nW~UOl zy1JTvyxayQx_2P~KGUmU0gPzEXdnUPUy&b4We3Yaqcnt4MXTgGJ*B61#jxgKXt^4^ znbjdP`w7N4*enTH7O02Vt|eZ&C+UmE`AZG^JH3_?&cH98gVJ4HtA4ND-cvhuLoz5b=k6@;t{^3{q&>s_8O8LA-et&K3iQ53%J#2Em< zEibb-u`$Zoe?N>+_akP-~eimy8yRT|f%N;^69N zJ%xT25PvtoBjuT^%=x?}FF#$7D^&Sc1HIiOcc5hJX{bniyZ`)z!yyZ(;V~}gyMASB zu-XG+$;-at;CFtF!=IKdF)76AZN$24KREv4IjpiWm|p6!!U^fd zlQaG)Ku0;g=oKHYX8V7FI1`#5s~kZm*IDz10zQ~T=q(z|h4*_t$YRr)E``$S%&KIp9!d1K@@SeAG7<}Cda+%EijUPo$AWiK^NF(9I2*d zG!`f8fRxFmO|oCJ=+|6>Bof2VMl*@DlCs7B0-RPFFP+v@ zv&Y;Iaa0W_8TI+dFNipuixoBGyz1+**+pal1h~u>vf%gzdEHIu=scqcv9mCm&XBp@ zc`h6aiiO6}Kr^E6k4Hn&6zJmY4j9ywSq`Qr~Fln#nnR%UqNmM&Q!#MXGMCd3(| z8oyN-oUg|P9Uwd5C7MmrGD*>D(xlS9azdLmV1Y^s0n^OFoUA&trSTrszI<}MoX@Sk zxoB;*>!oA;v>>5D>v@1s;(k7IT+W-&tBtkTqB2nH795tl2gzX63Vd%{N;1a$zhPH2 z&4Jlz@E6mhR;mY559N&BkaJ(+dK0p9pE$Fw8s2PRlt#D-oX*fc*B7L}FKnePcOE3< z{0PEB{Uf!Fh$RI`!+*2sQ=KuXN}>B>O%iwEE?}{$)5uvtl>pK-LF82;Uwo13{68+lMb0=L2neH6x7AA+sVkyW?IJqMULz zQIM0HP%iEq3W`3a!;Kz7F<4>hvsZ7Nt;u7-0lM^*Y2`D3IP#34Ozf|Q4-b}q8?vSm z{8P@L*Bd1HgbIN0hfoPDLXbl3gB6Hf6hU9|zeL<11pDYm?!%ddRT6GBJ!4Lw)|3&Km4t(qv`i>vBJwwO`pE9 zMPrU!Tp|7eIb&{AD6joQl1KnObt6>tJtm2%30nJkm?R2>99^z?kE=nDj~1zz2-58S z?Fo~!7?+NhMFOG)sy(U@{rVSfzLyHR{c8)p7v7HWmalw^nm+W-+%#YmO(P5!= z$W`BaQ!`AhTTVkyVHXbN8>9ph46qbn=qfTB(5U+A>Cg~#^3B89E0USWZowX&(z|!a z{eN*g*{NL3Bf1cMMdXV2erl{(op0bk|V z(JP{~tn_k&XGt&EG~O-cz&)>|*s`{LzpUdB&FI zw2HS~b0lrX;%5TS$XQ0BylY##cYof&>d2FoLTaeIx_fadWrWWEi#10|E9Jk(YRza9 z{(E$EOmq88KG=36?6NS7CUr5UC=9&8=CJhZZ1};G5-DYPMigCpjj1pN|D6B+_f8Lb zIHe_ExQ{lVlR@hAUFsqQeh0E5+9MjIg zW}qF>WhmrTjvM0OX=}-M72Mh7%n8?VCQLqWGVd;#!3yWTa&71IZuzA*p&3=fkD;Fu z1;sTU>JTQFo2A-|Kd}Wx9;L6rN&!(TG9fGL;~y8B$e5Vn!NJ3>{kT^gKCjedS}nc? zJJQ)^TRksf%lhYZ;_NQGqUe+)V}cPKfWrPHUWV1P!R|r>)`I$K;-=}b!n+=W`FBa2 zZB2_i0`ljQy|=NalbmMuuzwo=u(V=h#lK7yP{P8)^TndDipKi?7xHS(DW3ab*>wF=r1J#J zlZ8R)ek<1*#$4;uJ~C|VJlxIVa*7=s%)q(=+3AbxgtlFBk57)Gio)JE#}M+lx|6i9 z8yLU#;%VmQ7a{Xg>+Ej3N74Bs^@d>ERJ5^{#2pN2&ph0GlyJq%_OJLd*?gVY`~g&y zY_@E3Q9hG_j7ax6A(Vhbw`e`JNdxf*=lD8MoKIr2hc|Yb9A$_s()3nmVhqo}C+_`i zPt;-S8^^nB_iiE%%};|R{Z!!%^M3=}O~1{LI&KY2W}xy&9e;sEj@tW;C!iG| zgcH#N3nt%6@W&XM(D=)*RGAx&4GblQDGjmCU4BYBK#QSW;gX_)){fwI0DLw8WmzX% z3_>8Bs2c|GGZ{XAYa6QkU!q07k|5#Wh9)O}`W+k?kIjA`kfVP*@!~qNXZszg6b_S%lgT&w5tsIqV>+4H&+FhQl@Xs$V zqnex9cXxNKHrr6^6z#fECuK?i$wg|9`g8Jd$KOCOXR9^NS}f;^*?g}kczEn`re_{$ z8qX+vu7E673_Ji(?g88h;J&CD-|$Gc@`ND(sUfd-0C|ub(AUB6^X-wtbSU0N3%uZH zs%s(WeIJw*Ju_z55uyQ3~7+}?x;`$#)%F)^Y9eXHv z_~V7r1P&7&@=?wfJHK^a;vW#)w6dpR(%U&=Z4`TEw&O5K9T*#$F*b%Q+eN%*`}%v& zAhX(ywMWN@%E57{GG3ojI3=XzG^3PA8bjyOEnlG;NrAaTo2p`$CxYLxMr7Hevo)RIAU^V;P%mQ!-RE)a1Rd1>w zWp;F|*Y!P1Vn=4iq@;I))dZgs{XLbXj33jjii0u%Yq9+~B~|q=Mr5Zz`B?0o(%R`^ zHxhDXgW897Ut8?{*-wj^JIQ~nxLK#LFdZmsFR0X9hZb%LCPX93RY4QNVAPP*J#NU3 z?>suPQO9n#U}RrTRerNL92ww5z!%w(>V=g#vy@$lHxqb{orz9}A=^`8df_`K;kp~w z+dJgR$B-8b-eUIwY$&UKT}{XwQ6I*s z8uFKY?uA&`NCSpF-E({A!Fl~X`TFYp_XMNe!-O**cedc!WqV1ngE7BH2p5tOeSAW8 z@3gP3;Uue4HnM6FKfqq;i2z=?)E?7V-x(gRK+KMO}(VozmhV_ z&5wyi=d?|=;vlc)2baYepDYfzOp%2{^K5Py0-gfU&ZcL(FWAgJUy| z_y9rO9opH|k_)9wrf}OJ87e67EyOHaygTj-;cQ<-k?R;sWoULq{!Qs$wU#6ScIo<> zFgugp8&x;KWyycOGn%O`2{8)$@sgEkM}I`SKqj&gJHwF26yZTfbm{PKqy>Kj5HlW%0?bX z=-I?ox#pW@F-fCI!)~R>qFIck*pm^E|9OS5i3^6#umV96r59$&Aen$%`TMJ_`SqDF zC54aahEhQ1^(G^kICWa8(;eMJpRiO z0POtW7X*-S2+$4FFJ|(D^RB^$C$4eR=*;#Sd{kdw>K**>3DiIH#!aH}!*^2}KIrP> zm{TH~l>kCdv$+K@`m%O6#o-=tHi?o^hA&fYbfZh!xKv{e&6Itanllqr+s7R=#MnPG z!zT&Eg6AozbRj7MPS<3fv>OQ=Biua6Ef#CYMVu2v>Vtz-wCy__(iD7)?O_NAA-Jo@ zVCk>@gq@f$HBqDM(#B8CKb)vCf|7W`cpQ@q(Ri)PIO4s(Y<*=<+Vy z?|zxk>B$|tf!h%Wu!ut(fpa3z6xwV1Z!9=H9l7cu*VW!T>Nk|&X>mdXlyK9(k6)}i z{~=AG8Syi#AuR~LyllIHf69+eoH*FD$6*ICs_mKjQ>2phw}U?_01eX!N=Xwu#DKY_ zv$x8vgipm!vtVd`F%FJZ$BwzskwF)@_26!F2uD`8Q#2A*d%Aa&JTu$aYF4x)uE2h8 z&<=N2e5&T50}+ZNg#`IRFB|-@!HO%c{9ifF7ixCbc1r#T%x~3Gg*0XWoYHEl)#3lE zoq&>3)v-fTrOuaD$^hs3N{0C7%!(Q1^|fI2F*Q>X#!${gZ~o$ZelWfD_{v@-G6QC> zYHS5Bmx5eqhJ@DyZ57|pu>@PX1e2gf7S@mQKB-AILKq7i7QaH`5tRX1^bxgB80lkV zQHj|dXrtA?wa7F^=>$*Ozz{@ZvyDPLjH}rV&SM6f(XbAYnso}hvCaLZcHMR8w3O^~ z4sq5C9MC|(*~T3w=%+$05#Zb;u>L^oqAY$}n`m;B5vl^n`9vF6@9K}49=y>)HCBRq_MH4B7ob*bQsv07 z*cw$rAsm#A8tG;!kOhPqrNRIsGBVq0kdP+ni7 zCHy>yS0JY2#ucg_z(Btmp-&?<(&*5E6HJx}?;7})#kdTu>{Zc ziOF#qYlKy5V~i=@Y7CxOu^`Y>OZgSRpGiZkW3?4zv$ykwrQULbgeLjBIZjOmNN zptBWPPaqNMG`ug!d`&dZmx!f=ni_TGB*%A?Pb31-FS>M{BZPA7XUD{Q>RM|tOAr2c1SfmM?i4a>;xvT;XvlA`Y(kNsdt8qIS4d#6Oxt zH(;B&#vPzUE+v9#P*wnB0RKxy-_F@t6ah3oA%PRH=>Tt$U_1?gZ5MxlqAhLlYHJi0 zS)tAJv5`8n1ws<0T>zkt!LnWcGU^uvL9)Afj_^`hPGS2=eG?8YO$JkHo;Gl(xqVcM zUVXzK#s5j%fvKesM9Cb1T?Z>6@w|BO%1uxFI!{=0K-00;Z5oQvSxAw6UF85~R^GSh9ac)c8# zR8P(_6IJX$>&w5CFDQ~@(6-d=HMTHGL_5Bs$Uc(v0qypB$EqNnQ+J$qHMzlgoX%5z zKYrm+WcxgKTMjGZR2Y>40`hS%;;eba=QR)&VV{fM>UoHV4a1>OpNT4NyS@8y%x)hW z8~Xz4b?0>uFqP(0W9J@3a8~Q#0(Ot?I_k ziJG@Lp3$&|9z%8xjbzSH<7y!2Ow3l@-go9Ec`hj-VHEYWYIvm5{PsO2{zXaz&4icc zj%a%jcskyWLCPoTKCCT3t#<#*dXSkoBy#ffRBKG7HSu+_)EH`ZnuQ1bkb`@DZbNlE zr#+i|nu~NRAbwBRB(+N#0FUrO6C0K#*7ZC{!iw+l^9fW11_p}g5>nnh=PF=*GhbLV zjV|cF^`@pC5&uo)oO(sU$R}p|B2|E>u4B=pF1@WlIB4P0QXCcwD*6@7rqKV`&KSvg z=Vw;>41nV6B3@p9evpcOg~ev17ex6AIUJ;H?Cz$OJa8pgPJV9JT1@Jw9yTHX>P+?M z@AgZy(?Vxzh@9eYO6Q`XAp#>_)IvR_nAUN1)Sx z?Ryp}N&N-L}g+1SiQaeq)GT3VE_r=(3Hbww^3-vZ9N`M8N6dSJ&UBru9LFoU^?0Mk}37>dNVwE{JnClu_AIeA1FdcQ zJ+igb@=~5-*&#A*#%S8kU;7runO?BVcMVzIi7gF=<$=yD)8qt@z)bgSR=?7+y4za( zeRhlN>REIPDMgPP{q!?I9@`Se;*b-n2*%2XCFbs^y4y6F2(T5)z@H~y5-;; z?%si?mW?r;-f6YRVqi;xCFISMI71SQ-owma|7tJqlEBW{AL z5;TYRoPbTD_<6stSybIcqLES{(yBS2^wXwOYoXb6bhs5K@vY;q1wg z&W?^$27O?;Y#zzy&Y~hxKtsnVRqRqe2ZXrsorlSn16-jt{!{r8Fm3Codc+dZXwPFk zjrmDzPGB*Ys*%=&uFxwgel|G6nSQ&W@0z4Jv1N6}3Uq7izjmCQf9l3KWNp_6FwgmD z(ltP|_S~$`|KK3tTjhFYsJQw1hU$vO%us2W!V=sO)InIAY!wW`XS}qU@#%``2_Kse z@Dq=ox;wbgE5w8|njgv~Di1oomHc_QUPOLAD!^zm!&rhUNnN&O&~v^eym_YMFx84? zfh2QVz8rqu1*>6|-Z@)Sc(|AcE=NL7Mf)pFS5Ve>5HMe=59z-FYzi{7ttT2%hKNJW z?O%#5R1>~Bwvw$=TJCyq(U+cTKF!=HXdS#=Inr~zjAWJ5bBN2&aP@ovojlN5x~q(B z9~1sbR?oBowYsl$60t_Szs7CMnwh1TQWM7*2$wPV=0LWw#TArSNqygZg+Z;!LF8%c z**x#`?#0N@wnDR%JjW+mgQ8Z6Z@iY93|B>*fP#Ir+@fUi#)Zlt6lEV-(*iC4n! zDq8NbyJ9$eqc(JW>wKw0u?kk(!M{QK6xn*?k0XF??R${3TokWYEJ9vWhkw1bDH&o1 z>jM}Uhi-;H^1Rl|SlP(oXO{2lZ_y>+xBE)+qP}nwr$(EJs-}wr|P@df2DT1YVY0E z{r3Aj>sh}Av3FC3&n~Xco1-l>q!0^RR0b>GxA>1HLtTIPX2b6H zQpu0VeC?MkJR5Z>p-OT?P|8QjM?p~9Bh*;96H;y`9ye1ZKnPpiJYD9{!xz3-(Np5HI!)|$GpP~6&`V{*1sdTszMb<;sz4$TUgRvEIpGDZT=|#NA7MFEg$^0s$;of2`S7T|y(wJHw2hb3>#y8-3^|n11x# zjyMnfG9V1pAYd&G&;{{}li2#6NN2H(D|i{#9R_T7+-~liqf5qUX=&}t`UsarGveYP za)P7~iRPk45Bq7V;^ZB6AJ4AMrWt!Idlb!^&6%-2`xe-#*=M#sZqB1v zyLLQke7&IRZQM2c9k2g$55;Oa^A}s^Y_Wc;L2PdEF*_r^$^ zgkDfZ%j0$R6k9o#81EtFC(UXmcTV{ayg_ur}vq*lJA~REP zkS4=cPHuL0nHg=&%-2E@k_D%C_t8AZE~%jtP@GE@s~(J_iZmQPNPo1YCk6>}XTqv( zVjX&#snNtk?e{1hW+)T>bgV>>zVOkPb}6c8ydZQkw%$A9^re=t9+c|uUWcS|4GcIb znA@G(k0|2AKWvn(5hl?|hPcr9E*Vt2##Idxgi_kL1{S&6lmn+@FA74Hkp4RKUZG)G zx+})*x?$<%UAJ>~_%CKL+Cs8y^o2=kA*n3!wHC5nO$Oj*{OZC6m)c5?ufYQo`1@#3>(iJh6OlqcKcd5>B~qGVD#3p36K;Z(jD*kSq(r8bEpd+WSaQ!ObgE3*SG&Uw)aL4>X8TA>SG z>8*ual{L?ol%ypHyaDo?r{sVq6>%Q#a?e`c5ZR%fZL6SYLY~lY?@5{;0b0{JOULwf z4-Kpt+S)GpRvCC4d!uI{UzJj9{9q3!Y+mk;_s)-;mDpJbT;6^MQnCIW>U*Pl7X+t-5&{_ZKIh>0_E+6zN7cmDb_<_ z@pAv5A{Io^}mqw0k^gP`6Pb@?}u)}QV58W-|?s-_3jRDEeG})}HkqN(T_CTlAGn0SniiY(QutdEZUS6q>=iQ0X*3p@=L!JlK()&0* zcwD}=r{W7sZ8XGT$%z}82G+lEs?ZtMfFKF-c&;H#;%?de3VjFE3ttFNX0$h)TRSl9 z96Tq@{4w)(um4r5d3eYWjPY6x@CYm7uGkINQlP`WFDMQ_5jO7an`n`GGXBk~xPw-g z=&ZP}3Og3Y<)`*5T;wdJ6n`xEFlZYPBgpjp&2v8M5$}SinC#QD0q(4kOEd9f@A@NC z?Kz8pbp4h%lz$Mv5lK!}U5~6wG(a3Q8F~IR!DuKd+JjYFrVs=YP8*Mu>-fA#cF5R2 z{x?$KBK^LJoH@+1Yi>$wfGK}mp?egu$O`P5`}>1L(VN6pFU0|zPLR2|iuPtuZ?P@0 zgwP%F+h7A+$?6|;@Dzm+?cv)admcw?*;EikIItGxrW3o4VtD`L}wzS*AZ0m0?_mtxPjOC`5 zj2RRAH@3#7C|}EC6<-T0DJzC@+CxlRrW&|5%QJ_|vnoy3YiUHiDh2~(BgTt9l$k9` zBJv_=kW?hx29|L)kD-Oz7v#nW5xQh z6V-w;qh)woVs8TdJT(F6_9351J&}*>Ap@qn7XxNjy%;>=c(7d>HbL+&l`QH8HIL~n z(Defog{_m{j2cR0!qEg(t3D6-_PjWW5t=(cDNRU3`4So$w`rRt6fN33+r_eT?Qy6$;N8&~Q3JWvcS0qq6ZDwqlwTcHB5Vh9m+LyG=$eWqTYWmsa_bSU)e8cphh#y5C2Zt zT1in#(83#D_7_7K+^eVg^%;+33kE{L!FlYOAqijOXDTix=3tYj{ zc_d&OWqP_+f?=o0Y>UqIOj(;QSkPLqSJJuLv-G@&qRvPx9pN=4;x2rx#k2HSyH+(b zs;h_Fw_RiHzWR5Z5J?G50iwmH98}j%?e(L?MS(rG7HTQF%h4`k%)niRMYXZ_a7%{I z#Qql?@wGLy*DF#36Ga6VhOH@AviRXw4Ib-#sh!9VKBXkLbNmBukYq!5wsp)0@rW{S zm*8|svZ~V&Exb+?2HGu2n?s#vS5@7FN^l9ld&+F{9udzIy%2cKA1r!c(vJ)rE7NTN z_jp@mEaC_NFLcWtFDiv#ZSd!hT;;ajSK8ThCni@nYC}+xQ{5@B%P~>{^R#;&vWtXD zz=mwIY z*t^^Yyc6I|+FtMUJN!0iZ*PZ&f$5o^W{f0_18&=>CvQ=OJ>dWkDc`U^0^@-tg;6QtaQ$114{gbMt5E{0-p!)Ovq5=>dMx_qtTDw9c?SYkl-=`< z;n?^%CXWllcq;oK5R-_P@CQc3$Yl$#SZ|~OwvoP-)kMBzqH4R_y(q4tvnL(v84i@r z(<8YQT=wC{HfvdE%TKb?qm6CUY0<{&_q*6g7^9f52=`VmgOi=!zP&97NA`!SQ)|)Y z)Fj8{vgzBWF&U@yw%*c74!xqM6R=eki-Ss0g%AY5usQ?$K9;q1_e5|=_$(!=93cAj;HMiZZxs65|;Dcw;)j!cwqymDo94l$R<(_KA!oeO8iyyIEFj zyKWrITu*h&*BZ9YBg*t-#FCC6TK1$tYHfly>kTM#Np@4hO(c&OK7ab=&Vo6b&&?j% z!UvyJ92>cQwgCnph{W#nFE#nezLS<8{-+7NNqAXo1a^b!hdBZQrOHs9^ z3RakD_#@fH6TBBIF3YDFi&)Z`s_qpSQa{sBw)>GWTnA0%mmiI#FNr?#u%7UfAt5m> zO*Qx}AwJ3FW`DKPaKQ!gE0SCX()>xi@zabW9Q}QqCG5gYqqgi_O~VuS{-BttPjo$b zwpOAIA8Uf^h$Hn*n%#O>*)`nx$c{bzEbjAsKk6MUy?<&xqsXrd2xs-KV&lx}b{)VR zt_2ndQDucD&rX+_ow_OWwI zm)PU%yB|I6yUt~6ZGSk&!=DbcOl7XIEV{@CE9K)YtroT4O|kV zvoJPn$e2b@%0)VdqPIl|Hef=_3|A$%Fq9ja@mB8!gnYjRllAFO{i;)y>9E`g z-pU$K+^Q5BN9BF|lNovX{Dl%tvIUY5;X0EZf{;zS(;oO_pCH_k8;o@3q`K=d; z1v&{+LFZ7|M4g-~TIt|35=P02zIfr?XI8o}{Lds|cX!YtfvQ95k+_WRxJ|d(lHvf? z*aN%rC9x0n75qqt^i(~1g!(n@)#QYA8N@al_SsU6&RV*kLWT1=^CU>lmq>vs9;1EW zTf7u-&YTpUJtjM`3UwCpJI+EgA6K>+yXi=5DiQj}DSpaqpj60teB%!Rt4mI{G=}eA z=x9^M++4XEXmEv!aFk-qH!ORTnfL^~{^mIpANqgy zzSu?KP>|WjHL)c9oGaIOtFA)UeoUMx4&%<}D&G@n*kFp;-P-fHIs%PUG#-V)J}W1D zFkOSJbny0cMg5EQb?e+9yxAWdl{}4za+?+U5;N1JGJQwN0}S%TMK|wP?ypjLjk*2A zK=q5w=16V#=+22#n{RM=Ka89c0cjpeU=WSYhYOv?=Y%#*AGgH<-TcjiN^y9bwJ<_U z2xzw=CP%XU2H#B^ix|V_28w8zn0?d|_ACwiVNVcWmGSF9BCfEWAqd8YW&!kPFF@qswA+IM8Y`De?DvKcLE75eYw5~rat+=y;>tF|J4#oPrgE6y zsr+l$uxA;-<3W!cjubNQeUb7Ne$1VMf+lMQ{6G?4_I&3!rr2Dt9`}9IO8yw7kysjT zuDvrqnGm<+M@FRvS_Ck+Yaf_96juBa!6aaf<8Z)!3VrgK`>zG}Seez4%i6Lb3e=?k z6qMprFmk^e9vqi)`cVawV;c-h^S+H85b!ZESMuw4Rnecp>JX(Q6+_p+DzG+|RfO`x zZ9P7@&V(fzeY}v#S%Top+_lApPN@7PvRbjwAG~xrHHmgS6tDItS|(p#Q%EjfMyCca zgNy9we?kSpR3sxHD~rH@2@~==-rmFcs#-#1Li?g;qc0%^m{{OBa2d zYUJC6y&sm6LmJw(%zNhj@Tn_T}=dIb+I}+F2$wH@Q|yxXXOPS zX)IGa;Ng6b&id8&ixsJaLQWFCHl1}mhtI^hXs}z zL>ZQpo?9+|@R4G42zkq`@_7tL>XED0BUoT$i3wLNIq8kY@8ZqjgMG;2`FbL4#J{F5 z`phq^Cx4wxc-N3*Zf372HE)i))7+jB4bWIlHzO{4MCB{v0|+g_D0HuUSF73POjK_y zjVZNE8gywJdjv2R>@f1bOK0bpE_jw*-StW?4aEfW@6L8Wu$4em@QjHo@pB3jy&!(* zYkZwQ?+OK#rZlYFbG_rtXlX2z_MsHuqb`GsG$|S17i*C-d`MnwNFe+Y7aoo0BE#8RaoV;p`cL+>Wygqg4>34Lf#Qbs0^phpY`TJu`#?laoR1vEa-@2z2L#?u- zSsg}??>oC|IH94awehQ;91G#P%`x2 zr~3xH?gr3XCt7l%PrLy3Vv+EmWKDXuwDHve1YHOMT|~COTBnd(&h~`s*naZ$+M)xZ z8GGBU$eO)+cN#p=TdlQEW=Gl?hPk2pe7w(wKe=x%@ul9g6yoU62cBvlo?ba(v>d6a zr(*iu6Q+ls;h0Xu+W$Z9;4;9GgiYUyW0DbJipTzLE zTtd3KTiEXeAwYznkS<8(ohEkp-@aU_SYeqf;8!Ur8ygxocPq`P1QrE23d|Qk^ArLE z3G108KCd^7p9ocTb$x?_fj9f3or`I7|5?DdCICVRTU%RErc27e=K!BH2|6tLU;q3< zOlkL@Z_;CkNB@_w>-L!DyQw!{G)i)mDps1QF_eTsp@@Q10Dd3vdUN-f4tZN^wx27K z2XEKTv}*PSI`K~z%5$yOTdYS4=sxM?vOt)U4T6U3`yegwS0k05Uzs}1tG~A*vL322 z8WGBxg%z6XHq=(WK~CA~-xCDjUKyV~mfOTy83U)myyIwG`S@kC5W$a!`El>L{QY>5G;w!v(HG>ukET)9T0QSQEq5dQkW!>sHK=Bl;WFnETj$%XyP<2X)pqU3L$ewT;|E6FT#FE~^tgAxRz&4s^gDdB(3ca5!m z|7ZrZ49N0GaP$%IQ}eiA92hRp8b^Fkl6z~6S4i4YdcMW1x3^W%%w&QukB_~)pmfTl zk4~f}3#%XpcREE$x6>kx@{Vt4GVI}^{%Fx`#Ksx@28qb7}i);1})x98dTWt03|JH=Yn}(S#Dr&yjYJ zt26EXE<|O>9d8Wtf^ndtNC*JndeS=rV+po7YSjbe=9ZC&!NJ4I+K%gQSxImYRzv)& zS>>~plDCy=7lO=;bP0f8^ z860-yj-y#M&5L4fJNUv0%ve*gEU~D(rlpiTbv(v_bFN3L8cy_00pNrE0-`8uqobkF z$YfW0;x75QD9>@_)m#Ivrq zMHA~99EPNEABP@Q-01HzeC35Re*3OLY3UG-+Uqvm@CPO_7MY?nZ1=7~I<5MmHz%33 ziZo)e0dENOy8CNOqniKYW8jhU9aMRZ_g( zYM-B1E)U}+f1wGO9ed)A?(B}NukAzSxbSbYSBvtc>{XDRPf05#@Xd7=Y^>BdvITP~ zH@%rk1+?V)FO5f3c9)#_;H$OGTr@eIerjRPk0IBp9I62s3|#Egt@}lxkpdUoxcslR zuJW*FU!syxx`;x}D_`CDPaoO|EM6)dD9i+U^uGvOgeqjIgBFy%rt7D0L12?)zIL=P zGJa2&QbSFXoeij&)Ajl+CB{OW8obh&BnxX%_y47QYn=R`?p-0Q00->IdEAN@HT)Jn@OJ9}R1>&qxb`u!rZS z(+oE<(fBr}6sM{WpD5bYxDF_Ug53eB6GXY*e6Tg823<35y(q)BGS$sMDlMOZ)@BAy z$s4#R78k+n6(~iO4W)0TH5PvJ*B=q~x>`L{`JQ832uoSoBZqczOx(RCKJz#od8BPp zBg-XaluCW*o9yJN7S&P796RTyp{^&3mr`}H5fi_AoW7JVSn8@<*5d3PO!y{pC$sHC zEu?!J-!JKgfyU_OUc2BGQd11l){%ZINd_T=ppdP~?`CVz!jV+_jI(F1>8%Z-pWv+K zAGV-Wt|cR^u%=#~EB>?ooqNfSDR|3H;?B9DdcO`@`BkOsX#GXR$pxhvyiPNHH4{@5 z=cvPwkG6)pD|%V3O#NF)(@2&bYY93kg_`MCMYekb;azVWTJxIq%*&DA($iB6RIMWf zc}Q%FUWGH28XN`1;`@<08UI#N*uCSDdnH3FQ`2O9QutxMo4h;{1YrYTABBu(GLRG! zUaayUDY8A^p6T1IJJSo(3FZz9KA@&=R-fYWQ?a?Md2;&Vr4le($L zWF|c02MPauk??U(glfv+VDpBD)d`YI`c*ZV0x#oWq^+cO%2Y^mQ6K1eeQsJ1bFN-E zh+WK)^8Ue}+N-2MSvyl+ifRE5oLv{Y~Ia5Ub>hK~XHS#_4u6j!EOzga~S_AQ1C z;sj>CzPG=xLsbTjzLwfHCr^fa2GY-g55{gNP3|p(?k`b;ocC$Kw#I|$#-m!Jgh}>2 z4LJtAm2`3lnT5;1M;oGCMuBb^Xtt^Q`<=Mz>GR zlhpij`rbLlTDbZD_bR&+1d|iC!I*u+0YSD)*8%3W&8Z|A_4K=ideRx(7zsU(7DZ=<>>t2M-ss|urj}Vq`Ee^4rACOwY?UP zQa06MA(!Rt$uY7HU@7(k%~C`k&`iJ%;mQB*aJuYCkbzb|R#Or46{uLxr*`u-r+&{U zA%And;t^L4%^k~Gcc5qA6aWBvj6tN zk&^<^-W2~waavd}f!cSU=}*!VjnpZOio{bYlhruH0OV=0!CREZja^+u3_#YAQ+a`) z&DU9UQysX=iO59lnLSOLkIQpc{FK5=0DC@z-8IcTDD4YP8i7E0z-wAHI{%DYeo`#! zGDnla9$>0%S?bdw?k=R=#uUzr=a%mq3@cnUVl+%goL`sP8lcTMoaxd3n`L!ZUKQ4% zGvDuEAZzBTk>&Zsj$$r--*b5lIZ9JigW}>~^9_6Ad3K8Zo#00mRH4SIuTtiMZ$rWE zF^cZME^+A6t(dG)LxguuU#pNj+`;GAQF8y$3H`2280LJ6a@PZRx`bWjfEH-*t0+C|WZ3rh=dMJ9(xl!lwr8w|sxRjx z6U}w{c1@&UTr}mk#2ZWfBe=9S#m9eiTySBelZ87cgk7dvFcNAY(;}t?v-b|{3i$yk z%O~1#G-Sn;4A;)LER~UzHz=0h#SuC_^`o0QoGp~0oNNw@JM)5%J!!dwi&Y)&clSrC zwa%`%@V0Q|ed}{}HRCH;U7<{T3Cj^va(JPh%QfZZl>ApFlMfpzJ`I_|lzEf6SJ4I? zub5$ifw-$;Y7DIp&|BgXxdtUnDks%m>PbD65wm9}CpniH&vQC2iz2V>`ra<5;Wgyb6bG;-##pmhHuZC>Oat<#d!g1V_hn6uNDGv?< zGc)nMy!wmSPX!)4)_fUjxKA;b3*hTlrMKx5t;&|z;^h=UI5W;gX3G1t4jz&RIcYKC zaJy47SM;47cr4A7Nz1HM7RclB%rFn*rroUW3W3e{VGsg#YHgx`zMfuHRn?E|AvY%P zwqknq2b4X5oM$v+6G1-;HRNKbo^GQUqQOqJm@c^gB*VhfY9Cscno%DfgHh<4_nqXW z4}79Yxn|Ri8dhWcadpkCk)@sfX5xnc11zT}#ucJqrqXsK5$?Ib25< z{pG7F`UCkZ-sdE8_Z+5<_hB0vG5AfB_dZ$^;Qf{60@C{pG8)Ad%_N3NTi+rpu0`fw zM4#v~x^CKi$?J~moXm3AW>favT%k1WwUZ_quyOxI?(&rBlC!2yFzPvO9ZGT&g`$&4 zzNAf{>G6nqWA$|DOnAwqT}6sc{(ZN{;Bw++XZ8%#91Nd-H=XvxWVd_CXg=Vf4tO-0 z*!C8}dTK+!ukw^KCpt)*$UOyy99~mKp|}Ebf*e9R^>>)p@!z*FZ`sn{&`C!?7&CHf zSCNiV@CE(hlBxWb8}GBijdr(Io|aQ}2@L%&CVc{{>DAn?m=4h$4fF=1 z*f+V=lczYAUdKPNzw3;X1Pf!`IX@FB`4d24THk;Pn7z&kLFGJ$|aT9IaTPf)4-o?>0o%LMwe6AK$(wNKMYBq+#&hrME@(2->3X zeez(VoE(GAaGaA=?sC)ruR5?YaOHa-%+wNtONGN-IkV{Y1tG%Wa>X$s#@o~}Wy>u4 z(tPY2x6IQEi>JsU00J(w9cDeC2pjA>rUi+S-zcvt<@0YwT{+v$=7O2cszC>n=+m zL~(5B8|nffrnYuF_xL6r_}rlP+d^82LfMET-eRn2{ky{0EK#LSyMVhT73kODTK9aQ zL4e%U^WSLrK#^+AFXAt7usoj$Z-K>hMOl_dYw1g zdci){FC#C@b?q=nHaxIgisRx&hpT#oB_SXP+B*4m%wqLzw58eZbOHQ^dTnS^`P{@8{i@{cO{~y& zDK50os7mO(cgsnv@86{47m)y*30~;)7QfqK!qfi$BlK^$7FbDFk)`88xkeMtSYt=t zGz%uf8w+;~G=~@hC4{eGW-9-#(yNnsYBYN7G4@T5-pbySo7)`!e-i*kRVw7Gi;&w} z>;C!J_}*Q8l4t79P{Msg3+$9Z-JvcwsLFrV{HfXt5OnnegCE$q$sdiBMnRvn)TEkB z?Ua*ORmxNsV~Ok#<`z&Cx=_0aTzFoM=DQ)HlDU3MIK1ee{Eu3hvyr$n2uq+orFMD_ zZ=mcRbO|!!N#~8K(!uARmS;WzgC1ODtA+`a!P__1p z5uA<_oZy;4O(z!0u%Bm+R@i7#$Q&XOT5>8Pthaq<^R5YzI6{L1WG?Tl(KX5Gfnz}) z15ABQizFdTXOeZgM{N%rYE;3m=8S?b4rD#WEP;@&#ORs!oM!kcFmMLfXS!1LjBkwI z6q_B#f}Ag}rAKnCzWRou432_OzZf+zS8J&q?<3N*RAbXfljDJ;j{>l;e}(e(kxArX zoGo1|pieJp8Gb9;2u01NaE4w>8zEWhlcKd5JjH9;jWs<6-u`HXM|C0m`DWfun zLA~yO9FO+^^%?6!v_CCgiy3y?*B~>gB6lbt^{4`j&@%hV*>8<9YbaXS_kX zqma9W5Mq1BlQ2g7(i(DIRbt=N{ZzU^ij>|^<-c&(8al(Z^WrLmB<<|&hE`s$su$my z$`=Z!lH71TupK_q=?$gT7J(Nc>rqA-t$b?lO*kI&?@>!o`c%nDP*?2E+mlgSvE?PM zzMh`oZysh(gpG91Ls9*X_+dLzV2#^2mB3PD1m=g3#nYYe^}kBYir;jIxXRJW$)cOw z6FJ&;G|2EkE`R=R8vS{UEP|XFmtz=;)JYuk5EyxjbIHCazmFz1UP6cgLYR;A)SE9U zfxlvFI&H8px>{{`Odt>rBEft)f!8)zvDa0WG8YXtATIVs4m{+^q%wGt0q_3y<=&i5 zyKTq^6nx`Mqt%HPkj${?bRG(reNVA8SbS;Ct1|Yx)85*f#EuIU6lqWR5J$o6j- z07M^CZTf5%$%rKV@Msu9QzLLY9bBbT_ZO72#?t#1qZ>{Q0(-z0)-bgwP2``Jg4b7! zWtrFCE^fTT1>nq%A|#8 z-ho;)&91NOZK z!wk=ssxdDv*IU^Ec1EV0@1eydwoIaB2V63qxE8)--J#KR!FhzHsn)-;^L8$Ji0Sl+ zpGE(MY$RAd*qs}$8oUIAB-YqQUla(j((}!(Y841tNuwh~G90J40g@lRqEI_td(v6# zBoa71R|U81JUDeSzxh{BDp`F6Arg1*GRc7GsQ=K~`M8Ef0vL_$=wMUOD?4c~?D+~l6_cN&MFU4(s z{)`VnHdGydn^zfSU{FwZIa_dTI{wH?Hb_hoD$0D7QFDMZ&Je?ds9(sy%9 z%>n~;VsTmegGhDjR}Kkj*k`k(`v-)#X(+RiDtSrkkdKfRCl%s_VXY>sLAw1uLgM-gDJ2TDZOF(rkETmQ&EctaCl3|C^DR%S+ zW-V)zY9@kCtjKJ3(I)MWp)C;R!aGnCrXNdNk|NH;X{}-FzJCVjl^1Ix{Mg$c&pq3? zzdQG(`}xOnS^`C8HEPWNVEu%-c^?YPGgsFEmLKloO#eDsXa?PbpQ;n-i6Mlu7ky`C zC4P~8$Hr_eNN>5=0!!e%)u%Woc=3Ieq4LZ;q;-)$wmenXaN)x zY#GfLJY3D*0Q(P?yZX1Y+N1yIM9v$Ut#UY40Qy3M!@}~!qKR-g?flx^9~J7&l=1kz z!hqZ2`DR}b0RrHN2WVFwj>JIIh@ ztHG%4`QH4es0|DF_Iu!n?d?i6v^p0b@fNFq!VcG){!-w6-}D5EtWNbYD07xG;EM(J zr2c3vDBP154=o!1&wcfX9y`pTy=AXX`HSao_GIY=w|N_W^r*v^@`1LfC&seTun%=9 zjVavgkXQPX0aBi%y-PFE0;pzkeMD+tr8)7?H5&tcM)SeaQTi7OF<8Cq%-K51V_*JL z%vy?m2gIysxmm0j_va!X$EY=PNABmmXj?+^))w~2mZ4&iHM{{H2*c`-r`ye_Z|fZn zf6@y-?@wa(fwck9&wz-cn&p79AChR7(adgFC`L|d-S3G^W9cC?HXh*WkV!;P4J=qp zard~!Q~GuojtO)9SG&TAK-fb+&tuOtiO-V@ILAg$SKB}$OIKh2Z*?^z8yg!AzZZ_a zzJ7&vJ1||CJrbW^q;he0d%GLp@&`vm7!Ajex_ZK7LhkS${Gz?%f0*EI3CRiXo!p37 zQ<~vs1+pXvQ0<8jg5Ppjif5Ed5aLQ^5apt&peTe>W}b@p4_}F) zNs=T`*ACNoo0HSI0c`*J`*~b?;&g?1b8DS)25iqi8kn{yiuc~99^c24J_wmq(p99r z*<(p&;T7Q4Hxu zNRTlh^1BK}r$=)JtheaRlx~3H^GcmoQ}{J)dFJ^%+kYGlChw=YjVZsqW`y>-GUhfP z@Q*J4MlS!Lt^$N$05)YfaKL{u=~)lz@ziyIwr_NL?Kjb;U#0oL>=j^;a64G(zpo8^ zalFd2&3^7!cn`3{SRAP+kc1!fJwn0l3M?^`7fk<(#egG&0m}J5nC~9w?U%* z-W%O0^^|#=TtDf*k-H-I(y1X`iH0~45lmQCk2%ou5wCq2sss>H{BPAaOYYrx-P-># z8vtYDzbnF|q+fi1snS)hw?_Xs^#J`(wZnrGSd6HJM#R_nYSM8FbL_1Qi}}0#P9EYe zs-uq<9U>)rbz0TxkD;6uK_L+NowM^iEekRhz?mAI#|}#4MS~My#fpmZ)(_{bH8?%! zje7!4H>byj{kMeKT{qPDRJ4s&M;X(W?wGwbL);@Yiuslo0N|K4jI~cxVeaB7BL>{j zz1z&}EWo_!URXdgHZ~qfpbaf7B-hr~j-!zGy}G*cMV8_3O{24fQgfy*G^QN}Qvq7Q zC6;yH);3Bd_WD%*a1=?qycPydQcnx$TWG*c-rzN&E^ltIdaAqnBktm9; z@$ypcn!H?kDWCh-y^ro2*4G2maYpy+@#;qrtikO+rHB~=D+j}{;|acqxuQ_{!AxDR zZ)e)nH|p-@ZRHpi(TPr?X5V=jjn<0o-t`nNURZwCxYm>d*t-oBZM8@q7M8ehZ1w{;zkmQVs&DPuvg+zs(8a#R;Hg*p zSz-ES2iN;qjZb#Ev7&=@KQRaUBEg!`vOi_AUUFnxWd@2Dv4&7PE* zVbTw7(x-W2Ky^-_`R^ML#Da$mh&!Eu=RAw#;omeKqZr~bQ7nd-P6W6+DpM;Y zhbBTN_u*l#gZS){xCIS{#g8A0XWzUnGC#&VxaKRxOd%y5)M?4s42y0*Ea1`Y8h`iZ z8{WB(W-`H}VVFCr#$Sw-ATt+NGUxWk1P%5s-$-|qSN68_PWBy|xPzmUDR+yF*!+1p zVknuIllbsmnl zU6#CLoD&PRAHEf<;cNL$BB*vzG=n2mcnUXXE=C1YrxpS+k#91OHTJwzWwqNFLQAK< zR57w*QFi$5+yiY|HU3D9+PDo#gn{VyIUlIap)M=)>lY_}K`Oig$!v+VV2@CIt}j!U zMeN`msuL=Pj(zJAc@EI5VrVY)@Y}1p>^Ra2O)VF&U%#LGrGI|uC`oU{o+3x2EMEEw zXWU&BBeuA}d|#^#A)I34VaT0El3dlB`h0!~6o<~ZPgahCnc{N%)cwykgQsEL+@h?dokLFI!h62 z=ZnvWFIj73d$!DL*M`=`PC zClb_)5o$Y?dK6NAwr}M(b`L(omrHlorCQnqpP=Q@idglN2eTm3wY3njJEw?Z_l^)s=< z8#{wC;YapUu^-3@x5}=tF|uBRKC&NMLP=82k>uww!F;WOQ$Zn8AEuxmWW#GO`eekN zCWDauleO8Yx1H`Kzzno)#7e25^RqJyPCEm5#z`bwAh9HnAI(ZmCyrzm@29SHRZgx& zPrTQ27EAZr;!@@de&%5OoscZ)d6#CvrM>>q_9f<4A*h+6;R31Cg4XED?ryO>Gcj(u zsGu+SDgQ$ODUm9_1;Z=$0}YyA-f*6 z+zLrHkt7wqN+_v{&!M3eC=`S#)xP+zd3f@YcL?pjsPU9A8EG8(n#sXW8oFb*{r z$mtDk>fMmOOHJ`e@IjX#m4YU-DmrCQ{7j4QD4Zi!TO%>UWg)P7AKlA$HVHmIu-wSx z+gGFttY4Dqam$%=O3sQg5QKJTs7&TS0?j+8b>}N_q|Ie^dw42~aEe#jIK%O1#}{XF z3|$AtKjYhjElo|yf;Q{pJ#*k^QT%WTEeKFynw#5{ z#zIBLQw`>HbeYXk<_m9{ipyT|81Y+B=)%wP@RLXP&Zn&Qzjm0KheBXjeAmt8Qt-ju zH=SZ3=#2=-5W2Yj{k9;7)LP%$c4P!R;@iNqkM$a97#r*BX={Q62XI{7+_<3rNW@H) z-13nS&}xxM)5{g-9Zs!ALGwF?xSJj(Yxd%#%LK|4_O@NX(G{*AUc9#js^VW@P8NDA z8NcPBEuk(WBv7X+tr-cgH!T1m z#tvb$S;kr6;6LBb%eI)(aLLQuIa)4MDr7tR`3{uXml6j?g?KCt{Hy1TvV~ zDsnECF8iVDHGMp%_VPojX9?59vf|8HU?AUWE7>X7dbQZfU|)k^)2||={q#(lJde{c8$-186&cd)c1#cU zb~dXPmj-z2ZuI0sQoGEPA<{-))1;%pHxbMIJF zrdCvVg^T3V%CW*pa(Bx6+xUV$k&xUtSz;+~Q_V?+Hw&_;yjt zr_H)_pPV~kin6kz{;JUJ$vJ>d66r^*`H+|fc|?u0C!r|ZKX3Z7m<10##evnWKqw-+ zAYf-sev|fB><{4>Hs^e8=|Kcdoy zzi+5xjEif6sTPWsm*+>5OFLQw8EWW~wlpD=n48&0KcQ*fMJl;j`&va@m z{+DO78!D6+fPPk9Ip)XcX{s*qCC_eDIn;rk**BPyj06ci6W%pJPPCs2b!&9}4U;iR zK?d_7UuUhi!=3A`Bu*6EHDsANmrk-U<69Dg8RjT8*>!5gH~ufH``VHLrDLC0%n@#p zzdwhll6Vvcm>$3T*(u)?w5kk{Qi>;gg@MZbzTER!a*8gc+_HMvdsOC#r;n6=k$J$A z+G{5>nluOpwqU3IodQFdXle=Kl9J%r2P^M^LgXks9%aA8{rb@v>MAJ1J2OdRy^?$O z0+_yy5jEvkh%a`fO^2O47@Mr```vkCuT~M(-P8+)G*2woQNf*U1+p#nk&dO0ZXc)7 zQrr_IP(g>+LK@}J6`|oo`_qq6WVGRvgu-06fRd3Z(RR>4@W;;fayt|}I ze5B(v6A{(*OWxE$_f52Qkj4h~g@^H#E#G@em4P?N7< zfwNl@ySk9EjnZ~pNC0xg!QF5n%{r4m&)6uE9zcW-%N)f$;2(Bb-T0yZEVVUT3r;d< zWAtq3x$ED6*yog|c5o4f+Pl}dnuPZiQ=PPf`!TDpI0lYh?^nsZzBv#eIkHAW@3 z4^8)mhyLeOiDY;z*{@YE;ByT5DI8VUH+i=yt2oQOw_DMHnf9 zGV9@`B~l2$RyDv=rq4eS6-E41wj~#IAg<`E(l_|CYGYzPTr^NrQcI~EXl&5TNKyc; z@^l=(>Os%v4VqORFFI>%$82WKRDq*)>9K@r@X5+bgc|2}qOZg8nkOxmsBYW1xp+BQ zQ-3nfmR>J_?@D+?Xs9V$G-4=ljAp3L$sMW@#(T)knD`XUbABZ;`)FmZSk}ot8(&-> zShN+gSX|R!p`2v=r99)feGd&b4W(mMW)>6qZJ7C4s*68P81s*##`q+~`6xz!ZNYiT zjO8&?JpY`ZX=4QoDGTpj@*woLH1<^Ma)TLc)c#sS)HEf!SUW`QCno!T&i&jyHD3A@ z3Nzvz*NwS3p*3hQq>#1L>U)_?4w| zK(q{CJKm?my9ng8HvElSfuboyCbVOS01nw#l}e2gZV;1OTU!e% zl0?OLXuODhQK=eAgGKwbJ$W7?;4?L(Ff3|i-pSq%lD(|g+V5%|f#gfq3HJ0vZ<~7F zubve0iNn*g+K6^v0@+fRG)4r2zTxjHV_$VJXz6L-0(f~cUgBuA%L~`H*3VB^zdym_ z>w6azO2Ri49y%|EXfV+taE`wPcDkgtHF8b%{vL74klgDfdy$>H7dEtn+8a*_9Ov-N ze5s`{t&o&|s*JTv;Y6_!9!5pkG}W;n@F9{<&KthY??_B}i3l!J8~yA;`$9*AYB7;P zL>4;bB%KwIYH@|$=H@5=27Si!#PGbHCOf;MCKK#%Cs`onHi>CbPVaaVO18-THynU2 z7x1bZjCgF)oq5y9}-s_kcu(Kr)eMG>r(`Jyr(J1@f2%sAKCS%`j8(OX%<~lim~^MKddpi z0b$p|<@mwm0=Q*C9rGGrnqb2QRPuL5^QW2dFUEbO4 zf(y{ISGvQ=PSt(Eq}RMy00}_5=P2$7@LwoYL2!v z87@60^Y(BH?B)n5fIU;|Oq`1yo4kXeLlYixAZQQSiU{E^m(Nu$=l>NNi&)4l$=8`e z_7=^5fyIm`2<>HrTW{zLcQv@M3K1z0xH^JDg${2H0dKhZ4yEHH%6Z=0#cqyc+RjM5 zl%*d30Xeb*tlF*KZW%TpP&+-XT_mUu7I|G#WYgrb|L2 zCHuD8A#@5evlPaSE~?Tduh}8bfbSy_jjgZ?;lrTO&K5#RCsS?H;AbxeOuvin3KjOv zzkcbQSgLlE(u%PK;#~!au|{Q%WRbO}JlBsu-+EM?eisT9uyWAUjTo6Oje_O z$m)adt~e@I?{{mLBCHi}%LCq2{Ucy;_S*4&dbFlF;Bd6+Rl!AjI0c@2_U?qb1y?Hj z9K`^^hTHG!^K|w@LknUiXfLqijwf4%19YI9;W*q+{p9Z0yp0=feA zYT@yqTU7E%Nl0)FYigPM*EYVWYMzSfd*6?3^clr?r+wMF$ld?NLVad9G?}{Pa1lB> zKi|!Iquy?wP&1obZ=0i)4~-*6^QbjDxDWuLS{PA@S>$YFw*25B!#nc9z+XoxxRAzD zs?8U(O-nHT;*HQ{Mz-Q|rY91RU@(5M9uh`k$-u%0%G!n#bdw1_gAFaw1?LAcXzb~u zRL@3{;0^viFbD<^GSI4|ex7Wnd`bw4;I3hPiA6XDT6KnA0w z&S4|ZAWqpeu9Ubx-(2e34kbg7euW%PuXizD*YqoyM1||pXIHci5XB3BYbR?gtZJrG zAr%3AUwMnZnQ;zp=o_TO67_ntyU!ZacH2u87;~Rap1t8V6MdDu&-KEqda12VFbVV2 zX0?LF+Z$KhyWA&bEBsXAVu*>%J>59!J6eOo1L~;(vp4`UCP!m9SC*5hyuGVvJoh`3 z!?{v#VF@m^yg74*S_;9;2NQ?KQEgYHd)I|3=}OM;uBYdC5!zi@waNy|?{A#eV_zDK z?8y~17V-q3HOC_xF;Z9eFsHK%kB}S;ub7uI{rYZ1+@iU$)d}(L@DVFTB8fpc0AkW& zB-Hp{*{XJqLs$aQT7rQ>;vSlx+~KqOch0{6FJo&g@c7oDfA&E-nV~J9SN{oDuW`X} zz%p?KM~qE>qR+v_K5x5u$`gN7BeiE+#dEQGKiYq>I9Vtl6SeRhiinJ?fn6IK=zTMz zX}5QlcL+oMi7p`9_r_$I+MHEh#Bw4}}WX4d$B7r}4(RY+SFxNC$TMkcrS8^V@0C*cB%;nw#>uqg4<2BI=-6 zgR}7GmEnMQy@tRn+TlE2Fb#cyjPJ7H%YMSIPQ94PGNdwwr2KPRhN6eRWipw_1sWDGz#|Xa}YtiwxoV`{?rLM zHHx7U9U_e87cvrDP;?R+Cf>6+8g^*cwNV8Q5otQda!GBS?%_ubq8sc9`Nk2VXPlh^*PHoff)-zKqTo)2`<#DJp&IPUR<44Z%oB(f4&W8?aqC=&A|5TdfPpz>hF)6@HKcyz17CY0?&Vh$8Sq1f3YDizz~Gj= z^Lxv5Inow0hN3NzF>c*&%}NJ0+6R0W_EOu>J47JlRlcu&UQ>v)Xgmb9qxpoTsOl1i z3$!+Q84R7jjBiouaayV)S1!o8{6;Q#x^j7EYc!FE5EQBDm+sGUM7Kf2Na-68pfr}` zm=h5GeN@~m1q=HC;fiq(b~9M|-GU_CdF4t6*b{;0Kb8phrjfhS6_Wjtk|U1O=F6O=^Sykn!mzYw5^ z3p~mC=2|ijs2~AB=i=GW2no zSSc<*M8WR)Fq1Hp{g~r!4+)M$hom7Ys=6*au7O&|JG3ib((pEn#9m#A1cn_FYm2~A zVvv_nGM^=uv@V$%e;pxaKGC`bB?< zjmqrm8roayisNY)lgX;Hli87v@4*ePzGIKgxT$`yP*6BugA?Y%kxRoU?;%}jjU&rf zyX0k8dKV_qz#y16-rR*|0KMHbPgT~H$>pp_3PBCIG2B%xH^Pr+#G&Ssu?N<-r^Dfu zN3PP=`d4s`sfqgIUjY)80!WywX1^2i6B+{NObBC6_cuX3Gg5_%n-s-eZN6?vS4XDP zW>xlUUJ84!poIK@`Y{G1$jjlJ+6(vNMpHy{UdalW%stz~tFPv3#AdfGmx4`ta}2q< z`;)Iy`Xf8*!CzM=aEwZeJM)uy!|Hf!p{4IoWXkP}W10qrp80`L=+*aW4mPo?V6@SSOE^Ji=%uw#Zg-9%$9bb%#$d0tj)z?GAE zi^rwXd8uCP25-9_I$I7enJ9chl|iN2CDz?P+q!o>tXTj@cYxk^-QDe-A4h?XIomW= zqa(Y2t%=%i<|}G%hl!zQVH4yIA=3GZ=n%H&Q(E${!`}8qX|Py;#N3r*|6>o$4rkTn zYik4@Dm5{)_qiF6s2;dB`ihw8L31(%IBMu(1V*1o&S9ZmU6n^wm$L>_qm^Yjq|^qV zL(bXWJ`B^)`NPT|krt7LJhJcB8&yzf21+E=o^3hAiB4_{ee-b1{^495k*z$l6^c9x z@0#*;h|_>rX_<=y|GjNwfm^jU{ipe7c>TR>wQs4KEihz0KI`9xmpg&IdnKX4%U8mn&^_)t7=SklK#^1;&y}zkMsMm)9K3@IYc=&SfUadXX%jjye z8w%q(L7l3_TU<;dWxI|9pt80y5i31nFU29tI4#^D2Dmz0Ta_1n!-TrDj*Edh&H(AlnRI97;)1XpXnJ+;70n6-(+q)uW8Urw& zja@zKu|gW*Vc?P-RSBX2XLRoB`^kLuT6V5eGV<()m1Td>^w<&{#UZE3Tp4|hKR_e4 z$3J*CHpU-O8Pan!W%)zi{ikFF$tP$g2PRhp(GDLQ=(Wwzn9GPF4JQUQyw}+?Qh?X}U39ZaGcm2VDGd**7R_4S@ z(ZGYOt)qyW>%xwPR3k;7p7vlqxFHIy(2@Jq-uSySXW;O}li5m=v}>lG5W4~lkV6&N;7mLen5l=JD+iB*g7C#rK9Tp({x zzcVcgf%rIc?Msp6&*dq9)X0^K6|)?6v@dMN8si`lzM?mloxdO}+caNgrzr}McGoI^D( z_hMbqrGp6sk<(zpuHihg%vm5M;agHFH|=C%9h@o73CT)*B(`y(&bAlZg>n+G6Wo)r+c&7qZHizhXW>u& zap1`Akn~j2B0A%&M79$M83->Qyxf-Po0wHIji3%J*Oje(j^YpbJV~OZMiSI~k3LxG zD3H&B>TF~l=3QF@w5SA=%OO7 zFb37JyQc0Uo^QHap+$=#dF#Kl33Z?dhVhVLUm&3Y=&*VPGced|^#9uzz(G`YW_u2o zPy|h&z#0{}h7oG6NE^0ZMim}RwFZ@G7xT!gg(fgUcz=m=<*n_am#+xhxG1E?nSiqS zy$d1NOaQ9i=}H+FBF_&(WOEnJlwMl8u7~JzC((uJz4BqK`HjCu>g%{GRln2HJ z#$T2!h@79HopxhaWKXnsI(9&#FF^D$B<2u}fM z!We9D=_~so2w)6Errrh2*u*XUgo-9 zZu^2NG7-3(&&U&;MTCVzYij8Kb}bY*sYAimY*5AoHPlek)XbkjSx@D2KEnW^NwRM- zS2hHvZ`ZM=$tc4ci)^f82q-2X4uN#uNgi)1e67 zuhb2z9!8!k6iT7Ke=h_vjDcf<)l0~r9+UC$ioSM`+N}FO(kt2x9adrLMk7T{uCmYnZGfz{&Sz9Kh@ zf5_V7nEt7}?thJEjiLyCVv;gf_x4a<(zWN9c(lnRs=qvqf z`MU(^lb{UGSAVe1&7O6>IkE8_4|+k8FBClnHgEzZpK!Nu-=S4KU|D5gCVBLnINYnC z3Qclp7>dSfCe8p!Pc}MLIbj)9|5#0d_l~;UJ5YuIG1}HaxvjB69+RED$`IV z)C{hxg~yiiWP!b@yVElKMv97}j-K>D8OD3ko_6roM{S4;AjXv6`~4duOGNLhY zp^2$qth~R-5w~rML%`QM&w(usyPTIW#~u@h+Q~F>yyl@qwwcc7NXA(gfiO+b;bnHD zA5JlogVwv(v+fW{T>WUyrq0Yz(lc{{fsu81vy-&nh1c`;t{}V|8oEjXQkc#=jwkDf*S#KIj}j8!30USiPaDxx{%@>BFpXg7{Sv^6tCDkJs}~w*Xn}z@(|On z_L#&FM@(Qg#(4I_y=$j*iH-3P*JqvDDP=XOaPvjCo{gJ|CAC|{TkOX{9Xl#huUL7$ zT4-5AdP8`PK;$fX)#mpj|j3rpX6 z8%f@cOI0{Xf+>v$(9h9^^#4GRWQseRnwlbTIO^og3@2j!e^mM@vh6a-h;h8{|Hw}w zKQ~Q^1|DgNwU6fVMH#HxaGd&`6C8fyi^P`Bc1BC!<4MG7joL4xShGRx{&p!48BnTd zHorcKDO}f6?gX{Nyx+$=j+CkuaeFe1G%E4I1DQ?$%Y z)1{!W1Coty|F&6LZ2w#1t@&(xJ`1nG(2&S-pq0Vy_IpE;Hb;92#~@4%!jFOieQf%S zFZmLL+L}}IQ|^c;W_?$Q=j5)UJ5ox{4VsVG*$cCNh7sDBH0u*QT0;L%7Kw4jzj(DCD zy0Pu2lg!d@D0O^P@lmk(78MH_IRn@n1}CTD<*N(lCfy9N@7?RWk{E04ioL@}b|fG9 z!No;V`EUuNuhk-`{KsQSIGnd^_Erysk0O&v77TP)EH(nl-w~E0(v0s)xveS7dCkUy z7rCCyhY+>CzK?>Y0ZxN4|634-ZZt`R*pm=qNu}CY;9Lvj+cQ0~rfk^(0Gs+mya| zde%+E9${H;UswEhbclxWw37RK+E>Hnh3WVa9^Qy8ED8N)=M?n5JkQh47MRhYY!#sK zV3o+ICiV*Z`={c&PTSE}UyS5Z=^N5W`p;O((mOv(;h&sTF#sN%s9FzWb)P^PIqGW1 z1^1+swbfNL^#KBdC~h7e+<+e>;-(930Hl~C+V~>$5~W$iP(x?O3(&}hyb#wUl4X11 zTqcjX0tL(rtBM+`Ie#YxMvBqWDYJvxpgMk=omlU%e3QH5YTN^%4i+cyGQ$w|Uxt$X zJ!2(RczcE!VfL9pbAvlKD>nOjEr9j$iSgMlwHy5kmlz4bADrTHd^U0}?Pp6k0Jn~`SQapR-;=%WkZjcpKk zMotBy!Uzva6Y2GvU~zQSMg#4Qmz37giH)###TmprB&iA9Wzuz&q8y?jFwp|u1Wwf; zgIshuhTd)z>b7{47gM`TlP{VTXZ+*K$miRyzWh*z>iDqyZM=JQhTkfSi$-NVyRE|+zb zubQZN)ziC9(U>0A>>AQ|5>>X&jVE5BMi@;!gKNx;WF?G!AQHxH?LFI`QR*V73nK1g z=1Y`wWT%vM0P1O)GH!t;iR{e|Povo)laY#Z;4$nN+w zuHSq)ck8bT(>V*jsIfm?3H;Fkc{dZ=iLe|f+CB9nDMW1e;#h=$0(&~rKqasDW}Qe4 zLPp*~Q-^)wp?_gI(Fn(__OB?R7nzTSwzfktmd}C`Y2qIU*VKH?!lH-ozH(I(n~o2& zIknXSB)`?}fgIxIa^EAs{w14%;n~W@g&6H}`EzRKj%WRvR#elcYl&KCoG{rES(h(= zqY4O0uWsKCk6YhgfLb$;GgGSIlpYy45H57uq&iB&tj(;941)neySsA)?Iu@R{Z2|1 z3SeF96WNIV6bA{hKc5GA&f1Int0kV$9l`c1T>cd#U~|XW_)PuDw>()8F{ob(rpiNb z=q8`~_n5~%XyP-&JF38Y!$_SJh(U&8S-RjO=FHtDFl&?`q<&usUc3=j#PZ>jBz`vZ^xi3VbHY$tLREbGazVmJ>kH~N`R_uEm6qP3|6F=J zWmhKn4~hK7^2q#*M~&qTGOlF#5A*-3C3EUT5bloPE1>Db6YE$@cBkwvRba3*HN(Ws zwLF3lxVyQCG#&_d;^CB>7OyYwIrPWwb2Mpw3oMj|bdO|nJ;WF$jcw(Ir_vu_khohH z#;3y+>@r(vN6P2(7C#uWcv-amOmGcYJBdE}rSzYuoEp)Kol5tpg8tX2fCk(!@7uKm zagN^I+PT(!o{(0XHT(nK79GRuaulm- z@K9JxSF!+g?KH-Pfk;XKO_9~lQcF9f7U%4#>r?P2OLI}IATlBlpFOz}OiD7&C^U0R z6C`cgp9-DLXuY;(d43?5qb9*=koS=yl0C{MR!1(FDHFcJQOJXtDNppuyD`ohFN?v^ z4xr(aEL2-@?|A5^C3U8af&q_J^)|VBwEEVW{EObHz_Sxxjj^6=Q2CdpNz#Q1ll8u% zu~$plODJVUnCkwGs6Sdm3PXSn|*g{-vr z9f6y;$=8j`FU=%ywD*DUI{NxB7tW;P05mn*{iUz}4#zMkRbL{>{n6&%=^lE2dnwg! zzt1<5B^co2HF1ucWL$nFJr{Sw7 z^$b@Jl(Q1q{N+|m-^wao-#bbVK%kMUfh#VTI9(6oRS1 zGeU1B!mpJgfhv_>!AM0!60WJWg&1Ui8etwCswPG zADJmQTTA~Vt~|=#*yER1n~Bjx{z@?{1BWw-XT;?<*u@NwOZx!2H~PKPq=7}LaE(Hd z^7?_9P8ewqS%ahV2Eqb|AP((Sgt`l~puPY;bi)f8tMKsBuYWu9=lA#ZW|mDnUmsL2 ziF;o>{guD3$VLm#$cE@wTr+m23+BWW1{O=pEyS;#k~vEzVgjIZuU1bUpitE`wE`2W zA4zttxmWRVg9XL*PjzAUf*{L;^d zkT8pD>B3;cjv@UyL6CUJ_Ms2p`Eh{Z2QO56G{9_r3L|r<9E*m|-;m2{;3894YQ_mq z^s#%nYLoKnj0;E0+4p=wl2B{}+Un55?4w6ToDt!}y?&`wije$e> zZEWz)S@eZJoo5$|34uW-5d%GE@ewq!spLm4QDugel&At#_S_`sAMzXggex~gPDhiN55sJg6l{> z*B`sS?%{bnOS48=hE~^ehzuPrw=Wq!P6n&1fMzSund+{ALpmT=NG=rIM0oAvd`Y6G zlMhW}v5=?B`GO1SJrTz6J*EnHw5^RPcCdukbX*s08bY4Lyreg4ZPk=joppm;o|8yS zJQ{+ft222Pc!Z}T*LwS3$O*Uq7davR-^htrM>`gRUS-e)5mc(Jj&=W*6%W5TKl&N8 z3VAuVBlWQ)yYlczGv@PRaiCr(`u3Fa4A75msG=qn1h*qBN@58OQ^%nXc-+DLYvDCS zA+93_(;s$&=ZZ;_W+8>V5&p2EOkI+yI5KBODJBSO{D{TeHlSn1h>orN+B2gHW zXK!2#?)>m!Yv!R`oeA=SFEjP7EN~H*lnrduqXVBEz84S|{9w4 zVS+fDR_2k1sH>M;W{n9=L39@y_-aeTv5?%3a=WA5yhFDaTE$d->mq>xl6Xob_M;_I zGd7OfVLPc88WZgro`H;=V!Qm4xk5_nZxY*BSZqDaO~^(eO++w|rzvN6CFa=)4SF<&C~I+=LOZW|Hos+U+y z!U!t(V}1kgtHw2$yREg9aNK)KO5C|bms%+;ylKl$T`$8xLXcZZCEqfZN`K;ep0p9t zQe&qeEi6Hv|5YYKS)xTlq;40@GUtt7cAyU@bNnk3Lg`_b-xT_}YLnk_DQnpzX>Z)s z#}WC*2xA>WNktixe*dr%)7aecx;d1AhYRK7Blo*r@ey8@x2+7=0nE7Th}{1lS#_cR zmQ`;s#KN8j-ke?|LZp0FGU(0RaFy2T-&~n6Q%|7$Yo_+(f)?v{PIt!AgbBW3H$$*N zT1@J;i5e<>hK4RppgH*gjgB1`KTFTTI!3T-6o$TeqoE_FazK6rCBGxj{kFR2Xl;`% zcJ$1fU|%yd)*Gznp@3LAT5!k&liu`TunuCD(pMDn6ktUFj=2wUG*a-)BxjFy*5&1h z%Q?#iY=wu7&=p1k`=NX5j3pFHX6)KH-ubG7WevEz*;`d4hTmk1b+-pr40l-_`v-;o z=Il*&1*R&+kAYhRXY%|DuAVhXd!U5-^DVrAVMf8~nZO1x_jFDH(KB|dnP z?vhm=Vv@sO4yHaS%@1=ekmb8GM$OrnDbeS_^l0sFweEOjiM#1X2lJS<$QN+8av@b8-PHVLFD6Si447Jq^=Q>k$W(D?bJX7XE=@#O&zz<9}vRk%w*z z%_vY~#%NS&W;=)Nh$-gyteb%1hl8bRg%V8!FVsrxPcM7MYQHbM;E&e{9avef5Zd8M z46xNw;41|bS?+BBt>cv!0?CE~-srSjEluF-^hW?wPeo$Pm zxx^<=W&u~XqWG8269#!Y{$zu%V}-Vh7aWrlzu9!AsZ5@7|5K80{6tZE??lqg48)99CtQ6Ys(p`3)jCS#gfSqhz{3HTFYtjRWYn#cL@M9R`eiCo= zfuS*l5#%UA$ostaq+P}-QCRp>zT-gJ6z0@67N)Ew!u7W?nZAd&u>tETg;phTO6KFyv54yHyX5l`{YHe#aq^_ebMac__>~u#lErxg+J$X~s@bLH-?C&KTk#@= zn#C1Z`45w%-mS23>#YrxiihSaR(?6Znbh2yz82c^%Fdnl4?WVJj{f=*`_b%DgB%;6 zPDmKXrg3Tpz*k@VYh--QRj;Q@$`(rd&?0US}A0sMov}8WMsJ@~mgf78vogDjgO{vpf_4{h@X~_5HaRt21h^06|{yk z1&_N0CgkCxBO;1__3`=e5UJdPItu{(tpO#=P)bGrzSTAS2Hw64PmWZG}WC?^)}>lx>PN@(dMMs@f9~VJS2p>Am}^xY!1J_ zM^?3NH}Bo)k`FZt`G~NP(7SJY|EpaPdx?d%kkCG$9dUmMH$y#CtpAK}s zBv|P{2asLt@b9nuKi=lQKX}jQw`OPiP!0=PIIPFLC+;E*+e=TXvmE|%@tCOo!Wy*j zE&mZ4w&n8C+gn9Q2tw=%KU#luO?VV7!?&mBhaY1gl1d(yG$*{_b5K1oi-4k^=xW9O zSY|waH}NJon==`*j+J0{cGOTYKmXDDY~?N8}(eWVw`JB9{i|<%n6)F^B{ChOfutFJ7x*nYoe&Zt zX3vvUE-aZ=as_6ga6Q3Xg)4Y!KPC%->fo)?ak#`wKRAt)ve=c9ov$2#ve?PD&}Hbf%p6k zu2PDgN^}u_p*Pg=_idYPrp z$hp7P`g?apm&8K!&|KLsx|ySm5o}u*^k4C?T&i^!T43(cZwY;yOB`26&EEj4ekP#W z>s|iDE6AS6GplZWg%|D?y4J&ud_F;Z$dQB zSy8R)1cw;ds+NN7?awaU2|8;!dV_IIrR%TcL&apO*YlUH6}Vdd=0{Tq7gExL*UA)%%K+Fw za>OmAv!c+5*NM!c_~Pr{xzbjiB7;xVnB*GyUs3DSfu8Tyr8jc9BB)_);ECmmv&Fp1 zLEb|dY?RjuK`r9kb0%aqU7pP<88?%p`X6aMBL}Q$o;NJOrLgisg_Vem>g9|X1JyZa zH6w1Y8YHa>(gn)r&!`lqjs8Y-v<53(n+fC?QgfkerqA#y)gd9TY_F0cmAWQN_niY- z8ACT5f>6-V1jNKlvR*a5%jL!FPBTBLR@0izF^ro^e`Kmc-fU!(wZQ#~uh(RCSl5V< zkln!C{fOXNDOVv$_JT}zA#+m80zKJm-=`5Vc3@pjqHIQ)e7pPFREgt2{3b6h6 znanlSvwOiHZyWXIOL^!}z3_N#nFnrhRm(BSVx8@X=E^a}IRvKOJPLGSU`pigs@LFY z_m}{x`>HS0vL?*GNV3A9Um83{6Je@vigRQ;;5bn6*N?e0=}|3Y(I%SA)N6Y9y*TID z|Aep@@)<}7iMw@c6nK5Q8i3&#Jd0{PZfz&nqJD}AD~+o~_`Cnd_3>K|(rLR}N|Mgj zUI|sO0%jqOa^zBvFrFPC(~`b*sQ+RqfYu!cwI>M9e=2*ynbfM^1t|P-2QKBSQ}CVb zw5zke(LkX5TLP7(D3DoM>vCXNwOEC*&%2b6W1fbJe-Ryd`i(;nw zf4Ck0!JeM+Vej8tA-fg2*?4niM{Gqf#8zZ4r)Oq_s=LS90CK4poj(jCd8Icg<3oN!Z-#+ocxAQQK~*vJ4rX;Y_n z#|curUpU%3&Y1FMcKO~s&@7IQvec*6|3Ogw-lLr4G%DHlndgy+Gxji_^=6}{_SIbt zz!c?gP>PEp;1BJ?ck3NXFSzfU_}XB}riy3QPBGd$T4;d5>h`@j^OqMgAPJ86Me?TQ zDs#o#keBBajd*zqJl(}T>_J`9(*DqP>Dk*8SUNo77RwSJL~p!;cL-=4u0kzyA8 zbwJVJ+-LS^hFSycp{(+u9DujULLCruqwGhWwW;0F^Lul5v5>+h@}s;RS#sZ$Jux*G2^&YMoGnftMsA9kb5Ru+k6EV{Gz(h;Ufr2T@#HJu zep0&goXX6_(1)(>)HMr(63#|;lWeh92_3IX70_o;)hCwhPpA`UexHLv+qz;xxl*P( zO*_*?v24k?+V23f^xE|B*V#f8P~mc4|Do{e*@4;1lRNF^aoQSd5LzNkJddDxhb3#r z++ogZc)mKi-Ag)WP`A&CY`wkP$o8r94^wpwI8AZaxM<6>v)2*`*6=UkgZ_+y_d|e8 zY(jL$Ts32#j?PyoTBYbL2Y5t>J)_WhxF5GTI+u+|Hn9^l);?EdH!Md|>rI@sjtDf_V=}**(cf1j<~(~oM-33K4@EvmndSg= zSh0D1pGsdYFowDwxDRd&@e_;TeV_hZZuTKJ-wBWy;Vmt{X{(x}L-j4qr8~Bk&z=`6 zHp?)u!!vaFp<{AVM|ngK2q79pA0{Wy>NTp>sCAg#TCs<71#QQ+b6MD&8g#S2slRqu zFkb9ypOX6CWlnpHxfyeSrzaAJca!1qo{Xq;@G!{`UxHo!dZV}QL?zU8*X6d#di;7& zd|0kqbH9FmPW-MOf6N-q(>LXno$q=$@9I`apt+Kq>b(7NqUBpq;4_h1J-7z1>k<+b zHl+4CvQ)#B%`ze-0yMq7LtHZc@Siy=|k6D?6~F}&P){&fn; zxvX&@dx$h?+&eU*<$zF&k*C62DwU%(C-7~7t-atD&4VoG>i)S@qH=54)Yf2zcP$o+ zhm0O7v?jWTc%Cak8l05=?Jhb+ zs`d2lA~q88*nWcHE;`eXF-J?AC?mW2byp#r{S+5g>$|pgM5SD&w`}LeJpEt5#~W8Kf<)E>^yN zd$k8E^n|0|d!udeT8y8T_9x#v*}rWAsM}N1y%k3k3TSX0;pC!>AMtZEv#GbeGV}B? z>V3&uq&0Gql6U9E^_*(fLw+a9EMqg$d2YH*zhwBLsCzK*^uvC&$ITp#Zpm^uh|vp^ zwVLuomTo)0nR9MBHS`?$&vtf?b4}ve;FNAN{_RShdDT`)aEWwPo$qRLChL^>4Clin5!@T9x_pl8EW zBkdA)kts9RkVHJ*Z~Hzx$vx+c=W7K?Z)6D8F>TCSrZpe(5bk#=1i) z4(7#*MuuCDx9eVg(Zz9U!R+aoa?7-E$`=^HZOA zKCdoAiQ}i(wL`{TqB~YGB#?uUvd`@d+_aL(=Czv4K7L-o$jmLzRLUjB|xN zkixRzfUQ}yG+uW)gmDDTbaA3YYBh&>*umtDbmYd=s7JY{?UBcE%1r8R4ZC-wz7fH( z)8XRHU?Cm7o>kQI;*+}k|F!p?QB6hBx+sc(QU#>=7J1($0j58%_0ZeXa9_^wgOBeR93#Je=4ibOh+D0{Yy{I< z@(8s%R?%55-bHim&{#=5Z;%X+Hb73Uo$y*+fulv?UFoL>5j2Q^mcfV&f@Ogo_>@So z3@~y$26!rURi$bXnwNHZ16lSz9(pLsK=~%se`>6|xF8e;vy<2x8!9Zt+~oExy_-3i zC(HQkWG3#93ln49rp+iWBr|e9_nyYMG~rDK+$Pki71;&^qAkQvgf3;=(@RS%_p65E z1R-o2$3IaV?~%$IapTmMxr1XGY4Ntv2}94Xkvo6DXT*FCGPgTvWtu4CP$ZzFKBEWc z`gmLXs$AVa`rxdl)y7|<=a2-nPC-F}ztO)=(j?ukKS^n3M0*Cr>&>#*B;4vsM=I)x z%Awv57PTE1=bSg&?luj!Co*ybHsNqw!5j7OO8a)8vmO{e4!dI)dSO%Db1~HWH));X zwlog4XZ#j2RjuYh34-VQ1CMsa;nb_m0-Yj6hXU4-`@*hGE0p79<%NB-PVvzV{^t81 zSLF`4HVuR;YGLS~GJojn3q$8+eL&6URq#{?2fx&STCDMasc!bY&1mp(?}7TUh?sUg zqQG$AuLQWYXv0oR_O5eikFC@hSbEC+!$6tIG@vR;22hcclM9xkKkWx-mZb6V2-}|? zR(u~|=Ti|Qn4O%9{0M%DP6N?OS<%=HW(g1^k&)o(PT^Fyq=Gifk#s+J+S$}r79q#tzDJ8OL+ztyapZ()1^r{bX!go*%pj=idV|s3`wleI54eB5;m-L5whXL9=L!{uC9kW zhnw%lOLj+VlTc5698$~Mpb+5MtXOLf-3b;7?vRWTbC%Ec$HjS5<%<+k`~Fv_VN*#c zeJ=yxMg`pW9EX>Y^D`Zt8mSIWl4nLKtuT@`g%M>~_L|htN;(fOs-*m!?Aw|76XB#x zjlEAPpYoVTZ#cqz^}f6V+)U_k$7Sqr{!nLY#&2l`D}Fr)Py;Eci|mjPF@0t^(9Co> z|7du;BOYIcP2;dx`M3cnRh~W#vpcTa8Z{(@RlwTw<%zOg1tX3uIjRlSf*+UoX$^Sgop-A>@(RiIqgZ4AKQ$kL57bv(z~t+_!CJ_tJyh4@-G) z5*t7=-)t-iXSfB-x_|rCI?lp}kr^~51IPDx{-`MBQR-MqR#AQ?ukjN*?o>UNPcm9) z;-SgZz)tyGTidby;9TA~wQakWwsy~KdP-R)So3hvnJ(>f7@X|9EX4^n4B(5|LCn#HU|hfqI>I&`0ET zJS4wN_Vk&q1kmq8ajq)wIw;uE4+uOceE7h-Yk3A5I6`%Gq7b;1B zR%*^D!-Vn?51$tS`qBr-ENk%qkPs84w&B?kJ-xrZ8s8Mn&MK6~6tuw_T)Ge;tjL0Y zVNhN`z&f%gbbUsz`6k|r7<0vQy%77hawJ|9e6Nsv^R~YtpS9Xe8h02Id-_WC=zMK- zP&(=JTv8PR=0HVG{>)l_!1=dOVfW zB7b8&#rJpd*|s?2*pxrKKKzY>PR-T{dIC!O!j(duxASEGD^`(xxs=6P zOuH(I=hAT{=aFeu_EwNjY~UjIROVU!i)Ewwr;RzBtwTt6io&SH*BJd9&U1!H?jx+b z^bT-cJ+$hroJ`N~#yJ1L@tbh9iy3cMA#R!u+aZNxuoLx^i-5X`z)4Us>uf--M9sCt zHeF5Onp~?>g%!Be58a<@0x3btHu;khR+b%VL>IhVF&XiT8F! z<1kpHSHAZPD*K3nl`A;5QGC9P=Hc-<@ON?%jU zf#ImgCP9dzvQ)EH1Ofy`_>ZC{GTu`xvN!WCpX(T3xYGNN!#JFANlF=w`dSAl#|ql6 z6UCD*f<7+xog_p;YhMa}vh)%-4P)7T7zq7NG-h6ru5jiqj?Z2@bZbA}VyMi0>hO}y z5QOsKAPl}{sm*kikEkcJ!0f8dHm*Lx_&f!*roYwva#JYV&;j-)T52#JG#5TV`W3j8 zmsJ3^m*UTA4tBjq<6ixX=QO)1BO$PzjYEFy!DDOpR1cKc>OO^8C8dK6d_Rt{7T zB^|iUu^a6_>af@-4U|x@lA^MNySRi!mB<; zHF{owwtgbK;L3co(7qT`lANN!aerLiBa)Q3IqO#6ORgl*nn>4k68*(Q-H)PSi{86@w+xOJ9e4c1H>asw+e6oJsXW+)Q?VX8t z9dyLs9)&a;XlKl`HP$Y08BMhu_xW&7;ip5jimOg#nCoVML8M1#tfX8kU}{JS`+LT zevWxpedV=IKkmDMPPAS_#8}--@sEPp9EclBV+wYXN?)zHy6~%&f%r#vl&7Is3AP+$ z*)#n2Vr0yvw%PtgT>lH`^MA%-|Nn3PKO~(&*&nv$0P>xj9S%glgLPwYeSr=F+LgZl z0CgG9-~TTt{Xe4R{{_e%iSK4Pm{;6V&IWKcrVqDcmJ6s@PD{6jf8slqzjL{nn?6LUY=L}o2eL# z(5+`NSirx7L!(<}TLqXx_Ae`3-zTw6Pgva}Jo42uQch@u0m}x5){}NH6A8QYre`v> z_9`)x=RUEyylmoj!wgX&tk`#W2pTX}F~G!p^Al;-G@j=WaB}>@PNL+s=~&nfn5~oi zj>j=lq3hl{lT>YLuk0I9l(qVC+Uv=DZng&>5ZmC|{N6alS6ro?>%pZ0D^dGh;4HAx zL{hXo)1I6*U!I|qa`N*HG?T^ZAt9?j^n(ZO9TgOf*K?L-I^L?`8?!bn|JIE88fV#_nCfEKe|<-?>$j>CVye#t_y2L{=yjhB8*W2yDX7q|cd* z#>c57r%w%CGWzjxCJ@&j{5kj}C(fsp!y12$SsVUB$O=DiEcVr*Tp9A_tL5%;I=cPvJZyrLI-AKsy#fy4b8f*HAXTy>r`zvPF+m*~!;I zwJ%+lHPr&!WJni1mrq@w+MFPSrm)<%s~_9VQ+^NWCUby!CFoZMC&)m^fipU8 zRcJXr=ZiAn zaO>r1iSg+0YA!9WPh_>Estbx1TS<3jzFTy|q>b|W9vIx`eGx5VgsEb7zAaBC7&2Uj zb#VvAcw7Ey=aabwE6Q;l5wyfEO2_Nzd(uWCpVeZ)g08ctr<^wMeGe0OQsQ3!d{>;w z{}w-I$lb2K`a%4M$-K|fZ|DBZ&GP9tT`>dyn9Hx6_M#dV50fhz2K+|sA6!yaOat}^ zOVku9nb*m`{yh+NOALv`KS~vC;Yz9SUbxd=Kojt3W+3oJ%T%~svR4tVOafL{OuJHy zp~!ivf!CaAd6xUa)Wcc34e}LlGFO|)qmZ4oS)&Ts2MVoA(_oGD>Yf*vq)gAmHD)~V z%9@)vQU`~_BM0nfZCjH~7{&qgj~qd>3zA>+!7)lVhZQ|9(9cRa^?%X5O#4)xDLoKQ zXNP7KXU@}^ zalE^BHLLzF+^sB~vj)y+`8!#iZQ-cm_m!)3H7Vv!ey&?DU(YObNC;}Q+Y7+s)es5h z8yVsIyd%O4vkOY7Egx0p8oc-dr}&NLNpVgDa!UL_LI85+^v0#8PciP|n+S^uOn;${ zCX8t$GqMm*f?fjr#Js}1%0OVaJ|bSwj7POBNAWG<4kCX@fL7C32R3}PA27iDeudE7 z*L?v98ZT+QTF1=~AIVVxHDZA4GQ(q~+>IS}GaU2hS>|HAa~;#?5!=43_ItaYu{bU( zBDm%3CeD9VPHu1B>x$ld0rF0x4wmZ5xpw;v5_OVKv9D!wJMb4;j%!<_o>qAQHylyWUN5Sb5NnSFQ7WXYrcReH)?z^bY?Q z-}8rawM_kSPk9Vlj`TLvZXy;ndl*hXV2?qk8X{j;O}XuU7=s`JRoXgxHD;q4{2ctw z7jSwlXW?I@(j-gJ;L9(TOZ6RZe7m*g-8_9J zNh`Xi)W#L;B7vb5Gb|=DS*9>fJGT5B}2;Y$;Ju z54!<{7!~W*=l@SLD$)P<5ofE##~VPsUOZ)9g^=Q4BP)yc)h z1-n#WWtfbN3_S=0YHn$%(C5|XZS+n9G;y9;TnF_l4-b*$~20AwJ+^);Gym*f9oW!@>$(_gjX@u5=TESpKCz`jLakl;2f@1FnerPPOX z8188{Qv2p-W*P}+NR|-0PReY507yXzJ=AOOc=xrU)b63`4S(1BUVPzTC$Xk&?tqmb zR(zgRxnwU5{LR+qO#zyCq-{6L;37P}e_kJY6=jN#p)WahmMd9{wjGsoY|b88BoRK_ z7Mc0uSGj+;e66rc>qn&#R+vE%J4r;6Q*FidHhM$0&8Ab5fiX5JKlw=;%OD@|Fq7m9 zAIR{HK;xs(GoDQ8#&FYC>OUDZn3ca*Ur%~-s+r!k@jEcbN8G7k9&#?N&3I?oiHnj4 zZnkN@rZYpcYm<`Pj0bcj+9z0~F}Ay=uNKmG(ci>nlbfte4@-itr>rDEWh6|zv0cl- z5$?o{J1I(}8~B^UCT#iZJ{;ctk?V7>HTE%I%W&IN)?Z&=dJZx77y6C8j)}Ut^W8Px zNfD8JW>Cw!&OHgR$M*vyG3xl;jyCJ?0ax;_(&aFfhQ6)M=%Mi(-lyqD=t|)QsRDPG zt8-1?3JdVqwa?OWR>YTbp^0lVN!THCzS7g5I>qu|xxaE{v5D%|5kC2)^K`$VUlS@F zAD;S}pM2{*6dJjD|8C?&Lx(>lmO0p2#?d5y$l&X=V*F+vRmnT%84}GhEH_=>Fq&&i zuwf!K2J-_n)W6OWRwjP$yS1I9T3@>jwnV9v! zT8dfgyA(d-PXq{b<7O+E#V#lfFbsrVKo}P;;FF zneeP>aK`zhalmxmz1PA&;Nu(7&SuYE|E#q=YBdaGo%TX}^AAg0Q-a~=oHP`Txyoel zcOyEDmg*8R-m(O9jQRL4Ez@;rGl7{)wL9d0I(9!h*OBDbEqir5ttb%esH%04*_;1W zFq(I;`a z_o)wFxe;Fn_G{(EnJyfIzfWu(JsPJhHCtbK-|V|L-X6~x!=6TEjdMzQv83{ZqC+!;q&)52~$-#_VMD%c&0xxMp?Ys6>h#e4G6m2 z4~$`=Q@qw7m;C!cK;ik$o8JYi4f?-9!Z1GTqn)(N<6F2#suvlj8EdB`)v99nIquk? zTvh_dZUQLK>|u^@6hHCTO=Ktn+UEul79Rsat-Z4sddq_0Z#|dPD0vNf<6e5Y3?B5m z%Tkh^i}tFRp(&fi!Ndc)e4CL8JnrU4Jk@kH(qPmuTflfOw-bkHzRF@TuYThfnuD(# zZl_ve-Gx7nCwNUtXCc0|9E2I?bg}U2uGP(9k=@2GN?7htK7wEiLo+&#o9bli<5r6o zc`^%S{N0_?RGOva6nkn`mn%T)!#^h%XSS65=|i@B2pW4{{1x{$XKwf%@z& zweZLn$=JO6V^#dC=$Ai}4)6GyD1Pd4bcR0F!R%9sbxnLv$Me}NdtG&dR!`5H5G7S5y_H+k*bfG-=p82`ZCohhrt0Dtg^`sVE&_l3Gd>4?6)zlJ;7{? zio%zc9t?l|@9Q7-Ylq{Caxc4D;-I#Exk#1ITSm9g4lf!~9{VN~XDS)SUTOMrR0g1d zO~C&(r&vST`lTKXp>n>61VfQ76Xx1Ng7^_TsBCh(t3Xu0o5C1y6FFg26`TmS(MKkDVpV)X=}6<_rSV(10P7l7>! zz02-lTuagE_)PIkiG$~bxr*+ukm-HiW{FFGFONs-9(A}LAN0SV*b2r4f~9t)-;5XR ztz78MS4i{LHx+u!GWz(1bS^Kw`P}F~Q@+$ivseR;qcfOOTi#@Ry{{ilS@xo`DKt8c znauR&hF5=Vs_>d;mP`D`7&UxB07f8koxFHbwk9qbx*z)r;zyZ}V#8$3_r(kY4sa#T z;&Wdobr|`YQ(J<#6m80sg7)9RFzE5QJ+Jb5%cg_B!V+0aCPf6tg>`K6dz>b`#Si;a zXPyO%yS2)MJ4>(k%+DvBq*ak7Uctpn2Y3UD*4^%&ZgU3o6YB zdR6d)>c~kir~QsBe`M)JH(TLp^2=l?&Q?oihDbS&HyK#VU`6VI$Mjc0yGZvWP(rv} ziJtp{LFNdjXT+i`Q#XF2u-$-M?`&rCBlh0OUZv8NSKLelW_i6O#=d?Ns%mmZnF;WT zc9QmVd{gJn;#jBzgC4N`l+6||sSNkJFAi@ZGUWC+rS9xWL<7oLJTHBZ6UDp)gMYR3 z6Nr-2%I^a`A^bm(V43nVb( zubprH`7;_I1k}ZJyXg4NX4H!Q?ntyAK20ZFZEc#~yfGX?8Ga~%kxrU6j$BA^W%-r% z5%rs>la{!kk01iY|DHWV^GTr>6K7!F*~G&Nm&-BhxNdQ|-VU;fIJq!!GWyXF$mL28 zw!YSK20kml$)9>_|HDeoP=9@vqYpt={Y%Pg!Nn3ZGI_J!XH!1lvY%yR#l0eO`-d_w z!3eE{dH2FB##|brzYZ5tB&9mt)cz@q1e*t}3$9C=~)bGlzZrQq?^slCOz1QUZ z#tjyj^@oA(%luG$In_1#YDJu8-jm)l2a|mC*BLk~+yN&h%F0tdTP`p?BqW4As}N{~ zC#jw;;=8qy*p+GJIF#88v!mqXnC{`R-aHH5ygG1xtm`jZ=49L{?Tc9^@&&-SRvSdN zIzXLG68Vh;0}kY;Z{P4B0=O8W&E6y~n;l-bV=WY<$1&EZSn1#4B4?5Y!L8!yV({$0 zmDDWvSkcTJNQR9N7Dvf8K%B4CH;gLIi(%R<&skSujCgyqGC|cbv#&PFcl0G-48VzC z2;ng0TMF~LclT}n9Q4OE-3b)fy}3i|1{7>R4|y9Si;QguYdX?Afbg)gYDUE3G<;ox z=jqS-Oe8(;5}j2UIqgyR2psbSycM>j+||ypeC4FXp?|<;TylTc6C$_Z;9$(vPFZs1 zKM1r91*9~g&(AfXwc6DVvWgITdKsTDKU86EKvlow%&-4$(MlJwJ3K$iq#Cyc# zup!cfZB-ERfM`HFeYbJNfQ)Y}@!kF*lm-9tkzCFH`wJ3^H8}eYOcU_T*l7}zs``lC zB0HfAFyI|ZLV!D$6lg0B;7d9>PXKBFCBV54lF#4DDmi-Bt0>Jn*w;Mayp8Pl=M3}3UKP4tNibLLj6j95OA6X|A#+# zdAyn9!S$TQxY6U{`Z|eC5rU_2emR!4E6LCo5oXJ%ZC(0XXE^T3!$;q=YXX$hO<+0E zQIDxsC}YEc>Yt!(Eu;Pd^$e_DKuVv2nOZzJM~N@KZEGh~@xrEUr)afie|eNpC9eZN2euc#^{fd>=;6;D#MCB~|We@unj-C(>P4rl5w{p5JzyiOY z2?{o0!F*x#;I`=eF8ra4#IEyuepbv|0HE$t)mT#4lYMbf=U!j!#vs_Ed4CBi`FVn3 z;PK3nL35|u51q8854~^D@pP&DsHpcUu=nPqGf4!MR@}>TvuSck5rmHOrW4Gn>Y@ixaicOgXT+^kPi37+9f?U2na6Fh> zypdWVqJ<5;O7Xxvuo--rCdEK=wHj2qAZ9#hdi(4vN3wVde8JCYBB<3@jx+S~gYd1{ zAe&(;UBdkE6ZY0j?XnExqV(9-N?93VYCd$V0as1JkcwVHO{Xf}=f zd}sVu%wBmDW0^M5!pjIZTSVii$3!G{EK;?3%sv|}5(i-z$fqNOGjR?k3yr?-TB+?s zI{!Vf;u%dv9h11;*81g<+;)Xf^+S2`K$vY|?WOI|U-6LZ`AKBe*5!is8j*(T3#B0Y zRxv0abl6aF#+4E9ZIL_UV{tsh9R#Wr zYR&p6M&uvfks&mG8A+jbeJGkfRq_gHQ#R&2=Vs$Q)HkdtEx3koK+8Vam~vCtvz{9` z-Kwp4@M<$qeZYv3Yl0{7gn8Ob{GXig3uclE4}ABxdf4tg^sSP`Dr`;fhswR$PkX-6 zRb207IdwFYoaHl_{^Ef9o*5aYzV-pAJ2tRwW$1A*@A3}ybO{N6TUsoiy9s-$>aO82 z9-fe=<;`Kth2(HK*diFWWdhPJ+IFPNC_dO=>Qm*V8l*Ozc;33J8Pw*G+59DYcOSQ> zn8~MRpXcWNPpUAR_Z-NyZo+L;ILGi~u^@xk332;InrC-LZV3WRYZL2+^WP0R-UPgD zp+X?+5h>d9_nu`HON?j4+umJq4~qr8HN8~RStr0gtHCiz>%E#cB3qiZ-L+49 zWuJS<$PXNP)TsDieT{it^?cEg*C;4`+TC_-X+krV=3N#dh)#Xy+x$*dmFDac{cKGU>DqmkjV|KsA({25aSdrfT@nowkuNNEJ8Y6GxJ|O@BLrtdz00wR zSoHz7`06&zz5Ksz1=bt*E6mIf8n8ufYALAl*baQMx-D2{_^AuW%`(xOE%6=OBIl3{ z_w);XQ&&&vEVDy)#Y^sdgA)Qa6ehVGE&B{6w?3?4nCN;h9l&kbQDG4JTkezww%&1_ z5*OhvXOBX|W|>F(GwHqSB?l6k`-a85fNfys{)H_*slm|8Wp-@?Rn_!j_YJP7Ga^3( zLP#SXmrH|`KqC!Bn7rpDx91V&*wo?+$oB2(MZ&@hoW`Br=g*TD2s~^y^2MxLWFvXy ztO%R;p52go{RZ{39mN>%9D1)+WNh~FE4&_Any0)O^W)gp^`_Ny9ttmtdSGJr-6G}4 z)$_p`hn^Itd!6HIWuNPTSAFGIzH9pY(yy_EDHq3+l>A547cm%TjrDllmXDJcyq2~^ z-)%+5{rGyL%TI3@Z(KjPMf zZ2#f$(~5irjsx$7UHbAl*W8=i9%#7&preTspAE8rVSc+A-ed3n(1wY zY-gXkzGRVC$#E=EWFUAI!!-E5q@s3ix18bfOgXcj*<$ya;EN~=t@(E-weZ;dnI$qY zb&t#`(&6?oBL!h52B*ygHb1#YVh-(Szf7uMBK^Fyv@`|EqU4XMb^CsB>z25oYC zTE`+nA>YK|ywDIn!FRxdp0}&FZ`~Kb?ZBs&*LFNDI#J4@jf1!R(D(jzZ^+B(Cid@X z&hlpP4K)3fc0zZ4tL2CPXz&DKfI-8(qyz99jjQ|gjZZ--6g#rMXTzkDr0?jmo^sSY z#~Q2FSwJl`!WKLZT_WA~%9Lkui^5Foi|ase1b_F*t66>M1?BL@VEdCm=H{+9MkOWi z(K}RoV)NtAL{9LO_bMUntTb%v|M9iA7C@O=EBJwx10}bvA5E zrskgaguqoV@+$9V1U1G%-Rp9hH^Rj#44E)MLI{UNckP?hq+N%R^2Oms>7eUZ6`?0o5;)NYv{{&~z!MVTvfPVnS6q%*&a ze59?AVG_cl_Z$1gyWPl(0UySmosuRHH5`AdrtF)5O?Dt5V;;0_6^m6YIoCpwE2iIh zDZ8J5EtkX0rdzyM`C$Z}V;&msy7%|TTgKcwonPE#FMpZ~y6HjSrHO(G3z|-kWViS` zK8hUOx+$D2%(n%DR#sP828@}Ct!wmMoMxq{+ve@OU#J;=(z>^e6uB78A$WP}m;h6F zr||wg1&2oF*5Pq#caw|DOZ@SWu}IhtFKdn7vnDUzPdaRh}^OmFd3p0s^<@aiLNki<0`@ibHS`zj|tr1#R> zxN*qq4@zX8@|g=)1JYgWv!b>HwRi3>X(vwS`2)3MoA@k+(){Sgv6VmN^+-{aFY*1Ma z*ehv!eW+xsT=VD3kz#VS>dJ5PIzTY zFe^w7A3q$c(8TymhWytTYZ~kKoTQ=)0}Y1tz}$KZN=u1}h=?@QLzA2+U*(OyC>-}R z01vfl7qwV)MaPQgyanpl%`MWs>ueF=1Se~+r}Y9%nFZ>Ek3g=io2A+s5Re1MMP~vo zY`h>8V5l?*a<~Bwp*S2Es=^vP^?%$B>=05Yoh2~g|MigC;qkG%kHXIQNx!6?R>|Z? zd;7mT0#REu-?WOcuqb1HB(QzoRKA8?-H@;%xN)G}mtTO1VJW}XP$-o*`}BVSkzxQm literal 0 HcmV?d00001 diff --git a/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png b/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png new file mode 100644 index 0000000000000000000000000000000000000000..47ec05d5539c8aecaec638475ee204b39a5357d6 GIT binary patch literal 11149 zcmb`tWmH{1&@W8U;uLo+?(S0DrMSBnC@u#rP@IDlcXxL$?(Pl;*MqxU`hT9U?|Z-8 zthFWCnb~VHlVs*M2~$>-LPo$xfPjENmXQ|!4gmo<_wg?G8TR9@{0mL#;{oj~Dx>!K zqj-Nd3;n3$xkzZas5)4v}B+dEr1xPYO$gg!d4TdQfgh&h=X zyI4Eeld4(UnSV4wK(Mi}vF)|gv+(Yxhq7?)Cl!#gvhSz-V&~io{msI9P!!kRD7*>* zK?)%w{!Psz{cPRc1yh}5;1YaFm9{~j9|K926HFX4mBvvZgMmnjz!ig2KqDa%?oacX zM*k>V8V3d@|MTbXm}u~yV&j4^XHF*C{qoVINvA)PT|Doe@>V-r-c6V}9i+6mTl^v7 zceDCT7KA4HnJk?0>(|}J_q0FX1^=rDc^HvXP{_#1QP9yHlZ+o6ABP7A!&FvQx-(J) z==?_{F!RyF(Zwa-=lSm4=Iv~?2?Gnubx~R0EL%}pdJEJaAub^i8y5%ZUr<2#LHY3T zU}R}|Uqwgt-yiw{V-gc3+uY8k>ixK9TJ+o87!2AUDrdp?#p>#NyTkE8At5-)i7_!E zqN0$-#>Nkik6#E0BNdCDHL);3U3pGEo9LYn<3JqPrtI|ZE4N=hr0DA)<#`@xyw(O+kWkDUtiEb z6afZ0`mc}>I0(&3z3rh`iqq95;KjwyxVSj0xndPO_<4Npdxe8>-o+ypS6IQm@)t3^&w9Q*W5q1H<47M^m*!sJbkk2*k>aIgWlG1@CYNy!XLNZ&Dc zXU_bl&arn^4jA9>Bd@3Pm0*+jZ|)zK2e^rXDKa45CP_bi*un~0C|AaL23j}}kWQi?6l^b?Eg z$ea6IPXg`>-PXfRe7}xcfJnxN-aI^b3JVKidg^m&np~3ro}QNj1P(0bBbYtCy_eV5 zy$cI>OeE%C$&T?a&ogkZZA?!iOTMl(|BW3#Nxnb#oUxe9Z*zyIXfToLO)A>T$p1vg zeK)q3r>`i$--$Xe57CcFZHQ4`ZfJDXQTy)KjVt9+F1v6W#jrg6n>J%)?}TMun&*L% z9!Q<1v;OrP%!pSuRYK5%OS%q{xrY?LK15} zLb;BmM&ZWS_0~`RaSz+5M&>|1IL~P2a6(UEt?8>=59(F5i)c8740!sma|jOmwPuk? z7vuK!_Rq-3=98I%daW*D_?$Lt0N4{JXlNKvKEzyZry=ZtfnhJ$uW)L86~A_(+G!W5 zQ7@F7b=E&niHNWLhOWbgZx{M|LbK{-^=^b6p%94#g2hs+$M+fuKS?2i$aq87bf*#h z$6j;^2cXg|{@l=_Sh}kVw$DbFC^(q?5QMVm?l(eh_&Q>^Xeuqg$l{Ki>v^!x^ZU&B zepV zx^`EzDPxH8BAffdPp+yJ)IGuKcE|X%68vO3iORN1kVfI;`p9w@MVP{fNX^e2lU}Cw zml0KH9eRJsIUqY;(=M9k&dlGDC^`i%b`K+Zx?L0~*|xDvEIF;F=dVc)7A7Wp(!5zZ zE<|iW3&hLR97jp_S9@_)JU^e`5U z5Amp-CI{^6J@+nj?AY)EO~y=bM;4#smOrn~=>L|%HSQ0+Uk$Tof3t95;9jsBa9*PK z@0njly!I+nRm`NllZb?YbTu>k3UPa~$m|9qCMvpAXE`a{R+VM(mGh1~q=|T|RGZWe z(s`zEBT7f#fP84uD@SkLLr+^=tjZTCW(q z%80RPmNb7Tg30WD)=bA^c1D;ZT3pA%37yCk;cRT1Gb7IQHnO^3 zI`){T>i`a?O#DE3D(sK!7uvAVhyz^bI2w~#qaO1qlN+TWSTaE*g5u5k+`o}4BcO)! z`8~3CfTB&6CBZO-W^u7O*fqW_U2uj^QMjTKbuPKle!X#6@yTbk{xubZDnq+3$BvQf z`9_7 zPK54eDZ<@@y&|5ZjW3#YT6dN2nL#m44-b!pOEe3T{TV`$C#pcapcU7py%Qob8FU%Jhmwo+ZTLo?Rt(p@@e(I+e71~Lu=Lpc6 zgKsNU?H$i72erp4v(QKzu2!Iow$H-fqt7h%m#_1;q8?ku%Xtx^q$2kc1{!JnZJeuO?>P8sgJFoWyvw>bIke=;vCl78qXR7&Gb#12m{I6tt<=y*4Dls%)F4?tFo zKQZY-ew2h^Hqd8+r?pBSk?$8y+FmRPA&mJz&M2 zys{4GHva+{nwOiS@@ zXroR@=dQj5lX^v02DF%eP5c@c^QQy30Z;+h){Vc)Jf{h}T;8c3CO=NPWzcU6ne`it ziKN^gkgp#(GPA+AKA6NPLCkk&Ty0x}pa_h>c_Tj=kQi`zKVMi)jb-TnlPr!*2yWPm zd}-KM$kd6iBf@e{4@_k_d=21mR1Vkb61B;9?$2uvC{IrNYHQStJx7xTOu_uf0{2fX?`JOcYp0O)L#qL zXNl&O@;SU6k;eC0HF~jSpDiL8PgxFCf$4bj1txNK&v8881+=IrIha5eThaN$rV=M< zII!omZQ7Z-V+9hO^O5p~nK4vXt115wJzVI{XyRhnb`GI9OzV)db%&wsMVHgZrj}K&Y z)Y!Z~*&NP`f+7)BG_6{B#vPJdqrHD_Qu1g?9ZOznKT2U;HeM+)Wd5PUIZXc!bVxhc z-Z8%y4uAu3#)OeOFUSk}_# zqBFOvARsH96So0m7Ekpq=@sTWcNYR@OXF{n+gm# zSk78$v{mccCvWe7VFr7{sxR`R<;Vz{Loql5^SBe}9&y@5lo>(px=2q~z7NSoR|keN z&5c#j+5za={*a}=pTNXHU;mq;<(yquk1vA-*2!2Bg|MnIW#=_ypl zU`gIjU@k3c6~Vl@yxi^yM*QbDPfbmcgKr!kugvejo4{O}5APqdgG*+S$Nh<;(S~o> zaq(g2uLf5Jo3fpibh=D|b8U<*^z|9G5qVW*6#jd>>NKYF*m$j2 z31})v=yh@@dCvdNpoMmZ5Ybs$QTE$x%LZUQ;v4uVBFQOK-?nS8O+n&RfxX#jHko@J zZ7m?$SGf9Vhn5HFP?T|ygXx@CJRe~mVoD{)jvN0+Mt9sLN$g4+#8-%|zgsNnLf3FtZ@m-4d!^#z4WJ$A}wQz!u!z8|MQrpTk~?zNoP zjy;Vio4T++JWgmBW?8i{q$M}2V7m|vcSggdE<{yQ<<;fo?UNI^4@cU0e~k8~o2iA# zuyZV(*Li4Uq-S&Uo%lW*cFK4M6g?isDDU-5z>xvJzo7W%zDzdh3&qhw9~9Bl&n=qF zw635tk|;Azx@4V|tt~uh&$Lr|79xC9W0MhbY&*T)Ieqx#R=DjPRgvkWmu{B+LL>cq z^}=Xpli?A4hUEHGvoXi3QiUpfrSGFMWSo>(<9)=}?Ug!E2`i|DGjg8VtFZD7VsFIa-?#W(Fq+GzBv=gkjZNK2TQD0h#FVDYmVjPwY`g*u&NRv zLief42iQnig_iv@w?9(rl9o7N=y4214S$H2k&Xgsq@Bsfm1URZkmq}rR6@5S^3Q4qGwbTuCG);Ws3@!7l8l$l z9Z+@w5b}tqsbNx4QGLW(`+{D2DwvgeEj`=4A^*ZcZEfx5 z=4Lc}d>uX@@uJBRS1HM8x>TK$2NV)&C#<>7eu*cN2beHpe-eZTti3q&__uI``caXS zLlO`Wq;fkL?0OvXTc-{Hh2Nx|oY?H`?M3bw^RDUZ-xZ5yr;Al66f^iFK6DGXJ2n0Y z8DK_e!$teUu}Ta&o}dtCwYP<9ZhygV>}6C>Q(xc5BA36d|Aqyt?@SZ{BKWOX2{#=> zp)Hx$PREABUjs8n-Q%`4Lt>oJ3;)HSmsU>h?$qSuzZz^8(TRv&f%~p*ZUZwjY|QdF z`;~^FCoX(83*}*1StLnJ`s8$UbYP)b4GoRv)>hEYj%ptZ@I6&hw9opkVs&zl;4;5- zpT|Qz=Q2h{fW+h#Ja{j}6j`Gu(4H~wr$^023)Rs;@YVoebraE)eVTLsZx}*S+5mTF zu0B3K@7Lxye7j=*gEOc!^M`m#cjf=l<&|`p5r&_Oy=>R0Q&WDJV(*7m5f)j}c+~M@c3QFP;5wLIt7aLVx}LWhdi^ z_y)du=Sg&mjCZ64Tl$FOeK|#w4^{s>+g`X{r>CWb8x|JU*ViYIof;OB5Fd}>=H~VR zLL|7U_fbf%;K$0X;H%*OX>R7JC>HHyLfr{Ehh8sr?M14CQ(?Z0k)zJ2@_SF^(;+GZ zC@|N{f3OSBRsMiG;Pm(}tDkNc4L@Dn-!t!}hDwFibAl9S7DC}Fv;^k{zei6C=JSUa zqDig2R>spnl~?D=%`BJuV0I{l+X)xZMuj{VNqXBWPZTm9v zE54xW)Ii>rG^^eea$ojsf}cX!$YaG%t9R7aO4<@X;Etdm}Uzxz!H)x3RIq*Lv-6%(((G6{zQ;0KD6 z&In$oZWB=(iX=zHlAjKxNhI&65}fIUHBLA<@KKtKSQeBN6c-7a`+iDj2g~tJ6=x;Tz z3^owz`)ZF@38gy>v*Md(`e|%;SFQ}ys+;=O)(D;DbkhZ(Jaey4ykC0FXCfMq5+2U- zJX^tp(fNYVtIf&GD&7N2&Zp~d>`AqtLk}|itoZD}eR7qaQqo+#C*-m!r zQsv?9-H$6y7CuDUW8HN3ngioBCQ-<^9j@E}H+zfu)9rfJ)9u$H-HYQw0#pg2{;mS_ znkN{@QQn=y!_d4s{>)DeR@2V7&i8s`d0L}W(l7Reg3pr&i=c7R^K7bg)78i~4j&Lq z;(LmQnl%%nZ*1ib%FgA8Ww{}1z&>}y+869P^hE5|_p;JNrdDi<<%sedVdz-udDqUU4q9 zuj9WK^UBSA=b7DR7dmmwL{d{@3)QYkiuxAuxaKW{I8tS{^XJ5DZ(+;gYgbom3a1d_ zwIV8J2}R!-_hL!7VO8E zr`xh|h-&EI|9~JYUJ6;hJCEiQnt9z*K|iD8fY#r@>8G}G;ctO{%V)WnH`GmN9zdi+eq zyJi@U{b23q7Gs9j&A}aViZ@tC8=s9gFSt!R{Yf15-%~TyKvb`K(?bnj}oi}rRZ|Gnf&c=TU*EuC7 zWH?N7aQI;9G|?#abdvC-W?u8ASn_%tQtgR~mE_M(A*@RB@e}@V#Z~<-t;kecyR)G?6lnUITR<^iRN|TTW>iG$tnNHbeL+ScbU^d5vCy!t3PFg?6zbvo0T!kE7m=U!Km60dXwQjQwwXc0OcQJ#MlJQkVen zbro`4%j^vW?A#O6kykz6Ik~uq&(@3fy1X5k7gVSa>arQT`JMXV_Ezut#kx9k?}RW? zLX$ENfM}a~kX)k0=$yaqj+%o5yo;WW@Iy8nkyVp&Z{SzwDXN zF0?mB;l0#}@+s54x!F^-VL4)KjX*t;!u?!Is;0b*00rekzQz3xij;QbmT3$r`iMt{Uz&gn|{)RxjbR1gHIulaJ>m;8qL{} zM6MU29K*^+31*Y7+zwRK<{@r8=Io@(EEYuAbs8_oDhx<0CWQZ9vlEyO=_@IS%_7a6 zPhmF&^FQ8sz~EeG@ zvsG2SXC`k=r2j%?E%BYm zFd0j|e7hU2Z-n^3tBXF(4LNvpT-y>>@&5{=7jq0>8;*>1n3ZCj>|U;5rMN#mzkHOy z*;t@y@+zo9Tz|SJswJ<&C|FhHx?}4Mr19b3`%fHtu#T zmZ%oB-;ZL$tPYqa^Lq#@MUnK;uU6G^o>^lnf)yV{^ZaFZryZ7k3eYKppF?3$wEJ*7 zF6WAzLA?eyFKa=a;7Lnw^MuV#+)=+Sf~6_A}V#@B8!04 zV-Bu;*HW=B8WoT8$h>fYwE2B8fDkKhuLeDbVVO!<6AZsw-S9)#ne6m8DUa9j2|G zf%XFWzIdxmnI_(^eEGyHlQLo$Pu#UZYhqq2R0b{$p#C+WYwYeD&7|d>EMR~*Le(Zi6!!D+8vy!==#+Al%Wr=-uj9HZFb=m<1kHEI(RW^0P>(UI< zls$~GAR@>B^y{5%JDzHBkUCC=xW)2)D@7G1Y4&I3h^fw9VZH{-GaNVFV}*qUdvi3p9BU0RGSg3jORbHLCCAs1OJd&Vx$c9 zd&v3K1=Ue)l;2M`hv%cY$cBzL>mnfcv%w`RNCi8mMnwE%PSc)$(_eb$eiHWT2^JAj z)OI`HjG4me)>eRhA8CA0$<*9g_{Vn?mJ0Ca9%rxShIFs^szCw;iP;;EJMAG&v9irO zsmf$t)o)2v<;`7Azip9*9;+sD_|^w2_5zb$t`w6|e1rn5%X~p&2|Tq7}qb5#|CgMj&LlgNZc&8~7N)(q%RGnQv{;seEL(zAEycfHnUjnN{ zYI~F%`N%SaH!I{GS(_4V$2dT4jOYonhvfu))Q_fi6Y}_PReO3MVqJBH8-y{ z!8MiKywLHe&r}o5yxrr?9&=L7uU_(A#dU!A+r1x-<#t*3lvW#0xEQnw`8E&`2y_3= z0)z=={?5@yo6mHVfU~mT!gbM6q=gA=!&_-`T$5L=sSVNvat`AGk+To0e}I(6sX8D0 z+T+K;nR`_=nQG?vb{Zk?1AplxBD79GqSLeH07U-VNcRpN4@Ze8@1BOVdelHV z8x^$hy#VLIxcOr=?4h4DrHed4x)W3rEKQ?_dYK}pTrrm^o?n*TU z%cW}mgHZ2O_gRS@q!|^+yaX+?4oMZ!`9_D(@scjpd?K>~HCqx3BQc8)xIdd9>&`WU zjzmmYR~Wg<7OSy;ly1NhN=7gaZ>5H4N%WC$Y$dxPeuo<$kT>>ZH!N56vlzI}`wVw3 zq$eh(+MPdQq``C?ac^f>s|7m#&V?#TsjimYd6pV18cwr!f&oexZY4pDS?XY1@V^8Z z+`X79W;B&qqRzG|x#IL|mV7T@BQE?F@f_>uz(ig-KX~f>D!}ai=)ZWtWjs1nx|V`= z-id~+P*bp8Zx@NoxyCizH-V@cw{&aZ@YYm^&`ZUf>yu5PZHdETb%ENi)85lX!eYU~ z?mr#MTID{1sj3R2pzir+wQBIIG7h@$^{lRuZokM=bluBaRP>c^AzG$*VIu#KScpEg z(hrODoh2iFwr4(*LODt*QKU6DL>OIf z;aJa7s!i5+lx6UMeRfY{$fy^_uqAe2*rpje-z~4rp>wYjl~j^~QD!Afz+D)jK3{DA z7xl+YxtwHcd@Wv(Rp#*acp2|XHmd8|_|)T#iBVY!lIJtgSUf&^OO)zF_iHp%J8Uu> zyV)Dy)SfjuC!W9B(h17D6xv9uJ(d@d;6k?7Ny{u_&#M!lNKgumF@%XNuO}I7fF<35 z3A9WqI(m|H5IW+F?|fDJeeVOw9Kk_8q2FJ!gUG?G=>=HZxxZo4Z77ZftbeE3k|N`w9g9)iJ=Oy$_%V~FNbiH-oqv^T{#x&H zTwcjaBsOn=c?4t4i}|d|)Okg@zUp3ev1e%401y-^Y^LAml@p_U)4CJbag}eWR;ea; zxxoj?Q`0Jn;-kqSjTO@-%e?lu^l_`~ev(R%>+H#bE~sZksN1uNs}F{^APF0cE*rHP z+5Lx>G><#mp#BV)NWY&~YDjl>u6iEKlW2pR;e%Tr#<1I|5ir9%-F{0k0*hVz@ zf4goE8x4Wu5@9hu0okQk=*82jzB&{(?XKz9%x54o^g}zOWH=sE^DO6S2=cxmFlZ3CgWxs2ou&2j5=2X z$%^TjT#2-s?q;voOf=z$N*x;^C1(d;7R~91!kh7@IE+ZyfBT7D4^t%1p1?`oJtha^ zgj{z^G0zCmA@{FgQ{R)O0Z8-?(8ZA#TGK%Mq#Tv_$cc1Caksp|LS)#IRiH1Q(d zK#vKK-4plqAQydV^-rj%=cZmm@HZoKU631-(FC@f&XZPeL6^*ku0M*DC0xB6VCllt zy0bxudo&@RR7 zXWn@Nv4|VcIi~~3o(a0bMoc*XzQy^l`bam;!p9betq$Q9(%OseqSD#a0l0Zu_%P1I z;IBC$p~v$xY!>NDh4%$`k?z?|4}!)6`6-bg@Vq<=K0vJV;3;NJ z=|1Ztm3 zw(u6gu7ivXlFP?__hf-GVIl{!EEe8YxaqZ7w&$P{5JVyHJ*r8FSOMICkMefrQDrP; z^C;Z=!su1lA)GcD;-g>|j=~Qt72qty5Ik8nd$$|W7qXw$w7i*AiJV|cuCpGQHeJSZ z0wSft(kKeBny~^xF!dT~bcshcwO1VXBlRWZ56!4tncYuTt*>>EQf7e28J3qDrIqD? z?lRm>y);$#IZf{~mVjYHd6TVnp2&Tshj~DX4RbwI2l5#elf~T7C4)}~yJ{RK(gI4e zMtMY42jpSNk|i{hA8z-Id(w2{<5$ruIM3N9q}K<#o(=7x7~%*P#}ZMXwbGSY7!>E> z$Y$F|SX_~aj)SAQYjAK;2UXSRn_!hoBAy|Rw;joT?KVMCX{?ts>wdN(vD#>-G9U8t z=UYeL+8x;#f>wVDWB{L3kB%1{(zg^M4Hjjb)-mZfo-c?alw1nl5e13h>zyP)u#;~7 zV2EPTP8+y0PrXc|Kp*S!GcCJal!wtj_<6;Y?YGVvYOU~EhL;bVX&bVo`)~I76;$*7 zNMfK*@;@w}SRK8%zw2&lb1$#?4?_Es9duRCI8i*Yvw7|Z^T<;~&^LOGY!s)@gpd2vTbyrFBEoBLrVWJ{&1{f0xiz1kUHTD@hJ?Rt&59|Xh{LI z)WTRKNBqVzjE}T;%A=&CrLb4@{j1*tF z=3KCh|7^FHRnQ>A|2)WyAL8GP4(G2rs9T#lINz`{A-QQ~ecR-?y^)=XiIu&XwZr7L zaw!rL77~SXG8!&1V?E9;8beV_>yvYDS{lrRMoL+Bouhqd`r*!@13tFo??adDO8AD0 zmAxh>qK_}%&Z#xcRfw74w0$n}M8v8}nc`ITu)!A6Es2V+Sa;3;lxtmGeIQuFO>^~o z3WId~Xn&5x$)TvB7_mXAMfE4?FJH1hVj;Un{Lwk%mBIUefA>BY%>Cb&m5$syCqum* ze{Rt1W#9epN6A9DlP(Kbo&Wb`naB&qG&}zN2%jup1Y3l+L?y>yUgn*Duk~@Bhm~~; z$%9H67V3ged4)y)d*P4rKYggAJ`_1R2S-Mdoe@1~YH1l@Kh!k+pkzSlLoF|6BJ(tDkPSPFv-#KFVATpP6om|MwYe&R$MyXP_XJVCHvp zoPU>>cSDHj+Ivez{mqrli?+4`ipfuliri!qPMkk~{$)yv5_hHLsgpOGERTecIXXHn zc}U?>7?_xDnIWUcVNzlCROf&Te>m#L1h_f}cLUrhx1ZETcDR#U&?$PiSs?Ss0?I$5$>#jjsqILbws0{f?3hKGmyo?W-9u*%Q1Xy?(^ z*8V#-c8)VtH?VzE)t}`uahH4#-0v7o&dQ2!X}MgYhmY(?M^7ITY;Zn7^_PRVxw$## z>C=zr<{ZYHPLGd|M>bB=?ce`v)>X`HE>Z(G>Ss`e!S8S_z4OUxIhLJ*c6N509;r10 ze;P&dTUuLvT8EEF{q&VfVwL)Nj+@TV*f`AKgiD@C^-j*`(f=x=pzl??1r05&!lg?b zOo3n9+b?m`9bpQz`=hkS({tU}#N_F-XQX$k4U=EJio_Kv$4MLwSJ%}1^pxXq_SIvb z2kpWXIV|#+VwL6Py3e1y|%D{GL6V^_68MSy^2j{`=QpqFe0c%a_4XQ3{%x`zs8BQE@sBcOF&tedx1! z(J`uU@nW?_-q+5~itE;;9HsfUI;_;x)nC8*u5#(p9%12|)2r*gm3?W2j8b$*F3=Hs zw(q{}#+vKGxchkE7$2l!6`r%>oW7-H^((^8i@uaEE%x{JF*1b|kXBj9TU=_FfR0LVxA^X3NK-$*TRUaq0O7@^K{aWt* z^z>tNyZz?8@Tw@)s+#C1vE1#2I?C9_97D2g7I|S25xFSRPoF-Cy1(Bau6W7LPSDoY zR?8?aljnaO%%*bMMM6ke__DhF{>F3EPA{=Xd-RC0#pPS^OSp;?7XB}qw1 zmYp-=Ugh!oc%K>Na{2G5%@U49> zQI9f_&w?z2xwWDn^16{2wQS=SO1Y&QYrbo>#-@F9uU5T2S+QHDqvO9HrRE!9qi27U z?icxpxhgn+V_U@F=xM3pybn`_3O;IW&lo=oe=6V5eWhEP;x)qj_pTApW_PUEW zPiC|j#h?Z4NJe*7?7ym7 z^(y{G_3AA)=Iu7x{iiejY<%I2;jsL9~XEC2om2dLq6M@a9$7P3i zi87|SGv4euoE4qk=TO_u+5hP92aUDY7fByyrexq97q0x@wH6y|XDk%$rXinP+r036 z_SgB|N%5KMKd9Co23ybZd>g4uZF^p;v#}Zc{%^nM@l|E(MBC9&t!&Nz5W#@)&8C<_ zt9w-uDx9kAH)3W=>D2EOUzGo>GNn7Ht$Xd#S?%{4*Z(rAt1NHaqo9v}F5(b=i-Uj| zA8TSQjg5a7kjG?bT2_U8Z(OU_4D$ol_o!!W{^-2CVYM7sy76CW z>i_O2+DbCCv(W6$QgVC90jV?Fm+FpRmMT6bIOyKh^!cx|-r(+C#LrG;JLzus-*o@g zzooyiTB}g7<4aNDz^FvH{zRPHfqy^3qO%|sdWDQ zjs#URZId%dWh*?l zZ{I$g&hl@^S-I2=d;ImI*L#tg`oKo?Sm}G4K3UVo=hff8GnAH=k^o2W@bG+YYBHVf zujUS))Ln~CeGxp>+_*$y9> zH)&~|h4x%QQY(KSXfLb}s$M=^O&iG|A)SBgPAaoP*hcTr?DVGt*1@5nl`&%Ws%fdr zYc#Dte>VLrb+4_fJHpF5dNw2Xzb5|7e1|`O+0u~&)raVLN2!52&U1zexG#)R(b*id zj*V&X_?^N{+UU-I{qp4synup=3YU}=bAHz)b#>Z4o5;cXi1JBs-CLxW!sM;)=}*Km zu)i-3z2h96^f+RJq@FS~f%qYzvh0QQ}N`|9*dYZ;%e(O-oCPLY*f) zR$To2RD~WAF5|<)2^krCQd|9%Q`xw_jl$_XY z@5)!dcCGfy7iz3a)XAGURz2d17cUMUjy~vX%&D_+!4w|@mwfv4f!xjjOrBu}yM?i9 zdTzrUn0>*u##PnT(}VopQBR-J2r=#9FPYkk(gfJ*a}dY)Gd4Ay@#FW{Qrz>TvhNqb zUEf$|F0M5}GwoWA@vi)CJMyDPkJ64{D7h{VMu<6%(A_;lQmE7Pl;h&}w>K(RcheYk zC3>3mpBgz_>=G<0C@5=U!qN5qwvmeqLzI%7q2ZDE)$SqgW5;6VmTz~x&-Pq(GHr_S zZyu_gTr?7lT3Q>DR?*b_7IruJUjZJbYGp3yw-Nn2F_Ds+8yXkK+-Vb6Cpk21(sHU8Juc!9o%07dZ#Pe%QHPU}tnX06k?Y3{8ZaCR&(=54Y zkf+UW?f28@y2iJ_md;Lvn>V?7dU|Mc3b>SG46rIMQ&VRj>#yCj_FnBemvq@{HoBLZ zYYY<+8h@MnLUn2@|)yu;w-io%i*C_xHSe_wLucTk>FiU0vj9XFi_rM^#nla&mLcEi8^ulTFp}X1#S9 z*G5URdVaG!)!Nh3`1$i^UpH(B3es)!^Yac44%Czn6ciQRdaqSIb{$TRxSDTuX!Kjg zz}=n9FW$USbelv`i5{f1DqnXpXKDTvc%U=eXlI3iP2CJ$wF0H2$`k(G@)YrXr|}=c z<1NGg`g?a|N>BIN(m;^?P^>e};ltRsHO7K_{gotB^_9H6rRAR-yn(lQrNLw1F%r3h zeE%;SQEr8cK1Q?==EV(MUyfhP@s&U98HNRN5RaXc)XRzhmYyPR0D%71yCeYD`0%^7 z1nXtVr?qb#xHN(mo7Hhqh$%TO?eUv8JbLcq!mo$h2d3Xz^%N;YX+$@E3i6s`^I9L( zpKXob2pb!-WOg4DuqqxuMMFcwXPwNKeY|YBJ&^AEDUyN4<;^*j&Fuj}@14YB$GY-^ za&I;(ddelWaHG3kV3!pVFFdNB+ParsDTk0tWFfGSe$wym-I^GjY3r6 zRqyKM*`a}Xx6TsRlUd~(;+kpUUDoBEKSu2nrf&aL$n} zg*tre(M_K_uB2&RzJC28t`$usP_7ar3(e`Xan57P4=%+Mb{4|SU#d5%m)5;N&#;TH zW@!Gl6SFQ~U*M28)W49dW=qTsv=k;Lrr`QP6vB@mKmLx7$4=>nT=31#7N9&VMFB<` zQZ~D8`kU|lo~Hx+8wcq2?b}OF57u^?%un{OgE**gUS3}4is~mJAt9SzAGUzi|NZ;- zSEER;@nBte>+=D%|EOEXzQFBu_4UJ(lc{LY8Q1e^{Lj6;dJItgd1B%blDZtW&_9YE z6E6b%3I}F)lZy2PX;pOBEff12Y}aN80Rwv_6rI60ld+he9-VHp@1S!w*i zKx;pLW?^I77G=41*}nIupjbkolT%pvb+6$!?HwJoWZOSCHpU8_VP~Ka6cxQ-Z=c)N z@x@qB0HXo(n$+j+k007#@=;My;K1EhBD%V|VDU#Eg2yFiWL$bKj~@;J`Nilsbm$Pa zgu0&IlaA5Bx#3pP*nv>#Pbt6AYl_D1j4tjpwmf}OOntS}b?93DT#tXfPX!B?N?es` zE?&T2Ho*XL();(PNJ&XS8}V*mzkVfPmYLbZi3zKmoE+RdiS@Zwa6DpL9UZ`sl)Z5y zA(HE#Tq5-+Wgokqd2cs`V^8R|YW|ETIvvf{Wi&rCeSm99jz3aoTdR>gJmD;f*T5o9 zwP$&S2LuFA<=gy&Hd0^tI=jbgM3?Y&;X!xY@f&>~{SGT~Fkhq8-yHbeQR9$9ue(sWCix${=VN4-JF@rS zlh@lqsqAwG4|ciC&S+dpFYo3ZvUCpP&vF&%V{~8Zu2yokshj^jEGft{l4|u+Sd6OQ z{+<2nJU8j`8Z=}ew|F7J|JY<-k(t$#bMe2Hl<^7P+mKa-cvvI(Oc8|L09~K z2I;*0yX$jj$15WhpZ(TT{jC~&bqmv<4Tt_elHygqSwv2=#VeQV^EX;`6_P_nUp^#y zh{YEwoPRpJ5|^25eXQ>BPM?G3kA5xaujD#6X-zE)ze!UU_z!a){p=yt>&VEyHD>tX zRKri|jBS&;)z^jvt&U!KE2_VIm$Pr)9g`@sF2JO&^(cD~;IKIX5?VLP*HBesD!~NHHc;DkyHC z^k^F;hu5#1uYsXM+V4gs&BAF{Wfxo>md*|&1@9~&UF)OT_&oa5cT3$<_X}U$IZIR9 zEd-bXzmG2{G(Y@DJ!J}*_;@djO-z{D*iie)-h~hvx9EP{#b-BVOwhouWfu?DYT>k+ z!7n_O-C+X;GWW^T_#9fAYX|KxRqYC6peSP=W*Q#QMG-eel~yL2gP=UKq@sesvMzamHW#* z0euc4*Dxhxl{)VyVkkJ8`*v0R{Ug z3XFvo(ioh085cZ6uc^lJ$UYalP$Z`GOjzkThY&0G+KZ2UCtA$rigthg_eqyrmVU9F z$TV_u^krNiqjrB6tL2tuq~XssHSw_jDvPTC<2N$#c9%ObH#YuC?^pe&t*X+1z+tGKsA>t)VIcXB`3 z?LIP?5ln%9w6)}^tr)Ou&`vQfnF8fT;*XQvvq^fo!cIv=1x@^dib~Ds{MVkIOVGCa zrjq01DF(KfUb(WbckPde00RZ;!ux}1E^CCoK3kylo0Kjt0UG}k5)w*)vFO`?mbQC~ z@8T-@rd_@aJ8*|9f|S}79!zTa3@S^4+3zC3j0F*oePSc3#6o>0SuH`8durK(ozrVd zOkSE&PF}vV*qQ(A*|Y3GG61r3ZtHoO&sfMJsCUTbP|L{vP5J>PDCpqP&`3z%I(%GA zfrdg%NBQ{T=K3D*;6-mY#@t>5pQ#v_o&s^XI~4ED>{DSgXt5z+E4HQo*Dn_NAm+iT zzDjluj%>UGaP0XPElVm+NI1* z`JM3v<4&ZcD`Kww+oAe@rwM)a%H=_XzSr`P%+K;?gqS#J{8>V4x%v51%gW*i6vuR) zlUC&-jqm8CFmBe51yf(n?ObnRwKCadQ8i%A|A3Aih;P@m%3)(_Bityr(m0@Nj6I9I zmCB6dsW^z%ow+x$j4`QkZjU~FlGF2;J}))@S|ubj)IKAPOJ~c0QVchx_2fxf_%op0$;rtP`;L7o zi8t|Gv^Frf6MNd(Fu$vC{CyW779Abk6v27GccwnG1wo`fD)>kwa@$Ty>MUt_t8a$E z9CsTwI-HGJ9+&^JuHWHZBiQOSLw?fbFY9c3#+>{YDB|ty?J?5cacylH5JAp6JBxU4 z%*I1=PcA69`tCW|N5_wkgU;r(N~C$j(h3ESs`QC%Y&$oZzj%H(&A3x>o2B6i2C(Qz zCsz7X)py8fve<)aD)Yc1F}!)0Z!u zpD3ACo^z8Fj0*Vk$Mjpq^%{`MtoaeL!)IVe&{3XbUaS4I$?Fzm?l>E#eoyp> zR&?FN7IJxJoB7)HB>|eSXN!DiBA*J`z;Hng0h543Y4C&#Ma>RAtsj(_uA+tTKSi*0 zm{{2AH6!h5XaObj->(1u{d)>bNagCpf!-xjl!O7Gcf=SqGud{R*+!9T#ZJf31Kup` z8W|qGVQjqpakZ?XV%?SHrts8X&GBqI_)!aB4WA^H_V$9>7LgZr)r|G5^qVciT>vKT$WYmbeIv zc4Tp#JURO&J*#;rsi^2VK(QUX2Do472HmzbhX9V5!WHL^Zi$rkmU7G6W850=cZMeL z*J2@zhPb!hhs5lBk6(ZPrMFi_^x!9$S2rq2t%`DnO2$}-NtKTV-viv;DkwWDmrj|b#Y*Ebr-1*^dAF=EZCg@v$(&4Cr|nZ1;uML zfm&=%QEig2n9SPS+q?d{yE9VE{?8TN)Yk`b1tLFPd@?inNS;1@N~~1neUwwo$RDEBCbL6PMNX$DI;!d7G7oH$H$sT+KGz0V{GlnqXE({X|dkGHzZi zTad)Z@%&z?!7_HG{TzyZYogKntnC$TpLlgr$Gv)d?~J(qjJ0uD5d&k+t|2k};^ECD zdA!FJ+2xXWb`DBS79TU^ADTzL&a-5c~~=|5f;_MV=j`iMJN*Z;nPl@*(yf`WCP_ z3BK&w@>$`uA`=S6S?6KR>^#$M##E>hkqU+O}<*#+565v&*m2(n9%kyqD|s&CJXQf|B3m zdF2~F-R^ArA!W!VXSKDN;+Gq4s&dnz^hZWV3zz>yXDLdi(73NO&I?+Uqhv&TT>ms2 z+j6aY@c6OT;HRg2+S6*hN56kwieDeQc}%w`ESy(MvCC7OKJD(^E!x+wM+x8hwt8(J z30%Y1>FHG#d3v)ffKs4&cRNO_4YR-e`0=RG6lTv^Cb784$P2o|@ zine|0-CKn}I(iBx)bujkedy&kWN*vRSF~|oJ|9M*PZPh;)3m#Nt58m@dIcnDz&2j3 zOm-=$I1DtjV@!^8{qnfJz8e-67A}~@aALnw@Rct&TDxE?UWMnbI9S}OBoA6cxl>qJ zcn!K>jll_YRn8ZL7&v0gM*`3bY6A~)UB zd*?DVd1E*G?)~|5^$*{(^Fq;$5fW#5T?Y0v#Y;DrxXxZsR(^Jm62hdK8V!0ERG*n4 z>CLGAenUHZdqOVzXVz)jjC~i_z$NIvThr3_+y`}AR(7k8kB^xU(|&$EB}K*E#ZKe> z-G%mR8;j-gsjX;q&cV;0E1o^Ojld|-KH*Z!$ZV;P;E$MKg69oI1nR{8V^_~yyhx6k z2WmrCCge8q{l{2f;Lk1-n-;8yMA-kQrZeyPTO=gwM?1mZyM>4#r zNqJ$iR|Rt$8x{iCDa-FH!gp&>)3$SeFOy6ZPfD48xit1f{^@|IleCG-@*|Dm22W@IjzKO0UAdJA8~#UfP-Dk?&0A8t)WasHim zn<(G(!aU#%{|SHuQ5#Ap#x?F`A;kF$7cQ`%u{claDaT6%c8qfH@jby9Eh;K<83+_P z!4x<*KQCbYQ{vgNYcpHke;K@GW)}QM_TZsI)uy?9Hlq04oWjE4HlhKjVFrr+f9VTW zF)>C|CVUk1Wke+qY*=1irmfS%-83;Vp|$z~e*hx^{(zczie`p(bpYc@5^PozU}6di z3TppzL_rM>4#uGIe*30LNRtL@oSOt@eercH*T;3eVobAw1-mmqpOSY z&><7xjc~>Dm?);XQI8(&#@(9z(`W=~`NPr<^NvhiLSdd=zHsrPQD=^c+O3*Nuf@~P zj$fBkQK156!VS#DJq8fo^`H_~jnDbV`$NLR9XXuu?gE=gP`#6Q&sh6M65T%blR|G; z_-iNx=})EIH{g~%!NB_SrEKPuTJVA9hi4V01EyteSdYJn68e1fn`GbjY$Ic1V**)V z&Tw#We68+;@{foC(}4ppblADLE^fKgI!stcF;bq-aWm9j8;}3U1IC4(-C6D}4WtS< zsl-W3PS5wUpdeDxeZk;V^fe59h~$81(6m0nQJcb06?fbXc!(DCWD=;vY3%#m`oXOR zF!a=D{<^7}yh?5Q;+N}8mn?TtEhe_Oq`vTMDNU&1Nc7DN&H76sJ-#hwRR$*v^1C1| z4q>pOTQWx}-PcSrnICEYup}&U`gHW+wO>04u#%phuF&!2yCAP-It#2fpNbFVo2%um z?d|8e>H49PX=rK^!W+;jNuIV0x7OE!~?fa{C0dsKfcz>%S zlLHZhIssB3DPY}QHEy6e3=}gn_57_cG1zG;%!KT^Zs)_%AkrQF823Z7peL1y=O2qY zjWJM3ubog#p6SVqaVy2;fFy9ZUZ*Mj;1L1Lo?!scUTW|;n?9J0AoJRP}6{GWt zNBhz6fWD`fJq}8^1RK>nN$40wdIAm7LHrN|(C_0;;HcOiN%{F&MYGZvq^7xy#XZVc z9b}0zG#^(Od@vTgk$n|}mR|P(FY7_d)-PXJbYHr=yW>?Ol{m2?+&nx-PM!?y?$&wq z_;GUVJQ_dZNN60fv9Uz#g}cKr`^2qp#|R8pGtl=rgbjs<_V)5u>YF!2q3op~Rk&~g zQh1P-p6hvwpFiK3(_);!0wU(pYxPu!*2!>(0k#Ovf~*R5QYe>--O|*JHLp!yH+0uN zyF$<&VsAn|PoB7V@nZ6}O2h2nn3w<{jD&={l@F*OzP?<4iT8v6gLNewfX$6{tVVzw z8x|pSAp?dappZ|k@eyj&Z0fsZ;mYS0Nth%y5C*v5jZdR%=EmypzA zAjBjPr38qn7b@k|*w~1c+UHaZ?H7(UIew_eNz`vdPyWCBm{yPQjkV@+Q_<9n+YYVy?(2m0ak^N)QK?vPLQ%vRjUK)i9Aek? z`3^DL{_Urt_n_gXT#9}v=HiSXSls%P$81NB`agL>NqI=T3b8;>PtTH#mC?C{g{Q!i z9v&qc5#MdS5D*v^E&uV`j`94m?dL(koRbuP9e00KcevVMVX3)C!C$+7pv^dC_>0-^ z-=@0a+oGeRAERngQtnq&5CMXtJUq{XV~8$){P=MsA~E>jArAbi@WCzwSH(mM+;_^(H@#_b?S8FuK4+$Xw){=I#qjs= zT4R7b%#~Box9ghI0Dm4ou5E5k#U!7Ym{?5LA0xdExY5>D1#bFxWaP-vqq_hT!S2m5 zY0=8oW}A~*%z*wl1q3d;y;6YX1f7UON=on1tvK%uSGde!86_~EP^M9-@m|k~|I_Qa z+yyH{meOT)Jl`Lc#lf)yg{Pe%m)d&c1OvuA`b+=70Hz82J(UqF?s)|`jUQaRMUXOR z9cC90m`-eL%&v6_lw)%%^^)ihtH;Y2)js8bEsynp>JhGN1%(rdJ@Z92B?W~W_#Pk^ z_Gl&ch09!v{IvobR`4!>AFO6b{ovS-yvMLF!N=hkBBDZcklIi#;JpyR7{ajU3{`mE z^l@mmZ*i4KPh@9@0E82H6QH;Arly=!5>q7Nv6SV#{5!6qYQKI}8|E8OES?epO8-4L zXgivbPm~sx2>CYvA*AW%Dr-2EG}GF#1^4=&Bj7quxMERp$y*)Z?Z7)yhy`4KZ+T;V zWu6Gnd~mUG18@hR43b=6KqELPYIBEJP)x@C$F-{#%N8ZUoe*RkfKV6NXo|ZDv)Qn> z%oFPgDFQ5&f|7Cu$rt25PbGNEryu2aiV$z#pfCQ6#)}sQ4EiuSp99Ki;L+PqTiX{R z&s>37Lm1V~ag8hUqb+7aYaQO}^nh*1)%p_66o5r)HmVs8)@5$Gv7X|H2h47S5z9_< z13qcD>rgD*K%^$1SbkOa$6tso@3PeQAj|FWkz_T0wPmT*n{{E4$B%}Ev0t!X=iuU+ z{c@Jd7T_7QM$~=b7^c=S{W3xjMIC~LzIgd^JHupg4&nyD#bvwBi-?F|fhJGD3u?yS02QgvL2-wNu&R-U2eNm_cTm>-zB~jB4hfoj^5)hW)-$ zbppHnwn9}xvsF%|1rP+_#uf_Cam6(zUyR5>>KJo?Q#i|WAvc~FW)KbuEePDDmN%SapI*IXLv;|{XxEk#$ayjxsYu>1XaFUC)Etn<%Wyc{4rkeiHE;M6=9-tV(T;6~~uBN9)84xcInFISR!6E4sbW~68v0lG< zCViD1K&jnOSbQsQfN0hsby5M==!yvoF|eoDSzT52jJf%+aFmkNxk#z|E|FlPJ53+@{!WP*=V*Z+YoU3W6yyO@SdXa{xNA z8r|NT>mOW1Qph`Wf7JU_Oy8uhAs^7rC01=Sa&e$KN^a;@SW$0}$ zud=X}m?s$YKzIOnsHm-XKb(&k9~t>L?9gvlXm;xs8;yT(M1;JXThY60?C<21xSv1u zV9qyy%fVTIV}W^!Zi9OUqxpM*ZBpCNwDn|F*1kOLMp_jK8NfC4Krk$@1oPXs2a!;Q z{Qy}26BOzXLA3~IR($DB@kG(Bx?@&j`R>!z^yvPk*xE4?u4*fd_*D!e7Jvr;D3iP3 zYTU<LkMaIQo&G00U;BZQ{vEXGjwIjmEtN6sB|G>wlr63(19}ln7M>+=m zH=>@yB+XI$5Mt-xYCqjZj{`+81(L=$%Y$1deCd@UU#q)iX<21yX=+|L?|JOFe3Gz1 zUYWZ_Q}!cM!DU}lb0~=9*c%(ZN zdcC3F@bL0tTK<>s(95EKP-)wK@0_3P6UF4ds^Nu2-TR8kQ90K7nwrnTWaeEYh}1q} z6WcC`Xl66=qV+<+Hokc?sbiFpk?{r;VbrF1FK~MG=)9o&f+fbT3ZyZB)=G;!LELgc zrr=sm%3lvnwi#t{u(Jn0d&VhQ@fs;MGoiX)zh2O}5O0Bez%9ENw<+6aoIjr){=K99 zlyTFE!3o1S)oyu3nIrz%yr$!G*58bV?6IGbKv4p#mWsEoYyOmZ(XP-#O@g|MI1_-0 zju4`l875SSswUn1V`mM+y{aYVOJN9r~I zLxdcW)kHslM$H@~KjJKgRI0+E5B%R4C856fk<$}|H5cRL*#3Io@x$b8HJ?5Kq5&d7 zz_PU+SeGIKW?snAg+xX^tQlaHl+?8tsY7`5+jZ-~Khf4Os4p^xluLd$q_4V~8{Z=( zwWyCI3_vEbu*3!hzaSR1wzgIR4j6Qli>qgk@$REt-7McD3bOk6yZN-OJLIKiLL1!#-#;p{*H%u|P?G5d2Kgi{3_hIBx2g)UTy~{qfH^6$6-)s6YA^3rI zAeJc$O%q0#ny%4ML81MS9X8+Y-Mb<8pyCi5-L-wIT1vB3{w$J6g5u(eT3Q{x$ILA? z1HP|RS z*64i2`ZGrR4!`pn9p#%XjrfbY(?vyq;1zp*{W1^P#odq$@Up{Sle?XJJtLJ;$`^tT z_EMo;x{l{{dmdVLwrL0!-_|fe#Rc6_tZ*Ps?hHV8zHfmVP+5 zvX6zv|3Zc;B$*5B9|^Q!t~z#+9}whUU={R106j-)v9K9gHZBw1K)Hc;I~}VRlD#*E zyi;IJ18e1&w{jW_MmY?4y>^i+Dynj#gLnd$ZFF8atvpscdN5atrP3*Fshd1erR;Q@}}fG7WpQ3}1L zI*Y-9$QG9Rmedu(N=EnjYF34th7AaAh4F}DdrZ&o)qHm!6tsKywwA55XQ^GiDte>% zb-M*LZJhcuU7YA4XvoT(`ER?zIkFnafz!G1>j52l?twEib|5|>wZO&JI_i1OU(rmziWD}%J`v?4&H*O2TVPP3A)b_wXh5a@)QUfo z+o^<*8}5t|-uElXO9&*InL1*_vmqDG)jfBYUIwtuQay;dsfaP7pGLs|1tv==vC(-Xmz418 z)kF9@mqb$#3V>NBi*!bdnJ7FD=;jc@aL^FBx!~YC;2?lkcsujA^Z`M34x$G!Siq`a zPV52JKf`i@6(*sO2aL8qI0C@|c>)~e1$g?{SeVsYKpNaz@S39HVtl>9B#VnBRy~MW z@=>QHC%{)ky`OeQ+21pCvC1NSqSh7`MI$0c{{BwT+bek0oGj0TD5pjX-PW*t@dx)3 zy$yQog=W*{&!2-$3m=-4P(hE0-RAD*_HxT}d<;YQ2e@bE7O!=SofPc7zjbuz&lP2~ zTR_r)29pc#Z$F>TLwtTv5=2-K8_)dg6!nOlYSpA>Im(W9$gH~mtNpExts9c#hfi?N zw^BwLqFxEVjZmc!+k-G4m6F1##2MNq4BbXeU!T8+V)92Gd1|Y|wQHRwRB<@J@pNVP ztvv0cN%=6a(Wx=u&_hwtFO7PQ1r>bq@gtD$f(0AsqjxurJK_9=GR(je%tP>dF6!zU zUaNynxMe_OE7CKFy+42cJY{Sr-@HOVW5ly(&+gX`CKeY*Lpgn{N5VhmoTQn?)p%ge ztLphYpvwNoCbGdOZou2%=czLNyw)NZ-W_3{kW9{{9KDcoTk6d15o;d%Bpa1|36x$p}{V!fMcrm9lFlh=w}=A z`kPmZa$O;e97PC3l|NanR@b@bJUTId=^PD+Q9Lj&bh#;HsvNNK~AMc_ACc{Mu) z=PW=F?9I&0`&N55r4V1mn;`$|z^6VxN2OY8>4!!d3|CV&X6Y-uo#q60v7^j8E3?~&Gjwm?I==YxNBm;f;QROQ^B3QLyOlO? zaZC5ZtsTy1s5|LpSQ*HC-`->+S1=V^U0L;M3Sl^NhGlzibB)h~$S8ZMcrmVqlA-l$ zKjmQWb(VRa2G5{4B(4$5Gp&Gm6cclBUUKI5kdF8IJOjmjz0DM6k13g_p(;wOOSLLq zAJtVPtG^~+^shE-h!G1VI-EZ(7xHY4MnRHtdV0(NW)@RlaIy(RnI1$EyB;lD5N3hY zEDidSNVm;4IHANq>3__>mFvC?l-P@CoJ@iD5wW&`hJaI_TDp$m;v;nW0SwJN(Mp_f z{MxkZ7S;{9i)*>2A<@xk%0Hx+S3CxJ_4|fX<4s|CVb%eZ zpS@2ZHn>)M7eZjj1$La}#<}84J-cfs7t_ckf#8UJMwBi5C7eour1^j4d?3UMF|aSph#{n5Q^DdORyYD*V7YxEtm za-{8iyEBE#`bz!z2Yb_{yfVwFrunO|^NBMlC4fs94N#~*tuOWl;edkWsoC{OZ~am& zm=}L=;=^F>Z;R*8$$_M;z*i$BzeBHRXh3Qq6z*$OC`md3AuPCRg%i4xHYRkrDK^8#BLa!LJFq z>uG2xf9KW%w&s$^Okr^25JV?7C*)im`$plCsgL9z{HX?^B5y(T44x5TDPk_*lmYw? zsx#0UCXq$+E(@sF19B>Mp8Ny79r#`y-o&GQyg0!B^4+_z#Kc6es1N*AL)Es)(cas& zrZk&*yfO=ecKI8v6km)#U=i$cN|}jogD5LkSJ{W;{EA_!$COU&{}`}0_RrS| zg;m!SyHioU>)GyWW`vhHMgf+14MQ@pymdPt*;tIV3v_uF?o z3oX0Ys=`?|nDcRlqsP~|=Skh~t}CxU~| zKy1^lyh6NbznU7vF7`vXD7>CqNPFYIYe}T)fnd8*O!epGiU;%9vSo_C)5 zRk1PQgSVJ909$Nso}9EIW6e|*Pi^h&^~4X^B3$~)@;s++fmxF|`9ZwTP!=E-RGW^` zD6-cFHve0_9?qxe)po*(7JJ zs(!gm6rdN1{gOx7tT{#Vee^W=ZXRHj)Z)^sb|)Yph3JEUco=$rJN7q0~UFPxt(nb^!$3rP#8rD znbSBGub1e5@2h-(A+-kp1#?hr$defbr)roTxiaLz{mAaFJG(D+YAUHeQ=|Wx6U-Dm zZimEum`mgIq&vKiT#%Ulp1RW?QLf*}Gr~th7#pS&%J}xesbJQ##E~o5ApLl!6r`SZ9`>My-5?`#mnlpx=`tb#IE{PtSc<9m9@1^uS1IC~ zQ>FO#KL&+q>#)gLind^(R@I{8d2~-NYqI@GA~%NAe5An%vO9WNTr~cmP!Mjp_+pfu zj~INruzZWPs&hF#s;d493$^IbaZg|B2x;M>){T=N)K2<~A95?DjO9xq)x)J8Vv1!R zd^y(NNg+~MQ`yNG>JOBH&aa@P^joBSc@L^J;Go!3X@utk4v0K~6G!{Fnuu8tC%n?q z$f1cMu!y6{RA9qvV>hLfN=jlN+v1Qaqm+jzq|lcyUc6B0!r?^1B7>VioS*_0zJI{# zMS6Mwl1e|lH$5Tj>96(EZ$Yq`i0#;jHXP6ImIWh23gks$AtSK&ONGz^IKw~(MQ8F+ zOUcD*xeo~-izuj@xAR#t+qt15aB-3E^tA@aR&@1F7NH(}@~7mlbT zwWwab`gxB?_nj6qW8{A?mwHxCG9lR z+;l2*NnyIL2ak1u^xc-wU7*VC+cnNfO@^bmIJ|DMG}SjU0ErzyQPAuQ3&aD^1{P{E zBnuOyZZihTr6@%a#0IHMrC4#87m5fAApwA38{{QCA_ubX!g$xtGx!XoK3m|fp}XZ` z#R~!dV>(9n%X&z~N_2aac@uqhIL~G<)SgCVyKlB0W98DveS`6L6NxZ`*ycWG6m3>V zTGI}RYslcC=l##f1xh)}%d7q%6aWLCRd*1V9h;PFxE9 z!XRL{oe&0auX6BQqx)2cn{ba9PCFe#!^L6)&Z6=31BW6z>yJC7uC49%b4w0&|Lm}0 z16p3Dk_D%LRJX?)CByh*{nDFh|5TBo(&J}wlo}aWxujkj68F^@~I00@%)YsE$u%pDKMRo+(3wgz!)tG4N;{ zY&RHbdp&Zn4qN0T4x(}@Mc$E1>chdrmzkLpC3^X|K!u!#vJdb6o+H@7B|msR zmCcE{;>NN4>f25u<_~j#NV?-FC`17$pYD>q!w!k9!wiycLOA*2IQos?h|pQw$`CLN zMi8UI;1{9@IA91u>K4oOFWqyeW7OoM?=BEw_3PKWcq0R>Yl%IJSl6RORSXv>A%k{s zONf<*uF3uABNiC#Npy&u&v(fq)IfzyK=m&Pp5L>&FgY~bo~B|TM168d{m$BngW=#s>l z5*jU0a@I@<@jEyrjqSNpHz*>!=p-N_Kzv#QZ!DA1E&!69Fsbr+cj<~_at4y z=q$o6D3)zFEe*E-T@ex@L|>@YS8|_XOA}f>w5V!BnY4C1tLQbUEzsZ<@~+W&H~bhP z8VD#{*3oG|NDmSZLOyC58t33G<9rN)s!%^{#A9*SuwSDjTm>OeTXaDR1^+?4A=YNm zdx?|QtyJ%L+MtangRli1euDlFA3%c3^kPP{xPyn(X+<*Ds^u>g{vRy63v5TID*H|V zKR~&_Gd&tml=xTy3szX6q>AJ)9QQ|9ojYiYi^M4blW#_()#sXyn06-o7gQY+yfgZUP%qJkdhT?xe0|1Xn&T)ZG>!m6;yG$Z= zf8t2Q?N#SP6Nbz0O-T-FZtE?`oV?%p@BI=tb)K?JUv;N1ezIVbep<;i@yR!3s>u$= z6GD}B3o8Q++*u}*xV`mb8`q18 zqrO%vw>5#=by$JMf@2#7aBdS_J!aQ2OtrsK*OLK zMmiJ!2`7ABy~G#~O%pjc$G(R~szrVq@1&J1?a=+ZetqT{@j=NyvcF%SKlPNg$@9Jv z?rE^H7jW?)!waYNQ==!q%1_|JLd2QtR@06-)GI_U3#`2W{T=WcKujPba?7TPNJ!Jk z>ABI+FAV~fLNopbh9?Wq6F7j-V}wUVIc_uwAZspH&*K*EJFM6(T%iL|8=%hml0iAlbZY z#l^)0G8SRk2`CBHITR%+{;Rk&hm8Fwj#oT7)W-N^foUK93qM)akKcZOap%E<+ants zPgPKot;Fg)PZa>WKj^*~T-ng6xgESH5f536Im8-xiAnmDp`snt%fGlH7$#}8Kehg) zW1yLT#Cp+O`FV2kh68gy9|bKwqjEU;-X?pd(0#yUq=M~YXtP3Z=|Qe7UMmYfh#P`n zYdFn+Ul`}vbPo!Qc6+45?Em2&2}egXx}nIpxOxcaXaB`kJYFA&>H&`?hWyzfp31(4 z$hkNB%d1fI4*mY!1NrC$Xr;j2p{mS!^yue(Kd#@*Rw?Qzm_W5b)9Zt$-EMs2`}H-% zcM1-gs5PNO5)u&su(ygf{+wBWP7o(0ae){bvJqn&G(FH95QQ}C9woRjWB>>UvFz!A zj0sr_PqA!a{$EMNK8LUwr|0DoWOl$0dl4=W^a1qXZ94yEynKl>wO-rvA0x6={L%zT zdKED%;j+V3f*`-Mt2qcmS@r zHcJy^gDejxuXghwDHZC<@B&1Pp83O2QAjLp@P#1UrFLJl{1L2Tc%;C8LPG{$Pk(=` z=|$+2$wx+&a9@yyQ9$D-(l7QAoO_GcEG2Qn*-6|F`GI)rB-ha!^)Lz};wy7T1QMI910S*cA)5(rWgOB0duV?1 z@d^ogh!1^H!kH>~uNxl)c z3=45PU|8Y7edf2^#yf}C{89MbH>ZD0=A-NH9X;A8`gQ#jd)NMjLa+%qqR`&G0}v6K zgFf-Ndj|3_6ek)f^6`ir&CpKO!rJOAwmt3Ffkt^{K5qpEvBn<=sCxOO?tM&zZ(5=6XhlyS6^s=nEm(K6DDqq?~`-y-5O6Gs(w4LuG zxHMWhNQkuA)}+3J@ed+>oL!`!QvsEVikbYH(0W2`f{{4h-rILZB?l2qCBC&p3&sXIDSQ?eXnIB~XpyIL$4 zdlgp?SfoM-PC&0Zy_F3>H;x_x%1Tg~0P7*`fVdkaQ<@+Rs#&yu@L!^yj*hNH9fOSv zS>os1vyGtOYk;Vr`^7^7k_C#v@=u{jUY&Ij@MN5wLyUqij76UrJ(2s-40S%-kqBmD z34*N-wnm&F?$%+fG(JQVw`b4h#P=EHF*sZyHLx!=#F>q+n&eJja>omu#kK=cGO7Uk@X)2G5zicI-KRsQYm>g6kk=blj5$ zBA*9jkh<19aI)~aX9YJhD$y2QEpCMYQ;Zd78q`)NnAWX|Tq@6rJ$0G5NY+0p7Ggfb zNqeKq#cwl*w7Yd`Z)Y1&GU8W)p6Rgy3ylvd-3_4mzQ2Hv1RAc5Nd|X%oL<821?=sa z(O@klUq1B3b-*kYxudddqCD;mMIAHh+>Td89D-pm3MJNoKcQj$e_~U&j)P zV9r04+`80v7S=muLRJ>9=%oPM1}vJ%IZ#8eiNfIoW3=As2~S;jnd?Nr0of9IOlaoG zNdUdROuk)zR%Xg;BrBlB%hu1qEex%^A4-1E$hc@S0pjDvhWXCpl*i}cDbjU7LK;r? zrdeCIJ}`T1rn!mK6%A-V(cDDE#87;Imi=qdWm((@NDw2@cC=jbaafjMl}_a_=e6V9 zA8CzgFi(R53|Kh{e8c5UoT6y{_`z6Xm3mowoiQ@9LXH>iaHs>sY3ZgFlWbMLT$~zkMAQA#&$NfF&3p83vxa*( zRmWlTCSH9JfC>=wF1kq=H6F25pdO6@QK3+$2@nO471=5fVWR>gr~uTNQU@$0;<3Zz zCqEnk(}LsWWQN9$ojY%%+r#QNeBmerY6ROAT{Wup)UTE*IrI3o1 zrP0cO=w`y=p?a$8wp;Tk{v86on1ITl1}$G0Iq7$Ax3I7?;=*Q$5xXXU5x4c=br1Ao zbH!c@`=zgbgY>va&it&tJ_%d{P$GB1>#XRu6-a>boDQeAz^GQ=*f?Bx=;-S1iaE}} zcXxXXOfg}JDTjQ|*0nt6VzHHb({x+wPTmTXjr z5waGLZhVx$JFpLd&_i7TY!jJ-2r)YkMR(7(3RO+bkQc1!PoIY2?U2A(zbyjGzw7Mg*U-fX8DieD^Zf&ZymxhV(c;1hJFTUEDo>5;fm#)gw2b5LA9^3{boV< zYC`9XhY0N>`e!ypPkFG_*1x2Y^@P$J);bl`viMhk-B5>nGj0k5x(I&nx7`AsSliE! zFy6xO5Q#5@0Uw6}jtUf^D6k=}JTDRdiFp|R5;UCWWI8v_0kOpQ8gXJbu{hD37JE*F?KT>JshM~x98;gv%f`YsbP+_gdY%};%rRx1yM`X zvke|4}oa1+D#tNY=tTWKT}bCAO_Rz2}Kak=zuB^u{TtKTiHVsm}ld%$B0 zqZO$nJuu{GTj~$j;U|RW>>C)!k&4^v3knPzjLdh1zAXx~JTMlP-KxiRo&ve@@h3LC z_`%fnvK(PbW_%3Y$V5N)@qwB~f@A0zk&b5i9~r%kOpNgj3n2fa3dl^ z6cz^0thSQ)mxjhh#AT9*34jBv07YT1%6$Gj247tWyAC{%xFsy0;zg5Smd^@7I$(P# zqSXZChxoNmm6MKGBeSo!m6I#~Q@KzeL8nQ#1FZp^$c(AYb9?+M4 zn?7P5bd=z(2Dym(j|qzy2Nxp81yQ?yyWW15XtD7jdg1Q}Hx4rzP)*|PNjd07Tsg$w z1DJEKd-o>5@6hGBwv4{-AA~}larKylv}QY&BrJ9jxXKar0qpQR3Lhfp2Um`F0yj&% zYbt^w;!yy@CPEwvFglVD2O#Q%kV5MOV26}eU?ZX^v2w)kZ64GXN2KVf9Q_p(ELisk zZj2CXwzX3(ta#+8!YS9~KB1|wICcd005%MIYXX($o_(pRriNqlsq2(=m)q#UHAHBL z$`Td8fB1VdjA_j|w&-^Mgq?xFNDwp-WDw6A$1gM} zs)(??e`2ubdsO3>1gXm;<^wMW5FNSSP`+0KiGX_;4vMF!>q$Z`3WC2*$PEON2T-*Y zpBmEhFG%_UeDCMA*vtJ2Nek$k@sQ^R(?dzb29BGhMQc*@HD5er$8d}$jV-5{g zyo^0BRFS`X(N99$-~i>%I6CDRBErSux%z7SD3nZybtT%D7cb=SVw%8Er~(imtrH|^ z{hLf@DQwH<1W=@t!aaEVff#5&N!p&5^$(DqMt6m8NheFqK#xEMQhMlM*-E{o%JaJCIV zNC96e=_JV#fkN6JRXieJ&|Z=u1*musgD8#uNM{tqFJbZ_ctAbpf^Y?wm8BVckJE+q z6DZ z(5dmz^RNVw01a$<)L0jxen26@ifxL`At)q-E;T9NmgE+Y>b1BNXpuf;5DF5)4m!SI zj5XC^!o_x`_$ojoh=GAnw84g@2ohGn1(Ou*KFeFKItViW;)4ee-MD7Pha zGyvZTfq=5cj#uWO;~>3O=EOA)zPoJud~qwGMg(qw!XCK*%CNTKc!Ng-F_+4axpv!O z|0tuRLN$s`7(E<<7k@S?N zz_>j)fdE1XhQ5~bfKxEm+v$U_F@^&S*AQrxHSXVid~vxdKxpm7!qWCdUBbE3{e^;p zU>Uej(Z^sLfN=x^@@ZgqVA`@mRlowm0u-i>uUm8E5G~l30S01>YODHj-qIrk_XQ1Iq|b0sJy3P5}DP zW!;RaHQRv%m$Tp93ubv^N`Ut0KnG9aVRQi0p;>qC3qPjNdYw?AVChVk5Iw_5hqZ>5 z1IKICIhoOGUMb`wzyV9d!ldLUFv5?OR)0Z$8Y*M&vZ*Ap*)Fk z{>hVvrKRyjm)pU$jcn>_hs_C}4vh)&Q%Cj{!RuRvZYC=)&k{zW-(eE~V2E(nvtor} zdhzYUHfsn8z+>PYlgwLgk4c@p0laE*GaztG4XYD45D}Pe-FbBKWFGnGn=VvdWVj;D zvHo^<)WW6&4FUL&@CHLT_B%lG#f5Ky+xW-Q$c;Hj91SWw528o>{jHLqdk02eMIspp zd?s<%{d_T;3k1VwSBmp3>}$}_IOF5q|7x9n53ztGyLQL^GLyU})1a3)o=Jo_G4g|- z-N+~aVPn?wt=zc^vXqZtFu(#AD@1@PN#9k5rvfocW004{)p5B66IvSW8U#M1^COD4 ziXeqbLy;Jh85pC^L#eaFO-1KF!#|TsH-L*i_#h6Fp9L)*4#J}$>}|tcAclT5B{!1H zQ*0|_n+#kwU0_G;I~3ba;9+Ay^*@dlM?_&=phdl3b?#7J`wUG<&jd9{)R+dZF@&Nt{%RO`n0I!MqXZ*N) z*ry@K(~G~p|G(8A)6Q%rrP( z8q5#pw3&>z8%*Y5m(p>{KpCdyb;GBQ+bgLyXxj{7Ew+8}e#a*q=Sk*%QgXIV~MBLY`~4#iR+Au{?Lzb=PTXpk03?@12?aJ^8sLOV_ih z$5Ov38)vpo896#+yLn_RFd+G{?cH-yd_F8m8vr?sC+$D6k7r-NVPiUoO`Rc~Mf$<- z=x|am=TiM^5U6pHh}S@EnniedycZ^kZ~v*B{(mKO_4}S{mVl?QIa6B2N#%$#bX(Rj zav${{yH#@%3RiNmLe38?-TV7!69Oq<&&AdPmi4soY%Rvp~wYs5-~CudjVJ z5j#fn3*RE7!2*GmyAU=wvw#j_(;ECGYmWhfhI7giHwxAWNKxEsfSo+1KP*Zvbs(7G zK6Wy>2p8P%vWlm*D@2iBNrX~DmDaBsX7ygDmp4OasNW$H|KP!;eWRz_TUx3Rjdd14 z;c_TQfzAGv1A5SH5Ir&0&?1myd5h>c2s>McP|W&3k8_>zAEXEx?O*Bhl%}JJsYZ+pIZxTouiviCAOPhcN zZupre^ZxdKJPNlm?nR=*1P2G>3iNWpxI&4cmmIYeDu6&JQ2MtZ^?xeQD;ZA(JvF`! zdI?f0ZNKqN>HPWgUN`b%opll$_v}Og0!yU@cxH0W;!JZ$&|WL?Jt!>US#c^f`3CYq_CNc=bSj!V~He#h}#^nF#v#?MKYbLGYNt5dJw z4BMkbJvnsxm0~8sbBLIA6|f)BngjSpk!<&*J^I{%)^ zb9-fc4&?z84awbKSVH4O=+kMhdjSIJJeNK z?FbbkBo+xJ>VgV+-}8Kj{b(#1}Twm>ZdE;d=q z55*&N87SF_xoHjD>g22jA#6_IiUg@3=G~{jPqg(GLqUgce+-8ce6Zy8LyP%R`v56p zpLbsEC15;?B~C@pB(k!hs?jwvdW#WP@1guBLB~EnM#)X+K9ytIPY!TXbc7`0i$H$l zn;cV|-%7COzWQ)NO586jj5H7-AyNN>-gG)W`lCpkp{Q`?wwhGn4o4`}Av~0dh3}&%QmeY{#2ormJptiM zT>GT~mJba$u{r~3lki-0CXNX#WQVU4<)r9A+YX>Z92%8nEYIw!zYyaRg$em?1Oz8E zEI>i|_A_3F{$S*9+?XJ$YE1qLf=I$7bQ=?snER|9t}!sO=`in+-Hf^v=PB+JI0PjC zh#d`PwSOZ`+-#F)IDn%Cb1RZ$P%w5%5CbEdEEA6gZ}d0|aEaU#lya)U6LapUIuP8D z=ULeSHXX7ql3xh~tcKX$@dm7HZ1~tibWvihEHD+ zQP&Vw2U*I7Wm;&-(O8j51BT`9vSeHWeun{Q#F61z;5=?dG`~d6Pd+iMW0)?{aH;tQ z@@-%$1wEs9C1e0ZizoaW82>;*v}PMkmv7;uN3z2Zv)wA4M8dki0azg?4mfdIl>CwLD;z(cdyi9oyrYT_V<;zof`y2;Pl*6kr0h3DL z;^N}keOsWettn*-yT;JFIuQWi38W2U3rogse~UoKu%Z2KXxJeo#f;F9G(i%6xau2~ zl!TYM5tSnDd6GNGa^zzP8h>x=83~IU#;jdGbtD(9G{ONS&cCKY zj|hKt9~AZX;1Ywai4+_Nv?Hi0fRdct7r%pdJCvF11^E#d=loXoW?&jY_hBT9ldzyc zjuv18Bxt!Hb(Cb;08c5qO9ZC)6O@Hw5ZR%^Q9~065^4Zv23+Rni;PN#b6*0*;(EG? z3ltib<_a1YHHAvdNWjW0>8%gHNe$KnO5LQL=E=7Fpj zC-76mhmh_X$rtDo@F)PFqp$->26(oO`PHGkEr3Gne=JY$_oZcf-FDg(ngMCFfPnUq zMT6fQy^2<n;I7#qj#!hWkW&cCiEv>jl(jS1!S#0NbNwQtvj@S@1W()xH*8&HIZ- zGF!6-K)nO+fw?Le4g9WM``l;8T2r+1+QG^<#WmPqD+AcVn@8;l$N}FUWj|y+P$S@u zhG`N+5e?oe=p*6^g0}!ffprClI@m!dovH${cLjGV?;pn&4+pOYsJuDO=Rj!)IVY?n6Fe^w@(_-4GCl}AA$kF#1|zpT zIuZ$>f2-Q=)rSp87|TZ(ED`~oL6%_NP7F}%e5}svSBI<;A7 za)}2CSK3)n2FD@ZXBS-i+l*F42gF**_Yo6-n`PyvW8R$;y9cGOw+L5~%Ie5Jh)m8h zF>a&raos$$D*8Q5xm!gCT4YFsQxtJ;heDBsmvky;Dk@JL({ZrDuqM0hN}7d#q0sNJ zfkt%F=maia%uUGrrn_r-(RmH|e^eAGuJNcL8E+k0zexYE!MrCsC%1<*EhyMkbci8~ zjOjtT6`)E;a{|)@v1Sm8jm`-19E2;wD@GrFrjhuR$d-C`rKZAW@)*jnjbQ5W*Diqo z#1IyAwFErfD?&<9d2+}fO~3?Bo_v9 zspAp38U(SO`yo2Y0RKb23|Y$mDUlP0aTMoyb&Up&z#|i zj*cb^5)~#;>(_9JHv;**AZ4}k`*g)ym>MBNB03`gg+S4qJDzVvsRg7L{Kz7{0>T;m z3sOGdAhs+MDkazfshfx{0qQ5fI5-~A+zFk`c(hg*5D@Wd0a|KlY1#eqG)ATl?E1qVBoC=b2Fq|fH;On{uTt*fbWpa_YwXyB7y*1h1fB~QoLEB+C)P` zpeP(oJ^6K9M|79~1=W+7Bq@%4aOfaT@c#OjNZUC$d|p1MO&ygD%5Y*Z!p3d+{5fO& z#Y~ieRXF{DtCK<*8yToUU>c?i;h{oc?v2k+k~E2P>bhGh1ifhS0k|E{yQT%o4$t~2 zifV<~pOm;pz)@M^!*07?QG0w(O4~t~s^&Wz8SP{^Q4XZH4uQ3@0H2Q6LGn*lGkOEC z$kmfWUErWYJ70?jx3^OoOA%RbH86-_v=Rb(K_C%!=GLuSyMLFX)tvp}?S7*ju258* zIA9)CKO5ij29gZ`uf)Cp|0NDOGl_Us33FH6kJ$1_ez1}CknJIN z1It2JU*8ATg`y>RQHg4ua32u&2A~Thv@KpaL@|eSn8;0pdfP+IQ@g1KElgLJ=c(p{ zZNeib=gd6_X^PuupXro2fL{KcJ9~qhheoA;DrdOPPY!9MDl9#2Lep;c^Q4*O*DHoe zy6&x)v7SU&$f5ER>FDc-brhZi`M*vQJs%;XM94*I!SgW`eu%xSl})w<1q7sf^stkT ze={btMI-th62Ff>&UTO2aa$tra_ReKsBn*_kEe?JqJAqKtQA~uE|6b=Egp;~oWqQJ zz#70hCUkC@UB{NX0k!Bh{O9z4f3Y*0Hy=Z;lSc}P zqgnH+OAj@as&+FHh=QPwfwBO1oX7L51E+}*2_Jm2aB`9sC&&pVg4kwcyeTwF`q<%; zgba~)7dO%QQTpv9WdN!aT;aGnIA~hAU!8b zQ1_JU*n$VpS&+9RjOSgPy`vLXW`2J6tAfD){cvMmqbDT93A6-vxvxkqrlRmH zavv)VtTn^6RK##rj1|Eq$oGLXF5OP~_xbNdGga8861c__MfkDbu zm|ypCaU+2iX^f;;#aYI{mjVO5GH6m@y^wj6a~D@1ytL>hKSCT#M4qTki6|d}L{glQ zxD;SkQ#WRSDbw;0{D+DI=VZvy6t(T%oJEpO%JG zo}wt_A9}+2_e(1sZli*wZx>)KZ+w^BRXmz~eD|mDJ637b^gNa4YeKp?s3i$PwL2fEstKT!IBy%vaU zfc^P*-Ilh5Jc!_-os2a0oa?XYwI})6@F_!`Kss0We$lZWgJ9xCZY$!f<1JGfpc-Yx zlw=4JFMx(7w-@#XDST02)RRsI&kA4grgvQI#)xr~!h+39m7gySz&e1ReBC>KLWBju z=bqIFHmkyy1FRy6o&*mi#E1oMUc$XB)_E;%AORORxl4?5Bg?2b@K55&q?y1YnmL#k z@n(~W*KE&N*`w@0h)0b@7nccBeNODor3B|Pg}_?g4x9Bd?^V+H)ana@MJft|9ZKIJ z;9al-hM=gk+k}Z;D7NuD>w0oUhO3TcX?p~s!y)q)31J^2dRD#2h#!A| zoCa}8No7Q&LQYF!r3^niA51%n@Yp7F<8Cmfgjuk)N+RT}p9DF86x=XnGLmzfFOV0J zfvAeJv!||1AOxQL4J5y{Sx2LIC6z67F<{C-5zz;Wr6#&MIfu9)rf?|%$It7}V#8>vBBgP*yoVuOL;6`-2{qNC z3@0Hz1Jf`cT`+ccX0fU}I9gQXvr9!LzEDb3V#Me$0udvxHOcys7;{4w{8lK20s(&_ z)WW4FV3#u@D2Ry#q|u=(2MQn$z)-zpbBPAPO0#4gkl`hmsnATGHhJpk$xv$%5ijA* zSGBv4Qj%hT$WV;k$7^u}BwUbO^M6_Zd9LQQ(v}izFnnv~8<-;z1w@-YBn41StN}vx z&8~VgjJG0u>Bo+Af7Lu5<1qCB+Q~w(1F6^o&ash+JS@e~Sl&jFM>Vfie{&GnCh3 z&k^D*U56NPph+;$RxcS$3!6JjG=b6C0%-584=ljq8_WCUZG?>^`7p8^RF zpJ+JnAaPtF%*GEXx4KzO2n)n@;)eq+cXhkzW+?+nAhb3Hvu$7wK;#L*f?mz`1Th7v zIG!DV2NMS&rIc+uBI;n!KiVour9^_?Nve#9fnBA0#w7xp(N|ZLO-oS_m7Cay_#07~g1sx(W7HzhoT_M%ok7Ajh;*vvhib{TYkl}HPfHXlZ z!=)}14XlVxo=f}S&NfL+ z?7!LZP_K6YQ-z|fRfLPNlpe;K7;RgF^?8WrMCuQ?+UWd-J3Hw7(oMV`>1NS~)^(ta zCRicut4N1j!M8hnI&iv`;Io$YMI0w#XJmfp7jP^Hb^;dH@+* zzQ~4ozccOE@e6~OpXw7_J>EihN!bq~+7+HCVD_k}*$oHhwCS)0Nf# zj3oeL8j#Prq~3*@8Qx2G!n3mpCQQ=p7bi0nNiWA2sYcK|?!vD#bM6|6(ji3qO&luM zca%2kyOr2jE71ckz|53uh&CTj=U(y$T!tS@&|P9~#g-sx69DHYfJ7Et&E!*?@(hk4 zgL82JVB@5ABcL`DLxYv*e>clA7rfA3C(?Mh^_i>uqe}@^2?*WcN!9}G2hh3~U0u3v z7CLC;&OOi-A^{UM@2>s#8w)JRU-tC!1~ureptz>PCdT>I$~}*0MPYXlraKmf*F#UB zSSS!7Gnbx%O1qG94L(XJiN`)3-%Ezjrd?vs&MrXmEy6-Wg;m=y0T`tR>d4mszSIRG zkuyw0-|%i=5+*h`iT+q4b(1wf`M28cg_57QHbJ)BjTVhRS?i=|EUaj`p@muN1~E@M zom@nq-ihLs1m_W(3YiZn$VBj8GF`HUQ9DdSfIPFUR5$lE$yAH(H^(-b5J!N#xQFYm z*34!Ik`QVRpUW^rOv|d9lx5x@3^*19cnYarBDj54{~IkfnczC*8tS42gA=+RKYWGh z%9Lp-`Y^Zwfrk8>0bGkRmb(p!-9dyF!#EZI$qeL_z|m+rhC8zkXbG5};o1~tmGek{ z!^z}>lNo(j2{1^4--lbo6T1fKum)df!M>P^5yb{C9EZg7#?$Wz1`=J7Wr~Y0T71{| zdwE_lgn;ufFU z16?&JTe42=&a`;dozH8K*=?=a3QrVOMa3z4UIEAW`@$7KRU6FX`#miUqrFO!FAs z@u*B2!*Yil+0T-3#o)}0MPtjZ*j{@pj)*@n7^qz2Ix_5tcnjo6s?9zd8wIff5k|UW z{zotPr>UZ?Z{GYtA!cK6=EGe&Ohg5<#Z@DM@9_N(o}n9WdGo2^J#sl{<+Z;|wZxwT zCYyHCdgAl`K+jdvf_E*hdCYc+d|h_fEk_( za$moE`QlF7(SBN%m39X;1tJ066^aiPr$JH-ZwpQl^3Si^c;;2-{NWz2>s8SY zLR_j&R+QrB9y{*ms?Syr>HIl^t+c+lPw?nyt82u9$4~58SKR3X@HR3a^2#r@I+g(G z|Ni0LL~)Y`G#*N79%`IW|K{XhNQPS(vh;^1 zN3EAOdn7CJB<9=>Rfe?I5X;x`C10BkMI|3gdH3IEeDjN%`j&TNKi6LOJ>Vd>s%+HX z6VW^%#Po0Q1s(D@AMMITLHD{F*F~5*e?z}kD2rQN;uaOYQy;7Ke4j^l{^RTK+E1Ue z?n6<6Dign*pq@ZVh}&D2*^9>BoHhlutM@hPBJmx8yg;Tx>wE_K(;Mh6$Yv-dx?rI%2ex~Vr zvuX8oY*bGjHRhAiXGNzU?GBA*Yzl89Tup;RbZPyL}p!l{E*IkIy}C&<(5*pnmp*kes- z;UT-jOdS!2u6eU6hb7+|u(x7+9kRo?ispjt;@(?43SqEU&!kkg$y3TYf7fwg)GQ0u*>mnU zvqgfWArIw^t_}e7jDU`|XywIbB%pY|wr2H}k)0jxW>o+mjZyCEmUCJt4t- zxVl(usK=Im_|-4(-WUtB-^`N^ERBzD@$mRdM6~+#jh%~U_s!t-r!%kh(i@PRqVYYY zO#fhuN?d}kug)HCi=MGpt?ea7!M88*ygcIX@T4mppTmmZyw=p~za{wVHZL_VDvy~3 zaO*P3G?)x}^QMBH{=wV<1OwuAc%ouQ$;?5Qp!<;~+ScvHY=n`g>kaLx(F z1OB510lOr8KC5^+=}oX4-7o%p>V{ysfLzm35aTPFN9GG`UC!KXhm)Evdj|7R8^ko+ zjW$&}c~YZnQj22yz*_nr^VALBz3%;e+__QHo%;&Ko}A@rXWJR@UTDEWv6y(MDR5l) z`W}S@FK~?CaCg%{_Jw~D{hVdTEx83@5ruGeC!w8 z#^(6>PEvU+rWvxFdgvYhWo_+N5vkIZdj$bKGbdF4WF!n8OjFgkp?K^^j$&oId}3DP zXbofX&HK^ybgzzjn(w^1aUj)-`%kghY+poRZwZi)JL~rL-zPbkKkw@WY zcNpK4mN=MGC2=UMrSVd5wz}-WV3Ve`EEy&lx~%I(;x_^4t&@_v3hE8bJKl3 zEgP-=x$~o^0aMUA_*HPW!A6Zp-NUHbQ9QH087f`3ClJ@sonOCLFf-vMuRSJE5qcZO z%87f7q^AJw07%-YCX&sE(;?E|pZeEJ6PYK$`mEK@G#>xq(NpI+pu|)~A9j{^eoU#M)4v3lC-#sZs!J=jRq%FmyN#aHSe!k}i{RZvC=BG!; zijx~2Y${iBohiih=i?9Qk2$$o$@DTxCXEa6W%|udA7krWlu6@@{4y@NJni#r?=o*$ z-r}Z~OQ-3R^C@&3BDhD)949E1GxK=l=gerTIas@9cNb6&S4!IX9Efvl**`A*E-mTr zuFe_ZZlTyTs%ft>i_`Qstb~0tbc1cH+a+UPpV&4srd)Y=H$UEtl%6gKcjDl6|D?kNnUVCZD*86-JzYSt(k`_{K5x66Ab`}@ zt}ZTf!;I2ZQ4U5!u(Gp1(9KF>dfBxXJn;!Z4Ux~@NLopJ49c?52Tn)66wI9YC_l4OwLLyiuzg;5ld8Yp_}l*VUFV!z^Tum3S_^(^3*Egc(7Lid z=F&&WQSHit4>`>Lb|%I>Urha=&gR=SzhdWnLGs-zx;3Y*c>h{@ZOtscshgb`uDPY> zU(pA#;a0yQ!zgCXl=W0P=A&O<`fFCu*j8)s@%!_r*Ix~&?>jD@Ej)5=!{$Q8+kKO3 zd4$%=HO-`@+A|yoj6%7tgL**HJ;2iT~0gw z;S*oRslz83&F4+ud}@4G@^68<;FgXM&+fH{?j?;_I8NA(NhaBQt15?u9o!mqXy06S z@tD+m{sSd+H_T}YcHPRjd!Lz6&*2EG(MHp14SHo|!yR8dS*`@xs?bv%mF{Z(FmEc> z{jH*sG4ZC(o-poj-n-?)0|r)@>9lFfqB4@$@NC!_W0$4U`p%tuuU$V`e&^5K9vZGx zs$HvR*U2p?D^H%>2WPfsD|kWRVD~P473$4zy}tcyJLZL$J32-|k@G35RVem2nmJUG zTF(PE)DG!^8VAnBNw;gx{cd?Vm*d$?qbFQ~46yuynIQ4aYp&lffzpFWSfIt%8qkT! z*s3COq`E-(UQB*A|D(^YI}&`DGWc$vT35Y>Qsjt_16wuQdNqonv6tOO=V==y?VLHb zZkM_l*Hsvp|D@u`$wwTj~%j~R!Kj5&HQXNL+soaX$@)L8Igx67nD7h;G4njX~#@_O_1Tuy0@IzC#n zJ>Kc31l3x@jXifY1d4BL-?ORw&{eC0dnQ(>@&35|!QjcXB_Y)mi^Mm$hkvCqJqCM`$?yc6hbfl1Ej`U9&K&&+o&h zl06JppcPOsD>1$bTmZUh!$q%$*n$%Qb8OcFk9M`|W4j<_-;y9P!?Q(-w*}jHhJE%a=5$Q5VAq>C!snzIa!O+ji%<7^XO4^Ktm|T`_=OY}_XIs+>HTwd z=9+Sc%Dp*_w0(I>?0dIOj=bxqtXr`6+qJJ?dM_7BtIh>~# zv-jCWS)?&AnKB?0e5Fbvo7FX5*P8z1-q1~YcFw$l6tP-3GH06b}+i2RX*J-v2M(fFQ?FDxU z)v|Qr!AoW>{SH9Zftmmb&Cc$dz|fVCN{pU#L)q8ll1Xq?PF}9X2JJYpwSK)z{HIDX zlT;qAY>^3(pljsZc9X|>(Lb{3NQ$e_MTaea7z4b!^&cfjG`(Sy;oa_MYGcI^(Rw@K zt51c+4mw-f8|K35PbsNzYHk?jVQyQe^IAev(3$0k$XwsgZgOFsElw|7A)nc zjd=Q=sFS6RU`#jS5mE@X_+4a2btsIph&^HS{walHk4%p=N?hK*VYTkblq%|JSE5*} zm?o$z94*MQ36lp}1#(Bv0iG0&mVoJOY925#jy}jqQ1Lp-UGG!7OSzDn_ha-(#DnqS zGYQP&{s^u@opAoti;m8e%!PLz82I~c^!3qKyZ`3c!V%Z-dt?kNI|K`l+Pxaaz+vt?!SIto?kv-T(5p%yvNg~lh z^IHAp>-RI9(y6`*^4~}oc&T&i36!oZ8M%>PrzBh0PNxqGmxfQtyeW2Uo0Ue1!y6g9 zHPK}cwnc4UZu$#>jQjVQ>jW)b$7|$J9Ey|$X2ou$+Ncy}ZD#fAQL#ll^(}vshKM1L zYxI%_mVenw+Br^&O!NoHmTVlI8ag}!w=*T?V8JGdkt|I~Y{T1+}`*)D8z=yeAs zAm;`1j;kw`+Q%1P`~t)7R6!p>6b@Qq!jE5lzSfCJvDQWN`U^4Ozc?ACN`Ia9dDk? z-{C7g5&2R)wTL;zdhyaW$t7xhk;h=v^%?HHm+$Is@*<#PplxI=5AfM`Tp9;s~Ss_ zUldl+?75#fqZ<3O-99Vpr0EJpHuj%r>$zlVIc5h;R3e@yGMw7yHAFE$>H*?8lqxHv zDvFNBMDP)G#jje1v&r5*4l~Ru^5(RiM}O5vG@l3@efac-R7}({9V%rtW%e6PYyt)m zsPq$X!Zu*`75rAQ*hLs{1Z|isRP%4cm57^U`TYeR*d=)!8hN-@8$xkxycZ zf#UA~@9wp)LJC_x@x|Y4ZwS3|RF|SAWYlWS3P!frmnex7|RA6uiO zVCI6DuqF2d4%!`~){xmIRhGDDx2=Go617nM~X^wASS(BHD5n_$R#7Ie=^QmmFBbG2^Wd|-;QSt+kZ;l>&;TfuWxT8M|gOGrdi5j0DUlbaQIRq1y=f{(G7zyJWr6HUO;SUFje7P12QQQFE2{5cw}q9f>n|WOUSQdB`ySLdzPKrfU+DpSY5?(!&UE66i)ysMV^!npvHE&! z*U7nRoU!>Aj?tlEaC6%L*&9YRN8P;X3H=Gw@7Z5 z(A+T5xPXyR%{+&`mWtNYa_B-4(sME!iVTkCoJ{{U8@RZ-p@RlY!xMqm}{{LHPA zP$|}da+j2JHriILz>Hr=a(^*Bt?vo_=-k{X5afi(?E(BAXO{1LxXHsPsOHnJf%;qz z>V2h~^ExX#cI;61d|D|f;`k(V^7x8c#;{sK-n?9s)rMPw$u@RY$&8g)IXHG*3opqUmM-M;VZ;($V#>bc$ zo*CvERJO%E^0T%V$LUtbJGY%?|053bWtq4_i%puf+e7TTGj+4{=mbB!f@FcIHsA+0 z87uBk%F4k}c)W{q-OWQ`tKfE(m6}Xa| ztQ!hMCe_39Gd;bJY0&#q*{Rdr$@Za7yeV^jBhNXy$46K@eff5)dL}y-B<5Rs9q!B= zRi0J}om)Iwr0Osb>8;Y6)XY_U{cp9zb>IEpf~uJXP!cE)Elo8(mz9Dt;v!N{h;|?L zk$=a$CXYe<$LaKrMC(8mfV}h_XeejHL=G;Indl&h$g~2$O360Y(CGXFVULDIG?t_K zL*0S&M^TOZwAC_$ak^?xHr1QuBYhe|p8)HkQ=j=$70Yqr5uk3>h4V&I)1lPpNsTT* zli2C+e}*@o9p3U6J|GYV8`mF!0N~@$vpRG5UT@NPSF7{} z!W3SBWIM@Yx&saXDvTHrjqQa#;v#@HSOK~{nhmq`Nb+74f2{Fq1eHOg^;mJ{-~VX= z))37Av5DYsLX%5Z7hbyzkqMDYl60>$RXzP^cqURHY)8t?Z24c$3Ni|l%zanoD;aq) zjM?N0+4fNRlDL0km;UHuOby2xzkFdOsR1BX2_GV>#Q8X{Ae#k+)+6ijFo;n84A+7H zdNV+BQ^)5P!6f~g`?k@fAxm4#V7U9wlq(K6vwYS2C+-=Da9(eZ_9(l5oS(Tukw8eS zw9s|vwDHDk{ECY>qjezMDr(|^X<6urpe=qpJM}c5R-yxWa>tNW=@8XYvZI^Q29Sn%%&jh z`qOh&w|WH_LU+YjoQG)U%KrPyC6^L5$U8}1``fz|T$nG5S@4`Fii}0~!{A;rrGHAF zSzjNC5LCB!e7kz1OeHltoBKe%U?49#$Kp;x4ALv>6{(8GrsH{DTltk>5WKTdqT7xN zr?m`cRlkFdaGUE9?kAOgtta@*23apuJI6X6YPsVvdb;tB!}@*YlvG=fYu>(pTW~Ff zp60h5TZTr8#on&F+tSO~=J@-d z0h$ZHEk3%X)G;Oo?EZ3>M>ea^7hOqOw61@|t=;l$fYk2Tj~lYOB|5y=(E6x{#_62i zUCzR-iT3;<*OsX}wsRG0D$^^!9Wp^%kJ8rqJgZ2M)qs@w!s&G8I(%dV z)^X6?6RfL#eWyQzk&utc+n-~~8=vjHpEU9?YGkd5e7@GV@GSp=O|6e~-*46lwyk=^ zM8Ey2`yR`lNvYJ2XZoO8AVE4H@wq>q*^jADaNRVROW-mfHe86B$DF4t@-dBu97o2Y zbtG3)I?)1h8}mX#Vj?Rtf*s9dN4$muf)%*!QULUmiD$ymY0&XhRaMQ4>+54?yCvc@ z(95R@k`)B;6s{+tIK=2Ud{bMmg)^Wp;}9+dq8@S)iFn#HK_c6Op8{HfIuT=Vys*ny zVpJ7!UBsA+h5)$>lp}#@VlbuQ8pHSI=L(&`6i7@FBd!b}1H=@C zpo)(lP7HUHAt4q&gyb9`ma4O}5i&fW4G?fZ-SNBbuCJrqdqj4#Zr`pF!G};b!fZqH zfT6qa3z0C9+qa9}HKM4xOF|J5LzHj8m-t=+Y9AtT!y%1d7lK4&B@wlBq<|*HIXISj zZ;Tf0{QeKa!ll$cjI7JGMNQi~Y!*u{HpMhryzK{~ zxMB0=5Xg_hn+LGsQH2E7z82rq(z;eH0vZr-nxK_{Pc;wV%|Vx=F)_IATla_S$(Rj5sgjMReNY4!WB(_a7j7M!YnFB##pO9^yT?In`pP^#Jv^{zR7c*`RLP6 zob`9dxp&bSrm=HZehkg{`M{f}o;u=!VoUnzz~Cz$tc?Y6-((VXU-;e6T~XR=>HmJ7 zsbjf~_VJw1_JMCVTRXk)3#k2djrewATZYcWRpBb>go{7Ci|wV`zR};d{eIg-eW$7x zlh>x`KlEL`P1T9J)Ttxfy?Gk9l&P%@D+O^1kHFqw8<`(4{N&-s~Ce4v~MDZzC=({RS`Cpul|N&B_Cv6EPN;`%C0!#VPfigVHRK^M3!?)+?AK0Mp=r;yXPwF+SzD_TJp4*%Ab_1> z3424nhhgprr0$?A|uA1eq2S{a-S1ihM{Ncazo03zV zcv}i8B{bg5#O2dc(o*I`)v0gs{zIkqM*VN?+x|mLRS$Ve4*r}t&oS)z@jaz&<;Gu}2)gI{5BiWIyc;0Cy>BtmfLrXDL2^Xd=EW%XC!j572nw zZ#B>srR&BDR&{6Mh~?XY18RRcOUp zfIgan;jD@WUQ^omE3sB*oOhR&U1sfEmYM6G60CA^aBB}6afxr(=CR4E(=719nU3lD zzq5(IddHN*YmV3sq-p?mz!W?SlvrfRUd}De@O&q!39|D&kzbatER7c0bVv{x<`&N} zNbd5XMQyZzYXZ~k-mqEPAzY9g{YaH0HWp$gL(mx+7CU#JTkA`>oO_=5?SHH+c*#OawiY5zxj(YWT9zJv zaY-VeE0SzRl!2V$r&=X7=NA^{h?o#blW(TlO|_*H`~4IbcQh-m$V1sfVNHQtpYoZq z(ZAdFCa6CoA-aU{Yq_3`eA-YFuS%5cn?y^z@g2toFm8$H!}0&ucw>?&8Cz+OdlfP* zY>Zuz4Pw&L)8%=^A@BXF%Kr(0rLdC;#?D|}Uut|;Pk8SBl>X=I$t^A6u2#qR7_y4v zjtejX%4_Y)K|xd?0vL27m37xEhf)m*m&L|5%(CUx;@yg|A0NLNZ(32q7Z(~jo;~o$ zavY^SxO*ACcM*XnvJR=$3ZJeNyKnqPudbI}xcBWBNu@pKV;TpQ_D9rh>Isj%!mjW8 zV_s^!n0uX4%}|i|T+hnw19Bb}1G4hsyeIQ`zE4PWaxbtKG#;B+Z0;R<^He10+lNF| zt-UF}fKCD*%#KMlnUxHCOI!E0s!-Y}I5b*&0id>_XI( zJ$shy#=h@MAwmctdyx@B$i9`ONtQ{{Bx&s1Sd)fq-+Ac&UDw-ny@@gNJiq6h``qU~ z_nDufu{eL#{cqokKlRSn*58ZDD!Hz+L9`F&R$lKnIr~-rgw)Mv52l{nI)45-zv|7$ zhu&mGKTibm2_B+WuU9Y9*^2QR?eo-I6E)T6ZeLtEJlxLwJXx4ZB57n%bjVIT!Ghvu zV;g+a@v)h!3?;cn92wZyg$9hw(Wu5J2`1 zTj*QC(2}8NzopclZ|2DK@jX!UC3kms|Fsi#8M!uJJsLyneiSNys>@E*u<7tDDh7P4 zKy?M9xmmBxIVp{mqLkD%H@6~^n}Wi*^~(CAEn;)6+3a6DS^Z@Mhnz={}6lZQoX#tEWpH zoO}?pCpcv0g6!KLrK~T{RHDw-?kWCr@}VjvPVJoJv=~LyEQ1KD9x4qb&;1wQY|7q9 zvgD}S8=7Z$tzi9V>ICWD?BTt8lQi01PGBgnIIb*ONgwDDxopuEYk3MFh3bN&i~ljT zv6oaj9nFNEPd@x*7qkYrbT(GZgJ)mi`Sw0lO1Xb!D?xetl>6lemML1Uy!E9g^+r#A z&K`R^>}&mqr^dOug?cpDfbNW5QUrG9Y3fb!OIM4VqMCW zPc&zQh0aKYdhFzNpgJIcjUa9k07VsgMRr7zPi7Sc^5Zu7nqGgX=6G)R|tB>`Hzz~8Nys-cDEt7(% zx3(1@VH5LAN-(q;U~kpiRcA57AQccUhna#TjJ$9V6A$L0X7KQ=2!8g8g_Z^5ajHGx zEpj{zxYAPWMxEm$X+KXkx8T+YGk8Ga;*Mwcu)x45@E%|GcQ(?c-cAWHhTS&ciCk2s z2!P7l`=RUF7hbN+o98jP@vEDAz$5`A!bwRw^bukqA%38K>J?FwjpM2Hry^{kr&pS? z#j~&0()>MsErL@sT9IcQ?Z2GF;S*yupmJ45UTl;9;b+O8VU);g=mbEv4R-S!-+eB& zIBms$dvjq)v|IiOBWIf$IVCICV~H6UX3A+>ts`QaM{UL#lJwV~&5WaPfn!WcWg~d# zj1S8!(w?Cnl?64~QdQ8c+DS$n|28+&TxMEFX8lJ9__?5MHnE=Jw$mA5+`@_N9a~!g zv9gR}U{LrC24$OoyqM{K2a5mCpPRwsgHo4pGgtE|4kN@OHD!p8r~3@9ZE9+S!0hYV z+J_%}KVt{v7HFDFI9b!)bNf~w)S{aR@(94xoRmsSx6Tl{L$YF(UPptGG779#A zu2(v-fdz#%2#=wz`2`4a&*fOV8p8?1j?e3d%R!hgy>|9~LBT=B$c32EP=hGm7NI}u zWrB$}U)nlE+2Lc(tQQ$EC^oJXIh{%T#HbhtLwnuQ6vIZa0TK53HMTEv`FF1xo3Pv3 zVC=!Ydo1+1;$99Kb(B3aWkCulmpN~;R7Q%AS z3xUlJpFiJ(Nv?C}&Or>xu12f7#DcpNlm%;pTbO>B`U@zB+uzzX%MM+p`s| z<8HJ<;v9Wb7=5;8LqwqAcm=!WwfC+a?0t>Tx-zSHWmhdf(*6jmvHsC={;*!)gR(KX z3lUV4#aDlY#-vGumIXi>o+O&Z+p(4T;dfMj-MlqO8~&pjNK%Iu){jk2U-6LqWZpc# z4|7H#>=F;Vs^iDH^YWSSuh9wagd%H!7#rbx5RqM%;a_2%op zUjYr;X5q`qIr4$t(z(hD8WBGdU~iHoOtK+HvEUrD_oYuVGGKy?z`{(@)w6L!z(x%r zyT3|Hm!1Wio1nTD7Z=xmeKqo(&P8VWz)-jEQhZ%)?aWyXq6bY2vP_6N_$|b8*rp&q zm^QuQOZIwMV^}N$y#nwygcek`X8z(jX*;m#TQ#Jvr4Q#~QQGxC8c)R|Sq=#)h@aHX zADD01aV|wFbrF@(6IrWN2;f4pBNRo?pL4)a7*BEs6d!QFz{aMCF!1$j7>p$S`2)-@ zVz2_Wz)I1WR|y;#B_$(xIkl-Vk%c8tLr?UUO6&p0wAWnd>KbV=BJ39+XdfK0ZD&g1Or9pr};GNf=DLn{O z6O>Tb@7!5$7qye+#DG(APruIre*PkuP5@&%khgx|<)HnT9ad-YfkmqsBcBAr`+bi4g^Yv^K85pDx3A!^Yh&B*jysAw zrd0Jcv!wiF))iebb{~E>bZ0WpU-R6r5zO`~ykhJ(eX2W0 zn9kk1b87v|BVHi8(0v_UsaTcmO{@GxtdCAH4veLoVIYdQEfkgg0p@~M!6l{(hMsq} zCNLN8-m??e*TPs_1iEWC!h?A4G|9kF!CG%`v^0bFxu$P+B~Ui=&~QCB?M24 z+Ow3zGX5mYBR-7)ilHPYPf9Sr*P)w+3^hQV8}A!@Jy-J`n4`gaM;3u0OE(c09mKtN z3+jUX;HnE_40vQgJVMRvXEuB*yV)zd1eRqBh@XeT0p78Z(zLna4U zbPOgNXB8YT?hc-HK`3AQy-oO;v3VmY(<^X0^TcGUzL|LG^)VgohJ=RrY)s@?X(~+g z)Nz{rdjbdE-`LmrUrx=5AxY9-+Qq~}ENujJLFX z-oP~~KzjXDCf5c@4GVZ=`ez)X2vGgt z94-#K($~DU=SeQ)ul&eFa6IQB#5cy2+ zZsq9%1r;5(x#{=c?EE|jf63=Nkrm^v7^Yg(M$k6%7HP}wjrTpr8)Mpsw8Lm3ym?Pp zi`T1jB;I_p^-eZtV@8(K+92z@p~NluAMXe~A0=NtmPTSmk*c>TrO9feac3+ZqCW+TB}Ik zss8)0@Aw4%Yi%d}k1c}w5tT3UWOIJ!vtBo5kM$H19^a1clYDO(dFAhUeR}qGafYt5 z$8bEpj18_KB8GG|3h;Lm`%*)z*7?WZvYzQW#@#+zCUW-apn{IOyIrhh65aak<~9!f z**)|%{>R~m3>)2t`JF){zPkDp=$tN>NyHtlpa8reV7{e8IIM*O8OMgW~EEs*KtFPW|@|YZ*{ro2|cS!0u@8{>ob|FUWB5SO;j0{)I zXxH&qIIO*c1N>Oa@MFiSTQXVKwfnjnvNJNyKx&X;b#r%bH7k8vhR*bvIN2;T2G#bM zYt2gT^x2qE=h62^K{A5)zRJ-#LoQG|YBaj^)VKs5HB;-!&5v8?4p5x1ZBAWB2FieO z{JSZ$8OTPWfLa!6^{$v<*gzW;lr`?!4rX-Ns)5c)w_~)ln0Slj3UaM#UawrQ2*p*WJk4$;s)2jT5O6h}1U&b1L^JQn7(evEXj+DVcS!r0pr3?L$P#c#s)P9IFcAMn% zfTTB@N8DfbZKZC`@`#x4i;4E+6iy5|GltL4@5+a92Bs)+Y$`ZDQiErWlb3HQP9t9n z1Z(Kzj+HT%G@ie!G~`0%-ZzCO>D9b@Wrp9WO>eD|=)8i$Mrbt7#-69g{AA~?{7_+H zxj@`H9ZA^KrDddhKpqa{(4Z?mah(1^pGUF;3_GdxJ{+aOm%5-PPfYTe9BeZ?q^Kdf zKWKR^{oP&XVmV0pA1&=RCp6De`%4ivzZi$;_9sd@Zn2ktfB$MrA!*zp`<+A5JT-RE z+&O>d?icmb1a+S+`|n9G3`5|YAA;+;gSxP8(;5_`-Zoi;Fdm+z{y4J z52wb?E=q_dg)nLTo%)B+qpntZjDb;&ixD%4A~5@M9aHm}W9i!5*9*?%#3M>&A)&6lIDI{m`0pz@%b|=!qy6$NLa$#P z4l{j5KJ`F;4~fEBODIBjOx51Urara053{c2{eb4NSt)3+6n-p0sqQhmaZHe&_jsF@oF$QXl^J*SAHxzZyF!> z;?|S&(72|cpeA~$b3suu?-EWM@>E>Bo${tm<}o1?%M%3$__pCee4WUCPC~I+mPi zr1;yzg@dD~(gAGr9FSr`vxK-dxxJB9URUSG!BHwnyQaGxm=(0#%h9x{l(4<#Wm3O@ z-xMS@wSX7h$N+pdTVq+*k&NaO#`(?$59EhWaO=M4k2g6(u+gRYXvM0ZB&w1Kr=~j< zn>sr;_)If@Z=ueGg9q0;txg=LBSegfF+>Se7#!8sPU*s-ncR-ku?^jq-7~o?9<+QV zs0Heuu1o6{*wkNF;O`qtY}*%edLHgL=;O_|7HPL1BV_hQZ_r;g;J;&kY^?ryJHEZP z^mu#YXZooALn?Q4wCU68ej0{5|VhJ;z6H9}UfGajj`RYPOZP zJt{kV;L!H^_A0;U%t-V-c4})v86TvA!w=3DlNaafnqZOlSqwc~7uaxeB-He3)j%7i zAthR6;<8}FS93Nc>Z8dKW}g&lEGq^&v9 z#2wOKrVQ1|=f)ISQsz=qgU?$*Som?Z=$YmhSDLwp+pAH$mPS5zZzz|aPYiCBU5j=I zq})2=uB|-bZ6Ge!8MNAed;z7pmSGYU7F>K*fPp&LSqDBJ_`JE+JmPHk&cqMaRq3Pc z%h2aLg#SBs?AW)TKXJxRFi{~UBST+j7>m)?=1b&T@!f}sf2#Z*Jd5Hvf4(V8f&ZPn z1hht%Rg`C4myO5^BH3thMlk$f7;N8xPS`MEYnA}o7Q~C5lEL;D3n?e)Z=Pe~(HsBM z0t9_TtgvQQBoL408m|Rb()v#dSd&1$3S10fL%g|`VOD)nH)ur{xmNzxU{>hiu>O`S1Ctjjx({HtlJU;2@MjG&D9 zk|XJS$p`vYx|D}J}Pe$R0jKUsWmhiUUcwU~cK`j?U$ zpJ)66?87&fg8Z3JMr?0gAZUC^JW{g-A$vs=o%FJA2KGU$)UWP|8I9*`518c3!^*=W z2tqP_CO9+KG+Mo-z>c}Ek`|~&bNxh*J&#-6CE8q=u4()(m7LH|inBZk08NPIB2CE6 z+>h2W%ficRkBm#!zf?BeSF1^QYIzb)e$cwpHvfOfoI4K#>m2nwxLb}OTzA&yto0hQ zP35`z5V!2wAz5%GU7h!8isHn8qkZ54B{0^iyd7q&KeBE1uq`En1MlqIT!7gPf2X0P z?$RhW+{VO(%UeB@2D4w?Eih;A2t*{D*GMfjY;IOt9jJ5rY~Q~1A6@jo%n_1jbu-wwHdZ91|GH)AI(_k8u{tvl z>q8#gBinRkm|U{3*x$fst2)bfBzV!A6O*mQhoowzTQ9K9-csKmb*;Zy57S_vSLAR( znGT48lG&j z7Hq6#ZHpsE5Ls{lqrJ$y82Cd)lmti}T^j9U+B|Ycu@W6UurIc0b^Xx7Q7O*5bp^|U zjips+=NU1HK+hP;NxX#P7Oed4kln&>KSj)4 zcKmN)VLhkd?PBQ2%JINlw03ydrY;0Fy|Q6!A{gB>ew&@pJlZqfqlVfdjSQKd4J zy?d%#;b(2bGsz>Xot{ey92$fU|K-F3PP2*dJmyS?hh#D1HVPIvZqb*_C9eK*W3p9+ zY}x);Qo-f^YC%V#@D+nXQL&&E)nsN0U+3UM6IRu=&ruG6bNx6QCr;|-bAWImwLe$q z%pxRkUM%ltAh;b09{Pe$ZIvSgG0`i7T18hxd-EM0_uj>8t8zIz&-T!_7pIazFd zK{oJlM;Q%XKYEEwIj`J%jp*@x?pQGyN~gtu?N^PDZq1!g$Oa$OdY=T+Mi*%e%qc)( zDEo^WsFfYVU_h-%7}{8Yv41Oc_rV1-m~{9)CyJ1_$c7Z&WH!S%f3p&t>4e*jw**=T zneyAu9IzW9gfDulhX&b#{3i*W7xZw!1^$0svI|}{2ct2CSN|@WR61kEEmBPU8r#(z zY1e3FrAK17HqKaJJFa*UF2B{scoODqjoC zLQX|XH6@2n%Y2O6gU&;Qdd{^X21a{Rkub2w}O$7gKXS| z!Fy-1F(flJElV~jG#Cd>F^OF{&6U-pZ&v+qHOfRe&N6~9h)%HTI5>ZxJ&XR2GCzLV zA>m4p=*U8>(bm>Cj;>e!&WLWWbPO;z zh{(|u|JNe)Ve+#3pdQgw1L_C#Kn1>}rxFd;oBd7JBrU^CXE9k-caTrl_CP zBNA}tV^^2UmW6P0<(V?{=5J>42W@7sy#*K%he$J+j`uJMev4l&p2UpqOE3Y>71?x} zwY@P(m`%tp_z3*gd#Ie&!2b0k^yecA3O2xFT?Xj3_va4w)Sq3hGnt`lJsITZ^p5hA zb#a1#e`*`U-u2m%U=_O_w{bGi$PoC7kw11#vbOMCO+WA_|Nxq zHLchk^ZI+lRxz5iHNqbm_LP98ZVova7J(bj*D82vn<=8=j0#`#S7HfMbLZzy{M<3_ zY7}?pxuJmhK-5Y`E!o9MHObA!fVD*kq`WrY`B29W0BaXGLN0YUJy3a3CBe5rwZWL1 zrbWf$x5tcKe!)zLg4uI*vEF;Da^T2N5-?|noap^x+Hr3S>&|bDU!knNBj7hy-&)z@ zUy4UecK8_Er1_6O;G4t7o6mu$CeQ#4fP&9x9F*c=`|VlD{8<2 zQMHaTD}D3xT`CNAKFsDmeN5J-#?UcUw#vW(5N#@sqeC2p=ZZL&%fVjHzQ zk15pu+Gcj&wgRkX;AyoHY4*s7GX5EmUvM+8yrO3B-2u_?@bzmw1+){|5&F;uSA>`5 z`$9fihOT?s-xa+0BC*2~aZe8jKZ^bj^R*uiI+T zt8tkJzs)nOnIlOPOA$?53i?V@?drKvWpy0Q|%84$fa(UQ-UeHaxxKbh%U{b>7VW*|+rSFtj-ntrE(7&|De4$No;L$`?zQ&x3@ZxGHA0Hl%rC2`KrEl2 z_*nQkc+2J022WDQqmYn*W5@o$@6H{Ze}+939&Y&k*-NqnAW7Q_B?d(lE2~gi11aQo z=ryGD%=Z-5?H4(++DOQdtXh(yd_?ugJS0oNeJc6n5dzrrw3B~kyG7^l@Vne^+MVOB zT7256T}(Kv*}##l`Gr6wx=`Ie0^_iPrNde5nL$x$UEQrz$BU?A0AZOM-bB0hn?Y&q zZw!8b!l+|1f20NFv!Q%m)Z$D02*#Yhe2$VR0EL?=y3tQd6i%lAX@c6S5m^$Jg&sTM11L?k{CIj?m&c3~aXQ`ilL{>_QEU}R_! z+6x7|2Dm9fyQ_3=e@@TTkH&*X9-K|Q33G!`1NGQEob(AbUCh_zgZi2Z!aKhau@vDD z*fGKw`b${^XQN5w2|g3WV)Cn|K(<~q$SjI<{LZ8^TYt4xB1W%>)zO1-EEUJ{+E6}= zHH96U3K={in?*kfIZ^67|IuSpU+aB?ATUD!&FKUAIL&;pXM2H-bP!yu?;~xt9MIfh zHA3yqXA!{o>66DkBO(xC*EAkE2s)$E*sC+Ir^SBsFft^ziKxI)O1ybgyDNuBJeb{o z_S{{FxnM;TbzVyw99w{Xs^s6AH}x=aDScWpu=T27=E#F)s~@A@QfDJs1$n^x+4Lz; zbRsI&HHIU#0+kJKDVx^=Z*%UPF=zg<xWG5K0Jbsi`*8y7CI#wvWZ=4I*{oof=w3o?#P9S7F%YHnJBgWk1gW-7L z2TFiH={oT!6l6H8W?>(IEuP3lsfETm^)fUJL%@ut{`p5Oh@;}DCpc_7l{ zzu}AgFs^ec>R`ybilQWGC%GXYMjvML0HU9r|M}MA3^1G6R651u`biGW&rP4|>FnxVHFqkq!XpY@V^z)yHjS zE>`5@KOyi z{vvu#3^e<|u{H5#B2VPhN-f)|b6n{eE7$9$A1ew9X19 zf^&0cJU#zF(}5;DB+7WE;t9bH(vNM01hBeboW%@@2uLZBQ!0P{a6+Gh@TzM3`Yr=# zC^0ZF0I4qN)>Fdd>;kEcve^~-5IPfjI*cu!`UmRXK`6i7&$3?svfyKPH<0m(oEZ2; zkMQQ%JQg*E!mP3S0+82wr|mhQEC2ABw+@_n9Xs{HzQ>j+)9&&m#Pce*L3cGZ_1L)N zRyjiV-D!1EE+Imk&YG6kjXMJA=#D2hEtTE0`7d+`!xptm5(_zQi6QLln6Q^fmTrT3 zT;G1cN@z;f^PJfpnidFbFoCNlkW3T44-T3s3mq`vqh;tjvA^7|Y_9Poem{|Q-T#cf z&I7A*j7*8ce5jJf#`C6a5fx1DlVRt#a8wNidZ*pNzn%OuD$gls-)^X9zM-K7UR8ZF z1)8EA!iL1b&=#J^`8owqfk%vbH-vmPs%qvfZ)hlYJnJty+Lg%!AGh&qrKld8xh}EWR`!a?-33Q@fGwd zz~qXkoQ%Ehof$rwKL4v>(g!X*zzoF%i&}Y>h3eyRaLq=2a*7(yaDqcO_WzMz!rFdJsh378w)TM2-N-GP81Et^PfR z&i8v_N1;(mU+Ag*B+6I9JMr=}O$j3roDoE~H%G>nXUx0KH9G1He|o0Q1KL~=4T7f+ zn#ntJ{kdONUL1}W4n?`2_pcvrHK>I4vF9P#yQ#RQN91UW5V6kh6eb?P3{zBF&Cip+ zrIO!0Q$yh)ej+&IXz<&hnPxK>v{2WZE7sxDb`{@Ga61_t_N{XBTb)jpl}r}oREUG{ z>DtUrYHMF1Y+)tBy2o8@QdLX!Aaa4VRm>12BQ}U||pLX!F zgh3AgE-Hf}oC;j8K+I@*e{o-)c)z{~%|2~yScVd6`oUeohtHEy`*Q3Km7bc!O``zu zG(YMlYa1|UQmhV<=E~*wCVkQ}1`j?^+|6B=I67eHJ)U3(S*OczeAwCfIXty*^GFN( zo?($nU^6ht=uE)&%yc0%fgI+kL{F)J!5K85<~#}Yu|Hc3&^mpeA?yKpTVViR_DVU& zG4X~SH{Fr_3^0u;52RB~C#>#L6F~)jahEWa$WSlR$tRxHOKS6%F&9lM%4!Ms^4g8Y zs)|W!@|XzEy6EE5)x`=`ex3d@byMSe=oC4C@CI(avdYVQI#)X_DJSP#@Y^OPIgKPY z__A#UU$oK*MGZiG0Cz{k{wNC4{)cyQMjce?1p;=sjoAc%S??Tc0&cp(CIxT|oEpzM zc*h5ml=E(LB`oFkzcXIZOoxbMQ&shncp8!{Br|Mk9IkMZefRA0RTw0~+Pw(Cwxv%Y zvxYhd3i#?%5^=on-o8~fV6RG@I_ff5e&Lk&X8^QGmVkANbWQ`$tIGWdpL5G^TbM=x zRs^UBRHa_u04DZ0l}vc}LES&p2JxIb@cy%XC74tK_=e~AMUtvoyhDir>&dWoK#JfP zeE`_Y&CP(jyt{8lVy!-GVf{ygALZklRPhvx((>|-WkSYwYP)3fCy%>R|N$<Vx?*GOdW-}NB;66RsL1(6K;i$EJ0tQq!Qxk)LvjX3eIS@-(8 zBJ_C5a5(8c;ebChhn89TZ4Ll(lP!IvZ&}J5B zS{KQd?si7;IFzHSjcRnO%w?I}a=UMM^`7?V!XXTO4jJ)>grUe$u}-UAZdal*(F5G` zaM-gLVGRZu#^J?~W+6@*1w*3#70W9xwF)3%8xq0CPAYD6rH`dW7&^0BmaiIB>-&d>RKgOPo zF$bMQWf7nUn+l1ixOUe=a&&Z5TQv-1pc?Yu9L|g99IY*!9D?3yzJUz?zlP`@@CBC0 zV8;}iJ&=D6j+U#9v1AGBvO;bQJs?k>SIwpZc>r9YkRo~%_Ze07>M@o+!t_4!H*P6I zLq7$G{xyG3t{}BCbPl;tBMhex3gjIuK3dzISg7~^zvu+`LcAZ<&UGu&$?Lb*Msn6Oj8S=!Lm$&xi8PS zcY%eRH9JKE3b2fH$Jz&crtv#aC4k%C$6wdg{}GbeW(Q+S=$Pr zr+aLEmmapk#J7)Iz}@b%1>dM8spOWHkPDaIFP2Fjnr~$LH0=)MFWGKyj!Mh#6NQs; zuonw3HPj{0&F4+aGZFcBml<@tzTUOA4!={CX(}WHc$l;i2$N%dw%`d}-v0>3#fR3D z{botp6?&cUd;)6m^rArgHWiu*n)QJy0md)vMa{)f4=0!Y+eJ6cyy7ukC4noqL{!Y- zd56Ac6u=wA1ijN$lup}k(;NQV`x*JIJ&>S6~ zefh#0riqia0s{k*z#jakl*5t9J>Qf8v)#&dgIrmJ(RG;+xori)s0HgEXoQ-wfRy3d zo}PWXD1th6%A(Tsx%z*c{-l;=DSr}o3@NL5WL*Zhrh?QwJ@n^k`=Y%3&esm>BaQd$ zVD8aWpV8fjI|k~iLvlkwJWtjefp7>ly#X6Z!~J~D@aT3`KcM`JRkYI~M_->m)GP$_ zu0P4|?(&58jira#k8LYJ3JIm-RjRg2;D!$6gl0a~jq2hScZs#uoUw#6?-P#uulQz& z6bZ4$>Y=X@KA!k@tMbHBitJgq3JUCF1k`YTQZytArgxa@=@;9}V5v`*17D_m=dO#C8@eq3eW}u8OWcHOp}XLPYFI|q>cZ(T49qKL5g(2w z^nd(g-lY?h{g5Y;y5+G!QDsVhdI#@nBj_e37ZVNH;yL38c4^K}33i;APMbHWr5l(N zI;>-y=!skzr;ty%!H=EO$B-pFvn>la+YRa zHV>2OanQ5x`1bO(nMFV+i{>vp(L?rLt3ZjBK1Jy4OZk8zAVeEIVUX1})z~dY^0_dd zxgKg2u5?Mi1G@;+D3Rd+ z1Y?iQjS`|Hh0tIqATA*>26Z45weV!ZINY%Fy-~3j*uPr;Zb>MFcNn?=xd?1I1uD6+ z;cn2jKrz|)Ln=rP+shv56t;c+ddwNRO2H$&^h~-0MVxU-kE7RP&XgTSvHI}}ijB?9 z-(R`B-^^*@094S9RUO_^wSVBY?-H06`tZK^;V}Y&8cfS>^>CK}x`6)%=NCZ$6>rIl z$6tShAB{5pexhgE9>Q4rII^-QtmR#^^-nq%m70DgEU|j^|JmAhX|M&hf+>!F1MH-&q!}OCN z2LSs7!49a6InT5uZ-gG18qYX{iHG>Blyg7(B{Z1=o367Q;KaC2XWmM(w9tn**yb^) za&BJN7z;rG*pOr`BmFo3(*ihl#TIJuMeVW@CeW<9kjPh10Pm65mDCdM7#t7>QXQ2O z@fItE&;?25f+aZbhRo)tG9DNTgj?!s6<&tI2VV-5qiMCTK(e%rr9NDZR+nq3T=oug zU}q7>39js3`JS?D(1Zcu4hR|)HU3eMFwh1vrX)*X${QDed0VVM7)z+@{4CN|$xL-W zwwMfT1-dZUVE|xa*{p3Sy>^$mKFkIm4tqYjUKkixm?WDebAG(6y^LnS^G0s~gJP~h z5pqOFYmc=IFDkgz3rZ?#K+W^T)}z^3yPba*5IcaqH&B0i$n5Z;mU~cAE%-6(l=|!F z)ae#xh+^V}X!f-TO}SSBYm+%?JaY!!A_Pbu0&hI0l^AMd{bwQ@rrOXdZ2?4AkhWWd zzV-LNqXbMW_vi9F^rchOfawzQ;Gd1ZHLu>&NA6x*AMOM(z*uYbHyJV>5$+7FZz&2c zC>PDQ7R_rFM>-q8!wRHk@ATN`xetto6(NO!vP)3|Qs}h%-)p|=GFad+E1BWk08iLv z2DOv35!grz_H}1xzl0x#s8CzHk3OOOY0uPVq{BbL5fje|9F01Ura-br--ZNA6@o^x zFJ%G^=my>{j+lc@eOVB^ur3zkHthCn*(Vy9PR)EM^Po)hmTIol^^z~SG+<1JKc0%a zjb_4%-r1=i-!0ci8MA^M4{Q!aGDK8fBmJ*K!L&`!|8a1dGGcg&g}-t9tNB@@3ctJ7 zrhV_Nfhc`GI1h&cOw18X+pn$Fx4~7 z3M6e^BD=td$|KzLR4uegt`jchWl zPgV1X1uGv$Q@gCt$?7FUppaqG%j_a|WVRcvb>U(T;*_0;1{w&P`YN`~dUC75B_(6_ z^T{wd2rG7WM~EKKu7J38tr=R>P#A!IM>GG?WDV_QPtDyhqxErI`$9syg?wse01Iq7 z0bHd5qbo{~Yg7{K23uEMivG$Lr%G21I6&%v`vZ@Ifc9WKClCN|P4l&g1TIUVsd7DZ zv2x!^!qB?R`Z@&~udr0G%2E0j>N-$$l2y;3wg)ExOMOVFdJqWUgV9!u3hW+eKK&%K zRIW1gr3}>@y}y+U?9XGkk`qAV0eBHKM=q#dvWKjDjq$43fbsq}I)%|#CYvhBG#zp4 zib7CSg_YWJIp{Nf78!?#jiBR_AR|SM8&!TZuRH^)s=Od(T+>ia%~aXvf{BNB23ZvR+o8fg&d&oz8Tjqn&_(;M7utqz3>?rM&6EHL>Y1r)ZPxERjG(fO zE+#`GFo6&-zYL&f{%3<5*Ewqb4!Gc_ReHN;Q)Kx^#H^qKg)33*v&>Xd{|h#CriX-V zUgEXy5rGAhjxgz^w$k~J3~+!lds6jUVIQ0>a9g^5Jv?^B00OaRokFP|ppw9qLNCBJ z`Au%C)zDVy&KS^w4ZMHhWt`pJH@@wcS@8`~%?l}*0L}~g?1BP=Kgfj^Ams(&BJelR zoj+^J@`=2{YIj|y*WI?_6aWAb6$H`&Az&`c_KzVMUWYP$9Y7;2nuju6{10UTdQUQ! zhsIsSq4emog6S*>lrZo_2{rZRwO%#8?1H*<>L{Er(1tI;&M_vu%amsV0syd9#fP{z!*c{l`AK%{qCorA)}Z6GELY)dMX(ZJ737=IZM6w_%VVy zk}Z^~2B2dGBqi|r?<*acqSp4U^CjK1goKoegU<&E6N*aIb+dy@G`ZN1&SJop+~#A% zil#xRhDrwF*ZrXGGCnA6859TKC`4vYDL2>sdp94;`kt1xv;;tFucPeS;9$-GXo?ne zT~a3srsRl6L61FjH;d=6pknzj8BMdVS%^Ibxs=gL31+3xxBK?!LxN?BW`4b6Xc3SD zA#z#JoI4Be9G($KI1CS}i^H+0vkh1_rN$q$EsQEsSYUu_3Cuu4kSjkyA(urQLkxczCqWYMBX ze#n-!3f9s%%z-)=$TafG>$$$w_T^e;zTIaZRy7DCjIcmNHZZOC`$~WNh>)0AqfJ?! zT7gproHzF4`1V?&v2keYt)Um#HJI~I>xqCW?bq5y%Ajoh|4uJd(2!P)OjJhH85l_v z78)JKbkJJ*HpCPZ&>YwdFMiF~-EG_Y9M~3U&jquCczytMU~p{)=Rvm|A8I4#zGtUf z4@;V$gt*tbv;;C4?_SOUA(~%n+HyVZ0X^ma8gMXefL-k4_|9_yJ1PvE975c+PI=~{r#`J5$kxKM2Bx_rIGC!X`S zuMrS(eYWKl6=1{+vCMveNp30qrETCPYkdvZZ0kir5QBZhsj~uJvfHL&Cjv5uGR-rDE|xJP+3Aq4(YeQ?Dy3r{rcZ!)9TtIc?EoejjJaC z!b67+j{0$VRSZ!V-v;MGL}l6&fGjT7zsb4uX}Mk}Km#Zc;w-1RS|3{fUWL3J&k0cv z=vELME5A>xRF54QI|(EuqzD*flYn~(BWsbR{z&(`O|NSq;2^qxC zgBmIb0l~w=_ouQ}pF)cbqz>>nHj(^h^}n-;{ErfW7y)nS-LooeGryD+CLY@JFa!eJ zEewJ5*m%!KK4@HUaehsJya8_#!Y1sSWWB(%3(V3%&PZvvv{DVD8X)@KX|H*TgYLjX zjrnR8*G?9R1_K|i6ON=nO6LNdYToUuaK~MiCoa((kUkLdE+sIAtPCUTRbH?c8CsK& z>IiQVRanA)G;XrOmTb@_gRJWvw4ny117F|<{@wF3Ljl4Y>6@uskT6hqoI#Px=zA3a zHX^5_e;xnP+bVJd8IEzgjB^k}AwrRrpTncDv8~^}Hsz~k$rVxU8Bl?w1T~W7H9#_P zxWeF!q6X+Ctb|Wyti5>xY6*lZZidPYwujq``pnlbF9yJP2t{4;_wGALR!zl0FpsCX z;#CPeBrMm=A_(~UB!jC@woYLV_LK|C#os@4vIdfWQichmYbx#q_9p5&IvMFOz8N_4 zpKn9)RN?9#(B}x;0cii4Pt*mKCiLh={IbKkyXN!{^2(Gur*iq#!am3)hQzg2$G!i4 zwGQe!w3O`q?qiu=zY>n8D(~+-!*Cw+UF$RiPhc-tV?{3|Wm#OrBw3=Fj*N)G=nQ;D z80myT9U)ZaR?MKbE->&S22-^$WC=2B_;5{lOYGqmXgpq*DFF%#D{>01N>|MC=D&`s zEw>~G@ND^7P$)wQg~I#1Wa1!3fKT9IMF^p(xHwDR#OBo3DJQ1l@mkVLf0ZBD$R5TZzZ$stjF-L z5uMEgN_Gy6aFV6Ngq$oto&Z&UVIL1etsYxg9GUIs-DO4EuO zfQp-4X5DyMC61V~2qO)OguO2$*h>usO6PVtspdWyponTi;a#AlN{e#=_9&`tP_kns zMwcD}1V7(AeEmE|zRFLzgc~XTZA5m#WN|S(I`CEaudol-XD}F)rkYn>?3Np-2UJH1 zpBjH**`e_A{#k?BTJabVChXvYk@wMepyLnlK9SEpjuWPoz^$jF4DK|U>5>-|HgCsk z*#3&itp4w|8L-g&eH*V+r~uY?qy+%(0-G_}=#Dh54#1*2LVhcW3_848rOL+y$gT~> z$q<>KBnUJ8ME1DSl_gl7F!Kv8%8r7dr-!4mp?WGeCXshx=XehRT zyf~=IfV4F7%J;v8ybM|G=G;A*7$F+iZYGN8TRlL}27vXrVzQTRsd33plb;^$Fx1i6 z$?nDfD)z)a&O^yJb8qJLn%dcg4?ACNBGpahIzV9riA^a7Ba>w<#ai0bL~uy503ISk4g!NqzEFDifwS8L4pU8*Kxh^2T_iJ?-pO*#N^?fa279gN!WHhbOwMA#u{86g)F^7{7VA4+75gZ4dx~h9+By3@7ZL={=$c(XF5)NKA@j_kBC${*%PVzW~uLz`J%E z5eslFfW^^$o!BVwrB1a~ie(k^Q=?FN1TYC57vBbuV7LSW4PXu}288scc zoS~Y$8F)RFYs^PU!rdN5_5)lhy$3TS%u|^%Cwm;Lcq0J|d|2M- zm(KJ#OivFRYU{-@W%AnEQciHt@_N*%gNF(i4i*&Lgn=L-*c628w~yz%pi{``liARo z^D*3+C|O#-ck1YVl}mtmKu(gB`-(^|DgKR34Ff=oQ!MP0=xhlrD$i{S3%v&We4R8I z9z3y(a2SAw!CfIvf?H(vYBnF>_+}ShphUz93O!5~(-0myuXpAu^2HM;j^i8gG9I}b%Qt}87hQ5dIHR7re&x4nX3F^ zyIrZ+ySv~nY}!-gb@YHOFisynQc#;HHNek*c>VyY?CIPNN!^IVAKkt25{PpeC- zW0iRmiEZyD{AA@E9hFRSGBf=#*@T^IGI9=lp92EArl)zqOO51YeK@O!yuv~ghKxf( z@XjQyR#(~VC?}Tc_W(cy=`ct}3bjE)gkL@|>nSPYzshXN^5I;zAUksULRX`+Klp5K zNoP(jfw#vp3x_un3)+c%>_j=FlZ_v}kFu=FUz#6`U_Tyou z@wh`x8^5-{El|=k8==2jq!oLgonkap{->4^p(!V1$aVr3Ljt%AS%E&q^OqU zp6_9ff6N0rz5lCp81>l)IhyBdebDw1?F#WX&Em4Mpdf^XAD}pnjxvQ3#U+s)&3vs< zbQwB@U=0&HR^_lc^OscK;59cLK7fnp*&%(%O!;1@Jikl=2Aix8>poICXGl~a9!=_~5itwg+cHc|SmudbnLDzaUiRZPi6rv1n4DTr z@XfKR)9N-%S5(e9`~Hm)i^0TSn@N4A36yMh3K%$Lea~S4;Ai|R*0tO9($Qg& z#bkJ3Ec<6Ee*QsnD7=YC`~q&I5Sk}IjrZ7e`ZjQ>ocJZRJ=PRt_b&(nZ9Z5Galj@O zO4=uuJ}+4VF`riTl5%G~?-&wWTzvJC_J48R*tnY7HDO9}<&tjVVU8g93^gL+eN4Iei`zi^(ggYyPVG zN!7(sWZ##z2~q?Lu0?6!!%+Blm1ltb$EMON4(o=ukO1bi$(z=BV5=kn;Q;DD6J4nS zZtdJp-TZ$0&yHTeNCWB;(F(rX?!UEoDXD7UFv)}-5B2qNQqZ<^$2dKS!W(I5P~yKw zTN_*P{rCEn@=~w5Gr*b&(Xeh<1Ja4-gcxVx+Ui9LxXz#mVwMzK$j+57QO@ak)^E?M zR3_JDR(iL_Q0gz?r(>0&iOC+|yg;;}oJSnimv>gT8`}dCkOyi+8BMU0vPu^z>igl%kn* zo|&2XmCjN^@pKK;2I-lZ!l$BvGb=BrRrBS3UBK7fEd%2FN(VPr*XF*y@wECpo}Kp} zmCJLu!8ogZj9Od!(B3RsHvf;V%l0u|CB*_e@2#OfM+-z0L}r0ZKXV z*49ooHf{8>CX#L%5vlI0u^fP3`3rVz-)0a`PEK|zV8iWPd_mRn{#F-Xn(%X{qTkQu z#62ze?(ZR)hSwxshOn2E`+Hpggd8Zgtk-&-P3_X{lQ8`SA>PG^_9a9FP{?}T*)&y{ z%~K~{_Aqku@o9%a$K?a?{pt*B4wqI&@{?Dmc8|XXP0JV*zU~r6@fK};ec&TpIv`E* zsrN7{A9RGdHpQBgyjN0`|NE(>E~1jK!54ae9HE30Bws9 zWRXzQ)h``>TDw`Ay0#e7Tq>^IxWsAT$&*(^!MyD9fjLJLGLube;L{EIusztnzOfhU z1_d@7qfA*C6yLelNOd*`@eQ)%nP?o^@wZaicy8 zGpCw`yuad0vh2j_4>frN^o9!&$`wvOOD-SW)T{eW_i6oZhEJH+x6evrI*zjJ2h%$%@mlIGlXboP5%f%T=(i8zewfhB*I zeV`=sJ$KR;G0E2huf@GYu=1F22sI8n7$P@PsacKZ!x?J^*)_Uz_gGJ%4#s?odd4I} zEWoj3G?;zEuiL_q;fb_t*jhj80@U01ijw$IklIFRk3kq%^vpVA246bR_Nd=k!CuFg z>c=u*gj{fjJt8V}WNLpXKXrhPYX?FKjT>D}_k0b(x`d{XE3_IB_r?Dm1;uoiqu4xp zPhCp%iW@QlF-JW*-=E(n@i;aWr`^$hRj%;IO7yq6uHIdLO(|59a#^br*$^HnC~;(i zN<~uMLBDWUbKM4zhFFHV(b>MJ$Z?0yZ)`4&=G-DM;hbPv?4H65r*!z;{p@)&p!lNB zdbBoHEQTeyJR_e*6F&|AnQWt}+g>SO-Wn}zxAQ9F3>U1w23SHR!41jGDGGgmgw|7~ z=xUiG6$~@;2JkrM+GwY8R|Mrxcg&);o9AwX%NM{k7c9B=vorif8^#{x!Rzlw>SOy$ z$u!zKP2ZUS(jp2r6QYbYb}ytwxa9l7MCe;B9V8#KSwh%O&^8ke_CvKFkotZOv? zUm>=x=9s(p#5Xo}q~7a!LsP}o@@h0Sf(85K3sBSDxp~&p8Hb2#xZPiAb*7L3HhpYQvzs4+lA6(+VbzLz=ItwaBDz+Kp zec37gq)%%YjoTMBttsL9_;$TR<#Z8vSJl>z_Y*lgxH&s)j&~0z@^$}%_W)(fVDq;Z zRu|J6VmK5k$u~KC;?aKkgQrht>)I)5Z&YmTC-0U1C}D=_`yNEK;7?$1_1T(9Q#xOX zWjkrC00jBptLn8@$8Mt?sM}NzBRRk-n7deQNZebV;Yp}`b**rAp{)@XOSoHLy^B{F zgcX1ObQ=*Q=P1jEtA)1nfOLVhfs2Se+N~CF^T~{-Y&TKOK?k`!U%m0S-FrwB9~$j& ze}{6jXd3LWuoxH?f(Ie42Cs&PmDH(_P*-#Hc&~OVjkc>ehakd~$Qx)rx0pLXsUS@C z$ZEvm+d-7d^JNaml{IwFM8A8({!-iM<#X#MV7bQ*_* zb*myQcQRy{$>SKFDZUaD@4B>_FR@1jYYxMW4_gXPKV{kf+p(2iTCX;C)s+&XW8aoHC5 z=RuTa*ye>td9|2cI{yiAO!O>@!^t=ls0UH)2~jH8{&H){mz{cg`ZBLA8-JL#am{av zM9jt2`>m!gK)c}kX@C>yc!KG4&SLTX!Gc4ZrmY3ap%jOa0^6vJr3Y(DScT=Mo1-(` zQQ2CJ&DXq7Pks~Rj_ekdstw}N5$x)<7E!WQ)ODRLC6M-oD9#1 zo|S{_|dl zG4rlHSokFP^>||;EFd%*EcMkalqTc%X({sAr7uSIk|!d@DqE84&#X^07|A^nmbvj&oGvx>k8I$tV9`~QeMkmlG-A?tJ;pEakq$!i2?QB zQ5cY(k;Tjb4ipnveJ46r+IX3Yz8_hHiia+)pt0Iw^JHN-0_-ow_|VBU+De`BVxRcxd1{PE0Zsu&EyS1at2js z36^xL)u%gFPSGBg61gu}O}MqK(H^Y`G@|aqN81^w12GF7pNF^xJCXfJ`T+sil{Zv!+93zD7)B%4|Lt zfi2YIqj1XqI$)HV+YvhUSjvI#)ux(-8K{Dt{(>cqs=}QA3DHx$>Gp+|;KpFd5-E$j zYV=wE)k6q~swQ6csKEAeh5O_SDk6Yy;rn*ycCtogHBpsvjNzcD2xC!HcNMpUmM?9U zw2EB1e$(~n-A{-aU39 zRWUV;VGe>JrX;JAB2neWT3?3Dx_P*G#`FBDnrQQ;d1AtaY}n?i$xJKye3a~fxy|oS zLyiy8kvhRQY=}Lu_h?qDs@VG)DFP^nZS$ok|>C074YJJyo{M1+VZtCI3 Ni{i6IylL;T{{zK3R#E@} literal 0 HcmV?d00001 diff --git a/common/bag_time_manager_rviz_plugin/package.xml b/common/bag_time_manager_rviz_plugin/package.xml new file mode 100644 index 0000000000000..b12c24b728cde --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + bag_time_manager_rviz_plugin + 0.0.1 + Rviz plugin to publish and control the ros bag + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rosbag2_interfaces + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml b/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..2a3bbf667b715 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + Panel that publishes a service to modify its speed. + + + diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp new file mode 100644 index 0000000000000..dde3f10e9b554 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -0,0 +1,117 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "bag_time_manager_panel.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins +{ +BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // pause / resume + { + pause_button_ = new QPushButton("Pause"); + pause_button_->setToolTip("Pause/Resume ROS time."); + pause_button_->setStyleSheet("background-color: #00FF00;"); + pause_button_->setCheckable(true); + } + + // apply + { + apply_rate_button_ = new QPushButton("ApplyRate"); + apply_rate_button_->setToolTip("control ROS time rate."); + } + + // combo + { + rate_label_ = new QLabel("Rate:"); + rate_label_->setAlignment(Qt::AlignCenter); + rate_combo_ = new QComboBox(); + rate_combo_->addItems({"0.01", "0.1", "0.5", "1.0", "2.0", "5.0", "10.0"}); + rate_combo_->setCurrentText(QString("1.0")); + time_label_ = new QLabel("X real time "); + rate_label_->setAlignment(Qt::AlignCenter); + } + + auto * layout = new QHBoxLayout(); + layout->addWidget(pause_button_); + layout->addWidget(apply_rate_button_); + layout->addWidget(rate_label_); + layout->addWidget(rate_combo_); + layout->addWidget(time_label_); + setLayout(layout); + + connect(pause_button_, SIGNAL(clicked()), this, SLOT(onPauseClicked())); + connect(apply_rate_button_, SIGNAL(clicked()), this, SLOT(onApplyRateClicked())); + connect(rate_combo_, SIGNAL(currentIndexChanged(int)), this, SLOT(onRateChanged())); +} + +void BagTimeManagerPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + client_pause_ = + raw_node_->create_client("/rosbag2_player/pause", rmw_qos_profile_services_default); + client_resume_ = + raw_node_->create_client("/rosbag2_player/resume", rmw_qos_profile_services_default); + client_set_rate_ = + raw_node_->create_client("/rosbag2_player/set_rate", rmw_qos_profile_services_default); +} + +void BagTimeManagerPanel::onPauseClicked() +{ + if (current_state_ == STATE::PAUSE) { + // do resume + current_state_ = STATE::RESUME; + pause_button_->setText(QString::fromStdString("Resume")); + // green + pause_button_->setStyleSheet("background-color: #00FF00;"); + auto req = std::make_shared(); + client_resume_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } else { + // do pause + current_state_ = STATE::PAUSE; + pause_button_->setText(QString::fromStdString("Pause")); + // red + pause_button_->setStyleSheet("background-color: #FF0000;"); + auto req = std::make_shared(); + client_pause_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } +} + +void BagTimeManagerPanel::onApplyRateClicked() +{ + auto request = std::make_shared(); + request->rate = std::stod(rate_combo_->currentText().toStdString()); + client_set_rate_->async_send_request( + request, [this, request](rclcpp::Client::SharedFuture result) { + const auto & response = result.get(); + if (response->success) { + RCLCPP_INFO(raw_node_->get_logger(), "set ros bag rate %f x real time", request->rate); + } else { + RCLCPP_WARN(raw_node_->get_logger(), "service failed"); + } + }); +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel) diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp new file mode 100644 index 0000000000000..2cc4b98a6dabf --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp @@ -0,0 +1,72 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef BAG_TIME_MANAGER_PANEL_HPP_ +#define BAG_TIME_MANAGER_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rviz_plugins +{ +using rosbag2_interfaces::srv::Pause; +using rosbag2_interfaces::srv::Resume; +using rosbag2_interfaces::srv::SetRate; +class BagTimeManagerPanel : public rviz_common::Panel +{ + Q_OBJECT +public: + explicit BagTimeManagerPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected Q_SLOTS: + /// @brief callback for when the publishing rate is changed + void onRateChanged() {} + /// @brief callback for when the step button is clicked + void onPauseClicked(); + void onApplyRateClicked(); + +protected: + // ROS + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Client::SharedPtr client_pause_; + rclcpp::Client::SharedPtr client_resume_; + rclcpp::Client::SharedPtr client_set_rate_; + + // GUI + QPushButton * pause_button_; + QPushButton * apply_rate_button_; + QLabel * rate_label_; + QLabel * time_label_; + QComboBox * rate_combo_; + +private: + enum STATE { PAUSE, RESUME }; + STATE current_state_{RESUME}; +}; + +} // namespace rviz_plugins + +#endif // BAG_TIME_MANAGER_PANEL_HPP_ From c10879c0a60df8ea39d95e0a9dfefd9f873335cf Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Mon, 5 Sep 2022 15:39:47 +0900 Subject: [PATCH 12/28] chore: update NOTICE (#1138) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * chore: update NOTICE * Update NOTICE Co-authored-by: M. Fatih Cırıt * Update NOTICE Co-authored-by: M. Fatih Cırıt --- NOTICE | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/NOTICE b/NOTICE index 981e257f9bd21..0ac1d22befa35 100644 --- a/NOTICE +++ b/NOTICE @@ -6,3 +6,9 @@ The Autoware Foundation (https://www.autoware.org/). This product includes code developed by TIER IV. Copyright 2017 TIER IV, Inc. + +This product includes code developed by AutoCore. +Copyright 2022 AutoCore Technology (Nanjing) Co., Ltd. + +This product includes code developed by Leo Drive. +Copyright 2022 Leo Drive Teknoloji A.Ş. From 73bd23368e50bfce248f69595e992ef083a34b5b Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Mon, 5 Sep 2022 15:46:32 +0900 Subject: [PATCH 13/28] refactor(ekf_localizer): isolate state transition functions (#1758) * Isolate state transition functions from the EKF class and add tests --- localization/ekf_localizer/CMakeLists.txt | 34 ++++++- .../include/ekf_localizer/ekf_localizer.hpp | 16 ---- .../include/ekf_localizer/matrix_types.hpp | 23 +++++ .../include/ekf_localizer/state_index.hpp | 27 ++++++ .../ekf_localizer/state_transition.hpp | 26 +++++ .../ekf_localizer/src/ekf_localizer.cpp | 45 ++------- .../ekf_localizer/src/state_transition.cpp | 75 +++++++++++++++ .../test/test_state_transition.cpp | 95 +++++++++++++++++++ 8 files changed, 284 insertions(+), 57 deletions(-) create mode 100644 localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp create mode 100644 localization/ekf_localizer/include/ekf_localizer/state_index.hpp create mode 100644 localization/ekf_localizer/include/ekf_localizer/state_transition.hpp create mode 100644 localization/ekf_localizer/src/state_transition.cpp create mode 100644 localization/ekf_localizer/test/test_state_transition.cpp diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 79d0abc7b7c9b..0b1fc2517cbee 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -11,11 +11,39 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_executable(ekf_localizer - src/ekf_localizer_node.cpp +ament_auto_find_build_dependencies() + +ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_localizer.cpp + src/state_transition.cpp ) -ament_target_dependencies(ekf_localizer kalman_filter) + +target_link_libraries(ekf_localizer_lib Eigen3::Eigen) + +ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) +target_link_libraries(ekf_localizer ekf_localizer_lib) +target_include_directories(ekf_localizer PUBLIC include) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" ekf_localizer_lib) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + set(TEST_FILES + test/test_state_transition.cpp) + + foreach(filepath ${TEST_FILES}) + add_testcase(${filepath}) + endforeach() +endif() + # if(BUILD_TESTING) # find_package(ament_cmake_ros REQUIRED) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index c9a7a0dc6014c..0c3fa0ae8c3f1 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -198,15 +198,6 @@ class EKFLocalizer : public rclcpp::Node bool is_initialized_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - YAWB = 3, - VX = 4, - WZ = 5, - }; - /* for model prediction */ std::queue current_twist_info_queue_; //!< @brief current measured pose std::queue current_pose_info_queue_; //!< @brief current measured pose @@ -291,13 +282,6 @@ class EKFLocalizer : public rclcpp::Node std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); - /** - * @brief normalize yaw angle - * @param yaw yaw angle - * @return normalized yaw - */ - double normalizeYaw(const double & yaw) const; - /** * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_ */ diff --git a/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp b/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp new file mode 100644 index 0000000000000..732abd6c8e2e7 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp @@ -0,0 +1,23 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__MATRIX_TYPES_HPP_ +#define EKF_LOCALIZER__MATRIX_TYPES_HPP_ + +#include + +using Vector6d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; + +#endif // EKF_LOCALIZER__MATRIX_TYPES_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/state_index.hpp b/localization/ekf_localizer/include/ekf_localizer/state_index.hpp new file mode 100644 index 0000000000000..b2bfdb0bd9e2e --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/state_index.hpp @@ -0,0 +1,27 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__STATE_INDEX_HPP_ +#define EKF_LOCALIZER__STATE_INDEX_HPP_ + +enum IDX { + X = 0, + Y = 1, + YAW = 2, + YAWB = 3, + VX = 4, + WZ = 5, +}; + +#endif // EKF_LOCALIZER__STATE_INDEX_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp new file mode 100644 index 0000000000000..85a65828e4ee4 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp @@ -0,0 +1,26 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__STATE_TRANSITION_HPP_ +#define EKF_LOCALIZER__STATE_TRANSITION_HPP_ + +#include "ekf_localizer/matrix_types.hpp" + +double normalizeYaw(const double & yaw); +Vector6d predictNextState(const Vector6d & X_curr, const double dt); +Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); +Matrix6d processNoiseCovariance( + const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); + +#endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 41ae99015f871..00f6b760efaef 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -14,6 +14,10 @@ #include "ekf_localizer/ekf_localizer.hpp" +#include "ekf_localizer/matrix_types.hpp" +#include "ekf_localizer/state_index.hpp" +#include "ekf_localizer/state_transition.hpp" + #include #include @@ -406,47 +410,17 @@ void EKFLocalizer::predictKinematicsModel() */ Eigen::MatrixXd X_curr(dim_x_, 1); // current state - Eigen::MatrixXd X_next(dim_x_, 1); // predicted state ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); Eigen::MatrixXd P_curr; ekf_.getLatestP(P_curr); - const double yaw = X_curr(IDX::YAW); - const double yaw_bias = X_curr(IDX::YAWB); - const double vx = X_curr(IDX::VX); - const double wz = X_curr(IDX::WZ); const double dt = ekf_dt_; - /* Update for latest state */ - X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz)*dt; // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - - X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); - - /* Set A matrix for latest state */ - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - - Q(IDX::X, IDX::X) = 0.0; - Q(IDX::Y, IDX::Y) = 0.0; - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_; // for yaw - Q(IDX::YAWB, IDX::YAWB) = 0.0; - Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // for wz + const Vector6d X_next = predictNextState(X_curr, dt); + const Matrix6d A = createStateTransitionMatrix(X_curr, dt); + const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d_, proc_cov_vx_d_, proc_cov_wz_d_); ekf_.predictWithDelay(X_next, A, Q); @@ -760,11 +734,6 @@ void EKFLocalizer::publishEstimateResult() pub_debug_->publish(msg); } -double EKFLocalizer::normalizeYaw(const double & yaw) const -{ - return std::atan2(std::sin(yaw), std::cos(yaw)); -} - void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp new file mode 100644 index 0000000000000..0f8bb091c3662 --- /dev/null +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -0,0 +1,75 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/matrix_types.hpp" +#include "ekf_localizer/state_index.hpp" + +#include + +double normalizeYaw(const double & yaw) +{ + // FIXME(IshitaTakeshi) I think the computation here can be simplified + // FIXME(IshitaTakeshi) Rename the function. This is not normalization + return std::atan2(std::sin(yaw), std::cos(yaw)); +} + +Vector6d predictNextState(const Vector6d & X_curr, const double dt) +{ + const double x = X_curr(IDX::X); + const double y = X_curr(IDX::Y); + const double yaw = X_curr(IDX::YAW); + const double yaw_bias = X_curr(IDX::YAWB); + const double vx = X_curr(IDX::VX); + const double wz = X_curr(IDX::WZ); + + Vector6d X_next; + X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt); // dyaw = omega + omega_bias + X_next(IDX::YAWB) = yaw_bias; + X_next(IDX::VX) = vx; + X_next(IDX::WZ) = wz; + return X_next; +} + +// TODO(TakeshiIshita) show where the equation come from +Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt) +{ + const double yaw = X_curr(IDX::YAW); + const double yaw_bias = X_curr(IDX::YAWB); + const double vx = X_curr(IDX::VX); + + Matrix6d A = Matrix6d::Identity(); + A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + A(IDX::YAW, IDX::WZ) = dt; + return A; +} + +Matrix6d processNoiseCovariance( + const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) +{ + Matrix6d Q = Matrix6d::Zero(); + Q(IDX::X, IDX::X) = 0.0; + Q(IDX::Y, IDX::Y) = 0.0; + Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw + Q(IDX::YAWB, IDX::YAWB) = 0.0; + Q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx + Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz + return Q; +} diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp new file mode 100644 index 0000000000000..9cb7975a964a9 --- /dev/null +++ b/localization/ekf_localizer/test/test_state_transition.cpp @@ -0,0 +1,95 @@ +// Copyright 2018-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define _USE_MATH_DEFINES +#include "ekf_localizer/state_index.hpp" +#include "ekf_localizer/state_transition.hpp" + +#include +#include + +TEST(StateTransition, NormalizeYaw) +{ + const double tolerance = 1e-6; + EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); + EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance); +} + +TEST(PredictNextState, PredictNextState) +{ + // This function is the definition of state transition so we just check + // if the calculation is done according to the formula + Vector6d X_curr; + X_curr(0) = 2.; + X_curr(1) = 3.; + X_curr(2) = M_PI / 2.; + X_curr(3) = M_PI / 4.; + X_curr(4) = 10.; + X_curr(5) = 2. * M_PI / 3.; + + const double dt = 0.5; + + const Vector6d X_next = predictNextState(X_curr, dt); + + const double tolerance = 1e-10; + EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance); + EXPECT_NEAR(X_next(3), X_curr(3), tolerance); + EXPECT_NEAR(X_next(4), X_curr(4), tolerance); + EXPECT_NEAR(X_next(5), X_curr(5), tolerance); +} + +TEST(CreateStateTransitionMatrix, NumericalApproximation) +{ + // The transition matrix A = df / dx + // We check if df = A * dx approximates f(x + dx) - f(x) + + { + // check around x = 0 + const double dt = 0.1; + const Vector6d dx = 0.1 * Vector6d::Ones(); + const Vector6d x = Vector6d::Zero(); + + const Matrix6d A = createStateTransitionMatrix(x, dt); + const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + + EXPECT_LT((df - A * dx).norm(), 2e-3); + } + + { + // check around random x + const double dt = 0.1; + const Vector6d dx = 0.1 * Vector6d::Ones(); + const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); + + const Matrix6d A = createStateTransitionMatrix(x, dt); + const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + + EXPECT_LT((df - A * dx).norm(), 5e-3); + } +} + +TEST(ProcessNoiseCovariance, ProcessNoiseCovariance) +{ + const Matrix6d Q = processNoiseCovariance(1., 2., 3.); + EXPECT_EQ(Q(2, 2), 1.); // for yaw + EXPECT_EQ(Q(4, 4), 2.); // for vx + EXPECT_EQ(Q(5, 5), 3.); // for wz + + // Make sure other elements are zero + EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.); +} From f30d7ead2f5cb32dd232f4f82db0bf279e80e3d7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 5 Sep 2022 16:32:32 +0900 Subject: [PATCH 14/28] docs(behavior_path_planner): update pull out docs (#1587) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/behavior_path_planner/README.md | 98 ++++++++++++++++--- .../image/geometric_pull_out.drawio.svg | 4 + .../image/pull_out_after_back.drawio.svg | 4 + .../image/pull_out_collision_check.drawio.svg | 4 + .../image/pull_out_fig1.drawio.svg | 1 - .../image/shift_pull_out.drawio.svg | 4 + 6 files changed, 100 insertions(+), 15 deletions(-) create mode 100644 planning/behavior_path_planner/image/geometric_pull_out.drawio.svg create mode 100644 planning/behavior_path_planner/image/pull_out_after_back.drawio.svg create mode 100644 planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg delete mode 100644 planning/behavior_path_planner/image/pull_out_fig1.drawio.svg create mode 100644 planning/behavior_path_planner/image/shift_pull_out.drawio.svg diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d3690ca880b0c..a1e4c31c49ce7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -355,39 +355,109 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of - Pull out request condition - The speed of the vehicle is 0. - - The footprint of ego-vehicle is included in shoulder lane - - The distance from ego-vehicle to the destination is long enough for pull out (same with pull over) - - The distance required for a pull out is determined by the sum of the straight distance before pull out, the straight distance after pull out, and the minimum distance required for pull out + - Somewhere in footprint of ego-vehicle is included in shoulder lane + - The distance from ego-vehicle to the destination is long enough for pull out - Pull out ready condition - - The end of generated path is located before the end of the road - - Distance required for pull out of generated path is shorter than distance from ego-pose to goal-pose - - Generated Path is safe (This condition is temporarily invalid) - - It is possible to enable or disable this condition. (default: `disabled`) + - It is always ready if the conditions of the request are met. + +- Pull out start condition + - Generate safe path which does not collide with obstacles - If safe path cannot be generated from the current position, safe path from a receding position is searched. - Pull out is allowed by an operator #### **finish pull out condition** (need to meet any of the conditions below) -- The distance to the goal of your vehicle is lower than threshold (default: < `1m`) -- The speed of the vehicle is 0. +- Exceeding the pull out end point by more than the threshold (default: `1.0m`) + +#### General parameters for pull_out + +| Name | Unit | Type | Description | Default value | +| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| pull_out_finish_judge_buffer | [m] | double | threshold for finish judgment distance from pull out end point | 1.0 | #### **Safe check with obstacles in shoulder lane** -1. Calculate ego-vehicle's footprint on pull out path between from current position to merge end point. (Illustrated by blue frame) +1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame) 2. Calculate object's polygon which is located in shoulder lane -3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.5 m`), that is judged as a unsafe path +3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path -![pull_out](./image/pull_out_fig1.drawio.svg) +![pull_out_collision_check](./image/pull_out_collision_check.drawio.svg) #### **Path Generation** -The path is generated with a certain margin from the left edge of the shoulder lane. +There are two path generation methods. + +##### **shift pull out** + +Pull out distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one that generates a safe path is selected. + +- Generate the shoulder lane centerline and shift it to the current position. +- In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) +- Combine this path with center line of road lane + +![shift_pull_out](./image/shift_pull_out.drawio.svg) + +[Video of how shift pull out works](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) + +###### parameters for shift pull out + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| before_pull_out_straight_distance | [m] | double | distance before pull out start point | 0.0 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | + +##### **geometric pull out** + +Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. + +![geometric_pull_out](./image/geometric_pull_out.drawio.svg) + +[Video of how geometric pull out works](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) + +###### parameters for geometric pull out + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :---- | :----- | :------------------------------------------------------ | :------------ | +| enable_geometric_pull_out | - | bool | flag whether to enable geometric pull out | true | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | + +#### **backward pull out start point search** + +If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). + +![pull_out_after_back](./image/pull_out_after_back.drawio.svg) + +[Video of how pull out after backward driving works](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------------------------- | :------------- | :----- | :-------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_back | - | bool | flag whether to search backward for start_point | true | +| enable_back | - | bool | In the case of `efficient_path`, use efficient paths even if the back distance is longer. | +| In case of `short_back_distance`, use a path with as short a back distance | efficient_path | +| max_back_distance | [m] | double | maximum back distance | 15.0 | +| backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | +| backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | #### Unimplemented parts / limitations for pull put -- Note that the current module's start judgment condition may incorrectly judge that the vehicle is parked on the shoulder, such as when stopping in front of an intersection. +- pull out from the right shoulder lane to the left lane is not allowed. +- The safety of the road lane is not judged + - Collision prediction is not performed for vehicles approaching from behind the road lane. ### Side Shift diff --git a/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg b/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg new file mode 100644 index 0000000000000..802b6138d5f13 --- /dev/null +++ b/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start poi...
stop
stopText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg b/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg new file mode 100644 index 0000000000000..f89ddde14db47 --- /dev/null +++ b/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start point
stop
stop
stop
stop
backward_search_resolution
backward_search_resolutionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg b/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg new file mode 100644 index 0000000000000..53afb6db58db4 --- /dev/null +++ b/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start poi...
collision check
collision checkText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_out_fig1.drawio.svg b/planning/behavior_path_planner/image/pull_out_fig1.drawio.svg deleted file mode 100644 index f629ce734c10e..0000000000000 --- a/planning/behavior_path_planner/image/pull_out_fig1.drawio.svg +++ /dev/null @@ -1 +0,0 @@ -
merge end point
merge end point
merge start point
merge start point
colision check
colision checkViewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/behavior_path_planner/image/shift_pull_out.drawio.svg b/planning/behavior_path_planner/image/shift_pull_out.drawio.svg new file mode 100644 index 0000000000000..887294bbe5d23 --- /dev/null +++ b/planning/behavior_path_planner/image/shift_pull_out.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start poi...
before_pull_out_distance
before_pul...
pull_out_distance
pull_out_d...Text is not SVG - cannot display \ No newline at end of file From dff3102017311d4c44c20433cdd799de8b6e1abe Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 5 Sep 2022 18:08:45 +0900 Subject: [PATCH 15/28] feat(behavior_path_planner): use object recognition for pull_over (#1777) * feat(behavior_path_planner): use object recognition for pull_over Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Signed-off-by: kosuke55 * rename checkCollision Signed-off-by: kosuke55 * update docs Signed-off-by: kosuke55 * remove unnecessary lines Signed-off-by: kosuke55 * update warn message Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../pull_over/pull_over.param.yaml | 6 +- planning/behavior_path_planner/README.md | 20 +- .../config/pull_over/pull_over.param.yaml | 6 +- .../pull_over/pull_over_module.hpp | 39 ++-- .../src/behavior_path_planner_node.cpp | 7 +- .../pull_over/pull_over_module.cpp | 202 ++++++++++-------- 6 files changed, 162 insertions(+), 118 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index d11ed1f0db74f..baa35d2a88062 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -18,9 +18,13 @@ goal_search_interval: 1.0 goal_to_obj_margin: 2.0 # occupancy grid map - collision_check_margin: 0.5 + use_occupancy_grid: true + occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 + # object recognition + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 # shift path enable_shift_parking: true pull_over_sampling_num: 4 diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a1e4c31c49ce7..a2b299284cfad 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -238,18 +238,26 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio ##### Parameters for occupancy grid based collision check -| Name | Unit | Type | Description | Default value | -| :--------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.5 | -| theta_size | - | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | - | int | threshold of cell values to be considered as obstacles | 60 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid | - | bool | flag whether to use occupancy grid for collision check | true | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | - | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | - | int | threshold of cell values to be considered as obstacles | 60 | + +##### Parameters for object recognition based collision check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | +| use_object_recognition | - | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 | #### **Goal Search** If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is searched for in certain range of the shoulder lane. -[Video of how goal search works](https://user-images.githubusercontent.com/39142679/178033628-bec1bbc7-3b27-47b1-b50b-55f855e2e399.mp4) +[Video of how goal search works](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) ##### Parameters for goal search diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index d11ed1f0db74f..baa35d2a88062 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -18,9 +18,13 @@ goal_search_interval: 1.0 goal_to_obj_margin: 2.0 # occupancy grid map - collision_check_margin: 0.5 + use_occupancy_grid: true + occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 + # object recognition + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 # shift path enable_shift_parking: true pull_over_sampling_num: 4 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 0159c1a5956fe..fbe177291a879 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -57,9 +57,13 @@ struct PullOverParameters double goal_search_interval; double goal_to_obj_margin; // occupancy grid map - double collision_check_margin; + bool use_occupancy_grid; + double occupancy_grid_collision_check_margin; double theta_size; double obstacle_threshold; + // object recognition + bool use_object_recognition; + double object_recognition_collision_check_margin; // shift path bool enable_shift_parking; int pull_over_sampling_num; @@ -107,11 +111,11 @@ enum PathType { struct PUllOverStatus { - PathWithLaneId path; + PathWithLaneId path{}; std::shared_ptr prev_stop_path = nullptr; - lanelet::ConstLanelets current_lanes; - lanelet::ConstLanelets pull_over_lanes; - lanelet::ConstLanelets lanes; // current + pull_over + lanelet::ConstLanelets current_lanes{}; + lanelet::ConstLanelets pull_over_lanes{}; + lanelet::ConstLanelets lanes{}; // current + pull_over bool has_decided_path = false; int path_type = PathType::NONE; bool is_safe = false; @@ -120,18 +124,12 @@ struct PUllOverStatus bool has_requested_approval_ = false; }; -struct PullOverArea -{ - Pose start_pose; - Pose end_pose; -}; - struct GoalCandidate { - Pose goal_pose; - double distance_from_original_goal; + Pose goal_pose{}; + double distance_from_original_goal = 0.0; - bool operator<(const GoalCandidate & other) noexcept + bool operator<(const GoalCandidate & other) const noexcept { return distance_from_original_goal < other.distance_from_original_goal; } @@ -161,11 +159,12 @@ class PullOverModule : public SceneModuleInterface private: PullOverParameters parameters_; + ShiftParkingPath shift_parking_path_; - rclcpp::Node * node_; + vehicle_info_util::VehicleInfo vehicle_info_; - double pull_over_lane_length_ = 200.0; - double check_distance_ = 100.0; + const double pull_over_lane_length_ = 200.0; + const double check_distance_ = 100.0; rclcpp::Subscription::SharedPtr occupancy_grid_sub_; rclcpp::Publisher::SharedPtr Cr_pub_; @@ -174,17 +173,17 @@ class PullOverModule : public SceneModuleInterface rclcpp::Publisher::SharedPtr goal_pose_pub_; rclcpp::Publisher::SharedPtr path_pose_array_pub_; rclcpp::Publisher::SharedPtr parking_area_pub_; - rclcpp::Clock::SharedPtr clock_; PUllOverStatus status_; OccupancyGridBasedCollisionDetector occupancy_grid_map_; - std::vector pull_over_areas_; Pose modified_goal_pose_; + Pose refined_goal_pose_; std::vector goal_candidates_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; std::deque odometry_buffer_; std::unique_ptr lane_departure_checker_; + tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::unique_ptr last_received_time_; std::unique_ptr last_approved_time_; @@ -209,6 +208,8 @@ class PullOverModule : public SceneModuleInterface void updateOccupancyGrid(); void researchGoal(); void resetStatus(); + bool checkCollisionWithPose(const Pose & pose) const; + bool checkCollisionWithPath(const PathWithLaneId & path) const; // turn signal std::pair getHazardInfo() const; 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 93e05415595e8..d0c8d593a56da 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -379,9 +379,14 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.goal_search_interval = dp("goal_search_interval", 5.0); p.goal_to_obj_margin = dp("goal_to_obj_margin", 2.0); // occupancy grid map - p.collision_check_margin = dp("collision_check_margin", 0.5); + p.use_occupancy_grid = dp("use_occupancy_grid", true); + p.occupancy_grid_collision_check_margin = dp("occupancy_grid_collision_check_margin", 0.0); p.theta_size = dp("theta_size", 360); p.obstacle_threshold = dp("obstacle_threshold", 90); + // object recognition + p.use_object_recognition = dp("use_object_recognition", true); + p.object_recognition_collision_check_margin = + dp("object_recognition_collision_check_margin", 1.0); // shift path p.enable_shift_parking = dp("enable_shift_parking", true); p.pull_over_sampling_num = dp("pull_over_sampling_num", 4); 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 91f127a536e2c..cfe4889559532 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 @@ -17,6 +17,8 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/pull_over/util.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include "behavior_path_planner/util/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utilities.hpp" #include #include @@ -45,7 +47,9 @@ namespace behavior_path_planner { PullOverModule::PullOverModule( const std::string & name, rclcpp::Node & node, const PullOverParameters & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters}, clock_{node.get_clock()} +: SceneModuleInterface{name, node}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { rtc_interface_ptr_ = std::make_shared(&node, "pull_over"); goal_pose_pub_ = @@ -60,6 +64,10 @@ PullOverModule::PullOverModule( lane_departure_checker_ = std::make_unique(); lane_departure_checker_->setVehicleInfo( vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + + // for collision check with objects + vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + resetStatus(); } @@ -113,17 +121,19 @@ void PullOverModule::onEntry() current_state_ = BT::NodeStatus::SUCCESS; // Initialize occupancy grid map - OccupancyGridMapParam occupancy_grid_map_param; - const double margin = parameters_.collision_check_margin; - occupancy_grid_map_param.vehicle_shape.length = - planner_data_->parameters.vehicle_length + 2 * margin; - occupancy_grid_map_param.vehicle_shape.width = - planner_data_->parameters.vehicle_width + 2 * margin; - occupancy_grid_map_param.vehicle_shape.base2back = - planner_data_->parameters.base_link2rear + margin; - occupancy_grid_map_param.theta_size = parameters_.theta_size; - occupancy_grid_map_param.obstacle_threshold = parameters_.obstacle_threshold; - occupancy_grid_map_.setParam(occupancy_grid_map_param); + if (parameters_.use_occupancy_grid) { + OccupancyGridMapParam occupancy_grid_map_param; + const double margin = parameters_.occupancy_grid_collision_check_margin; + occupancy_grid_map_param.vehicle_shape.length = + planner_data_->parameters.vehicle_length + 2 * margin; + occupancy_grid_map_param.vehicle_shape.width = + planner_data_->parameters.vehicle_width + 2 * margin; + occupancy_grid_map_param.vehicle_shape.base2back = + planner_data_->parameters.base_link2rear + margin; + occupancy_grid_map_param.theta_size = parameters_.theta_size; + occupancy_grid_map_param.obstacle_threshold = parameters_.obstacle_threshold; + occupancy_grid_map_.setParam(occupancy_grid_map_param); + } // initialize when receiving new route if ( @@ -222,83 +232,39 @@ Pose PullOverModule::getRefinedGoal() const Pose goal_pose = planner_data_->route_handler->getGoalPose(); lanelet::Lanelet closest_shoulder_lanelet; - lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), goal_pose, &closest_shoulder_lanelet); - Pose refined_goal_pose = + const Pose center_pose = lanelet::utils::getClosestCenterPose(closest_shoulder_lanelet, goal_pose.position); const double distance_to_left_bound = util::getDistanceToShoulderBoundary( - planner_data_->route_handler->getShoulderLanelets(), refined_goal_pose); + planner_data_->route_handler->getShoulderLanelets(), center_pose); const double offset_from_center_line = distance_to_left_bound + planner_data_->parameters.vehicle_width / 2 + parameters_.margin_from_boundary; - refined_goal_pose = calcOffsetPose(refined_goal_pose, 0, -offset_from_center_line, 0); + const Pose refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; } void PullOverModule::researchGoal() { - const auto common_param = occupancy_grid_map_.getParam(); - const Pose goal_pose = getRefinedGoal(); - double dx = -parameters_.backward_goal_search_length; - - // Avoid adding areas that are in conflict from the start. - bool prev_is_collided = true; - - pull_over_areas_.clear(); - const Pose goal_pose_map_coords = global2local(occupancy_grid_map_.getMap(), goal_pose); - Pose start_pose = calcOffsetPose(goal_pose, dx, 0, 0); - // Search non collision areas around the goal - while (rclcpp::ok()) { - bool is_last_search = (dx >= parameters_.forward_goal_search_length); - Pose search_pose = calcOffsetPose(goal_pose_map_coords, dx, 0, 0); - bool is_collided = occupancy_grid_map_.detectCollision( - pose2index(occupancy_grid_map_.getMap(), search_pose, common_param.theta_size), false); - // Add area when (1) change non-collision -> collision or (2) last search without collision - if ((!prev_is_collided && is_collided) || (!is_collided && is_last_search)) { - Pose end_pose = calcOffsetPose(goal_pose, dx, 0, 0); - if (!pull_over_areas_.empty()) { - auto prev_area = pull_over_areas_.back(); - // If the current area overlaps the previous area, merge them. - if ( - calcDistance2d(prev_area.end_pose, start_pose) < - planner_data_->parameters.vehicle_length) { - pull_over_areas_.pop_back(); - start_pose = prev_area.start_pose; - } - } - pull_over_areas_.push_back(PullOverArea{start_pose, end_pose}); - } - if (is_last_search) break; - - if ((prev_is_collided && !is_collided)) { - start_pose = calcOffsetPose(goal_pose, dx, 0, 0); - } - prev_is_collided = is_collided; - dx += 0.05; - } - // Find goals in pull over areas. goal_candidates_.clear(); + const Pose refined_goal_pose = getRefinedGoal(); for (double dx = -parameters_.backward_goal_search_length; dx <= parameters_.forward_goal_search_length; dx += parameters_.goal_search_interval) { - Pose search_pose = calcOffsetPose(goal_pose, dx, 0, 0); - for (const auto & area : pull_over_areas_) { - const Pose start_to_search = inverseTransformPose(search_pose, area.start_pose); - const Pose end_to_search = inverseTransformPose(search_pose, area.end_pose); - if ( - start_to_search.position.x > parameters_.goal_to_obj_margin && - end_to_search.position.x < -parameters_.goal_to_obj_margin) { - GoalCandidate goal_candidate; - goal_candidate.goal_pose = search_pose; - goal_candidate.distance_from_original_goal = - std::abs(inverseTransformPose(search_pose, goal_pose).position.x); - goal_candidates_.push_back(goal_candidate); - } + const Pose search_pose = calcOffsetPose(refined_goal_pose, dx, 0, 0); + if (checkCollisionWithPose(search_pose)) { + continue; } + + GoalCandidate goal_candidate; + goal_candidate.goal_pose = search_pose; + goal_candidate.distance_from_original_goal = + std::abs(inverseTransformPose(search_pose, refined_goal_pose).position.x); + goal_candidates_.push_back(goal_candidate); } // Sort with distance from original goal std::sort(goal_candidates_.begin(), goal_candidates_.end()); @@ -341,6 +307,47 @@ bool PullOverModule::isLongEnoughToParkingStart( return *dist_to_parking_start_pose > current_to_stop_distance; } +bool PullOverModule::checkCollisionWithPose(const Pose & pose) const +{ + if (parameters_.use_occupancy_grid) { + const Pose pose_grid_coords = global2local(occupancy_grid_map_.getMap(), pose); + const auto idx = pose2index( + occupancy_grid_map_.getMap(), pose_grid_coords, occupancy_grid_map_.getParam().theta_size); + const bool check_out_of_range = false; + if (occupancy_grid_map_.detectCollision(idx, check_out_of_range)) { + return true; + } + } + + if (parameters_.use_object_recognition) { + if (util::checkCollisionBetweenFootprintAndObjects( + vehicle_footprint_, pose, *(planner_data_->dynamic_object), + parameters_.object_recognition_collision_check_margin)) { + return true; + } + } + return false; +} + +bool PullOverModule::checkCollisionWithPath(const PathWithLaneId & path) const +{ + if (parameters_.use_occupancy_grid) { + const bool check_out_of_range = false; + if (occupancy_grid_map_.hasObstacleOnPath(path, check_out_of_range)) { + return true; + } + } + + if (parameters_.use_object_recognition) { + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, path, *(planner_data_->dynamic_object), + parameters_.object_recognition_collision_check_margin)) { + return true; + } + } + return false; +} + bool PullOverModule::planWithEfficientPath() { // shift parking path @@ -370,7 +377,7 @@ bool PullOverModule::planWithEfficientPath() isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && - !occupancy_grid_map_.hasObstacleOnPath(parallel_parking_planner_.getArcPath(), false) && + !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { status_.path = parallel_parking_planner_.getCurrentPath(); @@ -392,7 +399,7 @@ bool PullOverModule::planWithEfficientPath() isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && - !occupancy_grid_map_.hasObstacleOnPath(parallel_parking_planner_.getArcPath(), false) && + !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { status_.path = parallel_parking_planner_.getCurrentPath(); @@ -432,7 +439,7 @@ bool PullOverModule::planWithCloseGoal() isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && - !occupancy_grid_map_.hasObstacleOnPath(parallel_parking_planner_.getArcPath(), false) && + !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { status_.path = parallel_parking_planner_.getCurrentPath(); @@ -449,7 +456,7 @@ bool PullOverModule::planWithCloseGoal() isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && - !occupancy_grid_map_.hasObstacleOnPath(parallel_parking_planner_.getArcPath(), false) && + !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { status_.path = parallel_parking_planner_.getCurrentPath(); @@ -554,15 +561,15 @@ BehaviorModuleOutput PullOverModule::plan() RCLCPP_ERROR( getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", parameters_.search_priority.c_str()); + throw std::domain_error("[pull_over] invalid search_priority"); } // Decelerate before the minimum shift distance from the goal search area. if (has_found_safe_path) { - const Pose goal_pose = getRefinedGoal(); const auto arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, goal_pose); + lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); const Pose search_start_pose = calcOffsetPose( - goal_pose, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); + refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); status_.path = util::setDecelerationVelocity( status_.path, parameters_.pull_over_velocity, search_start_pose, -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); @@ -574,12 +581,16 @@ BehaviorModuleOutput PullOverModule::plan() // safe: use pull over path if (status_.is_safe) { output.path = std::make_shared(status_.path); - } else if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + } else { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path. Stop in road lane."); // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = std::make_shared(generateStopPath()); - status_.prev_stop_path = output.path; - } else { // not_safe -> not_safe: use previous stop path - output.path = status_.prev_stop_path; + if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + output.path = std::make_shared(generateStopPath()); + status_.prev_stop_path = output.path; + } else { // not_safe -> not_safe: use previous stop path + output.path = status_.prev_stop_path; + } } status_.prev_is_safe = status_.is_safe; @@ -685,13 +696,14 @@ PathWithLaneId PullOverModule::getReferencePath() const const auto current_pose = planner_data_->self_pose->pose; const auto common_parameters = planner_data_->parameters; - const Pose goal_pose = getRefinedGoal(); + const Pose refined_goal_pose = getRefinedGoal(); if (status_.current_lanes.empty()) { return PathWithLaneId{}; } - const auto arc_coordinates = lanelet::utils::getArcCoordinates(status_.current_lanes, goal_pose); + const auto arc_coordinates = + lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose); const Pose search_start_pose = calcOffsetPose( - goal_pose, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); + refined_goal_pose, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); // if not approved, stop parking start position or goal search start position. const Pose stop_pose = status_.is_safe ? getParkingStartPose() : search_start_pose; @@ -837,8 +849,15 @@ std::pair PullOverModule::getSafePath(ShiftParkingPath & safe_path) return std::make_pair(false, false); } // select safe path - bool found_safe_path = - pull_over_utils::selectSafePath(valid_paths, occupancy_grid_map_, safe_path); + bool found_safe_path = false; + for (const auto & path : valid_paths) { + if (!checkCollisionWithPath(path.shifted_path.path)) { + safe_path = path; + found_safe_path = true; + break; + } + } + safe_path.is_safe = found_safe_path; return std::make_pair(true, found_safe_path); } @@ -1038,11 +1057,14 @@ void PullOverModule::publishDebugData() // Visualize pull over areas if (parameters_.enable_goal_research) { + const Pose refined_goal_pose = getRefinedGoal(); + const Pose start_pose = + calcOffsetPose(refined_goal_pose, -parameters_.backward_goal_search_length, 0, 0); + const Pose end_pose = + calcOffsetPose(refined_goal_pose, parameters_.forward_goal_search_length, 0, 0); MarkerArray marker_array; - for (size_t i = 0; i < pull_over_areas_.size(); i++) { - marker_array.markers.push_back(createParkingAreaMarker( - pull_over_areas_.at(i).start_pose, pull_over_areas_.at(i).end_pose, i)); - } + marker_array.markers.push_back(createParkingAreaMarker(start_pose, end_pose, 0)); + parking_area_pub_->publish(marker_array); } From da6f7f4cd977deafa3f5ea6d9cb48d7f2774bc05 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 5 Sep 2022 18:41:09 +0900 Subject: [PATCH 16/28] fix(mission_planner, route_handler): enable loop detection of route (#1705) * fix(mission_planner, route_handler): enable loop detection of route Signed-off-by: tomoya.kimura * fix undefined reference Signed-off-by: tomoya.kimura * do not publish looped-route Signed-off-by: tomoya.kimura * simplify the code Signed-off-by: tomoya.kimura * insert -> emplace Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../mission_planner_lanelet2.cpp | 22 ++----------- .../include/route_handler/route_handler.hpp | 2 ++ planning/route_handler/src/route_handler.cpp | 33 +++++++++---------- 3 files changed, 21 insertions(+), 36 deletions(-) diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp index 1c17012371e92..2249ec4a4750e 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp @@ -53,22 +53,6 @@ RouteSections combine_consecutive_route_sections( return route_sections; } -bool is_route_looped(const RouteSections & route_sections) -{ - for (std::size_t i = 0; i < route_sections.size(); i++) { - const auto & route_section = route_sections.at(i); - for (const auto & lane_id : route_section.primitives) { - for (std::size_t j = i + 1; j < route_sections.size(); j++) { - const auto & future_route_section = route_sections.at(j); - if (exists(future_route_section.primitives, lane_id)) { - return true; - } - } - } - } - return false; -} - double normalize_radian(const double rad, const double min_rad = -M_PI, const double max_rad = M_PI) { const auto value = std::fmod(rad, 2 * M_PI); @@ -297,9 +281,9 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route route_sections = combine_consecutive_route_sections(route_sections, local_route_sections); } - if (is_route_looped(route_sections)) { - RCLCPP_WARN( - get_logger(), "Loop detected within route! Be aware that looped route is not debugged!"); + if (route_handler_.isRouteLooped(route_sections)) { + RCLCPP_WARN(get_logger(), "Loop detected within route!"); + return route_msg; } refine_goal_height(route_sections); diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 9b9a034b27511..92737ca586fc1 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -50,6 +50,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using std_msgs::msg::Header; +using RouteSections = std::vector; enum class LaneChangeDirection { NONE, LEFT, RIGHT }; enum class PullOverDirection { NONE, LEFT, RIGHT }; @@ -87,6 +88,7 @@ class RouteHandler const Pose & start_checkpoint, const Pose & goal_checkpoint, lanelet::ConstLanelets * path_lanelets) const; std::vector createMapSegments(const lanelet::ConstLanelets & path_lanelets) const; + bool isRouteLooped(const RouteSections & route_sections) const; // for goal bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 2a9306238db09..1ed082216d3bf 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -61,22 +61,6 @@ bool exists(const std::vector & vectors, const T & item) return false; } -bool isRouteLooped(const autoware_auto_planning_msgs::msg::HADMapRoute & route_msg) -{ - const auto & route_sections = route_msg.segments; - for (const auto & route_section : route_sections) { - const auto primitives = route_section.primitives; - for (auto itr = primitives.begin(); itr != primitives.end(); ++itr) { - const auto next_itr = itr + 1; - if (next_itr == primitives.end()) break; - if (std::any_of(next_itr, primitives.end(), [itr](auto p) { return p.id == itr->id; })) { - return true; - } - } - } - return false; -} - lanelet::ConstPoint3d get3DPointFrom2DArcLength( const lanelet::ConstLanelets & lanelet_sequence, const double s) { @@ -199,9 +183,24 @@ void RouteHandler::setMap(const HADMapBin & map_msg) setLaneletsFromRouteMsg(); } +bool RouteHandler::isRouteLooped(const RouteSections & route_sections) const +{ + std::set lane_primitives; + for (const auto & route_section : route_sections) { + for (const auto & primitive : route_section.primitives) { + if (lane_primitives.find(primitive.id) == lane_primitives.end()) { + lane_primitives.emplace(primitive.id); + } else { + return true; // find duplicated id + } + } + } + return false; +} + void RouteHandler::setRoute(const HADMapRoute & route_msg) { - if (!isRouteLooped(route_msg)) { + if (!isRouteLooped(route_msg.segments)) { route_msg_ = route_msg; is_route_msg_ready_ = true; is_handler_ready_ = false; From 57c4b1fbc95748d59f0f77b1ac84a2c976f17fb8 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 5 Sep 2022 19:41:21 +0900 Subject: [PATCH 17/28] fix(heatmap_visualizer): add unknown class (#1781) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../include/heatmap_visualizer/heatmap_visualizer_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp index 88adf1d3d94b6..9ffb3349435e5 100644 --- a/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp +++ b/perception/heatmap_visualizer/include/heatmap_visualizer/heatmap_visualizer_node.hpp @@ -66,8 +66,8 @@ class HeatmapVisualizerNode : public rclcpp::Node float map_length_; float map_resolution_; bool use_confidence_; - std::vector class_names_{"CAR", "TRUCK", "BUS", "TRAILER", - "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; + std::vector class_names_{"UNKNWON", "CAR", "TRUCK", "BUS", + "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; bool rename_car_to_truck_and_bus_; // Number of width and height cells From 15b28b9e5e2bbdeddea3dc0046a8060e732dc9f0 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 6 Sep 2022 11:02:26 +0900 Subject: [PATCH 18/28] refactor(run_out): use util function to calculate stop point (#1780) * refactor: use util function to calculate stop point Signed-off-by: Tomohito Ando * refactor: remove unnecessary debug marker Signed-off-by: Tomohito Ando * refactor: revert an unrelated change Signed-off-by: Tomohito Ando Signed-off-by: Tomohito Ando --- .../scene_module/run_out/path_utils.hpp | 40 ----------- .../include/scene_module/run_out/scene.hpp | 5 +- .../src/scene_module/run_out/scene.cpp | 70 +++++++++---------- 3 files changed, 35 insertions(+), 80 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp index 0b8f2b4ef444f..50a0f8728f0b3 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp @@ -48,46 +48,6 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( return min_dist_point; } -template -size_t calcIndexByLength( - const T & points, const geometry_msgs::msg::Pose & current_pose, const double target_length) -{ - const size_t nearest_index = motion_utils::findNearestIndex(points, current_pose.position); - if (target_length < 0) { - return nearest_index; - } - - for (size_t i = nearest_index; i < points.size(); i++) { - double length_sum = motion_utils::calcSignedArcLength(points, current_pose.position, i); - if (length_sum > target_length) { - return i; - } - } - - // reach the end of the points, so return the last index - return points.size() - 1; -} - -template -size_t calcIndexByLengthReverse( - const T & points, const geometry_msgs::msg::Point & src_point, const float target_length) -{ - const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, src_point); - if (nearest_seg_idx == 0) { - return 0; - } - - for (size_t i = nearest_seg_idx; i > 0; i--) { - const auto length_sum = std::abs(motion_utils::calcSignedArcLength(points, src_point, i)); - if (length_sum > target_length) { - return i + 1; - } - } - - // reach the beginning of the path - return 0; -} - } // namespace run_out_utils } // namespace behavior_velocity_planner #endif // SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp index bb6fc7c237418..d9d6ae95c6e68 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp @@ -124,12 +124,11 @@ class RunOutModule : public SceneModuleInterface void insertStoppingVelocity( const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, - const PathWithLaneId & smoothed_path, PathWithLaneId & output_path); + PathWithLaneId & output_path); void insertApproachingVelocity( const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, - const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path, - PathWithLaneId & output_path); + const float approaching_vel, const float approach_margin, PathWithLaneId & output_path); void applyMaxJerkLimit( const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 9f629927036f8..7358c8b73924a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -101,8 +101,7 @@ bool RunOutModule::modifyPathVelocity( dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); } else { // just insert zero velocity for stopping - insertStoppingVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); + insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); } // apply max jerk limit if the ego can't stop with specified max jerk and acc @@ -560,17 +559,20 @@ boost::optional RunOutModule::calcStopPoint( // insert the stop point without considering the distance from the obstacle // smoother will calculate appropriate jerk for deceleration if (!planner_param_.run_out.specify_decel_jerk) { - // calculate index of stop point + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; - const size_t stop_index = run_out_utils::calcIndexByLengthReverse( - path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point); - const auto & stop_point = path.points.at(stop_index).point.pose; + const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false); + if (!stop_point) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); + return {}; + } // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( - stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; } @@ -591,16 +593,7 @@ boost::optional RunOutModule::calcStopPoint( stop_dist = boost::make_optional(dist_to_collision); } - // debug - { - const float base_to_obstacle = - planner_param_.vehicle_param.base_to_front + planner_param_.run_out.stop_margin; - const auto vehicle_stop_idx = run_out_utils::calcIndexByLength( - path.points, current_pose, stop_dist.get() + base_to_obstacle); - const auto & p = path.points.at(vehicle_stop_idx).point.pose.position; - debug_ptr_->pushDebugPoints(p, PointType::Yellow); - debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist); - } + debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist); // vehicle have to decelerate if there is not enough distance with deceleration_jerk const bool deceleration_needed = @@ -615,17 +608,20 @@ boost::optional RunOutModule::calcStopPoint( return {}; } - // calculate index of stop point + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; - const size_t stop_index = run_out_utils::calcIndexByLengthReverse( - path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point); - const auto & stop_point = path.points.at(stop_index).point.pose; + const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false); + if (!stop_point) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); + return {}; + } // debug debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( - stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); return stop_point; } @@ -691,8 +687,7 @@ void RunOutModule::insertVelocity( state_ = State::STOP; } - insertStoppingVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path); + insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path); break; } @@ -703,8 +698,7 @@ void RunOutModule::insertVelocity( elapsed_time > planner_param_.approaching.stop_time_thresh ? State::APPROACH : State::STOP; RCLCPP_DEBUG_STREAM(logger_, "elapsed time: " << elapsed_time); - insertStoppingVelocity( - dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path); + insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path); break; } @@ -712,7 +706,7 @@ void RunOutModule::insertVelocity( RCLCPP_DEBUG_STREAM(logger_, "APPROACH state"); insertApproachingVelocity( *dynamic_obstacle, current_pose, planner_param_.approaching.limit_vel_kmph / 3.6, - planner_param_.approaching.margin, smoothed_path, output_path); + planner_param_.approaching.margin, output_path); debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); break; } @@ -727,17 +721,16 @@ void RunOutModule::insertVelocity( void RunOutModule::insertStoppingVelocity( const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, - const PathWithLaneId & smoothed_path, PathWithLaneId & output_path) + PathWithLaneId & output_path) { const auto stop_point = - calcStopPoint(dynamic_obstacle, smoothed_path, current_pose, current_vel, current_acc); + calcStopPoint(dynamic_obstacle, output_path, current_pose, current_vel, current_acc); insertStopPoint(stop_point, output_path); } void RunOutModule::insertApproachingVelocity( const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, - const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path, - PathWithLaneId & output_path) + const float approaching_vel, const float approach_margin, PathWithLaneId & output_path) { // insert slow down velocity from nearest segment point const auto nearest_seg_idx = @@ -752,23 +745,26 @@ void RunOutModule::insertApproachingVelocity( // calculate stop point to insert 0 velocity const float base_to_collision_point = approach_margin + planner_param_.vehicle_param.base_to_front; - const auto stop_idx = run_out_utils::calcIndexByLengthReverse( - resampled_path.points, dynamic_obstacle.nearest_collision_point, base_to_collision_point); - const auto & stop_point = resampled_path.points.at(stop_idx).point.pose; + const auto stop_point = motion_utils::calcLongitudinalOffsetPose( + output_path.points, dynamic_obstacle.nearest_collision_point, -base_to_collision_point, false); + if (!stop_point) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point."); + return; + } // debug debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( - stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); const auto nearest_seg_idx_stop = - motion_utils::findNearestSegmentIndex(output_path.points, stop_point.position); + motion_utils::findNearestSegmentIndex(output_path.points, stop_point->position); auto insert_idx_stop = nearest_seg_idx_stop + 1; // to PathPointWithLaneId // use lane id of point behind inserted point autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); - stop_point_with_lane_id.point.pose = stop_point; + stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop); } From 06c3f553c61a2540a9080aeb8a7658e9c0b838ca Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 6 Sep 2022 11:24:06 +0900 Subject: [PATCH 19/28] fix(radar_fusion_to_detected_object): add message_filter to radar fusion (#1779) * fix(radar_fusion_to_detected_object) add message_filter to radar fusion Signed-off-by: scepter914 * delete comment Signed-off-by: scepter914 * refactor Signed-off-by: scepter914 Signed-off-by: scepter914 --- ..._object_fusion_to_detected_object_node.hpp | 24 +++++++------ .../package.xml | 1 + ..._object_fusion_to_detected_object_node.cpp | 36 +++++++------------ 3 files changed, 28 insertions(+), 33 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp index 45c3c4e86cc93..295df180504f8 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp @@ -16,6 +16,9 @@ #ifndef RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ #define RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" #include "radar_fusion_to_detected_object.hpp" #include "rclcpp/rclcpp.hpp" @@ -46,12 +49,19 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_object_{}; - rclcpp::Subscription::SharedPtr sub_radar_{}; + message_filters::Subscriber sub_object_{}; + message_filters::Subscriber sub_radar_{}; + + using SyncPolicy = + message_filters::sync_policies::ApproximateTime; + using Sync = message_filters::Synchronizer; + typename std::shared_ptr sync_ptr_; // Callback - void onDetectedObjects(const DetectedObjects::ConstSharedPtr msg); - void onRadarObjects(const TrackedObjects::ConstSharedPtr msg); + void onData( + const DetectedObjects::ConstSharedPtr object_msg, + const TrackedObjects::ConstSharedPtr radar_msg); + bool isDataReady(); // Data Buffer DetectedObjects::ConstSharedPtr detected_objects_{}; @@ -60,12 +70,6 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_objects_{}; - // Timer - rclcpp::TimerBase::SharedPtr timer_{}; - - bool isDataReady(); - void onTimer(); - // Parameter Server OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onSetParam( diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 57492a387a88d..2d3ead512f307 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs eigen geometry_msgs + message_filters rclcpp rclcpp_components std_msgs diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index ac07d7d0111a9..289b8f9a3de2d 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -91,31 +91,17 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( radar_fusion_to_detected_object_->setParam(core_param_); // Subscriber - sub_object_ = create_subscription( - "~/input/objects", rclcpp::QoS{1}, - std::bind(&RadarObjectFusionToDetectedObjectNode::onDetectedObjects, this, _1)); - sub_radar_ = create_subscription( - "~/input/radars", rclcpp::QoS{1}, - std::bind(&RadarObjectFusionToDetectedObjectNode::onRadarObjects, this, _1)); + sub_object_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); + sub_radar_.subscribe(this, "~/input/radars", rclcpp::QoS{1}.get_rmw_qos_profile()); + + using std::placeholders::_1; + using std::placeholders::_2; + sync_ptr_ = std::make_shared(SyncPolicy(10), sub_object_, sub_radar_); + sync_ptr_->registerCallback( + std::bind(&RadarObjectFusionToDetectedObjectNode::onData, this, _1, _2)); // Publisher pub_objects_ = create_publisher("~/output/objects", 1); - - // Timer - const auto update_period_ns = rclcpp::Rate(node_param_.update_rate_hz).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), update_period_ns, - std::bind(&RadarObjectFusionToDetectedObjectNode::onTimer, this)); -} - -void RadarObjectFusionToDetectedObjectNode::onDetectedObjects( - const DetectedObjects::ConstSharedPtr msg) -{ - detected_objects_ = msg; -} -void RadarObjectFusionToDetectedObjectNode::onRadarObjects(const TrackedObjects::ConstSharedPtr msg) -{ - radar_objects_ = msg; } rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode::onSetParam( @@ -187,8 +173,12 @@ bool RadarObjectFusionToDetectedObjectNode::isDataReady() return true; } -void RadarObjectFusionToDetectedObjectNode::onTimer() +// void RadarObjectFusionToDetectedObjectNode::onTimer() +void RadarObjectFusionToDetectedObjectNode::onData( + const DetectedObjects::ConstSharedPtr object_msg, const TrackedObjects::ConstSharedPtr radar_msg) { + detected_objects_ = object_msg; + radar_objects_ = radar_msg; if (!isDataReady()) { return; } From a4078bdf374fc617f5be531e50e892aefbbc53e0 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 6 Sep 2022 11:32:09 +0900 Subject: [PATCH 20/28] refactor(behavior_velocity): use state machine util (#1785) * reafactor(behavior_velocity): unite state machine Signed-off-by: tanaka3 * refactor(blindspot): use state machine in util Signed-off-by: tanaka3 * refactor(merge_from_private): use state machine in utils Signed-off-by: tanaka3 * fix: conflict change Signed-off-by: taikitanaka3 Signed-off-by: tanaka3 Signed-off-by: taikitanaka3 --- .../include/scene_module/blind_spot/scene.hpp | 39 +------------- .../intersection/scene_intersection.hpp | 38 +------------ .../scene_merge_from_private_road.hpp | 37 +------------ .../include/utilization/state_machine.hpp | 2 +- .../src/scene_module/blind_spot/debug.cpp | 5 +- .../src/scene_module/blind_spot/scene.cpp | 53 ++++--------------- .../src/scene_module/intersection/debug.cpp | 9 ++-- .../intersection/scene_intersection.cpp | 48 +++-------------- .../scene_merge_from_private_road.cpp | 20 +++---- 9 files changed, 31 insertions(+), 220 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index 4ee5d9d40dc4e..f694e4bbb6ff0 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -43,49 +44,11 @@ struct BlindSpotPolygons class BlindSpotModule : public SceneModuleInterface { public: - enum class State { - STOP = 0, - GO, - }; - std::string toString(const State & state) - { - if (state == State::STOP) { - return "STOP"; - } else if (state == State::GO) { - return "GO"; - } else { - return "UNKNOWN"; - } - } - enum class TurnDirection { LEFT = 0, RIGHT, INVALID }; - /** - * @brief Manage stop-go states with safety margin time. - */ - class StateMachine - { - public: - StateMachine() - { - state_ = State::GO; - margin_time_ = 0.0; - } - void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock); - void setState(State state); - void setMarginTime(const double t); - State getState(); - - private: - State state_; //! current state - double margin_time_; //! margin time when transit to Go from Stop - std::shared_ptr start_time_; //! first time received GO when STOP state - }; - struct DebugData { autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; - geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 3cb169c981e0f..996e2d901d18c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -42,43 +43,6 @@ using TimeDistanceArray = std::vector>; class IntersectionModule : public SceneModuleInterface { public: - enum class State { - STOP = 0, - GO, - }; - std::string toString(const State & state) - { - if (state == State::STOP) { - return "STOP"; - } else if (state == State::GO) { - return "GO"; - } else { - return "UNKNOWN"; - } - } - - /** - * @brief Manage stop-go states with safety margin time. - */ - class StateMachine - { - public: - StateMachine() - { - state_ = State::GO; - margin_time_ = 0.0; - } - void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock); - void setState(State state); - void setMarginTime(const double t); - State getState(); - - private: - State state_; //! current state - double margin_time_; //! margin time when transit to Go from Stop - std::shared_ptr start_time_; //! first time received GO when STOP state - }; - struct DebugData { bool stop_required; 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 75b0a1304b37d..12112b7d33c08 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 @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -44,42 +45,6 @@ namespace behavior_velocity_planner class MergeFromPrivateRoadModule : public SceneModuleInterface { public: - enum class State { - STOP = 0, - GO, - }; - std::string toString(const State & state) - { - if (state == State::STOP) { - return "STOP"; - } else if (state == State::GO) { - return "GO"; - } else { - return "UNKNOWN"; - } - } - - /** - * @brief Manage stop-go states with safety margin time. - */ - class StateMachine - { - public: - StateMachine() - { - state_ = State::GO; - margin_time_ = 0.0; - } - void setState(State state); - void setMarginTime(const double t); - State getState(); - - private: - State state_; //! current state - double margin_time_; //! margin time when transit to Go from Stop - std::shared_ptr start_time_; //! first time received GO when STOP state - }; - struct DebugData { autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; diff --git a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp b/planning/behavior_velocity_planner/include/utilization/state_machine.hpp index 657fab46555d0..b0b78d458e78f 100644 --- a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp +++ b/planning/behavior_velocity_planner/include/utilization/state_machine.hpp @@ -33,7 +33,7 @@ class StateMachine STOP = 0, GO, }; - std::string toString(const State & state) + static std::string toString(const State & state) { if (state == State::STOP) { return "STOP"; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp index f550c43bffadb..c7f50215ee399 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp @@ -27,7 +27,6 @@ using tier4_autoware_utils::createMarkerScale; namespace { -using State = BlindSpotModule::State; visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const lanelet::CompoundPolygon3d & polygon, const std::string & ns, const int64_t lane_id, @@ -115,12 +114,12 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createPoseMarkerArray( - const geometry_msgs::msg::Pose & pose, const State & state, const std::string & ns, + const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns, const int64_t id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; - if (state == State::STOP) { + if (state == StateMachine::State::STOP) { visualization_msgs::msg::Marker marker_line{}; marker_line.header.frame_id = "map"; marker_line.ns = ns + "_line"; 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 4314723be3f3c..17c9374fac1f4 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 @@ -69,8 +69,9 @@ bool BlindSpotModule::modifyPathVelocity( const auto input_path = *path; debug_data_.path_raw = input_path; - State current_state = state_machine_.getState(); - RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str()); + StateMachine::State current_state = state_machine_.getState(); + RCLCPP_DEBUG( + logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); /* get current pose */ geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; @@ -133,7 +134,9 @@ bool BlindSpotModule::modifyPathVelocity( /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ if (planner_param_.use_pass_judge_line) { const double eps = 1e-1; // to prevent hunting - if (current_state == State::GO && *distance_until_stop + eps < pass_judge_line_dist) { + if ( + current_state == StateMachine::State::GO && + *distance_until_stop + eps < pass_judge_line_dist) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); *path = input_path; // reset path setSafe(true); @@ -149,10 +152,11 @@ bool BlindSpotModule::modifyPathVelocity( bool has_obstacle = checkObstacleInBlindSpot( lanelet_map_ptr, routing_graph_ptr, *path, objects_ptr, closest_idx, stop_line_pose); state_machine_.setStateWithMarginTime( - has_obstacle ? State::STOP : State::GO, logger_.get_child("state_machine"), *clock_); + has_obstacle ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); /* set stop speed */ - setSafe(state_machine_.getState() != State::STOP); + setSafe(state_machine_.getState() != StateMachine::State::STOP); setDistance(motion_utils::calcSignedArcLength( path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); if (!isActivated()) { @@ -580,43 +584,4 @@ lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( } return straight_lanelets; } - -void BlindSpotModule::StateMachine::setStateWithMarginTime( - State state, rclcpp::Logger logger, rclcpp::Clock & clock) -{ - /* same state request */ - if (state_ == state) { - start_time_ = nullptr; // reset timer - return; - } - - /* GO -> STOP */ - if (state == State::STOP) { - state_ = State::STOP; - start_time_ = nullptr; // reset timer - return; - } - - /* STOP -> GO */ - if (state == State::GO) { - if (start_time_ == nullptr) { - start_time_ = std::make_shared(clock.now()); - } else { - const double duration = (clock.now() - *start_time_).seconds(); - if (duration > margin_time_) { - state_ = State::GO; - start_time_ = nullptr; // reset timer - } - } - return; - } - - RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); -} - -void BlindSpotModule::StateMachine::setState(State state) { state_ = state; } - -void BlindSpotModule::StateMachine::setMarginTime(const double t) { margin_time_ = t; } - -BlindSpotModule::State BlindSpotModule::StateMachine::getState() { return state_; } } // namespace behavior_velocity_planner 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 685c3210ba1d5..734d0d444e216 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -24,7 +24,6 @@ namespace behavior_velocity_planner { namespace { -using State = IntersectionModule::State; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; @@ -249,7 +248,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.predicted_obj_pose, "predicted_obj_pose", lane_id_, 0.7, 0.85, 0.9), &debug_marker_array, current_time); - if (state == IntersectionModule::State::STOP) { + if (state == StateMachine::State::STOP) { appendMarkerArray( createPoseMarkerArray( debug_data_.stop_point_pose, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), @@ -276,7 +275,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker motion_utils::createStopVirtualWallMarker( debug_data_.stop_wall_pose, "intersection", now, lane_id_), &wall_marker, now); - } else if (state == IntersectionModule::State::STOP) { + } else if (state == StateMachine::State::STOP) { appendMarkerArray( motion_utils::createStopVirtualWallMarker( debug_data_.slow_wall_pose, "intersection", now, lane_id_), @@ -292,7 +291,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - if (state == MergeFromPrivateRoadModule::State::STOP) { + if (state == StateMachine::State::STOP) { appendMarkerArray( createPoseMarkerArray( debug_data_.stop_point_pose, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), @@ -309,7 +308,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createVirtualWa const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - if (state == MergeFromPrivateRoadModule::State::STOP) { + if (state == StateMachine::State::STOP) { appendMarkerArray( motion_utils::createStopVirtualWallMarker( debug_data_.virtual_wall_pose, "merge_from_private_road", now, lane_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 1e326f4975cd0..884b60ab67f8b 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 @@ -80,8 +80,9 @@ bool IntersectionModule::modifyPathVelocity( debug_data_.path_raw = *path; - State current_state = state_machine_.getState(); - RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str()); + StateMachine::State current_state = state_machine_.getState(); + RCLCPP_DEBUG( + logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); /* get current pose */ geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; @@ -202,11 +203,12 @@ bool IntersectionModule::modifyPathVelocity( is_entry_prohibited = true; } state_machine_.setStateWithMarginTime( - is_entry_prohibited ? State::STOP : State::GO, logger_.get_child("state_machine"), *clock_); + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - setSafe(state_machine_.getState() == State::GO); + setSafe(state_machine_.getState() == StateMachine::State::GO); setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position)); @@ -603,44 +605,6 @@ bool IntersectionModule::isTargetStuckVehicleType( } return false; } -void IntersectionModule::StateMachine::setStateWithMarginTime( - State state, rclcpp::Logger logger, rclcpp::Clock & clock) -{ - /* same state request */ - if (state_ == state) { - start_time_ = nullptr; // reset timer - return; - } - - /* GO -> STOP */ - if (state == State::STOP) { - state_ = State::STOP; - start_time_ = nullptr; // reset timer - return; - } - - /* STOP -> GO */ - if (state == State::GO) { - if (start_time_ == nullptr) { - start_time_ = std::make_shared(clock.now()); - } else { - const double duration = (clock.now() - *start_time_).seconds(); - if (duration > margin_time_) { - state_ = State::GO; - start_time_ = nullptr; // reset timer - } - } - return; - } - - RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); -} - -void IntersectionModule::StateMachine::setState(State state) { state_ = state; } - -void IntersectionModule::StateMachine::setMarginTime(const double t) { margin_time_ = t; } - -IntersectionModule::State IntersectionModule::StateMachine::getState() { return state_; } bool IntersectionModule::isTargetExternalInputStatus(const int target_status) { 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 02f992743ed33..6e3d3169a6d8d 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 @@ -40,7 +40,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) { planner_param_ = planner_param; - state_machine_.setState(State::STOP); + state_machine_.setState(StateMachine::State::STOP); } bool MergeFromPrivateRoadModule::modifyPathVelocity( @@ -54,8 +54,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( const auto input_path = *path; debug_data_.path_raw = input_path; - State current_state = state_machine_.getState(); - RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str()); + StateMachine::State current_state = state_machine_.getState(); + RCLCPP_DEBUG( + logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); /* get current pose */ geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; @@ -109,7 +110,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( } /* set stop speed */ - if (state_machine_.getState() == State::STOP) { + if (state_machine_.getState() == StateMachine::State::STOP) { constexpr double v = 0.0; planning_utils::setVelocityFromIndex(stop_line_idx, v, path); @@ -126,7 +127,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( if ( signed_arc_dist_to_stop_point < distance_threshold && planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { - state_machine_.setState(State::GO); + state_machine_.setState(StateMachine::State::GO); if (signed_arc_dist_to_stop_point < -distance_threshold) { RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO"); } @@ -179,13 +180,4 @@ MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( std::reverse(private_path.points.begin(), private_path.points.end()); return private_path; } - -void MergeFromPrivateRoadModule::StateMachine::setState(State state) { state_ = state; } - -void MergeFromPrivateRoadModule::StateMachine::setMarginTime(const double t) { margin_time_ = t; } - -MergeFromPrivateRoadModule::State MergeFromPrivateRoadModule::StateMachine::getState() -{ - return state_; -} } // namespace behavior_velocity_planner From 90fb5e1a2adc5e6f3125936f027ef32b233a5f3c Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Tue, 6 Sep 2022 13:52:24 +0900 Subject: [PATCH 21/28] fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded (#1772) * fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix(elevation_map_loader): add dependencies Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix(elevation_map_loader): use snake case for variable Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> --- perception/elevation_map_loader/package.xml | 1 + .../src/elevation_map_loader_node.cpp | 22 +++++++++++++++++-- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 282c4e7cdb446..3b99d2c10d319 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -21,6 +21,7 @@ pcl_conversions rclcpp rclcpp_components + rosbag2_storage_default_plugins tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index ee41c6febf805..4474fe2740467 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -44,6 +44,7 @@ #define EIGEN_MPL2_ONLY #include #include +#include ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options) : Node("elevation_map_loader", options) @@ -102,8 +103,25 @@ void ElevationMapLoaderNode::publish() RCLCPP_INFO( this->get_logger(), "Load elevation map from: %s", data_manager_.elevation_map_path_->c_str()); - grid_map::GridMapRosConverter::loadFromBag( - *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); + + // Check if bag can be loaded + bool is_bag_loaded = false; + try { + is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag( + *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); + } catch (rosbag2_storage_plugins::SqliteException & e) { + is_bag_loaded = false; + } + if (!is_bag_loaded) { + // Delete directory including elevation map if bag is broken + RCLCPP_ERROR( + this->get_logger(), "Try to loading bag, but bag is broken. Remove %s", + data_manager_.elevation_map_path_->c_str()); + std::filesystem::remove_all(data_manager_.elevation_map_path_->c_str()); + // Create elevation map from pointcloud map if bag is broken + RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map "); + createElevationMap(); + } } elevation_map_.setFrameId(map_frame_); From 5b30e5012b930e046062db0846e66b722de2f42f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 6 Sep 2022 14:20:12 +0900 Subject: [PATCH 22/28] fix(intersection): use constant velocity for smoother reference (#1721) Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.cpp | 47 ++++++++----------- 1 file changed, 19 insertions(+), 28 deletions(-) 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 884b60ab67f8b..312fcc9944186 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 @@ -477,35 +477,19 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const int objective_lane_id) const { - double closest_vel = - (std::max(1e-01, std::fabs(planner_data_->current_velocity->twist.linear.x))); + static constexpr double k_minimum_velocity = 1e-01; + double dist_sum = 0.0; int assigned_lane_found = false; + // crop intersection part of the path, and set the reference velocity to intersection_velocity for + // ego's ttc PathWithLaneId reference_path; - reference_path.points.push_back(path.points.at(closest_idx)); - reference_path.points.at(0).point.longitudinal_velocity_mps = closest_vel; - for (size_t i = closest_idx + 1; i < path.points.size(); ++i) { - const double dist = - tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i)); - dist_sum += dist; - // calc vel in idx i+1 (v_{i+1}^2 - v_{i}^2 = 2ax) - const double next_vel = std::min( - std::sqrt(std::pow(closest_vel, 2.0) + 2.0 * planner_param_.intersection_max_acc * dist), - planner_param_.intersection_velocity); - // calc average vel in idx i~i+1 - const double average_vel = - std::min((closest_vel + next_vel) / 2.0, planner_param_.intersection_velocity); - // passing_time += dist / average_vel; - // time_distance_array.emplace_back(passing_time, dist_sum); - auto reference_point = path.points[i]; - reference_point.point.longitudinal_velocity_mps = average_vel; + for (size_t i = closest_idx; i < path.points.size(); ++i) { + auto reference_point = path.points.at(i); + reference_point.point.longitudinal_velocity_mps = planner_param_.intersection_velocity; reference_path.points.push_back(reference_point); - - closest_vel = next_vel; - bool has_objective_lane_id = util::hasLaneId(path.points.at(i), objective_lane_id); - if (assigned_lane_found && !has_objective_lane_id) { break; } @@ -515,23 +499,30 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( return {{0.0, 0.0}}; // has already passed the intersection. } + // apply smoother to reference velocity PathWithLaneId smoothed_reference_path = reference_path; if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { RCLCPP_WARN(logger_, "smoothPath failed"); } + // calculate when ego is going to reach each (interpolated) points on the path TimeDistanceArray time_distance_array{}; dist_sum = 0.0; double passing_time = 0.0; time_distance_array.emplace_back(passing_time, dist_sum); for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - smoothed_reference_path.points.at(i - 1), smoothed_reference_path.points.at(i)); + const auto & p1 = smoothed_reference_path.points.at(i - 1); + const auto & p2 = smoothed_reference_path.points.at(i); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); dist_sum += dist; - // to avoid zero division + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; passing_time += - (dist / std::max( - 0.01, smoothed_reference_path.points.at(i - 1).point.longitudinal_velocity_mps)); + (dist / std::max(k_minimum_velocity, average_velocity)); // to avoid zero-division + time_distance_array.emplace_back(passing_time, dist_sum); } RCLCPP_DEBUG(logger_, "intersection dist = %f, passing_time = %f", dist_sum, passing_time); From 165f29efd6f35c61c6cd42ef854cf4f46289d91d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 6 Sep 2022 15:13:21 +0900 Subject: [PATCH 23/28] fix(behavior_path_planner): fix pull_over request_length and maximum_deceleration (#1789) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/pull_over.param.yaml | 4 ++-- planning/behavior_path_planner/README.md | 18 +++++++++--------- .../config/pull_over/pull_over.param.yaml | 2 +- .../src/behavior_path_planner_node.cpp | 4 ++-- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index baa35d2a88062..8aba3d305c464 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: pull_over: - request_length: 100.0 + request_length: 200.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -9,7 +9,7 @@ pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 decide_path_distance: 10.0 - maximum_deceleration: 0.5 + maximum_deceleration: 1.0 # goal research enable_goal_research: true search_priority: "efficient_path" # "efficient_path" or "close_goal" diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a2b299284cfad..9d65317a11cb7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -198,7 +198,7 @@ The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle - The goal is in shoulder lane and the ego-vehicle is in road lane. - The distance between the goal and ego-vehicle is somewhat close. - - It is shorter than `request_length`(default: < `100m`). + - It is shorter than `request_length`(default: < `200m`). - Pull over ready condition @@ -220,7 +220,7 @@ The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle | Name | Unit | Type | Description | Default value | | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| request_length | [m] | double | when the ego-vehicle approaches the goal by this distance, the module is activated. | 100.0 | +| request_length | [m] | double | when the ego-vehicle approaches the goal by this distance, the module is activated. | 200.0 | | th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | | th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | | th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | @@ -228,7 +228,7 @@ The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle | pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 0.3 | | margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | | decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 0.5 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | #### **collision check** @@ -322,12 +322,12 @@ Generate two forward arc paths. ###### Parameters arc forward parking -| Name | Unit | Type | Description | Default value | -| :-------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------- | :------------ | -| enable_arc_forward_parking | [-] | bool | flag whether to enable arc forward parking | true | -| after_forward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | -| forward_parking_velocity | [m/s] | double | velocity when forward parking | 0.3 | -| forward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front left corner of ego-vehicle when forkward parking | 0.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------ | :------------ | +| enable_arc_forward_parking | [-] | bool | flag whether to enable arc forward parking | true | +| after_forward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| forward_parking_velocity | [m/s] | double | velocity when forward parking | 0.3 | +| forward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front left corner of ego-vehicle when forward parking | 0.0 | ###### arc backward parking diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index baa35d2a88062..2655383eda650 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -9,7 +9,7 @@ pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 decide_path_distance: 10.0 - maximum_deceleration: 0.5 + maximum_deceleration: 1.0 # goal research enable_goal_research: true search_priority: "efficient_path" # "efficient_path" or "close_goal" 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 d0c8d593a56da..37e3424efcf9f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -364,13 +364,13 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() }; PullOverParameters p; - p.request_length = dp("request_length", 100.0); + p.request_length = dp("request_length", 200.0); p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); p.th_arrived_distance = dp("th_arrived_distance", 0.3); p.th_stopped_time = dp("th_stopped_time", 2.0); p.margin_from_boundary = dp("margin_from_boundary", 0.3); p.decide_path_distance = dp("decide_path_distance", 10.0); - p.maximum_deceleration = dp("maximum_deceleration", 0.5); + p.maximum_deceleration = dp("maximum_deceleration", 1.0); // goal research p.enable_goal_research = dp("enable_goal_research", true); p.search_priority = dp("search_priority", "efficient_path"); From 61ac50bff83a611d26c1e75b64d7543d9252ea7a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 6 Sep 2022 16:58:39 +0900 Subject: [PATCH 24/28] refactor(tier4_screen_capture_rviz_plugin):apply clang-tidy (#1649) Signed-off-by: h-ohta Signed-off-by: h-ohta --- .../src/screen_capture_panel.cpp | 12 +++++------- .../src/screen_capture_panel.hpp | 12 ++++++------ 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp index e7500161237fb..a5e6868247daf 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp @@ -26,7 +26,7 @@ using std::placeholders::_2; void setFormatDate(QLabel * line, double time) { char buffer[128]; - time_t seconds = static_cast(time); + auto seconds = static_cast(time); strftime(buffer, sizeof(buffer), "%Y-%m-%d-%H-%M-%S", localtime(&seconds)); line->setText(QString(buffer)); } @@ -71,7 +71,7 @@ AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) v_layout->addLayout(video_cap_layout); setLayout(v_layout); } - QTimer * timer = new QTimer(this); + auto * timer = new QTimer(this); connect(timer, &QTimer::timeout, this, &AutowareScreenCapturePanel::update); timer->start(1000); capture_timer_ = new QTimer(this); @@ -80,8 +80,8 @@ AutowareScreenCapturePanel::AutowareScreenCapturePanel(QWidget * parent) } void AutowareScreenCapturePanel::onCaptureTrigger( - [[maybe_unused]] const std::shared_ptr req, - const std::shared_ptr res) + [[maybe_unused]] const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res) { onClickVideoCapture(); res->success = true; @@ -103,7 +103,6 @@ void AutowareScreenCapturePanel::onClickScreenCapture() const std::string time_text = "capture/" + ros_time_label_->text().toStdString(); getDisplayContext()->getViewManager()->getRenderPanel()->getRenderWindow()->captureScreenShot( time_text + ".png"); - return; } void AutowareScreenCapturePanel::onClickVideoCapture() @@ -112,7 +111,7 @@ void AutowareScreenCapturePanel::onClickVideoCapture() try { const QWidgetList top_level_widgets = QApplication::topLevelWidgets(); for (QWidget * widget : top_level_widgets) { - QMainWindow * main_window_candidate = qobject_cast(widget); + auto * main_window_candidate = qobject_cast(widget); if (main_window_candidate) { main_window_ = main_window_candidate; } @@ -153,7 +152,6 @@ void AutowareScreenCapturePanel::onClickVideoCapture() state_ = State::WAITING_FOR_CAPTURE; break; } - return; } void AutowareScreenCapturePanel::onTimer() diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp index b9178ab91f41a..a6e78acf29b21 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.hpp @@ -53,7 +53,7 @@ class AutowareScreenCapturePanel : public rviz_common::Panel public: explicit AutowareScreenCapturePanel(QWidget * parent = nullptr); - ~AutowareScreenCapturePanel(); + ~AutowareScreenCapturePanel() override; void update(); void onInitialize() override; void createWallTimer(); @@ -61,8 +61,8 @@ class AutowareScreenCapturePanel : public rviz_common::Panel void save(rviz_common::Config config) const override; void load(const rviz_common::Config & config) override; void onCaptureTrigger( - const std::shared_ptr req, - const std::shared_ptr res); + const std_srvs::srv::Trigger::Request::SharedPtr req, + const std_srvs::srv::Trigger::Response::SharedPtr res); public Q_SLOTS: void onClickScreenCapture(); @@ -75,16 +75,16 @@ public Q_SLOTS: QPushButton * capture_to_mp4_button_ptr_; QSpinBox * capture_hz_; QTimer * capture_timer_; - QMainWindow * main_window_; + QMainWindow * main_window_{nullptr}; enum class State { WAITING_FOR_CAPTURE, CAPTURING }; State state_; std::string capture_file_name_; - bool is_capture_; + bool is_capture_{false}; cv::VideoWriter writer_; cv::Size current_movie_size_; std::vector image_vec_; - std::string stateToString(const State & state) + static std::string stateToString(const State & state) { if (state == State::WAITING_FOR_CAPTURE) { return "waiting for capture"; From 8c198afc50e2c79293536ea7d64cf19a6ae2a00e Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Tue, 6 Sep 2022 18:43:04 +0900 Subject: [PATCH 25/28] fix(ekf_localizer): rename biased pose topics (#1787) * fix(ekf_localizer): rename biased pose topics * Update topic descriptions in README Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../pose_estimator/pose_estimator.launch.xml | 2 +- .../pose_twist_fusion_filter.launch.xml | 4 ++-- localization/ekf_localizer/README.md | 8 ++++---- .../include/ekf_localizer/ekf_localizer.hpp | 7 +++---- .../launch/ekf_localizer.launch.xml | 8 ++++---- .../ekf_localizer/src/ekf_localizer.cpp | 19 +++++++++---------- 6 files changed, 23 insertions(+), 25 deletions(-) diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml index 5d9e5a65cc720..750adca4c2fa0 100644 --- a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index f37a5362f2c23..3c109769e13ba 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -11,8 +11,8 @@ - - + + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index baa770d7dcf9f..f77e9b263a389 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -67,13 +67,13 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi Estimated pose with covariance. -- ekf_pose_with_covariance (geometry_msgs/PoseStamped) +- ekf_biased_pose (geometry_msgs/PoseStamped) - Estimated pose without yawbias effect. + Estimated pose including the yaw bias -- ekf_pose_with_covariance_without_yawbias (geometry_msgs/PoseWithCovarianceStamped) +- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - Estimated pose with covariance without yawbias effect. + Estimated pose with covariance including the yaw bias - ekf_twist (geometry_msgs/TwistStamped) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0c3fa0ae8c3f1..508e9740c0141 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -130,10 +130,9 @@ class EKFLocalizer : public rclcpp::Node //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_pose_no_yawbias_; + rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr - pub_pose_cov_no_yawbias_; + rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -203,7 +202,7 @@ class EKFLocalizer : public rclcpp::Node std::queue current_pose_info_queue_; //!< @brief current measured pose geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose geometry_msgs::msg::PoseStamped - current_ekf_pose_no_yawbias_; //!< @brief current estimated pose w/o yaw bias + current_biased_ekf_pose_; //!< @brief current estimated pose without yaw bias correction geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist std::array current_pose_covariance_; std::array current_twist_covariance_; diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index c122cdc895d31..9ec05bae8a864 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -29,8 +29,8 @@ - - + + @@ -66,8 +66,8 @@ - - + + diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 00f6b760efaef..2a2f60383042a 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -89,10 +89,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_twist_cov_ = create_publisher( "ekf_twist_with_covariance", 1); pub_yaw_bias_ = create_publisher("estimated_yaw_bias", 1); - pub_pose_no_yawbias_ = - create_publisher("ekf_pose_without_yawbias", 1); - pub_pose_cov_no_yawbias_ = create_publisher( - "ekf_pose_with_covariance_without_yawbias", 1); + pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); + pub_biased_pose_cov_ = create_publisher( + "ekf_biased_pose_with_covariance", 1); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -229,8 +228,8 @@ void EKFLocalizer::setCurrentResult() current_ekf_pose_.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); - current_ekf_pose_no_yawbias_ = current_ekf_pose_; - current_ekf_pose_no_yawbias_.pose.orientation = + current_biased_ekf_pose_ = current_ekf_pose_; + current_biased_ekf_pose_.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW)); current_ekf_twist_.header.frame_id = "base_link"; @@ -661,7 +660,7 @@ void EKFLocalizer::publishEstimateResult() /* publish latest pose */ pub_pose_->publish(current_ekf_pose_); - pub_pose_no_yawbias_->publish(current_ekf_pose_no_yawbias_); + pub_biased_pose_->publish(current_biased_ekf_pose_); /* publish latest pose with covariance */ geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; @@ -679,9 +678,9 @@ void EKFLocalizer::publishEstimateResult() pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW); pub_pose_cov_->publish(pose_cov); - geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_no_yawbias = pose_cov; - pose_cov_no_yawbias.pose.pose = current_ekf_pose_no_yawbias_.pose; - pub_pose_cov_no_yawbias_->publish(pose_cov_no_yawbias); + geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; + biased_pose_cov.pose.pose = current_biased_ekf_pose_.pose; + pub_biased_pose_cov_->publish(biased_pose_cov); /* publish latest twist */ pub_twist_->publish(current_ekf_twist_); From fcf812b72b621713dd3d3067ce41f833ba19c39d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 6 Sep 2022 19:17:24 +0900 Subject: [PATCH 26/28] refactor(obstacle_stop_planner): update params name for readability (#1720) * refactor(obstacle_stop_planner): update parameter name for readability Signed-off-by: satoshi-ota * docs(obstacle_stop_planner): update module documentation Signed-off-by: satoshi-ota * docs(obstacle_stop_planner): update figure Signed-off-by: satoshi-ota * refactor(obstacle_stop_planner): separate params by namespace Signed-off-by: satoshi-ota * fix(tier4_planning_launch): separate params by namespace Signed-off-by: satoshi-ota * refactor(obstacle_stop_planner): remove default value from declare_parameter Signed-off-by: satoshi-ota * refactor(obstacle_stop_planner): add params to config Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../obstacle_stop_planner.param.yaml | 64 ++- planning/obstacle_stop_planner/README.md | 200 +++++----- .../config/obstacle_stop_planner.param.yaml | 65 ++- .../docs/collision_parameters.svg | 3 + .../docs/insert_decel_velocity.drawio.svg | 4 - .../docs/insert_velocity.drawio.svg | 208 ---------- .../docs/insert_velocity1.drawio.svg | 4 - .../docs/insert_velocity2.drawio.svg | 4 - .../docs/min_longitudinal_margin.svg | 3 + .../docs/point_cloud.drawio.svg | 174 -------- .../docs/point_cloud_decel.drawio.svg | 175 -------- .../docs/slow_down_parameters.svg | 3 + .../docs/slow_down_target.svg | 3 + .../docs/stop_target.svg | 3 + .../docs/vehicle_shape.drawio.svg | 225 ----------- .../docs/vehicle_shape_decel.drawio.svg | 376 ------------------ .../docs/velocity_limitation.drawio.svg | 270 ------------- .../include/obstacle_stop_planner/node.hpp | 87 +--- .../obstacle_stop_planner/planner_data.hpp | 247 ++++++++++++ planning/obstacle_stop_planner/src/node.cpp | 170 ++++---- 20 files changed, 554 insertions(+), 1734 deletions(-) create mode 100644 planning/obstacle_stop_planner/docs/collision_parameters.svg delete mode 100644 planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg delete mode 100644 planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg delete mode 100644 planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg delete mode 100644 planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg delete mode 100644 planning/obstacle_stop_planner/docs/point_cloud.drawio.svg delete mode 100644 planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg create mode 100644 planning/obstacle_stop_planner/docs/slow_down_parameters.svg create mode 100644 planning/obstacle_stop_planner/docs/slow_down_target.svg create mode 100644 planning/obstacle_stop_planner/docs/stop_target.svg delete mode 100644 planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg delete mode 100644 planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg delete mode 100644 planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg create mode 100644 planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml index 82a77203343f7..55a21d6cd7b20 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -1,25 +1,49 @@ /**: ros__parameters: + hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + lowpass_gain: 0.9 # gain parameter for low pass filter [-] + max_velocity: 20.0 # max velocity [m/s] + enable_slow_down: False # whether to use slow down planner [-] + stop_planner: - stop_margin: 5.0 # stop margin distance from obstacle on the path [m] - min_behavior_stop_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] - step_length: 1.0 # step length for pointcloud search range [m] - extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance - expand_stop_range: 0.0 # margin of vehicle footprint [m] + # params for stop position + stop_position: + max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] + hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] + + # params for detection area + detection_area: + lateral_margin: 0.0 # margin of vehicle footprint [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] + slow_down_planner: - # slow down planner parameters - forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] - backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m] - expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] - max_slow_down_vel: 1.38 # max slow down velocity [m/s] - min_slow_down_vel: 0.28 # min slow down velocity [m/s] - - # slow down constraint parameters - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel - forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s] - forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s] - jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss] - jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] - jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] - slow_down_vel: 1.38 # target slow down velocity [m/s] + # params for slow down section + slow_down_section: + longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + longitudinal_backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m] + longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + + # params for detection area + detection_area: + lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + + # params for velocity + target_velocity: + max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] + min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] + slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] + + # params for deceleration constraints (use this param if consider_constraints is True) + constraints: + jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] + + # others + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] + velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] + acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index be62447684982..3b0a38d194a78 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -11,22 +11,7 @@ - Adaptive Cruise Controller (ACC) - embeds target velocity in trajectory when there is a dynamic point cloud on the trajectory. -In order to stop with a `stop margin` from the obstacle exists, the stop point (`v=0`) is inserted at a distance of `baselink to front` + `stop margin` from the obstacle. The `baselink to front` means the distance between `base_link`(center of rear-wheel axis) and front of the car. - -If a stop point has already been inserted by other nodes between the obstacle and a position which is `stop margin` meters away from the obstacle, the stop point is inserted at a distance of `baselink to front` + `min behavior stop margin` from the obstacle. - -
- - -
- -When the deceleration section is inserted, the start point of the section is inserted in front of the target point cloud by the distance of `baselink to front` + `slow down forward margin`. the end point of the section is inserted behind the target point cloud by the distance of `slow down backward margin` + `baselink to rear`. The `baselink to rear` means the distance between `base_link` and rear of the car. The velocities of points in the deceleration section are modified to the deceleration velocity. `slow down backward margin` and `slow down forward margin` are determined by the parameters described below. - -
- -
- -## Input topics +### Input topics | Name | Type | Description | | --------------------------- | ----------------------------------------------- | ------------------- | @@ -37,40 +22,87 @@ When the deceleration section is inserted, the start point of the section is ins | `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | | `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | -## Output topics +### Output topics | Name | Type | Description | | ---------------------- | --------------------------------------- | -------------------------------------- | | `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | | `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | -## Modules - ### Common Parameter -| Parameter | Type | Description | -| ------------------- | ------ | ---------------------------------------------------------------------------------------- | -| `enable_slow_down` | bool | enable slow down planner [-] | -| `max_velocity` | double | max velocity [m/s] | -| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | -| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | +| Parameter | Type | Description | +| ------------------- | ------ | -------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | +| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | + +## Obstacle Stop Planner + +### Role + +This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `baselink`(center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points for reducing computational costs.) + +
+ ![example](./docs/collision_parameters.svg){width=1000} +
parameters for obstacle stop planner
+
+ +
+ ![example](./docs/stop_target.svg){width=1000} +
target for obstacle stop planner
+
+ +If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.) + +
+ ![example](./docs/min_longitudinal_margin.svg){width=1000} +
minimum longitudinal margin
+
+ +The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. + +### Restart prevention + +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). + +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module, the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. + +
+ ![example](./docs/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](./docs/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](./docs/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
-### Obstacle Stop Planner +### Parameters -#### Role +#### Stop position -`Obstacle Stop Planner` module inserts a stop point in trajectory when there is a static point cloud on the trajectory. This module does not work when `Adaptive Cruise Controller` works. +| Parameter | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- | +| `max_longitudinal_margin` | double | margin between obstacle and the ego's front [m] | +| `min_longitudinal_margin` | double | if any obstacle exists within `max_longitudinal_margin`, this module set margin as the value of _stop margin_ to `min_longitudinal_margin` [m] | +| `hold_stop_margin_distance` | double | parameter for restart prevention (See above section) [m] | -| Parameter | Type | Description | -| ---------------------------------------- | ------ | ----------------------------------------------------------------------------- | -| `stop_planner.stop_margin` | double | stop margin distance from obstacle on the path [m] | -| `stop_planner.min_behavior_stop_margin` | double | stop margin distance when any other stop point is inserted in stop margin [m] | -| `stop_planner.step_length` | double | step length for pointcloud search range [m] | -| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] | -| `stop_planner.expand_stop_range` | double | margin of vehicle footprint [m] | -| `stop_planner.hold_stop_margin_distance` | double | parameter for restart prevention (See following section) [m] | +#### Obstacle detection area -#### Flowchart +| Parameter | Type | Description | +| ----------------- | ------ | ----------------------------------------------------------------------------------- | +| `lateral_margin` | double | lateral margin from the vehicle footprint for collision obstacle detection area [m] | +| `step_length` | double | step length for pointcloud search range [m] | +| `extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] | + +### Flowchart ```plantuml @startuml @@ -103,52 +135,56 @@ stop @enduml ``` -First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs. +## Slow Down Planner -Then, a detection area is generated by the decimated trajectory as following figure. The detection area means the area through which the vehicle-body passes. +### Role -![vehicle_shape](./docs/vehicle_shape.drawio.svg) +This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation. -The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity. +$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$ -![pointcloud](./docs/point_cloud.drawio.svg) +- $v_{target}$ : slow down target velocity [m/s] +- $v_{min}$ : `min_slow_down_velocity` [m/s] +- $v_{max}$ : `max_slow_down_velocity` [m/s] +- $l_{ld}$ : lateral deviation between the obstacle and the ego footprint [m] +- $l_{margin}$ : `lateral_margin` [m] +- $l_{vw}$ : width of the ego footprint [m] -#### Restart prevention - -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). - -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section.
- ![example](./docs/restart_prevention.svg){width=1000} -
parameters
+ ![example](./docs/slow_down_parameters.svg){width=1000} +
parameters for slow down planner
- ![example](./docs/restart.svg){width=1000} -
outside the hold_stop_margin_distance
+ ![example](./docs/slow_down_target.svg){width=1000} +
target for slow down planner
-
- ![example](./docs/keep_stopping.svg){width=1000} -
inside the hold_stop_margin_distance
-
+### Parameters + +#### Slow down section -### Slow Down Planner +| Parameter | Type | Description | +| ------------------------------ | ------ | ----------------------------------------------- | +| `longitudinal_forward_margin` | double | margin between obstacle and the ego's front [m] | +| `longitudinal_backward_margin` | double | margin between obstacle and the ego's rear [m] | -#### Role +#### Obstacle detection area -`Slow Down Planner` module inserts a deceleration point in trajectory when there is a point cloud near the trajectory. +| Parameter | Type | Description | +| ---------------- | ------ | ----------------------------------------------------------------------------------- | +| `lateral_margin` | double | lateral margin from the vehicle footprint for slow down obstacle detection area [m] | -| Parameter | Type | Description | -| --------------------------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `slow_down_planner.slow_down_forward_margin` | double | margin distance from slow down point to vehicle front [m] | -| `slow_down_planner.slow_down_backward_margin` | double | margin distance from slow down point to vehicle rear [m] | -| `slow_down_planner.expand_slow_down_range` | double | offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] | -| `slow_down_planner.max_slow_down_vel` | double | max slow down velocity [m/s] | -| `slow_down_planner.min_slow_down_vel` | double | min slow down velocity [m/s] | +#### Slow down target velocity -#### Flowchart +| Parameter | Type | Description | +| ------------------------ | ------ | ---------------------------- | +| `max_slow_down_velocity` | double | max slow down velocity [m/s] | +| `min_slow_down_velocity` | double | min slow down velocity [m/s] | + +### Flowchart ```plantuml @startuml @@ -176,35 +212,9 @@ stop @enduml ``` -First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs. ( This is the same process as that of `Obstacle Stop planner` module. ) - -Then, a detection area is generated by the decimated trajectory as following figure. The detection area in this module is the extended area of the detection area used in `Obstacle Stop Planner` module. The distance to be extended depends on the above parameter `expand_slow_down_range`. - -![vehicle_shape_decel](./docs/vehicle_shape_decel.drawio.svg) - -The module searches the obstacle pointcloud within detection area. When the pointcloud is found, the deceleration point is inserted to the trajectory. - -![pointcloud_decel](./docs/point_cloud_decel.drawio.svg) - -The deceleration point means the point with low velocity; the value of the velocity $v_{target}$ is determined as follows. - -$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{er}} (v_{max} - v_{min} )$ - -- $v_{min}$ is minimum target value of `Slow Down Planner` module. The value of $v_{min}$ depends on the parameter `min_slow_down_vel`. -- $v_{max}$ is maximum target value of `Slow Down Planner` module. The value of $v_{max}$ depends on the parameter `max_slow_down_vel`. -- $l_{ld}$ is the lateral deviation of the target pointcloud. -- $l_{vw}$ is the vehicle width. -- $l_{er}$ is the expand range of detection area. The value of $l_{er}$ depends on the parameter `expand_slow_down_range` - -The above method means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the deceleration point. - - - - - -### Adaptive Cruise Controller +## Adaptive Cruise Controller -#### Role +### Role `Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car). @@ -241,7 +251,7 @@ The above method means that the smaller the lateral deviation of the pointcloud, | `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed | | `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value | -#### Flowchart +### Flowchart ```plantuml @startuml diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index 6fe30b216118e..ed4e632ebd6b3 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,28 +1,49 @@ /**: ros__parameters: - hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + lowpass_gain: 0.9 # gain parameter for low pass filter [-] + max_velocity: 20.0 # max velocity [m/s] + enable_slow_down: False # whether to use slow down planner [-] stop_planner: - stop_margin: 5.0 # stop margin distance from obstacle on the path [m] - min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] - step_length: 1.0 # step length for pointcloud search range [m] - extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance - expand_stop_range: 0.0 # margin of vehicle footprint [m] - hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] + # params for stop position + stop_position: + max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_longitudinal_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] + hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] + + # params for detection area + detection_area: + lateral_margin: 0.0 # margin of vehicle footprint [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] + slow_down_planner: - # slow down planner parameters - forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] - backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m] - expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] - max_slow_down_vel: 1.38 # max slow down velocity [m/s] - min_slow_down_vel: 0.28 # min slow down velocity [m/s] - - # slow down constraint parameters - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel - forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s] - forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s] - jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss] - jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] - jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] - slow_down_vel: 1.38 # target slow down velocity [m/s] + # params for slow down section + slow_down_section: + longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + longitudinal_backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m] + longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + + # params for detection area + detection_area: + lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + + # params for velocity + target_velocity: + max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] + min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] + slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] + + # params for deceleration constraints (use this param if consider_constraints is True) + constraints: + jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] + + # others + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] + velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] + acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/planning/obstacle_stop_planner/docs/collision_parameters.svg b/planning/obstacle_stop_planner/docs/collision_parameters.svg new file mode 100644 index 0000000000000..a1f3c346bd305 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/collision_parameters.svg @@ -0,0 +1,3 @@ + + +
Obstacle
Obsta...
velocity
veloc...
baselink_to_front [m]
baselink_to_front [m]
max_longitudinal_margin [m]
max_longitudinal_margin [m]
reference velocity
reference velocity
output velocity
output velocity
travel
distance
trave...
trajectory
trajectory
Ego
Ego
lateral_margin [m]
lateral_margin [m]
vehicle_width / 2 + lateral_margin [m]
vehicle_width / 2 + lateral_margin...
detection area for collision obstacles
detection area for collision obstacl...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg b/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg deleted file mode 100644 index a49853974a2f9..0000000000000 --- a/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg +++ /dev/null @@ -1,4 +0,0 @@ - - - -
Obstacle
Obsta...
velocity
velocity
travel
distance
travel...
baselink to front
baselink to front
slow down forward margin
slow down forward margin
reference velocity
reference velocity
output velocity
output velocity
slow down backward margin
slow down backwa...
baselink to rear
baselink to rear
slow down
velocity
slow down...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg deleted file mode 100644 index bba4ad8a79115..0000000000000 --- a/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg +++ /dev/null @@ -1,208 +0,0 @@ - - - - - - - - - - - -
-
-
- -

- - - stop point - - -

- -
-
-
- - - stop point - - - - - - - - - - - - -
-
-
- -

- - - v - - -

- -
-
-
- - - v - - - - - - - -
-
-
- -

- - - x - - -

- -
-
-
- - - x - - - - - - - -
-
-
- -

- - - baselink to front - - -

- -
-
-
- - - baselink to front - - - - - - - - - - - - - - - -
-
-
- -

- - - output velocity - - -

- -
-
-
- - - output velocity - - - - - - - - -
-
-
- -

- - - reference velocity - - -

- -
-
-
- - - reference velocity - - - - - - - - -
-
-
- -

- - stop margin - -

- -
-
-
- - - stop margin - - - - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg deleted file mode 100644 index dd1054e17fb14..0000000000000 --- a/planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg +++ /dev/null @@ -1,4 +0,0 @@ - - - -
Obstacle
Obsta...
velocity
velocity
baselink to front
baselink to front
stop margin
stop margin
reference velocity
reference velocity
output velocity
output velocity
travel
distance
travel...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg deleted file mode 100644 index 941668f909b60..0000000000000 --- a/planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg +++ /dev/null @@ -1,4 +0,0 @@ - - - -
Obstacle
Obsta...
velocity
velocity
baselink to front
baselink to front
min behavior stop margin
min behavior stop margin
reference velocity
reference velocity
output velocity
output velocity
travel
distance
travel...
stop point
(inserted by other nodes)
stop point...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg b/planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg new file mode 100644 index 0000000000000..fb42e7f03a641 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg @@ -0,0 +1,3 @@ + + +
Obstacle
Obsta...
velocity
veloc...
baselink_to_front [m]
baselink_to_front [m]
min_longitudinal_margin [m]
min_longitudinal_margin [m]
reference velocity
reference velocity
output velocity
output velocity
travel
distance
trave...
reference trajectory already has stop point
reference trajectory alrea...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg b/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg deleted file mode 100644 index b7f2e2c5bcf45..0000000000000 --- a/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg +++ /dev/null @@ -1,174 +0,0 @@ - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - - - - - - - - - - - - - - -
-
-
- obstacle point cloud -
-
-
- - - obstacle point cloud - - - - - - - -
-
-
- stop point -
-
-
- - - stop point - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg b/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg deleted file mode 100644 index e0bd3001607b6..0000000000000 --- a/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg +++ /dev/null @@ -1,175 +0,0 @@ - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - - - - - - - - - - - - - -
-
-
- obstacle point cloud -
-
-
- - - obstacle point cloud - - - - - - - -
-
-
- deceleration point -
-
-
- - - deceleration point - - - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/slow_down_parameters.svg b/planning/obstacle_stop_planner/docs/slow_down_parameters.svg new file mode 100644 index 0000000000000..dcfcd62fc3a92 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/slow_down_parameters.svg @@ -0,0 +1,3 @@ + + +
trajectory
trajectory
Ego
Ego
lateral_margin [m]
lateral_margin [m]
vehicle_width / 2 + lateral_margin [m]
vehicle_width / 2 + lateral_margin [...
detection area for slow down obstacles
detection area for slow down obstacl...
Obstacle
Obsta...
velocity
veloc...
travel
distance
trave...
longitudinal_forward_margin [m]
longitudinal_forward_margin [m]
reference velocity
reference velocity
output velocity
output velocity
longitudinal_backward_margin [m]
longitudinal_backward_margin [m]
baselink_to_rear [m]
baselink_to_rear [m]
slow_down_velocity [m/s]
slow_down_velocity [m/s]
baselink_to_front [m]
baselink_to_front [m]Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/slow_down_target.svg b/planning/obstacle_stop_planner/docs/slow_down_target.svg new file mode 100644 index 0000000000000..203d703936aa6 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/slow_down_target.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
detection area for slow down obstacles
detection area for slow down obstacl...
Target
Target
Not-target
(Target for stop planner)
Not-t...
Not-Target
Not-T...
Target
Target
detection area for collision obstacles
detection area for collision obstacl...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/stop_target.svg b/planning/obstacle_stop_planner/docs/stop_target.svg new file mode 100644 index 0000000000000..838321bfd8196 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/stop_target.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
detection area for collision obstacles
detection area for collision obstacl...
Target
Target
Not-target
Not-t...
Not-Target
Not-T...
Target
TargetText is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg b/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg deleted file mode 100644 index 677e43bed4448..0000000000000 --- a/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg +++ /dev/null @@ -1,225 +0,0 @@ - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Reference Trajectory -
-
-
- - - Reference Trajectory - - - - - - -
-
-
- Detection Area for obstacle stop -
-
-
- - - Detection Area for obstacle st... - - - - - - -
-
-
- Vehicle Shape -
-
-
- - - Vehicle Shape - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg b/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg deleted file mode 100644 index 9847751a317e4..0000000000000 --- a/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg +++ /dev/null @@ -1,376 +0,0 @@ - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - -
-
-
- - - - -
-
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Reference Trajectory -
-
-
- - - Reference Trajectory - - - - - - -
-
-
- (Detection Area for obstacle stop) -
-
-
- - - (Detection Area for obstacle st... - - - - - - -
-
-
- Vehicle Shape -
-
-
- - - Vehicle Shape - - - - - - - -
-
-
- Detection Area for slow down -
-
-
- - - Detection Area for slow down - - - - - - - - - - -
-
-
- expand slow down range -
-
-
- - - expand slow down range - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg b/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg deleted file mode 100644 index e63c63d1ee1da..0000000000000 --- a/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg +++ /dev/null @@ -1,270 +0,0 @@ - - - - - - - - - - - -
-
-
- -

- - - deceleration point - - -

- -
-
-
- - - deceleration point - - - - - - - - - - - - -
-
-
- -

- - - v - - -

- -
-
-
- - - v - - - - - - - -
-
-
- -

- - - x - - -

- -
-
-
- - - x - - - - - - - -
-
-
- -

- - - baselink to front - - -

- -
-
-
- - - baselink to front - - - - - - - - - - - - - - -
-
-
- -

- - - output velocity - - -

- -
-
-
- - - output velocity - - - - - - - - -
-
-
- -

- - - reference velocity - - -

- -
-
-
- - - reference velocity - - - - - - - - -
-
-
- -

- - stop margin - -

- -
-
-
- - - stop margin - - - - - - - - - - - -
-
-
- v -
-
-
- - - v - - - - - - -
-
-
- - target - -
-
-
- - - targ... - - - - - - - - -
-
-
- -

- - - velocity limitation - - -

- -
-
-
- - - velocity limitation - - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 7d63be90db608..1294e6dccd5a1 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -17,6 +17,7 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" +#include "obstacle_stop_planner/planner_data.hpp" #include #include @@ -76,97 +77,11 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; -struct StopPoint -{ - TrajectoryPoint point{}; - size_t index; -}; - -struct SlowDownSection -{ - TrajectoryPoint start_point{}; - TrajectoryPoint end_point{}; - size_t slow_down_start_idx; - size_t slow_down_end_idx; - double velocity; -}; - class ObstacleStopPlannerNode : public rclcpp::Node { public: explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options); - struct NodeParam - { - bool enable_slow_down; // set True, slow down for obstacle beside the path - double max_velocity; // max velocity [m/s] - double lowpass_gain; // smoothing calculated current acceleration [-] - double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] - double ego_nearest_dist_threshold; // dist threshold for ego's nearest index - double ego_nearest_yaw_threshold; // yaw threshold for ego's nearest index - }; - - struct StopParam - { - double stop_margin; // stop margin distance from obstacle on the path [m] - double min_behavior_stop_margin; // margin distance, any other stop point is inserted [m] - double expand_stop_range; // margin of vehicle footprint [m] - double extend_distance; // trajectory extend_distance [m] - double step_length; // step length for pointcloud search range [m] - double stop_search_radius; // search radius for obstacle point cloud [m] - double hold_stop_margin_distance; // keep stopping if the ego is in this margin [m] - }; - - struct SlowDownParam - { - double normal_min_jerk; // min jerk limit for mild stop [m/sss] - double normal_min_acc; // min deceleration limit for mild stop [m/ss] - double limit_min_jerk; // min jerk limit [m/sss] - double limit_min_acc; // min deceleration limit [m/ss] - double forward_margin; // slow down margin(vehicle front -> obstacle) [m] - double backward_margin; // slow down margin(obstacle vehicle rear) [m] - double expand_slow_down_range; // lateral range of detection area [m] - double max_slow_down_vel; // maximum speed in slow down section [m/s] - double min_slow_down_vel; // minimum velocity in slow down section [m/s] - bool consider_constraints; // set "True", decel point is planned under jerk/dec constraints - double slow_down_vel; // target slow down velocity [m/s] - double forward_margin_min; // min margin for relaxing slow down margin [m/s] - double forward_margin_span; // fineness param for relaxing slow down margin [m/s] - double slow_down_min_jerk; // min slow down jerk constraint [m/sss] - double jerk_start; // init jerk used for deceleration planning [m/sss] - double jerk_span; // fineness param for planning deceleration jerk [m/sss] - double vel_threshold_reset_velocity_limit_; // velocity threshold, - // check complete deceleration [m/s] - double dec_threshold_reset_velocity_limit_; // acceleration threshold, - // check complete deceleration [m/ss] - double slow_down_search_radius; // search radius for slow down obstacle point cloud [m] - }; - - struct PlannerData - { - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag{}; - - geometry_msgs::msg::Pose current_pose{}; - - pcl::PointXYZ nearest_collision_point; - pcl::PointXYZ nearest_slow_down_point; - pcl::PointXYZ lateral_nearest_slow_down_point; - rclcpp::Time nearest_collision_point_time{}; - double lateral_deviation{0.0}; - - size_t trajectory_trim_index{}; - size_t decimate_trajectory_collision_index{}; - size_t decimate_trajectory_slow_down_index{}; - std::map decimate_trajectory_index_map{}; // key: decimate index - // value: original index - - bool found_collision_points{false}; - bool found_slow_down_points{false}; - bool stop_require{false}; - bool slow_down_require{false}; - bool enable_adaptive_cruise{false}; - }; - private: /* * ROS diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp new file mode 100644 index 0000000000000..11fcdcdf56828 --- /dev/null +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -0,0 +1,247 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ +#define OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ + +#include + +#include + +#include + +namespace motion_planning +{ + +using diagnostic_msgs::msg::DiagnosticStatus; + +struct StopPoint +{ + TrajectoryPoint point{}; + + size_t index; +}; + +struct SlowDownSection +{ + TrajectoryPoint start_point{}; + + TrajectoryPoint end_point{}; + + size_t slow_down_start_idx; + + size_t slow_down_end_idx; + + double velocity; +}; + +struct NodeParam +{ + // set True, slow down for obstacle beside the path + bool enable_slow_down; + + // max velocity [m/s] + double max_velocity; + + // smoothing calculated current acceleration [-] + double lowpass_gain; + + // keep slow down or stop state if obstacle vanished [s] + double hunting_threshold; + + // dist threshold for ego's nearest index + double ego_nearest_dist_threshold; + + // yaw threshold for ego's nearest index + double ego_nearest_yaw_threshold; +}; + +struct StopParam +{ + // ============================== + // params for longitudinal margin + // ============================== + + // margin between obstacle and the ego's front [m] + double max_longitudinal_margin; + + // margin between obstacle and the ego's front [m] + // if any other stop point is inserted within max_longitudinal_margin. + double min_longitudinal_margin; + + // ================================== + // params for obstacle detection area + // ================================== + + // lateral margin between the ego's footprint and + // the boundary of the detection area for collision obstacles [m] + // if any obstacles exist within the detection area, this module plans to stop + // before the obstacle. + double lateral_margin; + + // ================================= + // params for trajectory pre-process + // ================================= + + // trajectory extend distance [m] + double extend_distance; + + // step length for pointcloud search range [m] + double step_length; + + // ====== + // others + // ====== + + // search radius for obstacle point cloud [m] + double stop_search_radius; + + // keep stopping if the ego is in this margin [m] + double hold_stop_margin_distance; +}; + +struct SlowDownParam +{ + // ================= + // params for margin + // ================= + + // margin between obstacle and the ego's front [m] + double longitudinal_forward_margin; + + // margin between obstacle and the ego's rear [m] + double longitudinal_backward_margin; + + // OPTIONAL (use this param if consider_constraints is True) + // minimum distance between obstacle and the ego's front when slow down margin is relaxed [m] + double min_longitudinal_forward_margin; + + // OPTIONAL (use this param if consider_constraints is True) + // fineness param for relaxing slow down margin [m] + double longitudinal_margin_span; + + // ================================== + // params for obstacle detection area + // ================================== + + // lateral margin between the ego's footprint and the boundary of the detection area for slow down + // obstacles [m] + double lateral_margin; + + // =================== + // params for velocity + // =================== + + // OPTIONAL (use this param if consider_constraints is False) + // maximum velocity fow slow down section [m/s] + double max_slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is False) + // minimum velocity for slow down section [m/s] + double min_slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is True) + // target slow down velocity [m/s] + double slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is True) + // velocity threshold whether the vehicle complete deceleration [m/s] + double velocity_threshold_decel_complete; + + // OPTIONAL (use this param if consider_constraints is True) + // acceleration threshold whether the vehicle complete deceleration [m/ss] + double acceleration_threshold_decel_complete; + + // =================================== + // params for deceleration constraints + // =================================== + + // OPTIONAL (use this param if consider_constraints is True) + // min jerk limit for mild stop [m/sss] + double normal_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // min deceleration limit for mild stop [m/ss] + double normal_min_acc; + + // OPTIONAL (use this param if consider_constraints is True) + // min jerk limit [m/sss] + double limit_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // min deceleration limit [m/ss] + double limit_min_acc; + + // OPTIONAL (use this param if consider_constraints is True) + // min slow down jerk constraint [m/sss] + double slow_down_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // init jerk used for deceleration planning [m/sss] + double jerk_start; + + // OPTIONAL (use this param if consider_constraints is True) + // fineness param for planning deceleration jerk [m/sss] + double jerk_span; + + // ====== + // others + // ====== + + // set "True", decel point is planned under jerk/dec constraints + bool consider_constraints; + + // search radius for slow down obstacle point cloud [m] + double slow_down_search_radius; +}; + +struct PlannerData +{ + DiagnosticStatus stop_reason_diag{}; + + geometry_msgs::msg::Pose current_pose{}; + + pcl::PointXYZ nearest_collision_point; + + pcl::PointXYZ nearest_slow_down_point; + + pcl::PointXYZ lateral_nearest_slow_down_point; + + rclcpp::Time nearest_collision_point_time{}; + + double lateral_deviation{0.0}; + + size_t trajectory_trim_index{}; + + size_t decimate_trajectory_collision_index{}; + + size_t decimate_trajectory_slow_down_index{}; + + // key: decimate index, value: original index + std::map decimate_trajectory_index_map{}; + + bool found_collision_points{false}; + + bool found_slow_down_points{false}; + + bool stop_require{false}; + + bool slow_down_require{false}; + + bool enable_adaptive_cruise{false}; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index f41a97d8ff924..da485a4e52cdf 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -233,15 +233,15 @@ boost::optional calcDecelDistWithJerkAndAccConstraints( return {}; } boost::optional> calcFeasibleMarginAndVelocity( - const ObstacleStopPlannerNode::SlowDownParam & slow_down_param, - const double dist_baselink_to_obstacle, const double current_vel, const double current_acc) + const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, + const double current_vel, const double current_acc) { const auto & p = slow_down_param; const auto & logger = rclcpp::get_logger("calcFeasibleMarginAndVelocity"); constexpr double epsilon = 1e-4; - if (current_vel < p.slow_down_vel + epsilon) { - return std::make_pair(p.forward_margin, p.slow_down_vel); + if (current_vel < p.slow_down_velocity + epsilon) { + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); } for (double planning_jerk = p.jerk_start; planning_jerk > p.slow_down_min_jerk - epsilon; @@ -252,17 +252,18 @@ boost::optional> calcFeasibleMarginAndVelocity( const auto planning_dec = planning_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, p.slow_down_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { continue; } - if (stop_dist.get() + p.forward_margin < dist_baselink_to_obstacle) { + if (stop_dist.get() + p.longitudinal_forward_margin < dist_baselink_to_obstacle) { RCLCPP_DEBUG( logger, "[found plan] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt:%-6.2f", - stop_dist.get(), planning_jerk, p.forward_margin, p.slow_down_vel, current_vel); - return std::make_pair(p.forward_margin, p.slow_down_vel); + stop_dist.get(), planning_jerk, p.longitudinal_forward_margin, p.slow_down_velocity, + current_vel); + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); } } @@ -273,18 +274,18 @@ boost::optional> calcFeasibleMarginAndVelocity( const auto planning_dec = p.slow_down_min_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, p.slow_down_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { return {}; } - if (stop_dist.get() + p.forward_margin_min < dist_baselink_to_obstacle) { + if (stop_dist.get() + p.min_longitudinal_forward_margin < dist_baselink_to_obstacle) { const auto planning_margin = dist_baselink_to_obstacle - stop_dist.get(); RCLCPP_DEBUG( logger, "[relax margin] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt%-6.2f", - stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_vel, current_vel); - return std::make_pair(planning_margin, p.slow_down_vel); + stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_velocity, current_vel); + return std::make_pair(planning_margin, p.slow_down_velocity); } } @@ -453,10 +454,10 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod // Parameters { auto & p = node_param_; - p.enable_slow_down = declare_parameter("enable_slow_down", false); - p.max_velocity = declare_parameter("max_velocity", 20.0); - p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); - p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); + p.enable_slow_down = declare_parameter("enable_slow_down"); + p.max_velocity = declare_parameter("max_velocity"); + p.hunting_threshold = declare_parameter("hunting_threshold"); + p.lowpass_gain = declare_parameter("lowpass_gain"); lpf_acc_ = std::make_shared(p.lowpass_gain); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -465,51 +466,76 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod { auto & p = stop_param_; const std::string ns = "stop_planner."; - p.stop_margin = declare_parameter(ns + "stop_margin", 5.0); - p.min_behavior_stop_margin = declare_parameter(ns + "min_behavior_stop_margin", 2.0); - p.expand_stop_range = declare_parameter(ns + "expand_stop_range", 0.0); - p.extend_distance = declare_parameter(ns + "extend_distance", 0.0); - p.step_length = declare_parameter(ns + "step_length", 1.0); - p.stop_margin += i.max_longitudinal_offset_m; - p.min_behavior_stop_margin += i.max_longitudinal_offset_m; + + // params for stop position + p.max_longitudinal_margin = + declare_parameter(ns + "stop_position.max_longitudinal_margin"); + p.min_longitudinal_margin = + declare_parameter(ns + "stop_position.min_longitudinal_margin"); + p.hold_stop_margin_distance = + declare_parameter(ns + "stop_position.hold_stop_margin_distance"); + + // params for detection area + p.lateral_margin = declare_parameter(ns + "detection_area.lateral_margin"); + p.extend_distance = declare_parameter(ns + "detection_area.extend_distance"); + p.step_length = declare_parameter(ns + "detection_area.step_length"); + + // apply offset + p.max_longitudinal_margin += i.max_longitudinal_offset_m; + p.min_longitudinal_margin += i.max_longitudinal_offset_m; p.stop_search_radius = p.step_length + - std::hypot(i.vehicle_width_m / 2.0 + p.expand_stop_range, i.vehicle_length_m / 2.0); - p.hold_stop_margin_distance = declare_parameter(ns + "hold_stop_margin_distance", 0.0); + std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); } { auto & p = slow_down_param_; const std::string ns = "slow_down_planner."; + // common param - p.normal_min_jerk = declare_parameter("normal.min_jerk", -0.3); - p.normal_min_acc = declare_parameter("normal.min_acc", -1.0); - p.limit_min_jerk = declare_parameter("limit.min_jerk", -1.5); - p.limit_min_acc = declare_parameter("limit.min_acc", -2.5); - // slow down planner specific parameters - p.forward_margin = declare_parameter(ns + "forward_margin", 5.0); - p.backward_margin = declare_parameter(ns + "backward_margin", 5.0); - p.expand_slow_down_range = declare_parameter(ns + "expand_slow_down_range", 1.0); - p.max_slow_down_vel = declare_parameter(ns + "max_slow_down_vel", 4.0); - p.min_slow_down_vel = declare_parameter(ns + "min_slow_down_vel", 2.0); + p.normal_min_jerk = declare_parameter("normal.min_jerk"); + p.normal_min_acc = declare_parameter("normal.min_acc"); + p.limit_min_jerk = declare_parameter("limit.min_jerk"); + p.limit_min_acc = declare_parameter("limit.min_acc"); + + // params for slow down section + p.longitudinal_forward_margin = + declare_parameter(ns + "slow_down_section.longitudinal_forward_margin"); + p.longitudinal_backward_margin = + declare_parameter(ns + "slow_down_section.longitudinal_backward_margin"); + p.min_longitudinal_forward_margin = + declare_parameter(ns + "slow_down_section.min_longitudinal_forward_margin"); + p.longitudinal_margin_span = + declare_parameter(ns + "slow_down_section.longitudinal_margin_span"); + + // params for detection area + p.lateral_margin = declare_parameter(ns + "detection_area.lateral_margin"); + + // params for target velocity + p.max_slow_down_velocity = + declare_parameter(ns + "target_velocity.max_slow_down_velocity"); + p.min_slow_down_velocity = + declare_parameter(ns + "target_velocity.min_slow_down_velocity"); + p.slow_down_velocity = declare_parameter(ns + "target_velocity.slow_down_velocity"); + // consider jerk/dec constraints in slow down - p.consider_constraints = declare_parameter(ns + "consider_constraints", false); - p.forward_margin_min = declare_parameter(ns + "forward_margin_min", 1.0); - p.forward_margin_span = declare_parameter(ns + "forward_margin_span", -0.1); - p.slow_down_min_jerk = declare_parameter(ns + "jerk_min_slow_down", -0.3); - p.jerk_start = declare_parameter(ns + "jerk_start", -0.1); - p.jerk_span = declare_parameter(ns + "jerk_span", -0.01); - p.slow_down_vel = declare_parameter(ns + "slow_down_vel", 1.39); - p.vel_threshold_reset_velocity_limit_ = - declare_parameter(ns + "vel_threshold_reset_velocity_limit_", 0.2); - p.dec_threshold_reset_velocity_limit_ = - declare_parameter(ns + "dec_threshold_reset_velocity_limit_", 0.1); - p.forward_margin += i.max_longitudinal_offset_m; - p.forward_margin_min += i.wheel_base_m + i.front_overhang_m; - p.backward_margin += i.rear_overhang_m; + p.consider_constraints = declare_parameter(ns + "consider_constraints"); + p.slow_down_min_jerk = declare_parameter(ns + "constraints.jerk_min_slow_down"); + p.jerk_start = declare_parameter(ns + "constraints.jerk_start"); + p.jerk_span = declare_parameter(ns + "constraints.jerk_span"); + + p.velocity_threshold_decel_complete = + declare_parameter(ns + "velocity_threshold_decel_complete"); + p.acceleration_threshold_decel_complete = + declare_parameter(ns + "acceleration_threshold_decel_complete"); + + // apply offset + p.longitudinal_forward_margin += i.max_longitudinal_offset_m; + p.min_longitudinal_forward_margin += i.wheel_base_m + i.front_overhang_m; + p.longitudinal_backward_margin += i.rear_overhang_m; p.slow_down_search_radius = stop_param_.step_length + - std::hypot(i.vehicle_width_m / 2.0 + p.expand_slow_down_range, i.vehicle_length_m / 2.0); + std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); } // Initializer @@ -700,7 +726,7 @@ void ObstacleStopPlannerNode::searchObstacle( // create one step polygon for slow_down range createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, - slow_down_param_.expand_slow_down_range); + slow_down_param_.lateral_margin); debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); @@ -738,7 +764,7 @@ void ObstacleStopPlannerNode::searchObstacle( std::vector one_step_move_vehicle_polygon; // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.expand_stop_range); + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin); debug_ptr_->pushPolygon( one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, PolygonType::Vehicle); @@ -942,8 +968,8 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_section.start_point = output.front(); slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first; slow_down_section.end_point = end_insert_point_with_idx.get().second; - slow_down_section.velocity = - set_velocity_limit_ ? std::numeric_limits::max() : slow_down_param_.slow_down_vel; + slow_down_section.velocity = set_velocity_limit_ ? std::numeric_limits::max() + : slow_down_param_.slow_down_velocity; insertSlowDownSection(slow_down_section, output); } else if (no_hunting_slowdown_point) { @@ -996,10 +1022,10 @@ void ObstacleStopPlannerNode::externalExpandStopRangeCallback( std::lock_guard lock(mutex_); const auto & i = vehicle_info_; - stop_param_.expand_stop_range = input_msg->expand_stop_range; + stop_param_.lateral_margin = input_msg->expand_stop_range; stop_param_.stop_search_radius = stop_param_.step_length + - std::hypot(i.vehicle_width_m / 2.0 + stop_param_.expand_stop_range, i.vehicle_length_m / 2.0); + std::hypot(i.vehicle_width_m / 2.0 + stop_param_.lateral_margin, i.vehicle_length_m / 2.0); } void ObstacleStopPlannerNode::insertStopPoint( @@ -1042,9 +1068,9 @@ StopPoint ObstacleStopPlannerNode::searchInsertPoint( const StopParam & stop_param) { const auto max_dist_stop_point = - createTargetPoint(idx, stop_param.stop_margin, base_trajectory, dist_remain); + createTargetPoint(idx, stop_param.max_longitudinal_margin, base_trajectory, dist_remain); const auto min_dist_stop_point = - createTargetPoint(idx, stop_param.min_behavior_stop_margin, base_trajectory, dist_remain); + createTargetPoint(idx, stop_param.min_longitudinal_margin, base_trajectory, dist_remain); // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; @@ -1099,7 +1125,7 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( } const auto no_need_velocity_limit = - dist_baselink_to_obstacle + dist_remain > slow_down_param_.forward_margin; + dist_baselink_to_obstacle + dist_remain > slow_down_param_.longitudinal_forward_margin; if (set_velocity_limit_ && no_need_velocity_limit) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -1107,9 +1133,10 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( const auto use_velocity_limit = relax_target_vel || set_velocity_limit_; const auto update_forward_margin_from_vehicle = - use_velocity_limit ? slow_down_param_.forward_margin_min - dist_remain + use_velocity_limit ? slow_down_param_.min_longitudinal_forward_margin - dist_remain : margin_with_vel.get().first - dist_remain; - const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain; + const auto update_backward_margin_from_vehicle = + slow_down_param_.longitudinal_backward_margin + dist_remain; const auto velocity = use_velocity_limit ? std::numeric_limits::max() : margin_with_vel.get().second; @@ -1118,14 +1145,16 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, velocity); } else { - const auto update_forward_margin_from_vehicle = slow_down_param_.forward_margin - dist_remain; - const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain; + const auto update_forward_margin_from_vehicle = + slow_down_param_.longitudinal_forward_margin - dist_remain; + const auto update_backward_margin_from_vehicle = + slow_down_param_.longitudinal_backward_margin + dist_remain; const auto velocity = - slow_down_param_.min_slow_down_vel + - (slow_down_param_.max_slow_down_vel - slow_down_param_.min_slow_down_vel) * + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * std::max(lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.expand_slow_down_range; + slow_down_param_.lateral_margin; return createSlowDownSectionFromMargin( idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, @@ -1569,7 +1598,7 @@ void ObstacleStopPlannerNode::setExternalVelocityLimit() const auto & p = slow_down_param_; auto slow_down_limit_vel = std::make_shared(); slow_down_limit_vel->stamp = this->now(); - slow_down_limit_vel->max_velocity = p.slow_down_vel; + slow_down_limit_vel->max_velocity = p.slow_down_velocity; slow_down_limit_vel->constraints.min_acceleration = p.slow_down_min_jerk < p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; slow_down_limit_vel->constraints.max_jerk = std::abs(p.slow_down_min_jerk); @@ -1588,11 +1617,10 @@ void ObstacleStopPlannerNode::setExternalVelocityLimit() void ObstacleStopPlannerNode::resetExternalVelocityLimit( const double current_acc, const double current_vel) { - const auto reach_target_vel = - current_vel < - slow_down_param_.slow_down_vel + slow_down_param_.vel_threshold_reset_velocity_limit_; + const auto reach_target_vel = current_vel < slow_down_param_.slow_down_velocity + + slow_down_param_.velocity_threshold_decel_complete; const auto constant_vel = - std::abs(current_acc) < slow_down_param_.dec_threshold_reset_velocity_limit_; + std::abs(current_acc) < slow_down_param_.acceleration_threshold_decel_complete; const auto no_undershoot = reach_target_vel && constant_vel; if (!no_undershoot) { From 0b6b0a846a85f6658e6ec0c45952efca03bd30b7 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 6 Sep 2022 19:29:21 +0900 Subject: [PATCH 27/28] feat(rtc_interface): add finish distance (#1685) * feat(rtc_interface): add_finish_distance Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): remove unnecessary include Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix typo Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix goal pose Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- .../avoidance/avoidance_module.hpp | 37 ++++++++++++----- .../lane_change/lane_change_module.hpp | 17 ++++---- .../pull_over/pull_over_module.hpp | 3 +- .../scene_module/scene_module_interface.hpp | 8 ++-- .../avoidance/avoidance_module.cpp | 11 +++-- .../lane_change/lane_change_module.cpp | 15 +++++-- .../scene_module/pull_out/pull_out_module.cpp | 41 +++++++++++++++---- .../pull_over/pull_over_module.cpp | 17 ++++---- .../scene_module/scene_module_interface.hpp | 2 +- planning/rtc_interface/README.md | 10 +++-- .../include/rtc_interface/rtc_interface.hpp | 3 +- planning/rtc_interface/src/rtc_interface.cpp | 9 ++-- 12 files changed, 120 insertions(+), 53 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 4a1628533988a..1069a4f285223 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -36,8 +36,6 @@ namespace behavior_path_planner using tier4_planning_msgs::msg::AvoidanceDebugMsg; class AvoidanceModule : public SceneModuleInterface { - using RegisteredShiftPointArray = std::vector>; - public: AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); @@ -70,6 +68,14 @@ class AvoidanceModule : public SceneModuleInterface } private: + struct RegisteredShiftPoint + { + UUID uuid; + Pose start_pose; + Pose finish_pose; + }; + using RegisteredShiftPointArray = std::vector; + std::shared_ptr parameters_; AvoidancePlanningData avoidance_data_; @@ -89,19 +95,22 @@ class AvoidanceModule : public SceneModuleInterface { if (candidate.lateral_shift > 0.0) { rtc_interface_left_.updateCooperateStatus( - uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, + candidate.finish_distance_to_path_change, clock_->now()); candidate_uuid_ = uuid_left_; return; } if (candidate.lateral_shift < 0.0) { rtc_interface_right_.updateCooperateStatus( - uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, + candidate.finish_distance_to_path_change, clock_->now()); candidate_uuid_ = uuid_right_; return; } RCLCPP_WARN_STREAM( - getLogger(), "Direction is UNKNOWN, distance = " << candidate.distance_to_path_change); + getLogger(), + "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); } void updateRegisteredRTCStatus(const PathWithLaneId & path) @@ -109,15 +118,21 @@ class AvoidanceModule : public SceneModuleInterface const Point ego_position = planner_data_->self_pose->pose.position; for (const auto & left_shift : left_shift_array_) { - const double distance = - motion_utils::calcSignedArcLength(path.points, ego_position, left_shift.second.position); - rtc_interface_left_.updateCooperateStatus(left_shift.first, true, distance, clock_->now()); + const double start_distance = motion_utils::calcSignedArcLength( + path.points, ego_position, left_shift.start_pose.position); + const double finish_distance = motion_utils::calcSignedArcLength( + path.points, ego_position, left_shift.finish_pose.position); + rtc_interface_left_.updateCooperateStatus( + left_shift.uuid, true, start_distance, finish_distance, clock_->now()); } for (const auto & right_shift : right_shift_array_) { - const double distance = - motion_utils::calcSignedArcLength(path.points, ego_position, right_shift.second.position); - rtc_interface_right_.updateCooperateStatus(right_shift.first, true, distance, clock_->now()); + const double start_distance = motion_utils::calcSignedArcLength( + path.points, ego_position, right_shift.start_pose.position); + const double finish_distance = motion_utils::calcSignedArcLength( + path.points, ego_position, right_shift.finish_pose.position); + rtc_interface_right_.updateCooperateStatus( + right_shift.uuid, true, start_distance, finish_distance, clock_->now()); } } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 0706eb64c493d..5770c16beb42c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -118,17 +118,17 @@ class LaneChangeModule : public SceneModuleInterface bool is_activated_ = false; - void waitApprovalLeft(const double distance) + void waitApprovalLeft(const double start_distance, const double finish_distance) { rtc_interface_left_.updateCooperateStatus( - uuid_left_, isExecutionReady(), distance, clock_->now()); + uuid_left_, isExecutionReady(), start_distance, finish_distance, clock_->now()); is_waiting_approval_ = true; } - void waitApprovalRight(const double distance) + void waitApprovalRight(const double start_distance, const double finish_distance) { rtc_interface_right_.updateCooperateStatus( - uuid_right_, isExecutionReady(), distance, clock_->now()); + uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now()); is_waiting_approval_ = true; } @@ -136,17 +136,20 @@ class LaneChangeModule : public SceneModuleInterface { if (candidate.lateral_shift > 0.0) { rtc_interface_left_.updateCooperateStatus( - uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change, + candidate.finish_distance_to_path_change, clock_->now()); return; } if (candidate.lateral_shift < 0.0) { rtc_interface_right_.updateCooperateStatus( - uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change, + candidate.finish_distance_to_path_change, clock_->now()); return; } RCLCPP_WARN_STREAM( - getLogger(), "Direction is UNKNOWN, distance = " << candidate.distance_to_path_change); + getLogger(), + "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); } void removeRTCStatus() override diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index fbe177291a879..201791510d5c5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -200,7 +200,8 @@ class PullOverModule : public SceneModuleInterface const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; bool isArcPath() const; double calcMinimumShiftPathDistance() const; - double calcDistanceToPathChange() const; + std::pair calcDistanceToPathChange() const; + bool planShiftPath(); bool isStopped(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index f5af0308c22da..b1e9665eaa46f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -85,7 +85,8 @@ struct CandidateOutput explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {} PathWithLaneId path_candidate{}; double lateral_shift{0.0}; - double distance_to_path_change{std::numeric_limits::lowest()}; + double start_distance_to_path_change{std::numeric_limits::lowest()}; + double finish_distance_to_path_change{std::numeric_limits::lowest()}; }; class SceneModuleInterface @@ -251,12 +252,13 @@ class SceneModuleInterface UUID uuid_; bool is_waiting_approval_; - void updateRTCStatus(const double distance) + void updateRTCStatus(const double start_distance, const double finish_distance) { if (!rtc_interface_ptr_) { return; } - rtc_interface_ptr_->updateCooperateStatus(uuid_, isExecutionReady(), distance, clock_->now()); + rtc_interface_ptr_->updateCooperateStatus( + uuid_, isExecutionReady(), start_distance, finish_distance, clock_->now()); } virtual void removeRTCStatus() 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 41f9b43c79e1a..866463172c880 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 @@ -2084,7 +2084,8 @@ CandidateOutput AvoidanceModule::planCandidate() const } } output.lateral_shift = new_shift_points->at(i).getRelativeLength(); - output.distance_to_path_change = new_shift_points->front().start_longitudinal; + output.start_distance_to_path_change = new_shift_points->front().start_longitudinal; + output.finish_distance_to_path_change = new_shift_points->back().end_longitudinal; } const size_t ego_idx = findEgoIndex(shifted_path.path.points); @@ -2101,7 +2102,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() BehaviorModuleOutput out = plan(); const auto candidate = planCandidate(); constexpr double threshold_to_update_status = -1.0e-03; - if (candidate.distance_to_path_change > threshold_to_update_status) { + if (candidate.start_distance_to_path_change > threshold_to_update_status) { updateCandidateRTCStatus(candidate); waitApproval(); } else { @@ -2132,9 +2133,11 @@ void AvoidanceModule::addShiftPointIfApproved(const AvoidPointArray & shift_poin } if (shift_points.at(i).getRelativeLength() > 0.0) { - left_shift_array_.push_back({uuid_left_, shift_points.front().start}); + left_shift_array_.push_back( + {uuid_left_, shift_points.front().start, shift_points.back().end}); } else if (shift_points.at(i).getRelativeLength() < 0.0) { - right_shift_array_.push_back({uuid_right_, shift_points.front().start}); + right_shift_array_.push_back( + {uuid_right_, shift_points.front().start, shift_points.back().end}); } uuid_left_ = generateUUID(); 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 f43a683ba610d..703984da93564 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 @@ -55,10 +55,16 @@ BehaviorModuleOutput LaneChangeModule::run() is_activated_ = isActivated(); const auto output = plan(); const auto turn_signal_info = output.turn_signal_info; + const auto current_pose = planner_data_->self_pose->pose; + const double start_distance = motion_utils::calcSignedArcLength( + output.path->points, current_pose.position, + status_.lane_change_path.shift_point.start.position); + const double finish_distance = motion_utils::calcSignedArcLength( + output.path->points, current_pose.position, status_.lane_change_path.shift_point.end.position); if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - waitApprovalLeft(turn_signal_info.signal_distance); + waitApprovalLeft(start_distance, finish_distance); } else if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - waitApprovalRight(turn_signal_info.signal_distance); + waitApprovalRight(start_distance, finish_distance); } return output; } @@ -195,9 +201,12 @@ CandidateOutput LaneChangeModule::planCandidate() const output.path_candidate = selected_path.path; output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - selected_path.shifted_path.shift_length.at(start_idx); - output.distance_to_path_change = motion_utils::calcSignedArcLength( + output.start_distance_to_path_change = motion_utils::calcSignedArcLength( selected_path.path.points, planner_data_->self_pose->pose.position, selected_path.shift_point.start.position); + output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( + selected_path.path.points, planner_data_->self_pose->pose.position, + selected_path.shift_point.end.position); return output; } 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 450ba7f5e31db..31942a0ab61e7 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 @@ -85,8 +85,8 @@ void PullOutModule::onEntry() if (last_pull_out_start_update_time_ == nullptr) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } - const auto elpased_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); - if (elpased_time < parameters_.backward_path_update_duration) { + const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); + if (elapsed_time < parameters_.backward_path_update_duration) { return; } last_pull_out_start_update_time_ = std::make_unique(clock_->now()); @@ -173,6 +173,21 @@ BehaviorModuleOutput PullOutModule::plan() output.turn_signal_info = calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + if (status_.back_finished) { + const double start_distance = motion_utils::calcSignedArcLength( + path.points, planner_data_->self_pose->pose.position, + status_.pull_out_path.start_pose.position); + const double finish_distance = motion_utils::calcSignedArcLength( + path.points, planner_data_->self_pose->pose.position, + status_.pull_out_path.end_pose.position); + updateRTCStatus(start_distance, finish_distance); + } else { + const double distance = motion_utils::calcSignedArcLength( + path.points, planner_data_->self_pose->pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + } + setDebugData(); return output; @@ -234,8 +249,20 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() output.path_candidate = std::make_shared(candidate_path); waitApproval(); - // requset approval when stopped at the corresponding point, so distance is 0 - updateRTCStatus(0.0); + if (status_.back_finished) { + const double start_distance = motion_utils::calcSignedArcLength( + candidate_path.points, planner_data_->self_pose->pose.position, + status_.pull_out_path.start_pose.position); + const double finish_distance = motion_utils::calcSignedArcLength( + candidate_path.points, planner_data_->self_pose->pose.position, + status_.pull_out_path.end_pose.position); + updateRTCStatus(start_distance, finish_distance); + } else { + const double distance = motion_utils::calcSignedArcLength( + candidate_path.points, planner_data_->self_pose->pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + } setDebugData(); return output; @@ -421,7 +448,7 @@ std::vector PullOutModule::searchBackedPoses() p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); } - // check collision between footprint and onject at the backed pose + // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); std::vector backed_poses; for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance; @@ -489,12 +516,10 @@ void PullOutModule::checkBackFinished() RCLCPP_INFO(getLogger(), "back finished"); status_.back_finished = true; - // requst pull_out approval + // request pull_out approval waitApproval(); removeRTCStatus(); uuid_ = generateUUID(); - // requset approval when stopped at the corresponding point, so distance is 0 - updateRTCStatus(0.0); current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop } } 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 cfe4889559532..a44cbcbb90e12 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 @@ -608,8 +608,8 @@ BehaviorModuleOutput PullOverModule::plan() } } - const double distance_to_path_change = calcDistanceToPathChange(); - updateRTCStatus(distance_to_path_change); + const auto distance_to_path_change = calcDistanceToPathChange(); + updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); publishDebugData(); @@ -637,22 +637,25 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() out.path_candidate = std::make_shared(parallel_parking_planner_.getFullPath()); } - const double distance_to_path_change = calcDistanceToPathChange(); - updateRTCStatus(distance_to_path_change); + const auto distance_to_path_change = calcDistanceToPathChange(); + updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); waitApproval(); return out; } -double PullOverModule::calcDistanceToPathChange() const +std::pair PullOverModule::calcDistanceToPathChange() const { const Pose parking_start_pose = getParkingStartPose(); + const Pose parking_end_pose = modified_goal_pose_; const auto dist_to_parking_start_pose = calcSignedArcLength( status_.path.points, planner_data_->self_pose->pose, parking_start_pose.position, std::numeric_limits::max(), M_PI_2); - const double distance_to_path_change = + const double dist_to_parking_finish_pose = calcSignedArcLength( + status_.path.points, planner_data_->self_pose->pose.position, parking_end_pose.position); + const double start_distance_to_path_change = dist_to_parking_start_pose ? *dist_to_parking_start_pose : std::numeric_limits::max(); - return distance_to_path_change; + return {start_distance_to_path_change, dist_to_parking_finish_pose}; } void PullOverModule::setParameters(const PullOverParameters & parameters) diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index e36dd38516890..e3d41eab53c6f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -354,7 +354,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void updateRTCStatus( const UUID & uuid, const bool safe, const double distance, const Time & stamp) { - rtc_interface_.updateCooperateStatus(uuid, safe, distance, stamp); + rtc_interface_.updateCooperateStatus(uuid, safe, distance, distance, stamp); } void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } diff --git a/planning/rtc_interface/README.md b/planning/rtc_interface/README.md index 45c1d4e8b9a23..a02f4ef90625f 100644 --- a/planning/rtc_interface/README.md +++ b/planning/rtc_interface/README.md @@ -21,13 +21,14 @@ while (...) { const bool safe = ... // Get distance to the object corresponding to the module id - const double distance = ... + const double start_distance = ... + const double finish_distance = ... // Get time stamp const rclcpp::Time stamp = ... // Update status - rtc_interface.updateCooperateStatus(uuid, safe, distance, stamp); + rtc_interface.updateCooperateStatus(uuid, safe, start_distance, finish_distance, stamp); if (rtc_interface.isActivated(uuid)) { // Execute planning @@ -89,7 +90,7 @@ Nothing ### updateCooperateStatus ```c++ -rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp) +rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double start_distance, const double finish_distance, const rclcpp::Time & stamp) ``` #### Description @@ -101,7 +102,8 @@ If cooperate status corresponding to `uuid` is not registered yet, add new coope - `uuid` : UUID for requesting module - `safe` : Safety status of requesting module -- `distance` : Distance to the object from ego vehicle +- `start_distance` : Distance to the start object from ego vehicle +- `finish_distance` : Distance to the finish object from ego vehicle - `stamp` : Time stamp #### Output diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 6e3139254323d..6f3d6180277d1 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -47,7 +47,8 @@ class RTCInterface RTCInterface(rclcpp::Node * node, const std::string & name); void publishCooperateStatus(const rclcpp::Time & stamp); void updateCooperateStatus( - const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp); + const UUID & uuid, const bool safe, const double start_distance, const double finish_distance, + const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); bool isActivated(const UUID & uuid); diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 3285a547a1a6f..5990cbaa3c91f 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -125,7 +125,8 @@ void RTCInterface::onCooperateCommandService( } void RTCInterface::updateCooperateStatus( - const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp) + const UUID & uuid, const bool safe, const double start_distance, const double finish_distance, + const rclcpp::Time & stamp) { std::lock_guard lock(mutex_); // Find registered status which has same uuid @@ -141,7 +142,8 @@ void RTCInterface::updateCooperateStatus( status.module = module_; status.safe = safe; status.command_status.type = Command::DEACTIVATE; - status.distance = distance; + status.start_distance = start_distance; + status.finish_distance = finish_distance; registered_status_.statuses.push_back(status); return; } @@ -149,7 +151,8 @@ void RTCInterface::updateCooperateStatus( // If the registered status is found, update status itr->stamp = stamp; itr->safe = safe; - itr->distance = distance; + itr->start_distance = start_distance; + itr->finish_distance = finish_distance; } void RTCInterface::removeCooperateStatus(const UUID & uuid) From 77555c59f8459e37dc13ce57c9d7bb8436ee6381 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 6 Sep 2022 20:29:50 +0900 Subject: [PATCH 28/28] fix(tier4_screen_capture_rviz_plugin): fix spell check (#1790) * fix(tier4_screen_capture_rviz_plugin): fix spell check Signed-off-by: h-ohta * ci(pre-commit): autofix Signed-off-by: h-ohta Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/screen_capture_panel.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp index a5e6868247daf..0c558ca802c6d 100644 --- a/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp +++ b/common/tier4_screen_capture_rviz_plugin/src/screen_capture_panel.cpp @@ -131,12 +131,12 @@ void AutowareScreenCapturePanel::onClickVideoCapture() { int fourcc = cv::VideoWriter::fourcc('h', '2', '6', '4'); // mp4 QScreen * screen = QGuiApplication::primaryScreen(); - const auto qsize = screen->grabWindow(main_window_->winId()) - .toImage() - .convertToFormat(QImage::Format_RGB888) - .rgbSwapped() - .size(); - current_movie_size_ = cv::Size(qsize.width(), qsize.height()); + const auto q_size = screen->grabWindow(main_window_->winId()) + .toImage() + .convertToFormat(QImage::Format_RGB888) + .rgbSwapped() + .size(); + current_movie_size_ = cv::Size(q_size.width(), q_size.height()); writer_.open( "capture/" + capture_file_name_ + ".mp4", fourcc, capture_hz_->value(), current_movie_size_); @@ -160,12 +160,14 @@ void AutowareScreenCapturePanel::onTimer() // this is deprecated but only way to capture nicely QScreen * screen = QGuiApplication::primaryScreen(); QPixmap original_pixmap = screen->grabWindow(main_window_->winId()); - const auto qimage = original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); - const int h = qimage.height(); - const int w = qimage.width(); + const auto q_image = + original_pixmap.toImage().convertToFormat(QImage::Format_RGB888).rgbSwapped(); + const int h = q_image.height(); + const int w = q_image.width(); cv::Size size = cv::Size(w, h); cv::Mat image( - size, CV_8UC3, const_cast(qimage.bits()), static_cast(qimage.bytesPerLine())); + size, CV_8UC3, const_cast(q_image.bits()), + static_cast(q_image.bytesPerLine())); if (size != current_movie_size_) { cv::Mat new_image; cv::resize(image, new_image, current_movie_size_);