From 8381f8e8463abd3d72e1d4851ce41261eb075c7f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 30 Jun 2023 13:21:43 +0900 Subject: [PATCH 01/12] feat(start_planner): add offset to not finish before engage (#4121) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e9e28b8e62f9b..3540b616aa78f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -682,7 +682,9 @@ bool StartPlannerModule::hasFinishedPullOut() const const auto arclength_pull_out_end = lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); - const bool has_finished = arclength_current.length - arclength_pull_out_end.length > 0.0; + // offset to not finish the module before engage + constexpr double offset = 0.1; + const bool has_finished = arclength_current.length - arclength_pull_out_end.length > offset; return has_finished; } From 00dfda5229a006041f084c7b9bbb40b96c2387b3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 30 Jun 2023 14:01:06 +0900 Subject: [PATCH 02/12] refactor(behavior_path_planner): move parameter declaration from node.cpp to manager (#4124) * refactor(behavior_path_planner): move parameter declare Signed-off-by: satoshi-ota * refactor(start_planner): move parameter declare Signed-off-by: satoshi-ota * refactor(side_shift): move parameter declare Signed-off-by: satoshi-ota * refactor(goal_planner): move parameter declare Signed-off-by: satoshi-ota * refactor(dynamic_avoidance): move parameter declare Signed-off-by: satoshi-ota * refactor(lane_change): move parameter declare Signed-off-by: satoshi-ota * refactor(avoidance): move parameter declare Signed-off-by: satoshi-ota * refactor(avoidance_by_lane_change): move parameter declare Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 24 - .../scene_module/avoidance/manager.hpp | 3 +- .../dynamic_avoidance/manager.hpp | 3 +- .../scene_module/goal_planner/manager.hpp | 3 +- .../lane_change/avoidance_by_lane_change.hpp | 4 +- .../scene_module/lane_change/interface.hpp | 1 - .../scene_module/lane_change/manager.hpp | 11 +- .../scene_module/side_shift/manager.hpp | 3 +- .../scene_module/start_planner/manager.hpp | 3 +- .../lane_change/lane_change_module_data.hpp | 5 +- .../src/behavior_path_planner_node.cpp | 688 +----------------- .../src/scene_module/avoidance/manager.cpp | 204 +++++- .../dynamic_avoidance/manager.cpp | 45 +- .../src/scene_module/goal_planner/manager.cpp | 213 +++++- .../lane_change/avoidance_by_lane_change.cpp | 45 +- .../scene_module/lane_change/interface.cpp | 4 +- .../src/scene_module/lane_change/manager.cpp | 219 +++++- .../src/scene_module/side_shift/manager.cpp | 17 +- .../scene_module/start_planner/manager.cpp | 63 +- 19 files changed, 784 insertions(+), 774 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 454106e4e6811..0ec1fe0ca493a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -25,12 +25,6 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" #include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/steering_factor_interface.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" -#include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include @@ -134,26 +128,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool isDataReady(); // parameters - std::shared_ptr avoidance_param_ptr_; - std::shared_ptr avoidance_by_lc_param_ptr_; - std::shared_ptr dynamic_avoidance_param_ptr_; - std::shared_ptr side_shift_param_ptr_; - std::shared_ptr lane_change_param_ptr_; - std::shared_ptr start_planner_param_ptr_; - std::shared_ptr goal_planner_param_ptr_; - BehaviorPathPlannerParameters getCommonParam(); - AvoidanceParameters getAvoidanceParam(); - DynamicAvoidanceParameters getDynamicAvoidanceParam(); - LaneChangeParameters getLaneChangeParam(); - SideShiftParameters getSideShiftParam(); - GoalPlannerParameters getGoalPlannerParam(); - StartPlannerParameters getStartPlannerParam(); - AvoidanceByLCParameters getAvoidanceByLCParam( - const std::shared_ptr & avoidance_param, - const std::shared_ptr & lane_change_param); - // callback void onOdometry(const Odometry::ConstSharedPtr msg); void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp index 517057f8d6ac8..e4421459e61fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp @@ -33,8 +33,7 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface { public: AvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); std::shared_ptr createNewSceneModuleInstance() override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index 51fe30c8dc7ee..d2b1deb36bd30 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -32,8 +32,7 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface { public: DynamicAvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); std::shared_ptr createNewSceneModuleInstance() override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index 31920ecd640f0..d836f73df22bc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -31,8 +31,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface { public: GoalPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); std::shared_ptr createNewSceneModuleInstance() override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp index e58a8c07850cf..9af413d48b5c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp @@ -34,7 +34,6 @@ class AvoidanceByLaneChange : public NormalLaneChange public: AvoidanceByLaneChange( const std::shared_ptr & parameters, - std::shared_ptr avoidance_parameters, std::shared_ptr avoidance_by_lane_change_parameters); void updateSpecialData() override; @@ -42,8 +41,7 @@ class AvoidanceByLaneChange : public NormalLaneChange std::pair getSafePath(LaneChangePath & safe_path) const override; private: - std::shared_ptr avoidance_parameters_; - std::shared_ptr avoidance_by_lane_change_parameters_; + std::shared_ptr avoidance_parameters_; AvoidancePlanningData calcAvoidancePlanningData(AvoidanceDebugData & debug) const; AvoidancePlanningData avoidance_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index fa7011bcdce27..1171b92343e30 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -130,7 +130,6 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map > & rtc_interface_ptr_map); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index ebee8f3d3a07a..9dabd3d0004fb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -35,8 +35,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface public: LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters, const Direction direction, - const LaneChangeModuleType type); + const Direction direction, const LaneChangeModuleType type); std::shared_ptr createNewSceneModuleInstance() override; @@ -56,18 +55,14 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: AvoidanceByLaneChangeModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters, - std::shared_ptr avoidance_parameters, - std::shared_ptr avoidance_by_lane_change_parameters); + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); std::shared_ptr createNewSceneModuleInstance() override; // void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr avoidance_parameters_; - std::shared_ptr avoidance_by_lane_change_parameters_; + std::shared_ptr avoidance_parameters_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp index 71a3691b88748..f35f497df0e4a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp @@ -32,8 +32,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { public: SideShiftModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); std::shared_ptr createNewSceneModuleInstance() override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index 09c201b0b38ce..103e7dc375233 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp @@ -31,8 +31,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { public: StartPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); std::shared_ptr createNewSceneModuleInstance() override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 006040d8eaa4a..106263cc66ebd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -114,11 +114,8 @@ enum class LaneChangeModuleType { AVOIDANCE_BY_LANE_CHANGE, }; -struct AvoidanceByLCParameters +struct AvoidanceByLCParameters : public AvoidanceParameters { - std::shared_ptr avoidance{}; - std::shared_ptr lane_change{}; - // execute if the target object number is larger than this param. size_t execute_object_num{1}; 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 410c443b94ed9..7857c2962d001 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -121,22 +121,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), createSubscriptionOptions(this)); - // set parameters - { - avoidance_param_ptr_ = std::make_shared(getAvoidanceParam()); - dynamic_avoidance_param_ptr_ = - std::make_shared(getDynamicAvoidanceParam()); - lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); - start_planner_param_ptr_ = std::make_shared(getStartPlannerParam()); - goal_planner_param_ptr_ = std::make_shared(getGoalPlannerParam()); - side_shift_param_ptr_ = std::make_shared(getSideShiftParam()); - avoidance_by_lc_param_ptr_ = std::make_shared( - getAvoidanceByLCParam(avoidance_param_ptr_, lane_change_param_ptr_)); - } - - m_set_param_res = this->add_on_set_parameters_callback( - std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); - { RCLCPP_INFO(get_logger(), "not use behavior tree."); @@ -158,8 +142,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod }; if (p.config_start_planner.enable_module) { - auto manager = std::make_shared( - this, "start_planner", p.config_start_planner, start_planner_param_ptr_); + auto manager = + std::make_shared(this, "start_planner", p.config_start_planner); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "start_planner", create_publisher(path_candidate_name_space + "start_planner", 1)); @@ -168,8 +152,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } if (p.config_goal_planner.enable_module) { - auto manager = std::make_shared( - this, "goal_planner", p.config_goal_planner, goal_planner_param_ptr_); + auto manager = + std::make_shared(this, "goal_planner", p.config_goal_planner); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); @@ -178,8 +162,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } if (p.config_side_shift.enable_module) { - auto manager = std::make_shared( - this, "side_shift", p.config_side_shift, side_shift_param_ptr_); + auto manager = + std::make_shared(this, "side_shift", p.config_side_shift); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "side_shift", create_publisher(path_candidate_name_space + "side_shift", 1)); @@ -190,38 +174,38 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod if (p.config_lane_change_left.enable_module) { const std::string module_topic = "lane_change_left"; auto manager = std::make_shared( - this, module_topic, p.config_lane_change_left, lane_change_param_ptr_, - route_handler::Direction::LEFT, LaneChangeModuleType::NORMAL); + this, module_topic, p.config_lane_change_left, route_handler::Direction::LEFT, + LaneChangeModuleType::NORMAL); register_and_create_publisher(manager); } if (p.config_lane_change_right.enable_module) { const std::string module_topic = "lane_change_right"; auto manager = std::make_shared( - this, module_topic, p.config_lane_change_right, lane_change_param_ptr_, - route_handler::Direction::RIGHT, LaneChangeModuleType::NORMAL); + this, module_topic, p.config_lane_change_right, route_handler::Direction::RIGHT, + LaneChangeModuleType::NORMAL); register_and_create_publisher(manager); } if (p.config_ext_request_lane_change_right.enable_module) { const std::string module_topic = "external_request_lane_change_right"; auto manager = std::make_shared( - this, module_topic, p.config_ext_request_lane_change_right, lane_change_param_ptr_, - route_handler::Direction::RIGHT, LaneChangeModuleType::EXTERNAL_REQUEST); + this, module_topic, p.config_ext_request_lane_change_right, route_handler::Direction::RIGHT, + LaneChangeModuleType::EXTERNAL_REQUEST); register_and_create_publisher(manager); } if (p.config_ext_request_lane_change_left.enable_module) { const std::string module_topic = "external_request_lane_change_left"; auto manager = std::make_shared( - this, module_topic, p.config_ext_request_lane_change_left, lane_change_param_ptr_, - route_handler::Direction::LEFT, LaneChangeModuleType::EXTERNAL_REQUEST); + this, module_topic, p.config_ext_request_lane_change_left, route_handler::Direction::LEFT, + LaneChangeModuleType::EXTERNAL_REQUEST); register_and_create_publisher(manager); } if (p.config_avoidance.enable_module) { - auto manager = std::make_shared( - this, "avoidance", p.config_avoidance, avoidance_param_ptr_); + auto manager = + std::make_shared(this, "avoidance", p.config_avoidance); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); @@ -231,8 +215,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod if (p.config_avoidance_by_lc.enable_module) { auto manager = std::make_shared( - this, "avoidance_by_lane_change", p.config_avoidance_by_lc, lane_change_param_ptr_, - avoidance_param_ptr_, avoidance_by_lc_param_ptr_); + this, "avoidance_by_lane_change", p.config_avoidance_by_lc); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "avoidance_by_lane_change", @@ -244,11 +227,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod if (p.config_dynamic_avoidance.enable_module) { auto manager = std::make_shared( - this, "dynamic_avoidance", p.config_dynamic_avoidance, dynamic_avoidance_param_ptr_); + this, "dynamic_avoidance", p.config_dynamic_avoidance); planner_manager_->registerSceneModuleManager(manager); } } + m_set_param_res = this->add_on_set_parameters_callback( + std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); + // turn signal decider { const double turn_signal_intersection_search_distance = @@ -433,630 +419,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() return p; } -SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() -{ - SideShiftParameters p{}; - - std::string ns = "side_shift."; - - p.min_distance_to_start_shifting = - declare_parameter(ns + "min_distance_to_start_shifting"); - p.time_to_start_shifting = declare_parameter(ns + "time_to_start_shifting"); - p.shifting_lateral_jerk = declare_parameter(ns + "shifting_lateral_jerk"); - p.min_shifting_distance = declare_parameter(ns + "min_shifting_distance"); - p.min_shifting_speed = declare_parameter(ns + "min_shifting_speed"); - p.shift_request_time_limit = declare_parameter(ns + "shift_request_time_limit"); - p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); - - return p; -} - -AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam( - const std::shared_ptr & avoidance_param, - const std::shared_ptr & lane_change_param) -{ - AvoidanceByLCParameters p{}; - p.avoidance = avoidance_param; - p.lane_change = lane_change_param; - - { - std::string ns = "avoidance_by_lane_change."; - p.execute_object_num = declare_parameter(ns + "execute_object_num"); - p.execute_object_longitudinal_margin = - declare_parameter(ns + "execute_object_longitudinal_margin"); - p.execute_only_when_lane_change_finish_before_object = - declare_parameter(ns + "execute_only_when_lane_change_finish_before_object"); - } - - if (p.execute_object_num < 1) { - RCLCPP_WARN_STREAM(get_logger(), "execute_object_num cannot be lesser than 1."); - } - return p; -} - -AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - - AvoidanceParameters p{}; - // general params - { - std::string ns = "avoidance."; - p.resample_interval_for_planning = - declare_parameter(ns + "resample_interval_for_planning"); - p.resample_interval_for_output = declare_parameter(ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - declare_parameter(ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - declare_parameter(ns + "detection_area_left_expand_dist"); - p.enable_bound_clipping = declare_parameter(ns + "enable_bound_clipping"); - p.enable_avoidance_over_same_direction = - declare_parameter(ns + "enable_avoidance_over_same_direction"); - p.enable_avoidance_over_opposite_direction = - declare_parameter(ns + "enable_avoidance_over_opposite_direction"); - p.enable_update_path_when_object_is_gone = - declare_parameter(ns + "enable_update_path_when_object_is_gone"); - p.enable_force_avoidance_for_stopped_vehicle = - declare_parameter(ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); - p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - declare_parameter(ns + "enable_yield_maneuver_during_shifting"); - p.disable_path_update = declare_parameter(ns + "disable_path_update"); - p.use_hatched_road_markings = declare_parameter(ns + "use_hatched_road_markings"); - p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); - p.print_debug_info = declare_parameter(ns + "print_debug_info"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.is_target = declare_parameter(ns + "is_target"); - param.moving_speed_threshold = declare_parameter(ns + "moving_speed_threshold"); - param.moving_time_threshold = declare_parameter(ns + "moving_time_threshold"); - param.max_expand_ratio = declare_parameter(ns + "max_expand_ratio"); - param.envelope_buffer_margin = declare_parameter(ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = declare_parameter(ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = declare_parameter(ns + "safety_buffer_lateral"); - param.safety_buffer_longitudinal = - declare_parameter(ns + "safety_buffer_longitudinal"); - return param; - }; - - const std::string ns = "avoidance.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - declare_parameter(ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - declare_parameter(ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = - declare_parameter(ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = - declare_parameter(ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - declare_parameter(ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - declare_parameter(ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - declare_parameter(ns + "object_check_forward_distance"); - p.object_check_backward_distance = - declare_parameter(ns + "object_check_backward_distance"); - p.object_check_goal_distance = declare_parameter(ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - declare_parameter(ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = declare_parameter(ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - declare_parameter(ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = declare_parameter(ns + "object_last_seen_threshold"); - } - - // safety check - { - std::string ns = "avoidance.safety_check."; - p.safety_check_backward_distance = - declare_parameter(ns + "safety_check_backward_distance"); - p.safety_check_time_horizon = declare_parameter(ns + "safety_check_time_horizon"); - p.safety_check_idling_time = declare_parameter(ns + "safety_check_idling_time"); - p.safety_check_accel_for_rss = declare_parameter(ns + "safety_check_accel_for_rss"); - p.safety_check_hysteresis_factor = - declare_parameter(ns + "safety_check_hysteresis_factor"); - } - - // avoidance maneuver (lateral) - { - std::string ns = "avoidance.avoidance.lateral."; - p.road_shoulder_safety_margin = declare_parameter(ns + "road_shoulder_safety_margin"); - p.lateral_execution_threshold = declare_parameter(ns + "lateral_execution_threshold"); - p.lateral_small_shift_threshold = - declare_parameter(ns + "lateral_small_shift_threshold"); - p.max_right_shift_length = declare_parameter(ns + "max_right_shift_length"); - p.max_left_shift_length = declare_parameter(ns + "max_left_shift_length"); - } - - // avoidance maneuver (longitudinal) - { - std::string ns = "avoidance.avoidance.longitudinal."; - p.prepare_time = declare_parameter(ns + "prepare_time"); - p.min_prepare_distance = declare_parameter(ns + "min_prepare_distance"); - p.min_avoidance_distance = declare_parameter(ns + "min_avoidance_distance"); - p.min_nominal_avoidance_speed = declare_parameter(ns + "min_nominal_avoidance_speed"); - p.min_sharp_avoidance_speed = declare_parameter(ns + "min_sharp_avoidance_speed"); - } - - // yield - { - std::string ns = "avoidance.yield."; - p.yield_velocity = declare_parameter(ns + "yield_velocity"); - } - - // stop - { - std::string ns = "avoidance.stop."; - p.stop_min_distance = declare_parameter(ns + "min_distance"); - p.stop_max_distance = declare_parameter(ns + "max_distance"); - } - - // constraints - { - std::string ns = "avoidance.constraints."; - p.use_constraints_for_decel = declare_parameter(ns + "use_constraints_for_decel"); - } - - // constraints (longitudinal) - { - std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = declare_parameter(ns + "nominal_deceleration"); - p.nominal_jerk = declare_parameter(ns + "nominal_jerk"); - p.max_deceleration = declare_parameter(ns + "max_deceleration"); - p.max_jerk = declare_parameter(ns + "max_jerk"); - p.min_avoidance_speed_for_acc_prevention = - declare_parameter(ns + "min_avoidance_speed_for_acc_prevention"); - p.max_avoidance_acceleration = declare_parameter(ns + "max_avoidance_acceleration"); - } - - // constraints (lateral) - { - std::string ns = "avoidance.constraints.lateral."; - p.nominal_lateral_jerk = declare_parameter(ns + "nominal_lateral_jerk"); - p.max_lateral_jerk = declare_parameter(ns + "max_lateral_jerk"); - } - - // velocity matrix - { - std::string ns = "avoidance.target_velocity_matrix."; - p.col_size = declare_parameter(ns + "col_size"); - p.target_velocity_matrix = declare_parameter>(ns + "matrix"); - } - - // shift line pipeline - { - std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_filter_threshold = declare_parameter(ns + "trim.quantize_filter_threshold"); - p.same_grad_filter_1_threshold = - declare_parameter(ns + "trim.same_grad_filter_1_threshold"); - p.same_grad_filter_2_threshold = - declare_parameter(ns + "trim.same_grad_filter_2_threshold"); - p.same_grad_filter_3_threshold = - declare_parameter(ns + "trim.same_grad_filter_3_threshold"); - p.sharp_shift_filter_threshold = - declare_parameter(ns + "trim.sharp_shift_filter_threshold"); - } - - return p; -} - -DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam() -{ - DynamicAvoidanceParameters p{}; - - { // target object - std::string ns = "dynamic_avoidance.target_object."; - p.avoid_car = declare_parameter(ns + "car"); - p.avoid_truck = declare_parameter(ns + "truck"); - p.avoid_bus = declare_parameter(ns + "bus"); - p.avoid_trailer = declare_parameter(ns + "trailer"); - p.avoid_unknown = declare_parameter(ns + "unknown"); - p.avoid_bicycle = declare_parameter(ns + "bicycle"); - p.avoid_motorcycle = declare_parameter(ns + "motorcycle"); - p.avoid_pedestrian = declare_parameter(ns + "pedestrian"); - p.min_obstacle_vel = declare_parameter(ns + "min_obstacle_vel"); - p.successive_num_to_entry_dynamic_avoidance_condition = - declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); - } - - { // drivable_area_generation - std::string ns = "dynamic_avoidance.drivable_area_generation."; - p.lat_offset_from_obstacle = declare_parameter(ns + "lat_offset_from_obstacle"); - p.max_lat_offset_to_avoid = declare_parameter(ns + "max_lat_offset_to_avoid"); - - p.max_time_to_collision_overtaking_object = - declare_parameter(ns + "overtaking_object.max_time_to_collision"); - p.start_duration_to_avoid_overtaking_object = - declare_parameter(ns + "overtaking_object.start_duration_to_avoid"); - p.end_duration_to_avoid_overtaking_object = - declare_parameter(ns + "overtaking_object.end_duration_to_avoid"); - p.duration_to_hold_avoidance_overtaking_object = - declare_parameter(ns + "overtaking_object.duration_to_hold_avoidance"); - - p.max_time_to_collision_oncoming_object = - declare_parameter(ns + "oncoming_object.max_time_to_collision"); - p.start_duration_to_avoid_oncoming_object = - declare_parameter(ns + "oncoming_object.start_duration_to_avoid"); - p.end_duration_to_avoid_oncoming_object = - declare_parameter(ns + "oncoming_object.end_duration_to_avoid"); - } - - return p; -} - -LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() -{ - LaneChangeParameters p{}; - const auto parameter = [](std::string && name) { return "lane_change." + name; }; - - // trajectory generation - p.backward_lane_length = declare_parameter(parameter("backward_lane_length")); - p.prediction_time_resolution = declare_parameter(parameter("prediction_time_resolution")); - p.longitudinal_acc_sampling_num = - declare_parameter(parameter("longitudinal_acceleration_sampling_num")); - p.lateral_acc_sampling_num = - declare_parameter(parameter("lateral_acceleration_sampling_num")); - - // parked vehicle detection - p.object_check_min_road_shoulder_width = - declare_parameter(parameter("object_check_min_road_shoulder_width")); - p.object_shiftable_ratio_threshold = - declare_parameter(parameter("object_shiftable_ratio_threshold")); - - // turn signal - p.min_length_for_turn_signal_activation = - declare_parameter(parameter("min_length_for_turn_signal_activation")); - p.length_ratio_for_turn_signal_deactivation = - declare_parameter(parameter("length_ratio_for_turn_signal_deactivation")); - - // acceleration - p.min_longitudinal_acc = declare_parameter(parameter("min_longitudinal_acc")); - p.max_longitudinal_acc = declare_parameter(parameter("max_longitudinal_acc")); - - // collision check - p.enable_prepare_segment_collision_check = - declare_parameter(parameter("enable_prepare_segment_collision_check")); - p.prepare_segment_ignore_object_velocity_thresh = - declare_parameter(parameter("prepare_segment_ignore_object_velocity_thresh")); - p.use_predicted_path_outside_lanelet = - declare_parameter(parameter("use_predicted_path_outside_lanelet")); - p.use_all_predicted_path = declare_parameter(parameter("use_all_predicted_path")); - - // target object - { - std::string ns = "lane_change.target_object."; - p.check_car = declare_parameter(ns + "car"); - p.check_truck = declare_parameter(ns + "truck"); - p.check_bus = declare_parameter(ns + "bus"); - p.check_trailer = declare_parameter(ns + "trailer"); - p.check_unknown = declare_parameter(ns + "unknown"); - p.check_bicycle = declare_parameter(ns + "bicycle"); - p.check_motorcycle = declare_parameter(ns + "motorcycle"); - p.check_pedestrian = declare_parameter(ns + "pedestrian"); - } - - // lane change cancel - p.cancel.enable_on_prepare_phase = - declare_parameter(parameter("cancel.enable_on_prepare_phase")); - p.cancel.enable_on_lane_changing_phase = - declare_parameter(parameter("cancel.enable_on_lane_changing_phase")); - p.cancel.delta_time = declare_parameter(parameter("cancel.delta_time")); - p.cancel.duration = declare_parameter(parameter("cancel.duration")); - p.cancel.max_lateral_jerk = declare_parameter(parameter("cancel.max_lateral_jerk")); - p.cancel.overhang_tolerance = declare_parameter(parameter("cancel.overhang_tolerance")); - - p.finish_judge_lateral_threshold = - declare_parameter("lane_change.finish_judge_lateral_threshold"); - - // debug marker - p.publish_debug_marker = declare_parameter(parameter("publish_debug_marker")); - - // validation of parameters - if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - get_logger(), - "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " - << p.longitudinal_acc_sampling_num - << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - - if (p.cancel.delta_time < 1.0) { - RCLCPP_WARN_STREAM( - get_logger(), "cancel.delta_time: " << p.cancel.delta_time - << ", is too short. This could cause a danger behavior."); - } - - return p; -} - -GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() -{ - GoalPlannerParameters p; - - // general params - { - std::string ns = "goal_planner."; - p.minimum_request_length = declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); - } - - // goal search - { - std::string ns = "goal_planner.goal_search."; - p.search_priority = declare_parameter(ns + "search_priority"); - p.forward_goal_search_length = declare_parameter(ns + "forward_goal_search_length"); - p.backward_goal_search_length = declare_parameter(ns + "backward_goal_search_length"); - p.goal_search_interval = declare_parameter(ns + "goal_search_interval"); - p.longitudinal_margin = declare_parameter(ns + "longitudinal_margin"); - p.max_lateral_offset = declare_parameter(ns + "max_lateral_offset"); - p.lateral_offset_interval = declare_parameter(ns + "lateral_offset_interval"); - p.ignore_distance_from_lane_start = - declare_parameter(ns + "ignore_distance_from_lane_start"); - p.margin_from_boundary = declare_parameter(ns + "margin_from_boundary"); - - const std::string parking_policy_name = declare_parameter(ns + "parking_policy"); - if (parking_policy_name == "left_side") { - p.parking_policy = ParkingPolicy::LEFT_SIDE; - } else if (parking_policy_name == "right_side") { - p.parking_policy = ParkingPolicy::RIGHT_SIDE; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); - exit(EXIT_FAILURE); - } - } - - // occupancy grid map - { - std::string ns = "goal_planner.occupancy_grid."; - p.use_occupancy_grid = declare_parameter(ns + "use_occupancy_grid"); - p.use_occupancy_grid_for_longitudinal_margin = - declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); - p.occupancy_grid_collision_check_margin = - declare_parameter(ns + "occupancy_grid_collision_check_margin"); - p.theta_size = declare_parameter(ns + "theta_size"); - p.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); - } - - // object recognition - { - std::string ns = "goal_planner.object_recognition."; - p.use_object_recognition = declare_parameter(ns + "use_object_recognition"); - p.object_recognition_collision_check_margin = - declare_parameter(ns + "object_recognition_collision_check_margin"); - } - - // pull over general params - { - std::string ns = "goal_planner.pull_over."; - p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); - p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); - p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); - p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); - p.maximum_jerk = declare_parameter(ns + "maximum_jerk"); - } - - // shift parking - { - std::string ns = "goal_planner.pull_over.shift_parking."; - p.enable_shift_parking = declare_parameter(ns + "enable_shift_parking"); - p.shift_sampling_num = declare_parameter(ns + "shift_sampling_num"); - p.maximum_lateral_jerk = declare_parameter(ns + "maximum_lateral_jerk"); - p.minimum_lateral_jerk = declare_parameter(ns + "minimum_lateral_jerk"); - p.deceleration_interval = declare_parameter(ns + "deceleration_interval"); - p.after_shift_straight_distance = - declare_parameter(ns + "after_shift_straight_distance"); - } - - // forward parallel parking forward - { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; - p.enable_arc_forward_parking = declare_parameter(ns + "enable_arc_forward_parking"); - p.parallel_parking_parameters.after_forward_parking_straight_distance = - declare_parameter(ns + "after_forward_parking_straight_distance"); - p.parallel_parking_parameters.forward_parking_velocity = - declare_parameter(ns + "forward_parking_velocity"); - p.parallel_parking_parameters.forward_parking_lane_departure_margin = - declare_parameter(ns + "forward_parking_lane_departure_margin"); - p.parallel_parking_parameters.forward_parking_path_interval = - declare_parameter(ns + "forward_parking_path_interval"); - p.parallel_parking_parameters.forward_parking_max_steer_angle = - declare_parameter(ns + "forward_parking_max_steer_angle"); // 20deg - } - - // forward parallel parking backward - { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; - p.enable_arc_backward_parking = declare_parameter(ns + "enable_arc_backward_parking"); - p.parallel_parking_parameters.after_backward_parking_straight_distance = - declare_parameter(ns + "after_backward_parking_straight_distance"); - p.parallel_parking_parameters.backward_parking_velocity = - declare_parameter(ns + "backward_parking_velocity"); - p.parallel_parking_parameters.backward_parking_lane_departure_margin = - declare_parameter(ns + "backward_parking_lane_departure_margin"); - p.parallel_parking_parameters.backward_parking_path_interval = - declare_parameter(ns + "backward_parking_path_interval"); - p.parallel_parking_parameters.backward_parking_max_steer_angle = - declare_parameter(ns + "backward_parking_max_steer_angle"); // 20deg - } - - // freespace parking general params - { - std::string ns = "goal_planner.pull_over.freespace_parking."; - p.enable_freespace_parking = declare_parameter(ns + "enable_freespace_parking"); - p.freespace_parking_algorithm = - declare_parameter(ns + "freespace_parking_algorithm"); - p.freespace_parking_velocity = declare_parameter(ns + "velocity"); - p.vehicle_shape_margin = declare_parameter(ns + "vehicle_shape_margin"); - p.freespace_parking_common_parameters.time_limit = declare_parameter(ns + "time_limit"); - p.freespace_parking_common_parameters.minimum_turning_radius = - declare_parameter(ns + "minimum_turning_radius"); - p.freespace_parking_common_parameters.maximum_turning_radius = - declare_parameter(ns + "maximum_turning_radius"); - p.freespace_parking_common_parameters.turning_radius_size = - declare_parameter(ns + "turning_radius_size"); - p.freespace_parking_common_parameters.maximum_turning_radius = std::max( - p.freespace_parking_common_parameters.maximum_turning_radius, - p.freespace_parking_common_parameters.minimum_turning_radius); - p.freespace_parking_common_parameters.turning_radius_size = - std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); - } - - // freespace parking search config - { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; - p.freespace_parking_common_parameters.theta_size = declare_parameter(ns + "theta_size"); - p.freespace_parking_common_parameters.angle_goal_range = - declare_parameter(ns + "angle_goal_range"); - p.freespace_parking_common_parameters.curve_weight = - declare_parameter(ns + "curve_weight"); - p.freespace_parking_common_parameters.reverse_weight = - declare_parameter(ns + "reverse_weight"); - p.freespace_parking_common_parameters.lateral_goal_range = - declare_parameter(ns + "lateral_goal_range"); - p.freespace_parking_common_parameters.longitudinal_goal_range = - declare_parameter(ns + "longitudinal_goal_range"); - } - - // freespace parking costmap configs - { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; - p.freespace_parking_common_parameters.obstacle_threshold = - declare_parameter(ns + "obstacle_threshold"); - } - - // freespace parking astar - { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; - p.astar_parameters.only_behind_solutions = - declare_parameter(ns + "only_behind_solutions"); - p.astar_parameters.use_back = declare_parameter(ns + "use_back"); - p.astar_parameters.distance_heuristic_weight = - declare_parameter(ns + "distance_heuristic_weight"); - } - - // freespace parking rrtstar - { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; - p.rrt_star_parameters.enable_update = declare_parameter(ns + "enable_update"); - p.rrt_star_parameters.use_informed_sampling = - declare_parameter(ns + "use_informed_sampling"); - p.rrt_star_parameters.max_planning_time = declare_parameter(ns + "max_planning_time"); - p.rrt_star_parameters.neighbor_radius = declare_parameter(ns + "neighbor_radius"); - p.rrt_star_parameters.margin = declare_parameter(ns + "margin"); - } - - // debug - { - std::string ns = "goal_planner.debug."; - p.print_debug_info = declare_parameter(ns + "print_debug_info"); - } - - // validation of parameters - if (p.shift_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - get_logger(), "shift_sampling_num must be positive integer. Given parameter: " - << p.shift_sampling_num << std::endl - << "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; -} - -StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() -{ - StartPlannerParameters p; - - std::string ns = "start_planner."; - - p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); - p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); - p.th_turn_signal_on_lateral_offset = - declare_parameter(ns + "th_turn_signal_on_lateral_offset"); - p.intersection_search_length = declare_parameter(ns + "intersection_search_length"); - p.length_ratio_for_turn_signal_deactivation_near_intersection = - declare_parameter(ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); - p.collision_check_margin = declare_parameter(ns + "collision_check_margin"); - p.collision_check_distance_from_end = - declare_parameter(ns + "collision_check_distance_from_end"); - // shift pull out - p.enable_shift_pull_out = declare_parameter(ns + "enable_shift_pull_out"); - p.minimum_shift_pull_out_distance = - declare_parameter(ns + "minimum_shift_pull_out_distance"); - p.lateral_acceleration_sampling_num = - declare_parameter(ns + "lateral_acceleration_sampling_num"); - p.lateral_jerk = declare_parameter(ns + "lateral_jerk"); - p.maximum_lateral_acc = declare_parameter(ns + "maximum_lateral_acc"); - p.minimum_lateral_acc = declare_parameter(ns + "minimum_lateral_acc"); - p.deceleration_interval = declare_parameter(ns + "deceleration_interval"); - // geometric pull out - p.enable_geometric_pull_out = declare_parameter(ns + "enable_geometric_pull_out"); - p.divide_pull_out_path = declare_parameter(ns + "divide_pull_out_path"); - p.parallel_parking_parameters.pull_out_velocity = - declare_parameter(ns + "geometric_pull_out_velocity"); - p.parallel_parking_parameters.pull_out_path_interval = - declare_parameter(ns + "arc_path_interval"); - p.parallel_parking_parameters.pull_out_lane_departure_margin = - declare_parameter(ns + "lane_departure_margin"); - p.parallel_parking_parameters.pull_out_max_steer_angle = - declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg - // search start pose backward - p.search_priority = declare_parameter( - ns + "search_priority"); // "efficient_path" or "short_back_distance" - p.enable_back = declare_parameter(ns + "enable_back"); - p.backward_velocity = declare_parameter(ns + "backward_velocity"); - p.max_back_distance = declare_parameter(ns + "max_back_distance"); - p.backward_search_resolution = declare_parameter(ns + "backward_search_resolution"); - p.backward_path_update_duration = declare_parameter(ns + "backward_path_update_duration"); - p.ignore_distance_from_lane_end = declare_parameter(ns + "ignore_distance_from_lane_end"); - - // validation of parameters - if (p.lateral_acceleration_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - get_logger(), "lateral_acceleration_sampling_num must be positive integer. Given parameter: " - << p.lateral_acceleration_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } - - return p; -} - // wait until mandatory data is ready bool BehaviorPathPlannerNode::isDataReady() { @@ -1603,12 +965,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( rcl_interfaces::msg::SetParametersResult result; - if (!lane_change_param_ptr_ && !avoidance_param_ptr_) { - result.successful = false; - result.reason = "param not initialized"; - return result; - } - { const std::lock_guard lock(mutex_manager_); // for planner_manager_ planner_manager_->updateModuleParams(parameters); @@ -1618,8 +974,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( result.reason = "success"; try { - updateParam( - parameters, "lane_change.publish_debug_marker", lane_change_param_ptr_->publish_debug_marker); // Drivable area expansion parameters using drivable_area_expansion::DrivableAreaExpansionParameters; const std::lock_guard lock(mutex_pd_); // for planner_data_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 816ed94e5b822..f9a652a4cec27 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -23,11 +23,209 @@ namespace behavior_path_planner { +namespace +{ +template +T get_parameter(rclcpp::Node * node, const std::string & ns) +{ + if (node->has_parameter(ns)) { + return node->get_parameter(ns).get_value(); + } + + return node->declare_parameter(ns); +} +} // namespace + AvoidanceModuleManager::AvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {"left", "right"}), parameters_{parameters} + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) +: SceneModuleManagerInterface(node, name, config, {"left", "right"}) { + using autoware_auto_perception_msgs::msg::ObjectClassification; + + AvoidanceParameters p{}; + // general params + { + std::string ns = "avoidance."; + p.resample_interval_for_planning = + get_parameter(node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + get_parameter(node, ns + "resample_interval_for_output"); + p.detection_area_right_expand_dist = + get_parameter(node, ns + "detection_area_right_expand_dist"); + p.detection_area_left_expand_dist = + get_parameter(node, ns + "detection_area_left_expand_dist"); + p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); + p.enable_avoidance_over_same_direction = + get_parameter(node, ns + "enable_avoidance_over_same_direction"); + p.enable_avoidance_over_opposite_direction = + get_parameter(node, ns + "enable_avoidance_over_opposite_direction"); + p.enable_update_path_when_object_is_gone = + get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_force_avoidance_for_stopped_vehicle = + get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); + p.enable_safety_check = get_parameter(node, ns + "enable_safety_check"); + p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); + p.enable_yield_maneuver_during_shifting = + get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); + p.disable_path_update = get_parameter(node, ns + "disable_path_update"); + p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); + p.publish_debug_marker = get_parameter(node, ns + "publish_debug_marker"); + p.print_debug_info = get_parameter(node, ns + "print_debug_info"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.is_target = get_parameter(node, ns + "is_target"); + param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); + param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); + param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); + param.safety_buffer_longitudinal = + get_parameter(node, ns + "safety_buffer_longitudinal"); + return param; + }; + + const std::string ns = "avoidance.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + get_parameter(node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + get_parameter(node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + std::string ns = "avoidance.target_filtering."; + p.threshold_time_force_avoidance_for_stopped_vehicle = + get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_ignore_section_traffic_light_in_front_distance = + get_parameter(node, ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + get_parameter(node, ns + "object_ignore_section_crosswalk_in_front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + get_parameter(node, ns + "object_ignore_section_crosswalk_behind_distance"); + p.object_check_forward_distance = + get_parameter(node, ns + "object_check_forward_distance"); + p.object_check_backward_distance = + get_parameter(node, ns + "object_check_backward_distance"); + p.object_check_goal_distance = get_parameter(node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + get_parameter(node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + get_parameter(node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + get_parameter(node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); + } + + // safety check + { + std::string ns = "avoidance.safety_check."; + p.safety_check_backward_distance = + get_parameter(node, ns + "safety_check_backward_distance"); + p.safety_check_time_horizon = get_parameter(node, ns + "safety_check_time_horizon"); + p.safety_check_idling_time = get_parameter(node, ns + "safety_check_idling_time"); + p.safety_check_accel_for_rss = get_parameter(node, ns + "safety_check_accel_for_rss"); + p.safety_check_hysteresis_factor = + get_parameter(node, ns + "safety_check_hysteresis_factor"); + } + + // avoidance maneuver (lateral) + { + std::string ns = "avoidance.avoidance.lateral."; + p.road_shoulder_safety_margin = get_parameter(node, ns + "road_shoulder_safety_margin"); + p.lateral_execution_threshold = get_parameter(node, ns + "lateral_execution_threshold"); + p.lateral_small_shift_threshold = + get_parameter(node, ns + "lateral_small_shift_threshold"); + p.max_right_shift_length = get_parameter(node, ns + "max_right_shift_length"); + p.max_left_shift_length = get_parameter(node, ns + "max_left_shift_length"); + } + + // avoidance maneuver (longitudinal) + { + std::string ns = "avoidance.avoidance.longitudinal."; + p.prepare_time = get_parameter(node, ns + "prepare_time"); + p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); + p.min_avoidance_distance = get_parameter(node, ns + "min_avoidance_distance"); + p.min_nominal_avoidance_speed = get_parameter(node, ns + "min_nominal_avoidance_speed"); + p.min_sharp_avoidance_speed = get_parameter(node, ns + "min_sharp_avoidance_speed"); + } + + // yield + { + std::string ns = "avoidance.yield."; + p.yield_velocity = get_parameter(node, ns + "yield_velocity"); + } + + // stop + { + std::string ns = "avoidance.stop."; + p.stop_min_distance = get_parameter(node, ns + "min_distance"); + p.stop_max_distance = get_parameter(node, ns + "max_distance"); + } + + // constraints + { + std::string ns = "avoidance.constraints."; + p.use_constraints_for_decel = get_parameter(node, ns + "use_constraints_for_decel"); + } + + // constraints (longitudinal) + { + std::string ns = "avoidance.constraints.longitudinal."; + p.nominal_deceleration = get_parameter(node, ns + "nominal_deceleration"); + p.nominal_jerk = get_parameter(node, ns + "nominal_jerk"); + p.max_deceleration = get_parameter(node, ns + "max_deceleration"); + p.max_jerk = get_parameter(node, ns + "max_jerk"); + p.min_avoidance_speed_for_acc_prevention = + get_parameter(node, ns + "min_avoidance_speed_for_acc_prevention"); + p.max_avoidance_acceleration = get_parameter(node, ns + "max_avoidance_acceleration"); + } + + // constraints (lateral) + { + std::string ns = "avoidance.constraints.lateral."; + p.nominal_lateral_jerk = get_parameter(node, ns + "nominal_lateral_jerk"); + p.max_lateral_jerk = get_parameter(node, ns + "max_lateral_jerk"); + } + + // velocity matrix + { + std::string ns = "avoidance.target_velocity_matrix."; + p.col_size = get_parameter(node, ns + "col_size"); + p.target_velocity_matrix = get_parameter>(node, ns + "matrix"); + } + + // shift line pipeline + { + std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = + get_parameter(node, ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + get_parameter(node, ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + get_parameter(node, ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + get_parameter(node, ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + get_parameter(node, ns + "trim.sharp_shift_filter_threshold"); + } + + parameters_ = std::make_shared(p); } void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index b9ca4ffa964f7..08188343f1d64 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -24,10 +24,49 @@ namespace behavior_path_planner { DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) +: SceneModuleManagerInterface(node, name, config, {""}) { + DynamicAvoidanceParameters p{}; + + { // target object + std::string ns = "dynamic_avoidance.target_object."; + p.avoid_car = node->declare_parameter(ns + "car"); + p.avoid_truck = node->declare_parameter(ns + "truck"); + p.avoid_bus = node->declare_parameter(ns + "bus"); + p.avoid_trailer = node->declare_parameter(ns + "trailer"); + p.avoid_unknown = node->declare_parameter(ns + "unknown"); + p.avoid_bicycle = node->declare_parameter(ns + "bicycle"); + p.avoid_motorcycle = node->declare_parameter(ns + "motorcycle"); + p.avoid_pedestrian = node->declare_parameter(ns + "pedestrian"); + p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); + p.successive_num_to_entry_dynamic_avoidance_condition = + node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); + } + + { // drivable_area_generation + std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); + p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); + + p.max_time_to_collision_overtaking_object = + node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); + p.start_duration_to_avoid_overtaking_object = + node->declare_parameter(ns + "overtaking_object.start_duration_to_avoid"); + p.end_duration_to_avoid_overtaking_object = + node->declare_parameter(ns + "overtaking_object.end_duration_to_avoid"); + p.duration_to_hold_avoidance_overtaking_object = + node->declare_parameter(ns + "overtaking_object.duration_to_hold_avoidance"); + + p.max_time_to_collision_oncoming_object = + node->declare_parameter(ns + "oncoming_object.max_time_to_collision"); + p.start_duration_to_avoid_oncoming_object = + node->declare_parameter(ns + "oncoming_object.start_duration_to_avoid"); + p.end_duration_to_avoid_oncoming_object = + node->declare_parameter(ns + "oncoming_object.end_duration_to_avoid"); + } + + parameters_ = std::make_shared(p); } void DynamicAvoidanceModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index af96313aa0c19..caa75927f4062 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -24,10 +24,217 @@ namespace behavior_path_planner { GoalPlannerModuleManager::GoalPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) +: SceneModuleManagerInterface(node, name, config, {""}) { + GoalPlannerParameters p; + + // general params + { + std::string ns = "goal_planner."; + p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); + p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + } + + // goal search + { + std::string ns = "goal_planner.goal_search."; + p.search_priority = node->declare_parameter(ns + "search_priority"); + p.forward_goal_search_length = + node->declare_parameter(ns + "forward_goal_search_length"); + p.backward_goal_search_length = + node->declare_parameter(ns + "backward_goal_search_length"); + p.goal_search_interval = node->declare_parameter(ns + "goal_search_interval"); + p.longitudinal_margin = node->declare_parameter(ns + "longitudinal_margin"); + p.max_lateral_offset = node->declare_parameter(ns + "max_lateral_offset"); + p.lateral_offset_interval = node->declare_parameter(ns + "lateral_offset_interval"); + p.ignore_distance_from_lane_start = + node->declare_parameter(ns + "ignore_distance_from_lane_start"); + p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); + + const std::string parking_policy_name = + node->declare_parameter(ns + "parking_policy"); + if (parking_policy_name == "left_side") { + p.parking_policy = ParkingPolicy::LEFT_SIDE; + } else if (parking_policy_name == "right_side") { + p.parking_policy = ParkingPolicy::RIGHT_SIDE; + } else { + RCLCPP_ERROR_STREAM( + logger_, "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); + exit(EXIT_FAILURE); + } + } + + // occupancy grid map + { + std::string ns = "goal_planner.occupancy_grid."; + p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); + p.use_occupancy_grid_for_longitudinal_margin = + node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + p.occupancy_grid_collision_check_margin = + node->declare_parameter(ns + "occupancy_grid_collision_check_margin"); + p.theta_size = node->declare_parameter(ns + "theta_size"); + p.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); + } + + // object recognition + { + std::string ns = "goal_planner.object_recognition."; + p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); + p.object_recognition_collision_check_margin = + node->declare_parameter(ns + "object_recognition_collision_check_margin"); + } + + // pull over general params + { + std::string ns = "goal_planner.pull_over."; + p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); + p.pull_over_minimum_velocity = + node->declare_parameter(ns + "pull_over_minimum_velocity"); + p.decide_path_distance = node->declare_parameter(ns + "decide_path_distance"); + p.maximum_deceleration = node->declare_parameter(ns + "maximum_deceleration"); + p.maximum_jerk = node->declare_parameter(ns + "maximum_jerk"); + } + + // shift parking + { + std::string ns = "goal_planner.pull_over.shift_parking."; + p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); + p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); + p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); + p.minimum_lateral_jerk = node->declare_parameter(ns + "minimum_lateral_jerk"); + p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); + p.after_shift_straight_distance = + node->declare_parameter(ns + "after_shift_straight_distance"); + } + + // forward parallel parking forward + { + std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); + p.parallel_parking_parameters.after_forward_parking_straight_distance = + node->declare_parameter(ns + "after_forward_parking_straight_distance"); + p.parallel_parking_parameters.forward_parking_velocity = + node->declare_parameter(ns + "forward_parking_velocity"); + p.parallel_parking_parameters.forward_parking_lane_departure_margin = + node->declare_parameter(ns + "forward_parking_lane_departure_margin"); + p.parallel_parking_parameters.forward_parking_path_interval = + node->declare_parameter(ns + "forward_parking_path_interval"); + p.parallel_parking_parameters.forward_parking_max_steer_angle = + node->declare_parameter(ns + "forward_parking_max_steer_angle"); // 20deg + } + + // forward parallel parking backward + { + std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + p.enable_arc_backward_parking = + node->declare_parameter(ns + "enable_arc_backward_parking"); + p.parallel_parking_parameters.after_backward_parking_straight_distance = + node->declare_parameter(ns + "after_backward_parking_straight_distance"); + p.parallel_parking_parameters.backward_parking_velocity = + node->declare_parameter(ns + "backward_parking_velocity"); + p.parallel_parking_parameters.backward_parking_lane_departure_margin = + node->declare_parameter(ns + "backward_parking_lane_departure_margin"); + p.parallel_parking_parameters.backward_parking_path_interval = + node->declare_parameter(ns + "backward_parking_path_interval"); + p.parallel_parking_parameters.backward_parking_max_steer_angle = + node->declare_parameter(ns + "backward_parking_max_steer_angle"); // 20deg + } + + // freespace parking general params + { + std::string ns = "goal_planner.pull_over.freespace_parking."; + p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); + p.freespace_parking_algorithm = + node->declare_parameter(ns + "freespace_parking_algorithm"); + p.freespace_parking_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_parking_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_parking_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_parking_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_parking_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_parking_common_parameters.maximum_turning_radius = std::max( + p.freespace_parking_common_parameters.maximum_turning_radius, + p.freespace_parking_common_parameters.minimum_turning_radius); + p.freespace_parking_common_parameters.turning_radius_size = + std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); + } + + // freespace parking search config + { + std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + p.freespace_parking_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_parking_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_parking_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_parking_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_parking_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_parking_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + + // freespace parking costmap configs + { + std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + p.freespace_parking_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + + // freespace parking astar + { + std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + p.astar_parameters.only_behind_solutions = + node->declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + + // freespace parking rrtstar + { + std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + node->declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); + } + + // debug + { + std::string ns = "goal_planner.debug."; + p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); + } + + // validation of parameters + if (p.shift_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + logger_, "shift_sampling_num must be positive integer. Given parameter: " + << p.shift_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + if (p.maximum_deceleration < 0.0) { + RCLCPP_FATAL_STREAM( + logger_, "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + parameters_ = std::make_shared(p); } void GoalPlannerModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 1a1d680a99805..c808703b8f961 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -25,18 +25,16 @@ namespace behavior_path_planner { AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, - std::shared_ptr avoidance_parameters, - std::shared_ptr avoidance_by_lane_change_parameters) + std::shared_ptr avoidance_parameters) : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), - avoidance_parameters_(std::move(avoidance_parameters)), - avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) + avoidance_parameters_(std::move(avoidance_parameters)) { } std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_path) const { const auto & avoidance_objects = avoidance_data_.target_objects; - const auto execute_object_num = avoidance_by_lane_change_parameters_->execute_object_num; + const auto execute_object_num = avoidance_parameters_->execute_object_num; if (avoidance_objects.size() < execute_object_num) { return {false, false}; @@ -52,7 +50,7 @@ std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_p // check distance from ego to o_front vs acceptable longitudinal margin const auto execute_object_longitudinal_margin = - avoidance_by_lane_change_parameters_->execute_object_longitudinal_margin; + avoidance_parameters_->execute_object_longitudinal_margin; if (execute_object_longitudinal_margin > o_front.longitudinal) { return {false, false}; } @@ -84,7 +82,7 @@ std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_p safe_path.path.points, getEgoPose().position, safe_path.shift_line.end.position); const auto lane_change_finish_before_object = o_front.longitudinal > to_lane_change_end_distance; const auto execute_only_when_lane_change_finish_before_object = - avoidance_by_lane_change_parameters_->execute_only_when_lane_change_finish_before_object; + avoidance_parameters_->execute_only_when_lane_change_finish_before_object; if (!lane_change_finish_before_object && execute_only_when_lane_change_finish_before_object) { return {false, found_safe_path}; } @@ -93,12 +91,12 @@ std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_p void AvoidanceByLaneChange::updateSpecialData() { + const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + avoidance_debug_data_ = DebugData(); avoidance_data_ = calcAvoidancePlanningData(avoidance_debug_data_); - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, - avoidance_by_lane_change_parameters_->avoidance); + utils::avoidance::updateRegisteredObject(registered_objects_, avoidance_data_.target_objects, p); utils::avoidance::compensateDetectionLost( registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); @@ -117,8 +115,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( data.reference_path_rough = prev_module_path_; - const auto resample_interval = - avoidance_by_lane_change_parameters_->avoidance->resample_interval_for_planning; + const auto resample_interval = avoidance_parameters_->resample_interval_for_planning; data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); data.current_lanelets = getCurrentLanes(); @@ -133,8 +130,10 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( void AvoidanceByLaneChange::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - const auto left_expand_dist = avoidance_parameters_->detection_area_left_expand_dist; - const auto right_expand_dist = avoidance_parameters_->detection_area_right_expand_dist; + const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + + const auto left_expand_dist = p->detection_area_left_expand_dist; + const auto right_expand_dist = p->detection_area_right_expand_dist; const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( data.current_lanelets, left_expand_dist, right_expand_dist * (-1.0)); @@ -173,8 +172,7 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [&](const auto & object) { return createObjectData(data, object); }); } - utils::avoidance::filterTargetObjects( - target_lane_objects, data, debug, planner_data_, avoidance_parameters_); + utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p); } ObjectData AvoidanceByLaneChange::createObjectData( @@ -184,32 +182,34 @@ ObjectData AvoidanceByLaneChange::createObjectData( using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; + const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + const auto & path_points = data.reference_path.points; const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; const auto t = utils::getHighestProbLabel(object.classification); - const auto object_parameter = avoidance_parameters_->object_parameters.at(t); + const auto object_parameter = p->object_parameters.at(t); ObjectData object_data{}; object_data.object = object; - const auto lower = avoidance_parameters_->lower_distance_for_polygon_expansion; - const auto upper = avoidance_parameters_->upper_distance_for_polygon_expansion; + const auto lower = p->lower_distance_for_polygon_expansion; + const auto upper = p->upper_distance_for_polygon_expansion; const auto clamp = std::clamp(calcDistance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper; object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. utils::avoidance::fillObjectEnvelopePolygon( - object_data, registered_objects_, object_closest_pose, avoidance_parameters_); + object_data, registered_objects_, object_closest_pose, p); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, avoidance_parameters_); + utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); // Calc lateral deviation from path to target object. object_data.lateral = @@ -221,8 +221,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity( - object_data, registered_objects_, vehicle_width, avoidance_parameters_); + utils::avoidance::fillAvoidanceNecessity(object_data, registered_objects_, vehicle_width, p); return object_data; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 713188afb7f0b..721c7fa6fd390 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -487,13 +487,11 @@ void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * in AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map > & rtc_interface_ptr_map) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, - std::make_unique( - parameters, avoidance_parameters, avoidance_by_lane_change_parameters)} + std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index b410d4d75840b..f336d1325b688 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -23,17 +23,112 @@ namespace behavior_path_planner { + using route_handler::Direction; using utils::convertToSnakeCase; + +namespace +{ +template +T get_parameter(rclcpp::Node * node, const std::string & ns) +{ + if (node->has_parameter(ns)) { + return node->get_parameter(ns).get_value(); + } + + return node->declare_parameter(ns); +} +} // namespace + LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters, const Direction direction, - const LaneChangeModuleType type) -: SceneModuleManagerInterface(node, name, config, {""}), - parameters_{std::move(parameters)}, - direction_{direction}, - type_{type} + const Direction direction, const LaneChangeModuleType type) +: SceneModuleManagerInterface(node, name, config, {""}), direction_{direction}, type_{type} { + LaneChangeParameters p{}; + + const auto parameter = [](std::string && name) { return "lane_change." + name; }; + + // trajectory generation + p.backward_lane_length = get_parameter(node, parameter("backward_lane_length")); + p.prediction_time_resolution = + get_parameter(node, parameter("prediction_time_resolution")); + p.longitudinal_acc_sampling_num = + get_parameter(node, parameter("longitudinal_acceleration_sampling_num")); + p.lateral_acc_sampling_num = + get_parameter(node, parameter("lateral_acceleration_sampling_num")); + + // parked vehicle detection + p.object_check_min_road_shoulder_width = + get_parameter(node, parameter("object_check_min_road_shoulder_width")); + p.object_shiftable_ratio_threshold = + get_parameter(node, parameter("object_shiftable_ratio_threshold")); + + // turn signal + p.min_length_for_turn_signal_activation = + get_parameter(node, parameter("min_length_for_turn_signal_activation")); + p.length_ratio_for_turn_signal_deactivation = + get_parameter(node, parameter("length_ratio_for_turn_signal_deactivation")); + + // acceleration + p.min_longitudinal_acc = get_parameter(node, parameter("min_longitudinal_acc")); + p.max_longitudinal_acc = get_parameter(node, parameter("max_longitudinal_acc")); + + // collision check + p.enable_prepare_segment_collision_check = + get_parameter(node, parameter("enable_prepare_segment_collision_check")); + p.prepare_segment_ignore_object_velocity_thresh = + get_parameter(node, parameter("prepare_segment_ignore_object_velocity_thresh")); + p.use_predicted_path_outside_lanelet = + get_parameter(node, parameter("use_predicted_path_outside_lanelet")); + p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); + + // target object + { + std::string ns = "lane_change.target_object."; + p.check_car = get_parameter(node, ns + "car"); + p.check_truck = get_parameter(node, ns + "truck"); + p.check_bus = get_parameter(node, ns + "bus"); + p.check_trailer = get_parameter(node, ns + "trailer"); + p.check_unknown = get_parameter(node, ns + "unknown"); + p.check_bicycle = get_parameter(node, ns + "bicycle"); + p.check_motorcycle = get_parameter(node, ns + "motorcycle"); + p.check_pedestrian = get_parameter(node, ns + "pedestrian"); + } + + // lane change cancel + p.cancel.enable_on_prepare_phase = + get_parameter(node, parameter("cancel.enable_on_prepare_phase")); + p.cancel.enable_on_lane_changing_phase = + get_parameter(node, parameter("cancel.enable_on_lane_changing_phase")); + p.cancel.delta_time = get_parameter(node, parameter("cancel.delta_time")); + p.cancel.duration = get_parameter(node, parameter("cancel.duration")); + p.cancel.max_lateral_jerk = get_parameter(node, parameter("cancel.max_lateral_jerk")); + p.cancel.overhang_tolerance = get_parameter(node, parameter("cancel.overhang_tolerance")); + + p.finish_judge_lateral_threshold = + get_parameter(node, parameter("finish_judge_lateral_threshold")); + + // debug marker + p.publish_debug_marker = get_parameter(node, parameter("publish_debug_marker")); + + // validation of parameters + if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + logger_, "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.longitudinal_acc_sampling_num + << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + if (p.cancel.delta_time < 1.0) { + RCLCPP_WARN_STREAM( + logger_, "cancel.delta_time: " << p.cancel.delta_time + << ", is too short. This could cause a danger behavior."); + } + + parameters_ = std::make_shared(p); } std::shared_ptr LaneChangeModuleManager::createNewSceneModuleInstance() @@ -64,16 +159,12 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector parameters, - std::shared_ptr avoidance_parameters, - std::shared_ptr avoidance_by_lane_change_parameters) + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) : LaneChangeModuleManager( - node, name, config, std::move(parameters), Direction::NONE, - LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE), - avoidance_parameters_(std::move(avoidance_parameters)), - avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) + node, name, config, Direction::NONE, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + using autoware_auto_perception_msgs::msg::ObjectClassification; + rtc_interface_ptr_map_.clear(); const std::vector rtc_types = {"left", "right"}; for (const auto & rtc_type : rtc_types) { @@ -82,14 +173,110 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( rtc_interface_ptr_map_.emplace( rtc_type, std::make_shared(node, rtc_interface_name)); } + + AvoidanceByLCParameters p{}; + // unique parameters + { + std::string ns = "avoidance_by_lane_change."; + p.execute_object_num = get_parameter(node, ns + "execute_object_num"); + p.execute_object_longitudinal_margin = + get_parameter(node, ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = + get_parameter(node, ns + "execute_only_when_lane_change_finish_before_object"); + } + + if (p.execute_object_num < 1) { + RCLCPP_WARN_STREAM(logger_, "execute_object_num cannot be lesser than 1."); + } + + // general params + { + std::string ns = "avoidance."; + p.resample_interval_for_planning = + get_parameter(node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + get_parameter(node, ns + "resample_interval_for_output"); + p.detection_area_right_expand_dist = + get_parameter(node, ns + "detection_area_right_expand_dist"); + p.detection_area_left_expand_dist = + get_parameter(node, ns + "detection_area_left_expand_dist"); + p.enable_avoidance_over_same_direction = + get_parameter(node, ns + "enable_avoidance_over_same_direction"); + p.enable_avoidance_over_opposite_direction = + get_parameter(node, ns + "enable_avoidance_over_opposite_direction"); + p.enable_update_path_when_object_is_gone = + get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_force_avoidance_for_stopped_vehicle = + get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.is_target = get_parameter(node, ns + "is_target"); + param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); + param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); + param.max_expand_ratio = get_parameter(node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = get_parameter(node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = get_parameter(node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = get_parameter(node, ns + "safety_buffer_lateral"); + param.safety_buffer_longitudinal = + get_parameter(node, ns + "safety_buffer_longitudinal"); + return param; + }; + + const std::string ns = "avoidance.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + get_parameter(node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + get_parameter(node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + std::string ns = "avoidance.target_filtering."; + p.threshold_time_force_avoidance_for_stopped_vehicle = + get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); + p.object_ignore_section_traffic_light_in_front_distance = + get_parameter(node, ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + get_parameter(node, ns + "object_ignore_section_crosswalk_in_front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + get_parameter(node, ns + "object_ignore_section_crosswalk_behind_distance"); + p.object_check_forward_distance = + get_parameter(node, ns + "object_check_forward_distance"); + p.object_check_backward_distance = + get_parameter(node, ns + "object_check_backward_distance"); + p.object_check_goal_distance = get_parameter(node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + get_parameter(node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + get_parameter(node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + get_parameter(node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); + } + + avoidance_parameters_ = std::make_shared(p); } std::shared_ptr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_shared( - name_, *node_, parameters_, avoidance_parameters_, avoidance_by_lane_change_parameters_, - rtc_interface_ptr_map_); + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 91ae50e196b0a..5c92d7ced6036 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -24,10 +24,21 @@ namespace behavior_path_planner { SideShiftModuleManager::SideShiftModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {}), parameters_{parameters} + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) +: SceneModuleManagerInterface(node, name, config, {}) { + SideShiftParameters p{}; + + p.min_distance_to_start_shifting = + node->declare_parameter(name + ".min_distance_to_start_shifting"); + p.time_to_start_shifting = node->declare_parameter(name + ".time_to_start_shifting"); + p.shifting_lateral_jerk = node->declare_parameter(name + ".shifting_lateral_jerk"); + p.min_shifting_distance = node->declare_parameter(name + ".min_shifting_distance"); + p.min_shifting_speed = node->declare_parameter(name + ".min_shifting_speed"); + p.shift_request_time_limit = node->declare_parameter(name + ".shift_request_time_limit"); + p.publish_debug_marker = node->declare_parameter(name + ".publish_debug_marker"); + + parameters_ = std::make_shared(p); } void SideShiftModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 3b2917c94c8e1..bba6f2b7b0da3 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -24,10 +24,67 @@ namespace behavior_path_planner { StartPlannerModuleManager::StartPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) +: SceneModuleManagerInterface(node, name, config, {""}) { + StartPlannerParameters p; + + std::string ns = "start_planner."; + + p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); + p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_turn_signal_on_lateral_offset = + node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); + p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( + ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); + p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); + p.collision_check_distance_from_end = + node->declare_parameter(ns + "collision_check_distance_from_end"); + // shift pull out + p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); + p.minimum_shift_pull_out_distance = + node->declare_parameter(ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + node->declare_parameter(ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); + p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); + p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); + // geometric pull out + p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + node->declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_path_interval = + node->declare_parameter(ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + node->declare_parameter(ns + "lane_departure_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + // search start pose backward + p.search_priority = node->declare_parameter( + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = node->declare_parameter(ns + "enable_back"); + p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); + p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); + p.backward_search_resolution = node->declare_parameter(ns + "backward_search_resolution"); + p.backward_path_update_duration = + node->declare_parameter(ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + node->declare_parameter(ns + "ignore_distance_from_lane_end"); + + // validation of parameters + if (p.lateral_acceleration_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + logger_, "lateral_acceleration_sampling_num must be positive integer. Given parameter: " + << p.lateral_acceleration_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + parameters_ = std::make_shared(p); } void StartPlannerModuleManager::updateModuleParams( From 6744206515d4db4f92352c494353de5d65b9d3e4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:00:23 +0900 Subject: [PATCH 03/12] fix(behavior_path_planner): copy z in dynamic drivable area expansion (#4120) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 35 +++++++++++++++++-- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index a27010e4f8d15..819265b70abbe 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "interpolation/linear_interpolation.hpp" #include @@ -68,9 +69,7 @@ polygon_t createExpandedDrivableAreaPolygon( for (const auto & p : expansion_polygons) { unions.clear(); boost::geometry::union_(expanded_da_poly, p, unions); - if (unions.size() != 1) // union of overlapping polygons should produce a single polygon - continue; - else + if (unions.size() == 1) // union of overlapping polygons should produce a single polygon expanded_da_poly = unions[0]; } return expanded_da_poly; @@ -129,8 +128,36 @@ std::array findLeftRightRanges( return {left_start, left_end, right_start, right_end}; } +void copy_z_over_arc_length( + const std::vector & from, std::vector & to) +{ + if (from.empty() || to.empty()) return; + to.front().z = from.front().z; + if (from.size() < 2 || to.size() < 2) return; + to.back().z = from.back().z; + auto i_from = 1lu; + auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]); + auto s_to = 0.0; + auto s_from_prev = 0.0; + for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { + s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]); + for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { + s_from_prev = s_from; + s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]); + } + if (s_from - s_from_prev != 0.0) { + const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); + to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio); + } else { + to[i_to].z = to[i_to - 1].z; + } + } +} + void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area) { + const auto original_left_bound = path.left_bound; + const auto original_right_bound = path.right_bound; path.left_bound.clear(); path.right_bound.clear(); const auto begin = expanded_drivable_area.outer().begin(); @@ -155,6 +182,8 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ for (auto it = right_start; it >= begin; --it) path.right_bound.push_back(convert_point(*it)); for (auto it = end - 1; it >= right_end; --it) path.right_bound.push_back(convert_point(*it)); } + copy_z_over_arc_length(original_left_bound, path.left_bound); + copy_z_over_arc_length(original_right_bound, path.right_bound); } } // namespace drivable_area_expansion From 27c5a106f29702d0e55cff744ca004800b1495e7 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:08:16 +0900 Subject: [PATCH 04/12] ci: remove global codeowners (#4131) Signed-off-by: Takagi, Isamu --- .github/workflows/update-codeowners-from-packages.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 7c4132ef0c746..055d2b02a799a 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -27,7 +27,6 @@ jobs: uses: autowarefoundation/autoware-github-actions/update-codeowners-from-packages@v1 with: token: ${{ steps.generate-token.outputs.token }} - global-codeowners: "@autowarefoundation/autoware-global-codeowners" pr-labels: | bot update-codeowners-from-packages From 1e6449502bca25dcaf6a84f5728766312c28d7c0 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 30 Jun 2023 09:27:23 +0000 Subject: [PATCH 05/12] chore: update CODEOWNERS (#4052) Signed-off-by: GitHub Co-authored-by: isamu-takagi --- .github/CODEOWNERS | 407 +++++++++++++++++++++++---------------------- 1 file changed, 207 insertions(+), 200 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index dcf9c92f50442..98dc5c47506d2 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,203 +1,210 @@ ### Copied from .github/CODEOWNERS-manual ### ### Automatically generated from package.xml ### -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners -common/autoware_auto_common/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners -common/autoware_auto_geometry/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners -common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -common/autoware_auto_tf2/** jit.ray.c@gmail.com @autowarefoundation/autoware-global-codeowners -common/autoware_point_types/** taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners -common/autoware_testing/** adam.dabrowski@robotec.ai @autowarefoundation/autoware-global-codeowners -common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners -common/fake_test_node/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners -common/global_parameter_loader/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -common/goal_distance_calculator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/grid_map_utils/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/polar_grid/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_debug_tools/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/time_utils/** christopherj.ho@gmail.com @autowarefoundation/autoware-global-codeowners -common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -control/external_cmd_selector/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -control/joy_controller/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -control/lane_departure_checker/** kyoichi.sugahara@tier4.jp @autowarefoundation/autoware-global-codeowners -control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -control/pure_pursuit/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -control/shift_decider/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners -evaluator/localization_evaluator/** dominik.jargot@robotec.ai @autowarefoundation/autoware-global-codeowners -evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/map4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohei.sasaki@map.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/gyro_odometer/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/initial_pose_button_panel/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/localization_error_monitor/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/pose2twist/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/pose_initializer/** isamu.takagi@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -map/map_height_fitter/** isamu.takagi@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -map/map_loader/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners -map/map_tf_generator/** azumi.suzuki@tier4.jp @autowarefoundation/autoware-global-codeowners -map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/bytetrack/** manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/detected_object_validation/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/euclidean_cluster/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/heatmap_visualizer/** kotaro.uetake@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/object_range_splitter/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/probabilistic_occupancy_grid_map/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/traffic_light_selector/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/scenario_selector/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/surround_obstacle_checker/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/geo_pos_conv/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/gnss_poser/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/image_diagnostics/** dai.nguyen@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/imu_corrector/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners -sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners -simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -simulator/fault_injection/** keisuke.shima@tier4.jp @autowarefoundation/autoware-global-codeowners -simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai @autowarefoundation/autoware-global-codeowners -system/bluetooth_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners -system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners -system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -system/dummy_infrastructure/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -system/emergency_handler/** kenji.miyake@tier4.jp makoto.kurihara@tier4.jp @autowarefoundation/autoware-global-codeowners -system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp @autowarefoundation/autoware-global-codeowners -system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp @autowarefoundation/autoware-global-codeowners -system/system_error_monitor/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners -system/topic_state_monitor/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners -system/velodyne_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners -tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai @autowarefoundation/autoware-global-codeowners -vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners +common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +common/autoware_auto_common/** opensource@apex.ai +common/autoware_auto_geometry/** satoshi.ota@tier4.jp +common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp +common/autoware_auto_tf2/** jit.ray.c@gmail.com +common/autoware_point_types/** taichi.higashide@tier4.jp +common/autoware_testing/** adam.dabrowski@robotec.ai +common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp +common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp +common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp +common/fake_test_node/** opensource@apex.ai +common/global_parameter_loader/** kenji.miyake@tier4.jp +common/goal_distance_calculator/** taiki.tanaka@tier4.jp +common/grid_map_utils/** maxime.clement@tier4.jp +common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/polar_grid/** yukihiro.saito@tier4.jp +common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp +common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp +common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp +common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp +common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp +common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp +common/tier4_debug_tools/** kenji.miyake@tier4.jp +common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp +common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp +common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp +common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp +common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp +common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp +common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp +common/time_utils/** christopherj.ho@gmail.com +common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp +common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp +control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/external_cmd_selector/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/joy_controller/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/lane_departure_checker/** kyoichi.sugahara@tier4.jp +control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/pure_pursuit/** takamasa.horibe@tier4.jp +control/shift_decider/** takamasa.horibe@tier4.jp +control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp +evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai +evaluator/localization_evaluator/** dominik.jargot@robotec.ai +evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp +launch/map4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohei.sasaki@map.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp +launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp +launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp +launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp kenji.miyake@tier4.jp +launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp +localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** yamato.ando@tier4.jp +localization/initial_pose_button_panel/** yamato.ando@tier4.jp +localization/localization_error_monitor/** yamato.ando@tier4.jp +localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/pose2twist/** yamato.ando@tier4.jp +localization/pose_initializer/** isamu.takagi@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp +localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp +localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp +localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp +map/map_height_fitter/** isamu.takagi@tier4.jp yamato.ando@tier4.jp +map/map_loader/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +map/map_tf_generator/** azumi.suzuki@tier4.jp +map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp +perception/bytetrack/** manato.hirabayashi@tier4.jp +perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp +perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp +perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp +perception/detected_object_validation/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp +perception/euclidean_cluster/** yukihiro.saito@tier4.jp +perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/heatmap_visualizer/** kotaro.uetake@tier4.jp +perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp +perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp +perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp +perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp +perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp +perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp +perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/object_range_splitter/** yukihiro.saito@tier4.jp +perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/probabilistic_occupancy_grid_map/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp +perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp +perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/traffic_light_fine_detector/** mingyu.li@tier4.jp +perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp +perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp +perception/traffic_light_selector/** isamu.takagi@tier4.jp +perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp +perception/traffic_light_visualization/** yukihiro.saito@tier4.jp +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai +planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp +planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp +planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp +planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp +planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp +planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp +planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp +planning/scenario_selector/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp +planning/surround_obstacle_checker/** satoshi.ota@tier4.jp +sensing/geo_pos_conv/** yamato.ando@tier4.jp +sensing/gnss_poser/** yamato.ando@tier4.jp +sensing/image_diagnostics/** dai.nguyen@tier4.jp +sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp +sensing/imu_corrector/** yamato.ando@tier4.jp +sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp +sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp +simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp +simulator/fault_injection/** keisuke.shima@tier4.jp +simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai +system/bluetooth_monitor/** fumihito.ito@tier4.jp +system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp +system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp kenji.miyake@tier4.jp +system/dummy_infrastructure/** kenji.miyake@tier4.jp +system/emergency_handler/** kenji.miyake@tier4.jp makoto.kurihara@tier4.jp +system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp +system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp +system/system_error_monitor/** fumihito.ito@tier4.jp kenji.miyake@tier4.jp +system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp +system/topic_state_monitor/** kenji.miyake@tier4.jp +system/velodyne_monitor/** fumihito.ito@tier4.jp +tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai +vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp +vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp +vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp +vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp From e88ccad6e05e51471bf365c99c381f3a9c913c28 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 30 Jun 2023 18:40:35 +0900 Subject: [PATCH 06/12] feat(traffic_light_selector): remove traffic light selector (#4130) Signed-off-by: Takagi, Isamu --- .../traffic_light_selector/CMakeLists.txt | 17 --- perception/traffic_light_selector/README.md | 43 ------ .../launch/traffic_light_selector.launch.xml | 14 -- perception/traffic_light_selector/package.xml | 27 ---- .../src/traffic_light_converter.cpp | 117 ---------------- .../src/traffic_light_converter.hpp | 36 ----- .../src/traffic_light_selector.cpp | 129 ------------------ .../src/traffic_light_selector.hpp | 47 ------- 8 files changed, 430 deletions(-) delete mode 100644 perception/traffic_light_selector/CMakeLists.txt delete mode 100644 perception/traffic_light_selector/README.md delete mode 100644 perception/traffic_light_selector/launch/traffic_light_selector.launch.xml delete mode 100644 perception/traffic_light_selector/package.xml delete mode 100644 perception/traffic_light_selector/src/traffic_light_converter.cpp delete mode 100644 perception/traffic_light_selector/src/traffic_light_converter.hpp delete mode 100644 perception/traffic_light_selector/src/traffic_light_selector.cpp delete mode 100644 perception/traffic_light_selector/src/traffic_light_selector.hpp diff --git a/perception/traffic_light_selector/CMakeLists.txt b/perception/traffic_light_selector/CMakeLists.txt deleted file mode 100644 index 9a8555690beed..0000000000000 --- a/perception/traffic_light_selector/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(traffic_light_selector) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/traffic_light_converter.cpp - src/traffic_light_selector.cpp -) - -rclcpp_components_register_nodes(${PROJECT_NAME} - TrafficLightConverter - TrafficLightSelector -) - -ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/perception/traffic_light_selector/README.md b/perception/traffic_light_selector/README.md deleted file mode 100644 index d20d5dc790e3a..0000000000000 --- a/perception/traffic_light_selector/README.md +++ /dev/null @@ -1,43 +0,0 @@ -# traffic_light_selector - -## Purpose - -This package receives multiple traffic light/signal states and outputs a single traffic signal state for use by the planning component. - -## TrafficLightSelector - -A node that merges traffic light/signal state from image recognition and V2X to provide a planning component. -It's currently a provisional implementation. - -### Inputs / Outputs - -#### Input - -| Name | Type | Description | -| -------------------- | ---------------------------------------------- | ------------------------------------------------- | -| ~/sub/vector_map | autoware_auto_mapping_msgs/msg/HADMapBin | The vector map to get traffic light id relations. | -| ~/sub/traffic_lights | autoware_perception_msgs/msg/TrafficLightArray | The traffic light state from image recognition. | - -#### Output - -| Name | Type | Description | -| --------------------- | ----------------------------------------------- | -------------------------------- | -| ~/pub/traffic_signals | autoware_perception_msgs/msg/TrafficSignalArray | The merged traffic signal state. | - -## TrafficLightConverter - -A temporary node that converts old message type to new message type. - -### Inputs / Outputs - -#### Input - -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ------------------------------ | -| ~/sub/traffic_lights | tier4_perception_msgs/msg/TrafficSignalArray | The state in old message type. | - -#### Output - -| Name | Type | Description | -| -------------------- | ---------------------------------------------- | ------------------------------ | -| ~/pub/traffic_lights | autoware_perception_msgs/msg/TrafficLightArray | The state in new message type. | diff --git a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml deleted file mode 100644 index 44eec5284c03c..0000000000000 --- a/perception/traffic_light_selector/launch/traffic_light_selector.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/perception/traffic_light_selector/package.xml b/perception/traffic_light_selector/package.xml deleted file mode 100644 index fb696dd54dddd..0000000000000 --- a/perception/traffic_light_selector/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - traffic_light_selector - 0.1.0 - The traffic_light_selector package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_mapping_msgs - autoware_perception_msgs - lanelet2_core - lanelet2_extension - rclcpp - rclcpp_components - tier4_perception_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/traffic_light_selector/src/traffic_light_converter.cpp b/perception/traffic_light_selector/src/traffic_light_converter.cpp deleted file mode 100644 index afc8dab72e788..0000000000000 --- a/perception/traffic_light_selector/src/traffic_light_converter.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "traffic_light_converter.hpp" - -#include -#include - -namespace converter -{ - -using OldList = tier4_perception_msgs::msg::TrafficSignalArray; -using OldData = tier4_perception_msgs::msg::TrafficSignal; -using OldElem = tier4_perception_msgs::msg::TrafficLightElement; -using NewList = autoware_perception_msgs::msg::TrafficLightArray; -using NewData = autoware_perception_msgs::msg::TrafficLight; -using NewElem = autoware_perception_msgs::msg::TrafficLightElement; - -NewList convert(const OldList & input); -NewData convert(const OldData & input); -NewElem convert(const OldElem & input); - -template -std::vector convert_vector(const L & input) -{ - std::vector output; - output.reserve(input.size()); - for (const auto & value : input) { - output.push_back(convert(value)); - } - return output; -} - -NewList convert(const OldList & input) -{ - NewList output; - output.stamp = input.header.stamp; - output.lights = convert_vector(input.signals); - return output; -} - -NewData convert(const OldData & input) -{ - NewData output; - output.traffic_light_id = input.traffic_light_id; - output.elements = convert_vector(input.elements); - return output; -} - -template -V at_or(const std::unordered_map & map, const K & key, const V & value) -{ - return map.count(key) ? map.at(key) : value; -} - -NewElem convert(const OldElem & input) -{ - // clang-format off - static const std::unordered_map color_map({ - {OldElem::RED, NewElem::RED}, - {OldElem::AMBER, NewElem::AMBER}, - {OldElem::GREEN, NewElem::GREEN}, - {OldElem::WHITE, NewElem::WHITE} - }); - static const std::unordered_map shape_map({ - {OldElem::CIRCLE, NewElem::CIRCLE}, - {OldElem::LEFT_ARROW, NewElem::LEFT_ARROW}, - {OldElem::RIGHT_ARROW, NewElem::RIGHT_ARROW}, - {OldElem::UP_ARROW, NewElem::UP_ARROW}, - {OldElem::DOWN_ARROW, NewElem::DOWN_ARROW}, - {OldElem::DOWN_LEFT_ARROW, NewElem::DOWN_LEFT_ARROW}, - {OldElem::DOWN_RIGHT_ARROW, NewElem::DOWN_RIGHT_ARROW}, - {OldElem::CROSS, NewElem::CROSS} - }); - static const std::unordered_map status_map({ - {OldElem::SOLID_OFF, NewElem::SOLID_OFF}, - {OldElem::SOLID_ON, NewElem::SOLID_ON}, - {OldElem::FLASHING, NewElem::FLASHING} - }); - // clang-format on - - NewElem output; - output.color = at_or(color_map, input.color, NewElem::UNKNOWN); - output.shape = at_or(shape_map, input.shape, NewElem::UNKNOWN); - output.status = at_or(status_map, input.status, NewElem::UNKNOWN); - output.confidence = input.confidence; - return output; -} - -} // namespace converter - -TrafficLightConverter::TrafficLightConverter(const rclcpp::NodeOptions & options) -: Node("traffic_light_converter", options) -{ - const auto callback = std::bind(&TrafficLightConverter::on_msg, this, std::placeholders::_1); - sub_ = create_subscription("~/sub/traffic_lights", rclcpp::QoS(1), callback); - pub_ = create_publisher("~/pub/traffic_lights", rclcpp::QoS(1)); -} - -void TrafficLightConverter::on_msg(const OldMessage::ConstSharedPtr msg) -{ - pub_->publish(converter::convert(*msg)); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightConverter) diff --git a/perception/traffic_light_selector/src/traffic_light_converter.hpp b/perception/traffic_light_selector/src/traffic_light_converter.hpp deleted file mode 100644 index 8c6c6545d9dc2..0000000000000 --- a/perception/traffic_light_selector/src/traffic_light_converter.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAFFIC_LIGHT_CONVERTER_HPP_ -#define TRAFFIC_LIGHT_CONVERTER_HPP_ - -#include - -#include -#include - -class TrafficLightConverter : public rclcpp::Node -{ -public: - explicit TrafficLightConverter(const rclcpp::NodeOptions & options); - -private: - using OldMessage = tier4_perception_msgs::msg::TrafficSignalArray; - using NewMessage = autoware_perception_msgs::msg::TrafficLightArray; - rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_; - void on_msg(const OldMessage::ConstSharedPtr msg); -}; - -#endif // TRAFFIC_LIGHT_CONVERTER_HPP_ diff --git a/perception/traffic_light_selector/src/traffic_light_selector.cpp b/perception/traffic_light_selector/src/traffic_light_selector.cpp deleted file mode 100644 index 599bcc0f389f1..0000000000000 --- a/perception/traffic_light_selector/src/traffic_light_selector.cpp +++ /dev/null @@ -1,129 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "traffic_light_selector.hpp" - -#include - -#include - -#include -#include -#include -#include - -namespace lanelet -{ - -using TrafficLightConstPtr = std::shared_ptr; - -std::vector filter_traffic_signals(const LaneletMapConstPtr map) -{ - std::vector signals; - for (const auto & element : map->regulatoryElementLayer) { - const auto signal = std::dynamic_pointer_cast(element); - if (signal) { - signals.push_back(signal); - } - } - return signals; -} - -} // namespace lanelet - -TrafficLightSelector::TrafficLightSelector(const rclcpp::NodeOptions & options) -: Node("traffic_light_selector", options) -{ - sub_map_ = create_subscription( - "~/sub/vector_map", rclcpp::QoS(1).transient_local(), - std::bind(&TrafficLightSelector::on_map, this, std::placeholders::_1)); - - sub_tlr_ = create_subscription( - "~/sub/traffic_lights", rclcpp::QoS(1), - std::bind(&TrafficLightSelector::on_msg, this, std::placeholders::_1)); - - pub_ = create_publisher("~/pub/traffic_signals", rclcpp::QoS(1)); -} - -void TrafficLightSelector::on_map(const LaneletMapBin::ConstSharedPtr msg) -{ - const auto map = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, map); - - const auto signals = lanelet::filter_traffic_signals(map); - mapping_.clear(); - for (const auto & signal : signals) { - for (const auto & light : signal->trafficLights()) { - mapping_[light.id()] = signal->id(); - } - } -} - -void TrafficLightSelector::on_msg(const TrafficLightArray::ConstSharedPtr msg) -{ - using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; - using Element = autoware_perception_msgs::msg::TrafficLightElement; - std::unordered_map> intersections; - - // Use the most confident traffic light element in the same state. - const auto get_highest_confidence_elements = [](const std::vector & elements) { - using Key = std::tuple; - std::map highest_; - - for (const auto & element : elements) { - const auto key = std::make_tuple(element.color, element.shape, element.status); - auto [iter, success] = highest_.try_emplace(key, element); - if (!success && iter->second.confidence < element.confidence) { - iter->second = element; - } - } - - std::vector result; - result.reserve(highest_.size()); - for (const auto & [k, v] : highest_) { - result.push_back(v); - } - return result; - }; - - // Wait for vector map to create id mapping. - if (mapping_.empty()) { - return; - } - - // Merge traffic lights in the same group. - for (const auto & light : msg->lights) { - const auto id = light.traffic_light_id; - if (!mapping_.count(id)) { - continue; - } - auto & elements = intersections[mapping_[id]]; - for (const auto & element : light.elements) { - elements.push_back(element); - } - } - - TrafficSignalArray array; - array.stamp = msg->stamp; - for (const auto & [id, elements] : intersections) { - TrafficSignal signal; - signal.traffic_signal_id = id; - signal.elements = get_highest_confidence_elements(elements); - array.signals.push_back(signal); - } - pub_->publish(array); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightSelector) diff --git a/perception/traffic_light_selector/src/traffic_light_selector.hpp b/perception/traffic_light_selector/src/traffic_light_selector.hpp deleted file mode 100644 index b21efaa2ec2e3..0000000000000 --- a/perception/traffic_light_selector/src/traffic_light_selector.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAFFIC_LIGHT_SELECTOR_HPP_ -#define TRAFFIC_LIGHT_SELECTOR_HPP_ - -#include - -#include -#include -#include - -#include - -#include - -class TrafficLightSelector : public rclcpp::Node -{ -public: - explicit TrafficLightSelector(const rclcpp::NodeOptions & options); - -private: - using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficLightArray = autoware_perception_msgs::msg::TrafficLightArray; - using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_tlr_; - rclcpp::Publisher::SharedPtr pub_; - - void on_map(const LaneletMapBin::ConstSharedPtr msg); - void on_msg(const TrafficLightArray::ConstSharedPtr msg); - - std::unordered_map mapping_; -}; - -#endif // TRAFFIC_LIGHT_SELECTOR_HPP_ From da9c9c58a73cef13295f1744282f5ca32ec5b906 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 30 Jun 2023 13:23:39 +0200 Subject: [PATCH 07/12] fix(yabloc_pose_initializer): disable downloading artifacts by default (#4110) Signed-off-by: Esteve Fernandez Co-authored-by: Esteve Fernandez --- .../yabloc_pose_initializer/download.cmake | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/localization/yabloc/yabloc_pose_initializer/download.cmake b/localization/yabloc/yabloc_pose_initializer/download.cmake index a17910156b574..6ce997b3978d5 100644 --- a/localization/yabloc/yabloc_pose_initializer/download.cmake +++ b/localization/yabloc/yabloc_pose_initializer/download.cmake @@ -12,12 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifacts download") + set(DATA_URL "https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/136_road-segmentation-adas-0001/resources.tar.gz") set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") set(FILE_HASH 146ed8af689a30b898dc5369870c40fb) set(FILE_NAME "resources.tar.gz") -function(download) +function(download_and_extract) message(STATUS "Checking and downloading ${FILE_NAME}") set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) set(STATUS_CODE 0) @@ -38,11 +40,17 @@ function(download) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) + if(DOWNLOAD_ARTIFACTS) + message(STATUS "not found ${FILE_NAME}") + message(STATUS "File doesn't exists. Downloading now ...") + file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + list(GET DOWNLOAD_STATUS 0 STATUS_CODE) + list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) + else() + message(WARNING "Skipped download for ${FILE_NAME} (enable by setting DOWNLOAD_ARTIFACTS)") + file(MAKE_DIRECTORY "${DATA_PATH}") + return() + endif() endif() if(${STATUS_CODE} EQUAL 0) @@ -50,13 +58,10 @@ function(download) else() message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") endif() -endfunction() -function(extract) execute_process(COMMAND ${CMAKE_COMMAND} -E tar xzf "${DATA_PATH}/${FILE_NAME}" WORKING_DIRECTORY "${DATA_PATH}") endfunction() -download() -extract() +download_and_extract() From 879a837e7524762d0c4fe700b9a0138e2a18d4bd Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 30 Jun 2023 21:14:17 +0900 Subject: [PATCH 08/12] fix(lane_change): fix visualization when path is invalid (#4132) Signed-off-by: Zulfaqar Azmi --- .../src/scene_module/lane_change/interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 721c7fa6fd390..2c1ba45c3ef84 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -232,6 +232,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); + setObjectDebugVisualization(); // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); @@ -249,8 +250,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); is_abort_path_approved_ = false; - setObjectDebugVisualization(); - return out; } From a9346851684d6c9884398fa86a533c7106c5fd53 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sat, 1 Jul 2023 19:27:36 +0900 Subject: [PATCH 09/12] docs(planning_validator): add unit (#4126) Signed-off-by: Takamasa Horibe --- planning/planning_validator/README.md | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index 08ddee246317f..da1e572bb383a 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -63,15 +63,15 @@ The following parameters can be set for the `planning_validator`: The input trajectory is detected as invalid if the index exceeds the following thresholds. -| Name | Type | Description | Default value | -| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------ | :------------ | -| `thresholds.interval` | double | invalid threshold of the distance of two neighboring trajectory points | 100.0 | -| `thresholds.relative_angle` | double | invalid threshold of the relative angle of two neighboring trajectory points | 2.0 | -| `thresholds.curvature` | double | invalid threshold of the curvature in each trajectory point | 1.0 | -| `thresholds.lateral_acc` | double | invalid threshold of the lateral acceleration in each trajectory point | 9.8 | -| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point | 9.8 | -| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point | -9.8 | -| `thresholds.steering` | double | invalid threshold of the steering angle in each trajectory point | 1.414 | -| `thresholds.steering_rate` | double | invalid threshold of the steering angle rate in each trajectory point | 10.0 | -| `thresholds.velocity_deviation` | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego. | 100.0 | -| `thresholds.distance_deviation` | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego. | 100.0 | +| Name | Type | Description | Default value | +| :-------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.interval` | double | invalid threshold of the distance of two neighboring trajectory points [m] | 100.0 | +| `thresholds.relative_angle` | double | invalid threshold of the relative angle of two neighboring trajectory points [rad] | 2.0 | +| `thresholds.curvature` | double | invalid threshold of the curvature in each trajectory point [1/m] | 1.0 | +| `thresholds.lateral_acc` | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss] | 9.8 | +| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss] | 9.8 | +| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss] | -9.8 | +| `thresholds.steering` | double | invalid threshold of the steering angle in each trajectory point [rad] | 1.414 | +| `thresholds.steering_rate` | double | invalid threshold of the steering angle rate in each trajectory point [rad/s] | 10.0 | +| `thresholds.velocity_deviation` | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0 | +| `thresholds.distance_deviation` | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m] | 100.0 | From 06184cff9f804b3a700206c066729ab4207bd4e7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sat, 1 Jul 2023 20:41:23 +0900 Subject: [PATCH 10/12] docs(mpc): add mpc document (#4011) * docs(mpc): add mpc document Signed-off-by: Takamasa Horibe * fix format Signed-off-by: Takamasa Horibe * Update control/mpc_lateral_controller/docs/mpc-algorithm-document.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * update Signed-off-by: Takamasa Horibe * update! Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../image/vehicle_error_kinematics.png | Bin 0 -> 53288 bytes .../image/vehicle_kinematics.png | Bin 0 -> 36676 bytes .../model_predictive_control_algorithm.md | 389 ++++++++++++++++++ 3 files changed, 389 insertions(+) create mode 100644 control/mpc_lateral_controller/image/vehicle_error_kinematics.png create mode 100644 control/mpc_lateral_controller/image/vehicle_kinematics.png create mode 100644 control/mpc_lateral_controller/model_predictive_control_algorithm.md diff --git a/control/mpc_lateral_controller/image/vehicle_error_kinematics.png b/control/mpc_lateral_controller/image/vehicle_error_kinematics.png new file mode 100644 index 0000000000000000000000000000000000000000..73c82d8ce128520d3ba64bbc90f87630439b4fc8 GIT binary patch literal 53288 zcmeFZ_dnNdA2;r7s3g0xB9fL(c9cl&MMbscxSGl-DylWA_;2e* zYWzvZc$g&qXPvXs@zWde%X_0mFn-VGqN?wrYj5e|X6kg2%F53E%0)qEbEk_J?VPRc zUB=cHE8vI3FP+wRQFgj$>T=26Zs+MsS1#fYsi?$6#KimZ-ib)|KL{3;>5qM~Q&gfq z{`x_gzTiL+v4Qm4Z^gqtQBm!rI<9i$wEKgzkDR@i98HKj3QFt}|_A12O|%MU`f(f&Rh4 z!Bw=UA9r~!B_*Ye^z@ZxnHsaZL#oz&(})gMyA~bI7#JAnAe$KDvyWG-R3OllHO7aL z7r$aoimUR9^{j8_U=Qq~Fh$3!V&5(K#HZ)I6{t6CxOC}K>4@uElT7{7>U?5r_pyXj z-LeSeimX)D)n&FXUXeU_@Zj{^+~s>}7J;YsGDWjh(<&S~bcj>IJI<}cCwB98ArX1` zzm%kp(N(o6f(v*j)L_$axmo7duU|`|cO6qZaU$jM;~Vqy?l1L*pMKyKd=gSidw(BZ zshn0$@5SAsA_ck~vMTE8n==Y$*^eJT-fSyb#}HDhO+!9*+@pc2=w0vLy^BgnsHq6& z*tB^w$H4=SuV2RZsIlS|Gmpzsv21qCZ?m{~F*-W>h=IX<%U~5tX_R=pC~aBEz?3N~ zwQGK2L4o}3n3#~r$g?|{7_YwD79C&uV7F7AA?=nej9a(fY?S?Qy;D#9qJaW)ybMl> z#nFv_Pmj!aX10#FZFR>VSki4Z2&>v9{3hb*ix*)&OS4^0dCL^7^^Lr1$mdjcFEn~> zRQS0xV8cQ^Wm!vBPHw_oes%2fcS9BY>)vQfq4>NuJIzGd^3k?@8NXF|Wo6}g?;@Q@ z9TBGZmUl}UDqJ24$&tfN8P2^`jO-j79$h!LtEsE++Pin7c3V{N?}yraypa-FMt_Z4p8DUvGo9u8;5x$I*x1Nn4&KozWI}PQcZ+*ZCLqrQ&#VVzF6r*&7ZH%AJf#NbItepHNq?I z$Hf>gjSZMod!sLag8q=7KR7hRD<_w5=7ruuy;Oo7ORx%m626Q4DNec#mz!Si77~gm zC@82d;$)0J+EAdQ!o?Q({dSBg>#bY2L_|ccnX>Zj-hC1$OMddSwIJ`0`z;C9vKvw| z=h6aAJEstuYehL>hKbD7A-O`G!i=F)BS#mXKu z+RRYe2Bx}PWG~s<5A0D~*c2WfZrl9E^z$k$ZzK~cbitr0!#F)JFMjO?y05=~*WNvF z7AF`x5qkd!=i=8Wpuv)dJyyXZqgL+?k;`7-3( zZ=3Wiq@}0-@$Fk_q~Egn;?h#fyGzVk3JZ^wqN_-QkCtQS#xmZdjy=xtj=A`>=CpG-@m3iBTtG)gGnv-WjYiY3+PRFiXx#B%py=Qr`shD3; zG5M^K$Ib3;Gc3QLpy25+G$L!4fg;*ikoCuc+ndD2k3i)!WxcmJ^dsaiLnv+Sk3ysO_8pq% z&vW2QcV$*`!lf8I345zC(*D;mi=%00*cXHf%Ej*+O=zQ z4TU~@_^^CcWLwz!7mX*>)HZ4znqg5@Rjo}{^!wn^Xb5k$!PL}Les#VF)k(=- z?|bs1W_rvHIai0l@4G3b**TkC{pCh#g&EltNl$Ejt1p21V`OGVe^@K$`qyNtzpi&+ zV5y!ttHoADRdwQ+-}JHR+1cB7?nv<5{AsyTH`!NHT~~K;b$LNofoi=NZNU5Yl11~~ zo8Mn)t-xv0&#~K4U0uD}b3(SJ?J%SJ2^EKfAI3pcN_j#cDa`K$4yIYNraP=lYXAlZfxh@j^bqF)yde$j~_EIF)8Wk{S=IS z%AZ1AVUvA3E>6wNto!L)A{|fH$D5l;>5#LoFR-m>WyD@rPw%Lq;e+C{-(2(;nLQfz zVnAJq=SeLncyQqCYwfdVkKs5_#_1{7%DOt2Iw%V0N&2t!D7PaQIJ2zX3$Vw2Wo3h` z>}Y{l&(5rRgC-S(OT4#2X5yzS7C6Tp^hcOpNMn+IpqF5DM$yQ~rjG&7nzp@1k-Ua{pV?8-J ziPe10!xX>fG#YG9@6-Y-E2~M<>kG@1vBkZpd2DcWV&X}Ad*N7N%2*BU>@+d*UAXw~UTJHie;O9JN7uv(>+0&3p;>$M$6Eoe z%q7c6wy&?ZOsy#Nn*09i*9l)=g}%PNb?euMw|d=@O0GZV=cgD%x1+n^nJyn6pNQM2 zp|=9JWWq^*H4B>ii#g|b#Y}O+Ki@szy}B^4ypT+hxp3ivZOhx6W8MRDEG#VZ+&Qiz z2BBeL6P|_B)6;LvE9qoqWx0O67RPHF8*|^idlzq(>AT{Yl9H14S0mb#mGnLg+tnt@ zqOtdUR}d{Ntwzi6)@Y9DOLbs2 z0HqsT(?{0(T~3bTyUXlcT>Ur`>G}ByA3rML@XczsUoJ4rcMV5_AGh+GwIV0Dg2R41 z+}?Zb{FS=KrmJa1XRJmCu}~HtzG05cH#Rjn|M|*uKOte$mMwjE&l;Jw7kI3Wbn^$5 z9Z^zRgI!Aj0KsGslaP=YrKlyC@<>a^;rM<0__3=cXf5?P&y-}^4l~qdeKdqM`ris(O&#AkCS#iAOHu% zJNnL_WK&I(#BoB&iOS2@W|~(@yNz;w`}R%1FgNP)V?N3~w-=4F&n@@L%6c|LbI~iC zD63tos;UZF^zGph6T4j=ww;-c?YN`k#8b2JXRQ4yy*XquV`H1BzjIDtTX%PN+VZG0V;+x#e8T)I(MHksBEUvkZ*@AFn6qXIQ;ZHAhAg1dNlObs)z$RRisP8bM!uM7^p&RE%5 z+fiCowGkz2+xFq^hYzy-UlMDU7pCkz9|UXc@K09HX+3FfzDv$+lyBd@5Fj0b+Qa=n z8txC^P*>7SpSl-$kTw7t{^R#=4U8-7pZr2^4Ej!ZO?I5P2wvDQ*TEl+B>KSv`HNqq z#pQgt<=vy|6_#%O{HdpxVRRDvjJXx#+d0|SHR9D8oMpn$8x zKcjEo7Q1L^1QY@I=RID!0f#8dwneOnvNFcJefvQ2Iq{4$SVG+}Q&!NP&I+rRfvE-c zHHYzMYd{jdfB#1h`L5j9QC3bQ|YM^#q+DkHfE zxG59d9+Z{2kcf!>NXmR90c0&LEkJ3cFADokY<3;_rK+d5jgX#m6gog+*&R!64)a08zv?(q5Pf- zx@fM*AWVO(DEX=K@`cUcE1jhi_lukJ{y1Z@nQ?0Q(~GkilYXo9l&FAQb6q(J1gEcj*pA%(R^T18L@*?)@37rjlMqXkt0X2+Ex}de_6ew{lH~!)E>(@8Z(ed-~-FW(RFIKz^ zQ|Y*t7I*AniCI=UXRCM_xr<-4!{@Z0Yx7N-tl@ZmU%U;2@!w_3bLd)w)uAa~$}d*; z_Aa2vfT7`oDt<&w-??`WFi6GJlqXi`LP|zPB+foirlX@Hu#C>BQ%VK~?2jHj>haxy zf>-ZW8+S>({d2T9auuGb+i1yEksAB>660`s=!s>k>KZS?J>Gddk>X z>%<9~n>TM#r5bGATM(>q0Z=dfM>Up6A-TTT;%8%{s*cXLldn|B{B^U?luI~2v!{Dy zjg`A*vO=BWYLDXN7rC*oO-)kIkDGPUp48BwRgV?w8_DV57ZqhpsFQsD<&RxQt9QZw z=JwoZ+i5+$w-`}HGq3F1W<7dNnyUAfddPEu4+3v}>~eSm`erN1VVA>!abRuz=V{`r z=d>)*a)6EK$wBPFwO2+hnX^kuVq#*-=>6uEaY&9{C~Lm>>g7v;cK4nwX6EKAH9m9f z0;pewWcrLIb)es|Vv2&_#CAXbnm*or`v%i|HQi+Uj8ynz>j%uOt%KbEla2H+ttF3r9)26 z&h|92+rn<5Vf5E0*Mr7$TW^0*XIVBpbL#y0n&Oq#;t8zO8;>c=RUo-2Of$;N{&gW5 zJ|MK?;|@?UmIaTYjz-9w+W6(jNK>(~9y(v!N&w;NlG{?5A*6tt8vpr;rRtdUg0 zu@#LKaB{n_(c|3QxNr`s+lh(c827t&^+bf5T(K`&=ucpFDCFgp0?P27X*|2!y}j53 z90|Z!`_w6?nyH`oJgjVNT|njU-oIaTyHlGW71@$yC5}mm|2Ymu&%;yNaY5GM{Agla zq_~%8`KM3gwc_>K`ug+QG@pwjuHUO#!DgFbE_*KyrmGx1dOJ4u7~oQTd_3Soo8#I* zfNP8I;KH+G9a=!6yLa!t6C2y%=(Ut~GC>;J0$6xjMh1OQSt+xY{1ME5AaDYk3RY%Y zi5O^Pq^Y9v>Cl*xc%B>nPjW3XIQ4aRB*= zb=5v3)4Y<~ul8?C*12q(t>@02bED0 zOyrf4n!6Wtz9ZsZJ-%(6&$Z7rHRs-0Gm46e8hZ&G!(l=Z1O`%*aj{f?AuK%n$F?=Ro z7_Wq)&Y%!g*VOc&JW^i1{F@Zfr331`X{+>-S+>D(4Gq3Mdn)}5sej9t@6g{4$f<%a zg%UI^3*1thbO!T86`@^inP=^HxKRQT_en~_68g?bzZ$NJIStajwcKbyA+`BTU9%vi#u|Md|7Fdcd?6(l>SFPo@sJl(rsFpMlpqX2ZHcud^ z7_SVS<&<;1#l76Q0p;o4Ae7ZA*w$7*6H-;yHQ_$LR7~+g2ZSE<5EHfJPyIyO>^^Y< z-FB?!D)G!%tFiMx8kFxkVurT6^?;uI9R{3ox&hQjfLfvFjE}uvF$ccS>C>kjW`@p0a2-0~(cTo2&WM3T2PX}EO+kSjV2Up)jjZbiQrsYTN{|D1G@Y@fpba_ z#ISBOD!e~#)%0~k6(&l-jT<+jl9ImM-mSj8v@GdTvV;KRQ4{tdVC3Fpi(|&y3AS z-7cg*ZL{JPKilcZfv(y&wmLc{?Yk^(lo1;j$GS~G|1h8umI!0wJ5DN&5UQ~c9pd7` zWMAr#v)$QwTcP9~hdl2Prg`VijW##p9yx%M9XmkN$qmmoDYStA@173E;;qpo5x z38VlVEQ2=z#1%aMo`HkIt7eyX=c-lmR!G>ntE;{?Ha7DUk$$z$bko(ew5%q&%dsYq zn*!=A%`wL?xP=UJ4}GS8^f4-#_%y)-bSXqgSiCqUt5ELA_ zm-w7pg5}a3hIrG8_6^wp{#S?f_8&N~Pe8z8WpM_|3;~N2y{kA8pd#qyyC8&u34KYF ztDGHay<}_q82keL^{qpf(kWeCdr#r8@^1T=tn;f}kyeIfp?MN|`0PNrcu78#=5Hfo z75>21k+dgIZoWF7Lv&QYL$oDq+){*;F8v423~d-7@uj_sGsEE4%%g>_7VK%z6Ybzc0%8v~0WF{`_njevJy z{`)s=2_!X-m6;|F%zv4yLo8&nf->3rP9@NDs#Q>Pl!-Xx8J2`Uk71%iKs&6o4nC%pG9bWXS0$gasc{( zsHf8X7SpjN?Wb>f=kr@bA)Ih$WoQ35=yK}9g_9RA?ml_)BpOIea&kS0Etoms9;ndy zvY#t-mi}dT>jzR(Qv>|wgZw%{r(d4WiP&!Be#BJG;yyb&dpE{OKf%*`_wVm6UXGh8b@1R_^lYc@ zPn&QAfY3})#=yk)@85@N!Y3^~UljYcwvrwyI>=1D->S1_lKgR1)%6Pt3p9$nKjJET zh&cJ+kk`F8Z$x14poQ`9@|t~oedH0GG11rugwH_~fY$cXFzmSGjG}`oG^8fvn9fm&q`SWK|R)T`zt1i^XAvJuqpiXnQ8y3Fupz8%xj0iS4V2757_RK5u7V zxV`_aKH5rN9znS=_wR?|13(|_PIPPU>MRYApZLTK$>#dURY7M*Vd-%#WK!a4Twp!POs{;}k9!U*qB70XW(| zW5Vrb*TkTj*s z(QG&hH?Y%_i^CZ|nwlyBiQQ*gtO#2f6JgBV{0WZCjHjb);`Gc6^lo)+?I_CFz?Z}x z3=~)#f#{K7^X@n;Ed?Ip*nQYBK390&w~koig^9YPv+Mb8rg&f4vWIDDfyE06#a*95 zn1M4CJI{A4ZUZlkaqUh~|NpZ9C*MnTKy^Rv<;ED#VA*-;^5q_^(E!m2VT)FBdEJ6s zx#9i=QMWq_-U{HTk_lT9>W*4i@cql9M>~uXqM{Caddh}`gg_IACQX{(`}g-Sv;Vrv z0nV|Z?mpj?l+Vwr8_+a7({XradRqJZd3ASpX*hk4)6;b^ z1ku-mBO{X=vNmOV^NT6d($P5$HS8lGo_E(S0<`hUL`c7OHDgV3kBe-k!7C}ji|c)1 zabRAQ!9KzONkDkY# z!zK*G2!V-*qeYopldqRhkA*S)mZAzpKGURh4LLzLhsNF$>oHwXOepR$g2B4OgmYm* zLU6f{VV<3n^F zHLv*m`2>6{{d`wJqNG3-EY46k!7H`^S#SPtRz20;y?Z<1eVLbDUk`qHGOwkh`h_KU z4~%mNh5z%Kf4;sz^q1nFraBO;OhYNgVjz6SD$;OM;v~(eatN- zB6NF$s^43nWBHFjb9Hrf#L^fYwS#zc%+$0i_XaIQ2fP6BFK`&%y^{cd4Y_p-=X=4* ze@$8Rg1qxr3Cd*!NlZJ`8WZ>$`X@RAKUCKSb5)B)k;t_)oXoX5Ug$<+gm9Sx^8Eds zhepW`i(s#y;BJ2Yet(;g+Uu9ZSrQffxFD7QgW?rt6~Bnk78qstrB!imTwC0Ev=II= z3{fZ)Py)8$+`>g_9r;O%GBfiv^78cjGdx_@<)E#tU2-|2FtBO(32LOdS-&&q!gxBI z;OJv&ojYNl10)k66vcLPX1FQKq3h#@BK6JQKF-w#_U;V^4unNMRLmifvD=|yuLvjl z1y&YzDq!UW(D-|19tVm-_kOKmNSI{rwl zm(e^h3bqfae&|&v@zvI^U%$#cQu%Y{x1e})oN5EOq=5b#GA7{-+0z{1pDL_n77)NL z*4;md$xWqeKD;slJ5T<4ZPAfr@5xPAe_|2*{%uJ4wYswLueilqgIC~EDD5`}uTqDo zD-@avgn*2|P{G$EJI%GVj_hLe!c?aT5tkFm~ybcsHlBG=K&)8Txd_d+sn&4BuKkOnm z#P3FLB32qc7s(t!!xa)5eQlLYTecH66%Aw-(oTxm*0=-?!q-d-k;8yIS?W9@Z_&R}d=ozO9sVP7G(DKnBx?WzTH^&9Zww~Dt`Y&Kzat|T@g~HwM3tX7aRT8JsMsq0 z<&7XV59Hh;oyHi?~z>uB2y_L0fmGPe|tkj^CsiR!OLqpa!Hc(E9 znjRe&7k5eKVW|A)HA@kgyAIQTbl%&weFpM7_uiHb8-<>D``$gtD>N@E?s)I&7ODuZ&DfvHZ+V|BZGHYY-Kn#W(f!E%NgicFk4+C2$`2;)t;YnI9?< z7HfWUR-M5O9A%I{QQ81(%qf`W+S*^&QWR8ry9TkdKq`?6iyO;Csy=__1(U*utS)-D zA?3Vi^s#GbM_`j6Mc-Rmjssa^!mq~1PjtG-;-6Yo-`$<2 z2_K(B&=QH11asV~;pJUexpZ`Iz2@vdw)EdxYyx!kF`!iBD$v*-n7`p_{7tz0J^fXdp)&5HH{U{abe@h`9K@uq^tCCd(E}Pg<%St8%Q1? zXw|=7{{60T84zA#8WTg(-n)l`W&3uRk~iusr;5|s?1%*{T)cc67>#&L0LG0*gStRZoUgp`%=7DT8{C-qz%>EN(4xtadHeP@=38iJ zD6(`P21+me&Aa4(Rnan6Y^_dbPwlWM_vM_{<0np-LC;=2-0L8Hv@q7AzXf?7#APnx zK(TDwX49f!5sG#SSLj<~BZd<{f@KMHvsE)px_Gk+pjn`HqN(H9kYE|w6ez)t*rVa7 zILtgIE-p^6u$031))-(AF z8xE9Jv^=SfWC`rGqDvK(!!zAZdB{jXH8ja;K`;s$dl z_Lg5B$Ve5w57v0y-fl#o6*^;8b#>Q7CHH_=#}XGjAxOTR!?87KtxmcBB0^Sb^hpwv zdi`4HXNIwstE(ii9~rK8QTYd5kK2L)5R0LqLB-50#V@4R&_I0ilh$9q{)~*wBxhKD zp;`ZvBfiF7GJ!ZC2>YpedCB2Gn;{)Sni@q|O@&Jhis)F;k`M{^pf-q{OZu6{bW@|i z{hk{O10@vAGib^`nz3q6pFK0h552sQe-BBW4f|h9TDbuZ0caygh1^J8fya~sOz?Bu z=-{KH{U9;69Yu*LDZGG;!QtVBeVyC&Gfu-~LFEUBh4nm%HR4rJ@clKW&*;H&H#U}7 zD3~GpfOf>iV^HLsqe@>+(;v=B0TkboXD9?XU4hn&!Y6iHQqpMhqfG_M`R6Tpvj63! zFmS?9?~p_hpwqdKSr5Pu+zgHa`RFEgPN&8-=Y^bD>=I&W9)B|{NgxTx@Q2eliwDb9 zS68Ryc}EyL`fp7BL1K*$7=&1wTelvx5&x7rpLd2F1p-xprHL?31oy%3Par0M_i|Z3 z6ifC3XT@GCWALKD0AUv*nHGj#WHjfy2@@S&K_LlB6o$sK&V{FyvoeTCK~%B1e3^h} zjB5GWW^*C}MRV$_rg2P}GyzAndkzai>|JP|E2@P23bH>TT&+07wUVse%Lp0kBSH!b zAQZqi3sVCz2?=5K1NmduL|t901>#;meM)wgkf)!^C?$jQ=Se~WHUZAe^!rMuxQu}b z_gtqwntZo0!YaRiKXp&oSYgELez1m{KL-!e1H_8~HpVW0`}ONHG?QzJ42Oo2&(*Pn zXfXGi0Bckb$Q+3VL;2?xs+3+rKU^@d76l%;`X>@pU{x4eJ`Ce0Po4lnLBKs?Y1w~L zT*<%o;X$-zbXX!3lIraHe781HZZ4YJL^VwonhDaoJrfLwV89x;FPNYXG$oFaxA(Gz zarX_BFjjiD)xaSv@|s%{P?X3lmk87SeqX(?i3cLf6?e!nO|AbJ%kmY@Vi2si|CH$-@d zSM*bWO;7Jq$m>L@J={Ctjnl4)W|ndbd**d$lleSGJ3Awv^%Pqb`w zT;H0*PQn?R5x08=+Y*PHWXvFpqM}+5Kt)DywQWyY=7r~#fjqf7FWV3xz&R&i1T#K8 z$@8<{gUaR|HoTJ#@yR(LPmt}A`6S^!0tVBwv+tq|V&davs?W;gf%T?^bS(Ev5z2St z#yaGe`jfNslFy}-R9r@P1&}8i7gW=I`}P^)OU|`>kv}X#92Sy^lD>ZPQxTUJb6aR- z@Y7g+awt)N2p`?17)mn6%VWQx?RQWB$onf5S4H9W#C=n@^nbV8*di364f3L7YT}b( z*g+nfb-Pz++%L*mSXlVCH7634M09fiNkp#7EVCxs#(ZN*TxiFRa0q=(LOvIEUguDE zA^{ADb>w@-8vCBBvE*g`qbL3Wu_!1UUy>EI;76b{eCnCd=su$H%Z;Wr6D~5 zr=9Q+{Bqp)+BMG*F-h%n=STpS1gw&Ewn0z;9wo61;j7^zcXqJes1T|=C?VmnIDH9f z1JR@)ams#Cwpat24pZ*G;6OJ$@b9(pS>fCS?SM$)p$9bn1jv|^};TzBiQ(&=PlIG=-XvS`l;P5S~{;k9KfMg<_J&U$o zj^w$1f%}Bdg%`oU6`l9AV1HU*rD4Z*Nl4s5{tIe-qx{S^gd@dNk@tme#O*!)56%+_ zmLw)_k#GL;s^nZ^QW6)q6Yx0x84Il9MjHUig@6dQz}n)-LzJrD?U(7^0E7N*&I~}o zkkFE&VC zB`5o36t7|Qpn~j;NDQDcIA|&+qT+IyVlVJU`pcKM2~pr09>+L_~o|f)tP<&$216kLsJt;6i5{> zoJ8IfqEm5>@wMRJF`0?aGYs>TAh=Uj$0$Tt1rka_mH`R?k_ncdF3ruU1Mh}zq!k8$ zCDtE6lQ`0r0Lb=oac$2TJpazsHM!-t85VlpZOeCaMr?wHhUV$hr@0x6*E0_uCAQx;;g0ilMAgKQ(MP@vf*@;nk*hEq`$<~+B>FZlMGzQb3BB|}n; zNq(yyC)Cxgdi`(nG7CDGQxh8vbl{~yb|{$7mwBN^LaHr7MU+EedLmOJaqyrhz91_wMqJfGWG39@qRa&-Z&B0;?>uU=&Nk+=2Fl zem!1i?;8Wa*mgHmg8^i#*y5!d{cY!v`fi1%66-*{)BFYm;W_ns)< zzWmopu?Mm#X&aOUMamw~_thPiPHv=aY)r1N#3B+yAFdB^%>c*cs8cd;{_IbNN{2oS zr~|+37~T_k4ajQXJ$7EdR+=_>KWGtzIP8S5DPnnh_w6G&hMWg2Owkh%?2&Il!@I`;v(5awAwuYSnv?w0~AZXS@)$q4(I>@Y51lNfOe9$ z&7EV85?pTK{az8bZhdWU{{q<_y^|!kp-^u5#7iF!TMoG**)+T6VC2bEQ=ht`tGW=KoR?2!u z2c`JMHm!-xhnPxc8pPoew6dJegh=t6g2*i#IUktcwb+tyeMZq@=*60QuynL;EeVNC z3B4;UOhk_%my#fvp38OM0cQgP?Xj>eda$yw`Mf9RN=-o5v$p|?^?>T@ypNd}@{s@N2`m-yuv4>5RONN8Zl zsS3k1Ds#the3{~{qKNkmr;N+%uPE>Z^e3(}dsvzS{tQPy|ewqDdi(ltrLPKqW6-`h@X5XD6iBnWV64lfoQ z8fr$|a>HB(gm9pupb7nI7WqO>i*_fKWndVnF9|h$`7*m9GPA-i^Qvqj(Ss3xWnyFV zsBAyn!+58*Pa0kacwVmi1TVB7;tNvAUOjK|09Oj?2N2RjKB0^<&rJ#F@3%}sOw!0} z z`mHMG8%^0tIOkK^3%wwR!WzgjD-R8tE#eiA&WkTyc1CO&_6@9Mq_H6|LS1hQmggg9 zvFhb^kqvSgO>#p^{%-z&R=BH(5F`B0Juq3`z7^XhWT*zOLQy(gDl2#jN1J5WG3K%? zs+sNyT{vN5Gn8ALT%{o?EnNi#7p)F(Dih&D;0-_*QE(-y)n5v1et0+LT3T7iSD4;_ zKe!n|uHFDI4~h0CS4v(1~ zA!%TDvG^gZFbx#&m5vRKjE6MHm$UeLxZgoqA7&qVC%MYg4PTi2w2X5<_*A#CkaM2l zzm`*C>sGCfvD&)krp?~;Wg;Ze56K(np2&1j@$t{@Q%5^mvQl>}{>*_`4;v6jUF12> zP}{qqs13x|I}U3TLkyl*>P+1T_Ocm1`rN= ze%lu$RjZK>;N$CU_tR!%D<7D0oc(*Q-H+k}Mtd0PZL;P@1+1*euIx_sWD{E;9qJ>% z2yg{FOm1>O`VOfdz&*|?V~rgwEWKl=u5dAM=(~8lFmVyC9Z1MN-XD<#?Uw)_bZIU5 ziFG)}_jfaS9PS^;K_~)D%G1;HX-38#T&8PoZf;f3N-hi{DDHY(mXv?_?fc1DoAot6c8r!pv=g8{5Khc;KM@*QQUA65g*Q?%^IJPBF4*D<0n~E{CYhFX>=+X1 z`Dddym;~}= z28R?C$IK$H(_C%686vz0)sO-v*&*1kw*i9@9@et8eObIAdSRh);EEaCL5*vfSlWKiKTBb{DLZj_)4k{laQF$NP6Y=QbQ=QVV6qW${!I>4@68Qskq zpWjABt~wynj%7iFJQJQQ1S|=)u-|)}Eps91f?Xk`1TkkP#4-f#;D5UQ7!~vRNv@by ztwRc%T=sz>J~?bmxo=EaY?_|^&D@an|2-FA_I6ZM2^5L>*_`4!V!)$Hfis=*5Nt^S z>%zTSP_;6&3!?uF)F-Fo58+7R28-P<8CONQuUI$ahQN*RIr89U11Y`r`wAZ=PZ8Y- z+YHqTCHolX1OLu1)@EkseRkV_gUjSTxVS2)FLb2>%Iebc@_=G`aWV3nL-Z;ne}U84 zY)j$x8rZR;(W9SIZ!NLw(qo-5vj#>crS&RXRj-MpG)tK4+E%*$eP`~a>zY-qczpIj zZ%)2ucg#$6MP`$$;UH^*Q0Sk+<7>8R|8ub6=Hjh1%IbKYl(^D0hLrd@XS!o*_r1pC z){c1<&v_&dEbZ0V%d+?GiGspH=x{tRP(TjxpsWR@>sEMA+8dDz~UI7MLE{s5MNw_&ANrYq-cqW^u**a1K5Qzt8R-D(a zr{Bi7k7*+p)hV++a#b7lH;Mdt7IK|EdsbdvK2F+^7w$^orG%3oEwe~G#L$ovDn;kl zN6I8^_(0Jw3Fw04A0WK4D*DO;uRT4zKhM_qxxO=J&Ed@%r>phOoGHgi92_1tM=Al$ zqiSURx;3S3rXsBK2&E8?58=8K4qYE+$35ri&i?lb*Mou>RM(XHd~|&A3|1z&V7V>9P3! z;o-34WbV`CIRk?npZ^KzI|OY-PvZVL;ht*38zk#th@AaUwi&rK3utPEi4YA7M z)DVXaq+EM13tSY$9uEvK4!X#;@GRP?{{fNOL{Gm5BnDHQiHV7XrOsv;J%QPafC^p_ zd01XPzIDRF!g;6abULN=%7Im|`87~}zgbI6UcRxDX(RP9#Ijm*><8d?Ls0=%CS#^g z{zz$R7}+_IU1uE6oIRTbvy?bKNNcc%2SV*Q8p&06XP+*yrTqQVIa0~D?B-#LD>F}|B|Nh-u71?4-GBw~taO=Yp5;!nL zt*|+haW zMR^mU07{+n_|%j$lalhK>Ti@^r}|oukBw9Ck%j3@SSX3>;Z_E@*Mi|0kG}x=#g|Dr zz5mjcD;FuNDH^ z^=|$jg(St+U-vr7!~n?_;nDHY{4pntao}qQH#aNPFu5_ycj(XsNkbR;pV3_mJ0TkU zhXIKEA|(|I-{{BJuh(K@V>ut)e(>V_`Sb9Ox=4r?qPE5ZX-1S3DV5Ef-0ZiG?!rAG za$A!ioZeoGyql3I)}*8)Ef5$HE9ruhNpf<(a~;aCCx=xNwNs)7-ZX1=gu8R{s7^#LEzBc!CHez&zvOik_L=MTJfix%Ed1qwoh zl{PmokCB1lF!E%`)lBjVCEZF zmQCyCu7Oyg;!tN(xGh6s0JWClXm~_t0B(kEyh&dEk!Tb{ew#k$E|u8k4f&8ZxF^&|A6gp93#iO@H!!q-}A@i&j+(Ot1G?cvZ*9uW5&^O~cwcpCHF_Hc4a$@e?tHAsv;)aC8bQ<-m zfO9*!Kk-Uyhtcx{ue)Aqio-HE_y__yBns5ygZ{--vHJCHk>oV(_+#&6ovDwRZHB8d z-msO?Fa8nnn6yapnLS59ike!mVPI=Vz#kpjFQM(~Yf2NJy?8-ddHB(?I8k$VT*BY9 zaia+;yzK26e)oB@*PW8mLQa@VL@$M&gECEh`gG$fy&62#;2fUIKr+z#_kSL2&Q}$9 zx~4SsCUyswEv}W|zF-;dD#yz>^MkEhXB*)87TV8FyX_ABB=0A|7>$D{DSgxp^$)3$ z=tzhLqe}5G6ne+MM<#riRK2o0ayRG$tG(0 zh6gkGNHL_S?abYBJ3Xq}q8s#Tc~HyGtfR;W9J;Hgr|iZiW^#*#?VwG=$<~W&{5vNq zz6mM-8KV74ux~-*U=2T-H*GZDcEmP$)4H5e%PjH@Fv>k}IRz;4G3Gab01#X;#N>RQOpqj3%CXd`N5jpa zmJq`RAO7_Itqzw(`I4yDU$3OUB$QBRIqi7}Eu!0WVAGLc-cp}RyBu@L1X1>IT%aMz z6~t<)>+1*VkR9z+(E%EUYDL)5T!&vWMu1GZ2^t(ZhbMv*d2{iX(i7_x#Xj2XyKqL;q&Y4HCj z<4TEn3d#-sl7%bvtGF)kwV?sm56B3WYIhHSD<&%#q5MpmG3EG9_i2%Yx_{lE4nJ<7 zDB_%^X~Zj8TBgM`#dYR~GpJr)y3Vo#^rQ#Pl=yvka>r}qV$x-WgoM^nQ+N0G&wi~O z%>9ZPk$JIZ8@W`2EQU??{n}a$y_fo>V8LIlCER?r@LXSa{TOp)5Ia6Pt~NpZvzB1T zQzqbam*WCebv26WD3Ua57lx5u+Ss@?^;_^a>rX#^Fk`A3Uf^!` zD_z}cM*0(y&cqIYZ+GVeo-iUq(hVtgBl~zbc>QTCa}-xQ93mM2JkP=; zloXJR`$#tF@KEHcJOZxo*}xOJPWk9B-pls(B!Z1)Cglw8l%x+^TN9AM!AmPcm;|JK zSX9K7P*;Jc5ji?Ld+klB(qTaJ0QC6eHC9*&+=hqN5FrBZMDC4NL~>(Tc3i96OhK7V zd=wo*xcV4wx{;m=2++4dah~MffcxZob!fQwl~^{7hG1MF5cUy|Ybi%~@z3DkKPEVR z@>8|q7{S|(jg1xNJJ%w9fM?sFC$P}0Cq}klx|-*y0uXb*j?FF`l6%>y3_3h@eOEUQsD$FW|e@TQ<3_^0d z9y^N>C~*G$Ef6dO`l|8g^JA1%dt}ffU@tW_#R9?LQ?Q30b@lg+0*r!TT7`$JKx6|p zTN6-P0uPqhdyuMezwd8aW(ax*8k}7Y=9dcS$3Or4Lk1Eq3?aLO9$!*Zqt16)v1_9a zAFJ4jC*Xe!Aah7!kP8XGAV|F6lEowRS^x`2=;fe4=#aShO-mgh5X$AkhexD*Z*Hza zpKZx=-h*O!t3HDpS0zN`2oJwH3yrJ}{#6-ja!Uke^CsD&Fe~n~?M9_ESd6h$0 zz=mPz)Zkd2EA}PPE8qr#RiLGP2K(Q@qOz7}nj04Wyb5V30&B^$yaon{;z6>z2Cwd+Xkmzqsz3hYnn#6+ zNy1>Ty6Fpz=t}^_{=G~ynm1q}>LT1~Vdl7X<7jO_ZHp)?EwQL^Ckuk*0>Ub|B766T z{<<~C$d~7cNFnzhkXgr+LX6>d;=?Cg)@3{PITg54)=S|g1&qWuZevEURmP{MEpP>l zXnCkpxe(cqQh{Ge>#)Z{XxcH=DtNFFE}ikiO$7)f_c~%@n=Z8a2QREzpc!>0MSTix zv#YepHj_-ydC^!KCmMij8kj&hB~MQ6I~M(T^6V=;Iop&f$I{#DBEDJoqA!y#3Bp05 zO`vuprHc^4tsQcDvpbo#T)@=gxYW);ON|bM=RM5N&x>6A%mOrsu?|uRe)7q1`)?*v zU=^aWz-J_xjhjY5rZ@NLrbWTk?d+wwoT)DoukrNsbJO~Lw-=d_7YDF_(nxFUyRaKg zn1D?jbj$iA!gs3xlmJa9msme%gFdI;G|Cvjg;8|r?O83gxYnnRi_QPB5SlU3(jANx zxpm4PMU{HHyOkgwqt*7|ZYVRB2N>Y z6!(>B;Lo8!;)xaAGfl>O1O>OCSw|Y%-o^4O(%f8AXhfbXmF&B?4?qByT%da){^n^d zcynFkcGB}U9T&hY#!NqoFL}-f9LA%mxa#k!8~3ZMEC8n;{{R<$SZ8m3HW48kPC6iw z2$de~L{Oc`!{m8ckS}Z{6Rbz~GW_1~SjI2kzpm!iG6gj%Qr4vrBvQ=pPucSlGZ< zT`6^zp}{CxXFSpes0D%2f3LJ7A&oOrb;i;y-+J=|AH(nX4I|WoGa{`}7`KI~ z3T0)ydZqg06h4UT-37tQOJQ19fOD>r%v5wP`b{;gSvn1z@mcPMr(cp?;e&p6iic~B#fla zt}f9|-PGxs3KxV4LTVK=mJ8%Jsh)57l_b=?Tg@L+$y}& zWG%w_;Qc6M0NSmnaW_@JmU=# zEu&dBKR@g+MjN~H_Ltp#jCb$dOKG%u29^%aQthW!d!bYtC|bn&@s)I_sKlcMawXaa zaqFP{V1w%{^9SR)9s|1IX^jeaz7#SE$O$o zltm7NSyY6fq2c)CB+0-+FT(RtBJrz=2d$T!%1|h#|_}ndUBNr^4hUyr^Rd<_9A;#0qeAq9M!=@nswLZM4SJ7HQ99nVk^i1iSt99 z;K75;&_6|-de!nCD(_^~B%_ArKLG-yHRRh+9P`sI&UW>Eun+s`mC7hKok}`j|j_L`0%6z{rzGK#jmQ-3)3u z4-``Gy2a@gy^nL*umnac-kW`SMoU^$>VzX|h^0(5CV7x7fr< z0t0;G3zBh9U%l!>O@r=|M97*oYaFg#y;~m6=Q?`&`PlpvstBl^v<5UVT(R2*P|1Jt z{wKh#!?5)+>{5EN503{sN|}Ai6j7Fajfe}Pwxm?A2Lw#a&RTxHvv1wHbyXR|#j1PO zMVaI>xUrNz1Q$mTgxukR6#{c04XF@tIDB8?HGnY??&a96f-ZDG*ev4xGE>g^Pq-9M zZWbpEf|fkR4d3=gaWUmu(vh`- zPOkYEUfV4p6bC4l!5eL7~X2+Yhz@B;_McI9f+@4i@fVm(})}di&-vSS$X-~*H@eY*X!~AZ>@FB z$;k-;Tt=#N2Z;UU&Gp6z=~CzPD!Uv~yPNlUiFAVw>Mn?$Yqw|7QBeeu4&n?&uoveL z&NTqnB4P!Y{Qay2mGLikri|C|h_zWc?aah^7pE3@S}=H=n`M4zHT$CXYfRLN>1avYbTn51*@kZP32v*Csv-_!VaZ?80;mrYTs+Z6lyxf-(>EiZvmL&0M-mPUkY@LBWNsv+qt3(7DQ%}E|5Vpr zMeIo%9vy8Y_G@o%91*hsu*u0$oq<1Oz~|U8O(3bQZ~_T>e8NG8PV?Q#Vi}mNnVBh| z>P#kh-s<)&>#A}~?~%RUD35awS$^y>2o4uL6u{M~pU1j&?~00cQZjTqU~6YmQu_O~ z*SpG!iqOeXg^newJojwd*`Y%R@z248=n2Y9nAUVE6eq<0H&;Jg%Tu4kdO0$O#Qf zqy$$-aGLh{!8yeMv$7K&|Fg_+d)uqjy48dt&nI!{jD^i2oltHJtWcU@(3s51j*B8@?Qa;a)DZ~`p}scoAG5DD@ORx-Hs?p(~vg)IF_p$H*SPB2q0Iy%ggIHB&yqi`*XFUw?svC2l(EtVJw%4(z0Ng z{sG(04l`6oKDl=EO3JD6XUZxoKVU!l`@)Z3`-YAE<2)d86!5YJG`>{*f?%yO1ONul zl?s=#Q4a{4(His*W-ta2_FNE#dJ)ITFz#P5N7G2<0A5)QOeqkF;AZ2-wFHPtrVMM_ zRzLA>dFU0deFF)_3jmvB&=P+I&_^lhgjeYLW%lR)(*l$#bAB2Bk_q&L z-#?d#?hE0|ee}}^q=xBP?N)nyZ_!$9sFRnyA(%Suy9Q$@%o!?M&cMzel`1gvj2=8C!$#DQPt*cJw z?F8zrnm4?)vX&RAgA(=wF?<21yC$)&vd(~1)T@_Du$^(P&%wjLysfvU_8H$AVIXbJ zIO?Lv$o%4BKTsFfdmO(@292RbXbuGbB%Oc^Z<~JZZgzI?g$oxNE>_j|OXIfwu30M+ z#e}=^%E~UN2B@v$>dmV=+iT>7wH_W#m%I%GD|ld(SO8ZkTqq?RCXRhifBm)D?O`+F zZ78XP2tisy70P#gzqQ@$IZQfwtX-S!evJEO!ih)spZoc*$9~)RV(llq8Q2g=Jr{p=kR2Z55 zUX&U9P`Na9{%515Bt+5Oq4ho$AFocqL2e;DY?2MY4EvZG&6LS>ZjiCk zqfmYHBz`nwpgkmI00y7H6D4Q6t49Pu}GygDBSYl`XK-B!>D5k)L>fQ&_-N@ZY24`{NUabh}H|o zs~&KlrGxu)+Cq-{*l|+v+t}qkxZzxydt0e`xDf||8M$9aFLzaGYV03ju;LEQm4icR zCtsqN>+5;@k}s29uv=0^*I{L~ZRS7AP5%i11%YlN3h%=c9b8eQ0v$Vwb18_~1jLEo~8iI`GSbBbRkMGz>^q0IKG!}A0g$qZ#Qd3(C zBAzGC?-J~FbZ!-WiwpXEc(mxbK7RSa;cvJrrM~Ac*OJmwLD!f46Gp?OMeS@A7J!mz z?WmlrERA5hF42q3n>Nz2)>=PqKD?umk%%zpIS~PnuM~_AobTScF<`3lEm(W1!33kY z9<{CZH&bdbBgUl=$eI`@U)t8Tv5Sk#rFH@Ptvhun9k|fCMQGlI7La5s+Wj0ds1Va}lUA4GH+6x4lvt4GI&W}Wf7>o(YN)VzoD`;411C6$7mgolc% zH+N-KexCr&024!9vot}!E`Ts2Ww0DQdUxQct9P z9cqNI%a;Sun7lf4Z%(hrWOC@u2m|#mRp81p4~fiJ=RfyzewW<+~3sw7T(iM0k34lTEs0-Z$yh5HD4b9wdCjLTOcDLY2M!6f=S3x z$Ra_SLDep*HBr2616)COs(=^*HKjGpD2|cHx5DfVT+U8!v(mmSw*2KTUAh63+x&-= z3-3xxjq2>a<}#Ejc|>f1@epBr+&=2r?N3dM4P^c;4uXg_OFRepawOuB_{!0t+huf2 zJvRUh1R?{W8!~iwRYA2V(oszV7f{exXkCW7hCva?9t~5kjWr&l^#f~uC0Xsr%}T!s z0tOxTN0xv#6=;^&A`KIMHMp&9vW-x`r1J@V0E0E+nf%`@%zs+<38ryeAjI#ZQ4C>Z ztFNcmz`-G>;Pvu_AfQbg> zYSN)a{CP7SH}Yr(j(}K!kOTDD93V7jt5*5@&+Djow)6O68PI7&kG2oS{F^}*smZCU z)yBzhA^57e&uXnsY?`VpbU2S8uzNI~=W50-$0kgg=%uY&6gB_jR0iD`B>;MLB%5=s z$}1D=g3)jwIuh93-l^&S@SYo@?G2bx*MoLpr~|%6^c8e)mQl}|Pgfa9=X3!YdLwf| z4<5~m9DO8NUHp z9U!*C()j`dX!cZJI;!c&FEkN!OCFWkYk*egAzUY*rz!dR_^D7^LxlcI5dq|I;GYZu z#F@lG{*#)SIbp!Y8<}dZYqIfC{!x-W3Y_oD%Kdxy=I-C#e{V*_?C8|_eRj@N+MKJ^ zOWjZUR-XC0{QM&WmZ_UoCAFA}B$^b;= zLP&;*7JcU?D>7zdiCw{$5BcxkTM@zt#LP_JUr=0|^6=cz+pm`nAR?k7I&t%6Fh)GU z0U(-dx&&U z%8OZXzttzNW@+s#k?rVCR2$Y<(ISk$06gd-3Dn9@-IBCOWxsd*p`D-XlGYsH5YRXP z4N@GQBypl^AQAwM{TVBOt+#7_|Mn5g8&w^aJ_+#Wp<7-m||l^!qb}r)-1z;jG0Cr4b-3b=b4ziaUSxp z{2SFGw+Cwm2@PF!4OEm%c7HGHRM81S>QIO8^?e}c(%B|mX;R!s4jVYLiRW{kho3-468{e zap>jDBs+rpo&6pI1&O{psAfrdHLjfzREvWG^*GNfEAG_yD zli{7J48H}dSfT_pA)EBL)pN$2H=if_y5DGBD9%a%{h|+`B1J5KGAZ8qmyNB$!M6Kp zscGT68XDe3+`+WBtxIy_oW|`_H4+_5K>jrNc;X;U#gK`92ZK*8m5h0@Zp6bQwq_8m z@$a(jrV1CxuC(Jge*3IVB;`m_l(dHN7!)!+v^Wmd6m&7|*ts+8>FTk|tbfAsGM{}4 zlpyGl{;yC{@cj_Kr^$@y&h2xACWyYdc~pwGSy?AGi-EF7a9r^GFXs-y&7^0!CWv}_%;}ua>g31a;!P{-GEhv@bHhA^WfsXridb^_sZtmq2geRx z04z_If?VP&ICAyAurJw$(jXH4tP-H^5zYJfVlYl zx`KTvhj>!Qqya?#m&t@oY&))=v)ZYBR|Mh@%S)ZEnU`{XlapRJhMiaQ1K8^#7S#c7Tj^r*_;jimElx%&xbijmZ z)4Il}mQ_@A?%%%+P1viY2cwJB?na+I%bGhy1z-RwFEkvjcU>`a(-8YFXq-BVm-$it zzf&|-YySy2Jj1)Rk9q5Cr8B?QV`ju2j{@3Ys)cDP&i&K}^^P|kZ@!A63Ar*nV&8S& zow%Myh#eiG&%OAJPhl>z#hvij;Ejk{fR30hzHya*$lN^dn56(3Gon-$3dW!Z@8HZ-szV&TaB86R}5a-RH;GPvK3Ww zR~Z>7P+XDPdq$78GW~>@FXM9sx-(+mrs?Ojo?rZ}#WZj1(*4NG1lkAObv#O9~h#az*#S!ESID9?6y&W)Trj{2aPjnj+bg zHUKdlfIA&yL3qw@W9K3h8Ac+PfHfkEr3;x4>qJ*J=t$${?%zt7eMmn(RCGC1V4_#1 z8GD3KrQw3zhq**#Oli2OYdrBPrMqg|bNZG@;M-pZbVtarF#ox<{vRvWk)uVqF70@r z{+ReP-ztLD&KjWq2F-t*k+C^3F){Ga6SEIJx_8en~Po|l_+$>h~PL=(XgG|#OJ@VVX zzcnfdzd# zzV%2gTgphfqqK-$RHLy*3xm;oqwuwkej)28sVWHGb7>9)B5bK=#SSn->$xchkC^2? zY}M0OGi4`Y9MTGnZy7Qvpcc{EFiHgM*F4t4X00`F000&}t0?tx(xH$?gWx?m>Sj^w z??(R;;5&1HF`=o8fQaIOArr7*=mo^>S}6isabd>qp85x0`9NwPpue3Vb zHnsUiGaVI^G$v{dXSu7((SDw8%wL&R)Sw;7a@-%v|C! zufEQjyjy6zfRGS>hDjbh8gpI;Iqc}8{Td;2L;fw|kiw7Y%wK^Q!8L-AQBs ztpqQGb#6e)!N$BjGexVEXU(VrW-p12M=#OOsl_1@i z19IL+2M=cb(UyAtt8SbDOI!(iCdM?PNkoed7{rADzj7FK=I7*-Hm|SsRhQwht>6HF zNom+g4jMGAM5R=1)?Z;LB84O_qeucGA}sCff6!RPywO^CC4~^$zVE^d3}c|{qW%hH zB9M(hlzU9eOJUBmiAb4b@29lym>XGWTsA8cW4nVdPLNyyany#c>xbzNnca80;^sL! z4C?a57+&j#j#j6AFGb6VMuxm5&XKqS_USXer07}BWoBm(Ni+AHZ-(EC5{C!>3 z>LES3RIJ?sg5lzl#*mhGwZ5yRkK7v_Cn=EOA40SS1vRM{6m1@fU39EWn3K2egCEgb zG#D}HVs09Cq@vtA^%Ov+MoXsKgn><_mxO3QCZJ#aCKam)y2iny6KNS zzI$>M^IDdMrHPrx6G#G#EWm03nxL-Tt=@m}tz|>3=Tcs|tz4<6tLy!rl#awJL%TTV zpC6-M+79Inf7JwLU)dKywfj2>6iaqoLXpTAw_F$svuTRH$47g2*(qbIT@#T=?JMRLzb6!<0I*xYyLw;r!r0musrz7A^&sjT0x% z4Qzn0+M~G>74)*;Q_BWu0Cx%tBQtZD(A-N+y?gh*VdnTll|v?NY~NH#wk9FOf}JHS zLjO%a_k5(n$oflk0&NgigF=I&VqoWwa7i2r0uCJ5>hGU!(M}DhiI$rhK$twnwu$;y zVM(c}4YjpHwtosmCF}Y#Yqbc84jpQE3&zohcT!_7D9gQ!*&Qb;dauvl>DzkbK$kAg zSEqA(C=yOEg~~6bv?)A2ibjRwnGZEeJtxr=hDn4QkdoK#KV%XYzC$oFAUc^Q;SNcD zMr}1Hbh>J($6ycXHsZwioSzXyqIYvE@7aK7?53-fuNR!XJ<-JE8 zX<3)ATD9SHvd!WwnGys$#r;CgFir*EUG-Evr!4oJ+#g7fR;;c%q%IU&D+(coPaqY$ za5$+Jg9c}zj|?rp(|LM}r|Xweu%RfUupExqg7l2(Q2m^P@er6OMv|BgL)555L`-n} zyi0KH2M=B$sS>~T0YQE|bVlwA@;eVNcxk-sM0s+Iz8`ZbBuJWLkv<}2i)mxjp#R26 ze@4e8WsM%aKV5t*;olj6bR=Y1)VR}i+XuXl3~Rp>$AB6g_*d}Xz1nO$s2LG$x_Qpw zweLo>9N0v!>bXI$p+h_PB$?5PA^Evwntd|+&6~quVuXOsOn~jt8iCfE^eCe&thw7p z<-v+5vDl*li7Y-hlP)e^iH?|EC2QLn};GXLoKSO7~jpPmUg9#sBL|UyWqj+TN8|R>>&rcUv@oBlA8+y2L>4McX-#A+F)zW#c3zL3Jqy`9un3zc zj+cmTV15k`?P)O}boyBxG(F;sKz&Si2B@w{tFUj3Zr;D!mei&MJRQc+R3Jqh8gW@q z1pLm__{LvZIY21uf3={TXnN##i#|_Y04&HbHk1K6FKa}1Z&s$u(($!JRpv1@XlfEL z$WpboogRt<4kG<03pZs#3bYyDg8khYU{e`c+y$*cqO6BNS1G&3nA<^jnpXe);NRx= zC@wLfBB%<+Ni3l$qqwBo6tM|qUNNY@Xm0q#ZslcVIs*r)LC=e; z0F*;uUHUn)jcm#mMLNzx?t?qX;0)p+!rXq_!V@P4)g~UAdCX{5{Fd3#U06xZ@4nyD z#%0x);NDT_q+WbFmLhieXiKqR| zH=NO89v6)jnC<4tQI?UJ<#n&Fm*$=>9a5K{UVI(t#F(3-HpPET)-l`hw&^Ly)SaJa zc`nyqrut#UkvB$1(hBvj86Drn*jes$%q*XbW>*LuOt6Xl<@>zni3g!VWwFJYIr&YSkS zt+O{pH+ua7angI4-lobb5WVlhg_qVckNwm6QpxZqD$~XAM`+ zPp0MtO{{*e8VMLKQ(|0sC^2#3qtEHYobWa62`XERFvUffuohg5{5&6ml49iRJw1PN zFjiM>d2sV4OdcLPs-(K8ua{5E54D=QZH@KBEy0WXB4Q5_oWLESGnjj8bw}f0^*$uN zjzav3{qi?l9~}D}k%x^2dRwYT)Np6Fq}bBZgtE58XN;u(`WEwDT#A1GGdy-SF;M^{ zR^;IGr?%C+W5rN>(b0c?iMQ1Gop5&9b{*xMV`^Q$ew);+MQvRPCQaz;n6H)}5np_Y zM{z}Bo8&UryB}A?g>QHyViFRKIrhbHs1%UkJiX>*?bYazf37T1+o6n}Lrm&5HHkSe z9JSX{HX zEqz_Q>x^SqT=IXeZof(s{imOMpYW^1k)Hz$*$|dm-p0|3Mnfb>w3K8X0L}weJ4+@m z^RJtDbo?1)9w$-Pl$4yhdUe%H?XfC%)pHh&0J5-)oG^X)bS4@J+A@buPePS(*}Z5y zcnghYe8&9VtMrxE_$yn+oPhL3)b-z4f4unWn>9Hbq!lE;MQi>@Xul%v82l+V8>7f- zI5|k>ZPZF%MNLilO|`3k!s7*(U$IEZptWz7R<-Ju^|JIx#yv5&CR9;oP`zV`IOVAK ztPas0c!XRtN z`Hs**b;zlH{bFub<(inRM~KX*N_6S8`M^WI#(MWUT0P*v>cPS2pZK;i4Mn3ax{<)N zJE%lnf^v$Tu8xiZM!gSq87dC1`mw{_y%iSp(uM$w4HdBfKr%DUY&!o^ulvJ?T|JcSujT54&YZ%s)K3C7byK>k8*C<&N32;my zEsNk`$k82v1wl?vTD*I3Y@2Or9aOgjjNEcE8ZqrdacBvttJZej`iksu#AI!s#aGw8 z)B;<;O64%q?kG7auz4DA6l=aTc$zpipmZ!}R=xnzaE{M%&fLhL{Knp8osW#RqPFvb z)TNL9V=hr>FAepJGmlg30SKp8{@@awzk(?rn? zG76yNlc(Wzq1wfj`7zvfse?ua>IWbVpp>4|OCjdk|LRunNm`>x0WM@HYJXglXxZm5 zH}bLK=a8>2y?d!wz4d}(lTFNw+YE4dV}p|=%88gPxPkq@k5G4x*1vw{ZnU?~?jA%o zx%|9`Jg{G}_aU2z=K;N8#K{k?wmg)WEE~>m8Uq zf-`;^h_9wtb;+nH7YVnkEm!uxC|r?U-eL;NS9y7iUKEg$SjZ>pwXqw&wxUVfw#wID z#i~zT%U2wKXbQF{L8r+WI9Z5>Os(7zR0%(r-W~Ln9q$a)_EkzRWy*TCN8Udo zk7T6;#0gWMjeL@jb?i+T+I&@(2GUym%D{KT9g?ZVlMIFVr~24MTIdJj&BUNTVq17p zjN)?ckFNp{lrJp9s~lgxaPE;s$A?G1Xz}qvPi4no*KV(H)uar3yYbgDp`iq~ZP3ER z49Nq6DjvTufX)sUb;-^x_bASFzaD83Q0J?{w?`EsX@Zx`l#%>boHjpV=b4GNVHQ4X zj@HGuF!xUtrvyRw zX`Au{?&s&@%`Fq$U;qb_8fAAdq!#pl0OJ!^oHI#X)`o{1rhM_dti3d$C>Dzru@5*s1~*%IewYF3FLptU#L!pDM7&VJ+S*3-1Kh@zZMaxo zp|hTxi>N{7$3aM?dnK+SJEmU=n;g~fO0G)OJ;k|S;&`B``Jw+2@0cUW$w#Cq5IaEg z_z6~ZKSP6FX+H|dEFunVVNBy)xd&~8$YT#4Z0qd%(0D6$oztW~-k^}P^V`)|OJ%B` zdQ|Mc*tCGaM1&gPge`Y$>HXkEzl-RupjN;D;AxbQzQALQ(mO??1^8qoduhB|H%1@b?SL;S)p;kIUNnEKjNuu#X;Y4!;A3Jx0l!+tLE+!_vhCBtLG3cK zNa@UB?dW)Ea|dB|gvds6B7^kkU8*uees7s+c<61Nj+KvM-t3j>TXgHn-gWP_u_)q@ z5KdyDLcz}1ZdVk($LsNYyp#kFmV{QVO&C^ zI>(!c{Q%bsrP|`nS!-|D&){}15<~C7opw7*6N}+MH?#u_5orlJxjg@FIc9L`SZsn! zQBPYL9%?bjQeQdwX5sNhmctO-Et27#zutB7=!p}cMS(~A{H!0~I^X%Uz5C^6ZJ0Iy zVUh2O;GQeWez@>{L!Yxa6m$f#n70H9ziokOe}}mqY?z?(&tuR)L6#fH_6V@b<^~_z zT81S%71Ry;La_w;#=#RgGW;1C8Tzqc#UKvs_sU9s)nQ{-EovT+_cQ-&(@{62$qQ$tvLe=IFcUI6jSTJR-)NerDF$UB14|+NBh^t5PCT-tepd-g)_; zL8RqoqlU`a2jQe~VlI-)RxuJ5? zrXwQ8H~ToP-^|U*2}|Fw2{MhsibXdfuSY5Y2&GeD{iMj9XehfysbIp3u4fgx4aChu z=9Out+U`n8rjt%jta*?qLl~zXZW+gxyDM46PkvqG85?m72750L-?jsLkv$UFJ! zHLu&OoEL7Rc7|#MrwEF2u*#8u`@F^zuy(TLU7{;U-b&z5+G3GI>*$!p-z?p=%`2z( z#fP(16ule=w_a=aoqFkNCnC1@$vD8hpaJ_k*hhq(DWb>#a=+;{Mj1^Di2m=H%h+g zINC0x`lkZw2!9ItLsDZi4^y?%M+x|>gyyex8O${WBzj1i?uCfN}@B`6FTqCq48XSIz`;p)t%1{DqA$#uMEA%HavbiQ{?7&5P|I2Tk=cp4z&nx< ziR$B5rev*+Zl3UHsz>vGhN}&8G;EFTu$hoO`0R@V8RC@&4Od*f?xH z{nz4LcK6~9gEBh>d?F3U-TRbG;UR5G&J&E;B#;gnP|aEkc{0f}TyLfzgb)*Gx+Lmw zYPj3voDuZ=q7(ze5X*|N4CUR~@twBcPU2-%=WEy5@VaFK5-l{bzb(ah+I(GJh}lss z4MWHB>eh;RLG4?G-t+1_m6(dY)=EFHH>==@P|PaF&OHCw#rSpo0RNeWay(cjv4sz( zP{z3E1%~Yedhjp|E7DB$rWEFg%Zwp#M(WZ5BL8MQjV!^NqV7Or_h(r(k5Fs|^}BRw z7&r4@ZqePRt6Nc;($5RfXFql7J(44PL^j^$afk~iP%ufIgv3M$bs!SU2=&m5Wtz&~ zy)OCfv@eLJx`X&by9>=j9CL^XjY@ZFIwC>$sgP#0^1NYKg9W4&ID(Nwr`PN}`RTfa z;h{j}Z>TV7$M-ItBeq5yb?keX!9&(gnT4&ZXimI+d`MAKdmOCyPga;8DU61e0*^ly zQ7n}u!M;MXyfo}b!W_$E3b!JHftj+Jh#yiP(Jwf@x3x83!8qJ!eRss;o-B4=Vr$5JI3 z`yZ|!DAu%CVC?|vL~P22cxz5Ya&K73;}il(;SLRA?T=T8NkJ>!xoNlc67U| z(;t!YN`K@vp|cFOSFC$;91>9;@lTDA49(`eQr11}jh{FcJ?3_;bvrPZKwv+pz$V#g zSkA>cr|t7A)^TM!Vl~LD0Ren1GDCrBi7$i=zxxrDnES%P%U~szJX9`X?n<{PGkKpY zWQq<;8m$fAF$`(n?LgzIMdja|hAbMca`$-eR=#Z3oalu|J+@LQ#j9JKPE+%npfXJ6 zA#gU4sZfmEzM(qvB`}4(_-tLV)^EY@?sfcFKR*pD2MIdxqWG3e{stHCv8r!n;v=U< zP5KdQaAWv}5XB^?}fTUR&K0xSt187R72 z@)InnN}Dznem_*NYrs&k1VYFdLAsrt=TcgWq3)p{9_r27jm{gD@Z!v?rI?zI|5*yB z!?0Ad%dbpwKUgh(xhig^6uR7)3A1L&0xvNfz|bI%t4}2k%1gz|nPn_syLqT6Hr#bo zqnGfZw)Jl5TeE8%154A;Fwv>X)=kQCQE58$3;Q%9hA2b*0lv5oIl=8bp|Ruz;=#kO zE%`CGam=mC_|Gf~6EJDua`T^FkSjpU%t$o6F)Xp<>d$32k298wT^gEGEv8i|4*57} zr=^^6pR+nQIj20omEv$ULwRhV)*@&?&_OI9xGo0GHRR@OBal|`D*4suPs=?_L<%ZT z6B)$LCXWmXSHy8mWFLfR&IzCB6S^80oS?9#WN7-b`#I*jFj|_U9j(!R&x?y*(b3>M zNLMU@24#2~-6|u7;wFOEiW%e|aJuxpfS@7BW;&Lx>~u3makz@h-NVY3zqu)txKtNU zU@t^CvmBRgk}W>hu@euQsNQC&-znHc8JWghozQ7ooi^43TO<6c)l@!i$&j&y4N6P6h8jQh<5cBKM%Kfr-krvLgO(Yu&R5^dwx*Gr-U@Rdg{1r7{e zLSZj+w#faM*IOhrPha=kAQlcfA4ND(9d zBFHwta!B^1(g4pC@Et{Gd((|Io0@HM-ClQP>VLCL&!5W_9~+=Z{77q7Al2Y^$JYJ+ zC7=VugM2MLJtS-pLj^6bb{eKc;P_1%J5k+g|OjIX@b1 zHgkb52Ea~J0BeS@;qVWC<_0Ha(gLKxJXtCgy4^Qg?3@nSMnVJ)boR0p?IUndz z%yxxt=+I#duY|jee6e-ot?qU69EOq|F7+NVzQAjmjNf|xcEDVVJhQ(VM4 z@h!t#KUcA=fmzQd*E%p?E-u?>od@y#O|<-DU+y+fm{)}GgohzawD`mm(8jl0M48Gd z6a#+JJ7^2wl9YU)1{B3~Ot-_R{lLH ztB(4;3eZ)n8ZJT?KGE;0d=C>X;X^cBZ)~1^ZXv>PUd9=-^#gq}?AJ%HTC?UdB?jkD z;U%Z#kDwbh`O`*I)o#{QR{V?)2?T_699LJj@v?tDYdKR1q5|$=y{Z%ZBnfgq9ZPd*O}1YvJ-&I zgvv!SI~+8M)+5xO01Q}TBR|?ywjpx1>$_Dwl#`wMp0f7ch9XTsZi-XT4Cv=@Gd8{V z`I5D$%CITfg21|xY5wNg!*&xZ_#`~@Z8XKrK07vc3@p9+G$SL40Q@hV2my2f5<1F3 z1i^=W*xGHM5y$5x2_9V5_0fr%edF1;U3(^i~@MpPIfVCDz zRx`VuZPPAXKAgo6H8DYR7w8iwk?`gF0sFHiY9GYNm_ue*;%9M)b3nW!Labh5Gf@tjZZ7q zou1@q{O;G}k{PS8Xt4x`FTL@tIbGf8paFhJ^ZJz$bs5&$;vb5Xea?U&l;aI%NB-6W{ZUSS zbR7uT^~jZHi^Z)R*zH^1c5T_m#m$U3pWwD6>0V`D7$QOaIN1aUYL(!N<_X0uTjDzwu;gKr?H23L>~XOJ!du|GXM zLtqm{!1QzaOa&v5sn;Q@Va_2PI+rCSuj~u*vt05Zq}dV?g4={hRM{9HnDCY@yiC`# z0VhknpQ7TXg~$3y1h54Ba=7qbXzRKTz1fXFP--)zB_nw_CviH^%u;3a*Qwf}(uqLjSyF9Z5ad$! zOjTkKzy(aq4)aiPA^_apupsHIKj=U6a1``X2_e`zjB5JNLOe3`x|_denDotCShX=4 z1`9I_dXfm|O9;{!6axdU+oZG$Tpb%5yU1Tw{_JLhRSRrf+qZ9DA~lT|X4O99&k*3Z zVk3hTYt8Na#GtIF$~04*E8PvgmMZiJOhc%VcPO!@Glz|4tPDqbOw$CbLb@4~ReNB} z4-iV(;faz4nn}0}kB5PKO>OW+&qA@d^sKpfa)Y7cqFv#C#l!LptL;8hL!6y76Jt!1l1031i+-%;QFl;(p6cVYX;c{m}F~%$Pue8P4c2sfZL4 zHnAJylM&I;W7U^kp(T6&-Ztwor}U5D!5Efs8bcv4RX2XTULsvJTnxwW!qgso1MUQB z=v&3BG5JrEb$Gn~9kZjgFHD@iB`hGN@;ZW5M?4&$9b~H%_++^Han7gsvOpdxz#hu{ zECD>X+yhkB;d*D8hG&09Bo9KS?-2Tl!vMuKv5zg$tbAjQoz+PgefyI7~t4;58x{Bs$`v1LoE++nR~kUs?#y!5TvPOSn`ROj(74%-eP2Hx26ZKDlaKmn{Jn zXc2OAa?7nvf3`&E)r-LeLjOOoML04r=R~XdzsXVG{TvieW+YYq+PXRMGK1vhqS#_c zj**9Cx_TL0pkXCY(&T|Nhh-RS)j*;Ljv*Bh>0XS8B}t>Zr4|R+ViLn)io223dOpQp zkq8azU+Rk{7~#C-9~A`jN6eMP{k^_c`G0vvk52Ls*&qiH=0gv-4P7JFO!RQH22_dt zp6Lgs@zaGICQ%epfxZ7Xc+}|8O)2V6akc0>=Vmt_!3$8^_-?|_wxqJ}>uNVmvi6CY zreYP1R7dc70AT`~NP4M97_tk0oCVOIReZ4V)d%>l`uZk+uAn1ec2UFtReeTV!yV2W zVI%#wO6bS7)$Qzdne`h`Revm06+NA3B&aI{j6q$>Mwoq*w}XzKSy|0r)wM^D`JgMK zkDDBkzb!u>!UBJ>)wc{zlBjxp-D&UMq{hhQG3KiA^FG4lFNW9XJ_(Y)B}@n45v`#=-=>xrv5glJ~9rnv)Aed9X)Rg)rkW2M*pr)QT^WB&3HY|L0HaE zo5{AKKQ{IZT_3~x(DlZ&Pi|H5TH|I%Pno_kal^0G303c!i26!&<^KMMP9-W3ZPwz~ z@Sk9jGq)Ep>a)eHe4n5`PF0SWQA=ONG)BH;Rf~vB#7DwJEBC>&E)?8YyluZd+{-KR zd+O&3Vf0=t6-?*Yv4&f8(G$d&9Nd8vb6g415 zd@gbpm7mPIu&(uM``SPw#b%RQ=in;=;^dlGJAl(ENDAP&?0EP?vs)bPvx}@iRFX~t zaG5GAW#SRuwwj)KfDXSHRhC!@@vxQ;4i@$|K3a!XAi)FBx4rVfa}JyO&f7aO@H$u)<};8cgmlCcON5_UUJz zT3)_*rf%Y^+=z1iE7`FC8v--4mW~|+*c^nz@|jdbi#%*4uwC{D z1JzAl+w}abEEsvg8$z20PQsS>1pxRG<9X4=04*|sEbD!WD+hQ?$h@BBwcp82?JHe} zW^;)<48M(lMMsP+&bU~YP)4BhAlnM$L1q$B@c`F~F-MwBy3=z&U7P>@?(<<(;#hc6 z-=iwU1aWOafHhO(1s^`hcs1H0Sp`KyMG$Cz`BjhXjp2$7>dXq7*m9`^fWb}}mH=U( zxGsZ;ilrXqCRQ^hWw#m#4-YH>dI?6-z-pV-{Xvb%gSV~4$>>MSnqN9wjQsXDIY)>E z(uV7BqdWn%0>+z96+x4R+~CaFv+vL)(QN`4`KHZh+|Wj^>zdCqmHbtzqx-EK9Lz^A z2W1i#h~z}r$wMZ75{MwNrqD=yZXrdu@zxmCfCW*jX1l4oTnP9&q%}9tC0N-~?;bwB z_viPa;RP_0z7{nK?H(FaN14>Ykreq4bB#Ni*>sp)(;K@>PHUQMar6y+7!`KD-7FOJ&Q9F6J4=xu)v9HN8rn z4{B;@c!;oNLvUseoQ@JA$pQflj2W|9? zuOMs;I_0|!e2MoFtdx8b&Of&d!Na}6uhB#8T6**F@3w8}yG3Js)H7!G$4#2lxuPfR zp)N8TtSNA8zVd7<9p%sVm|8y}&jHG#_IhdY@PpkyzM&vKsQLEo3x71WaK#`5 z?II-yreY>F4ogH)oiG?e!zQe}va(t)-Bpp^DY?%U5_x1r6SSf@#6V;S26}{615`!k z;Mq$A)EnWb`6-UHu5uUIt>T=IDmsB_;?pqGP+Yb@ew;%J#Brd-$LYlt>p-T(>ymU! z9~;ZxPQM1OtqH{l9fB0_#kqIt|4t7J{n@gDe2aES4n4;Y=U9QkAaeJCwQ}r;=O-;l zsopxg*T?o-?<6eT##;dfAbptV;WeTcF>M3eSNnbojtQ$I`PK7m@Gvd9K z|4>b7mH2b-< zJaX&F&?aN9^qD*;0Ek|5Y{&9MW7q85^E_F>;@pVL#a9U+7$glW=yo-ff5h8Y)&wyh z#sP6-vKG`6z34pLJ^%9k0}C2o%_=PSaAV&(*By3hIRFx}Bg`7-V`jqdctWR#ji8dx z+?QHYq%NzyV-rwz)n)bBC1vhM?IOiigy$`r*p@5_JXjUN^Z$#{@SYm{RNoNTQUh%v zWhHH7J{wfJQ^TUn!HFT6d@QbzXpH04b$i>=UF+@!;OOdfSC7P7&8*#(q6Y+~%@<=zP9b~$%m@}VA)H?NGctbinBZsu z6-oo}=uR5y8tYKsxUm{J1|;Vjy>U6Ng)0d3L}0CcBW-3gKSn;we z4NVK2e5$`!jiZA6VkE$AcNLyWpQ6}V3W;5(yQ*%>_B(Y8=vzk9@SS913ziPBGO{fU zD+w}*;KbZ6r>n0&OW|TIQlVFge|8px(?jyEMGcR#lf@0Bn#DV~Gg~xNLv3R9Tw3oY zEAYdD>!JN4CJC@F&sDg)=g%)Of9^c3?nQQFX(IJ22%!|2Kl5|NfFRdc;~;;+Fertrzkm-XC1JA6yH;=*w&-_HMZT8e zT!WLRoc7a3$yk!KNFYZvPafo|weNOzk(JxMf;$DKRYlkRG@`e&Tea%ymEshU!dSEz zdIy0FCn4wl{wMZ&CU%jn;%4rl3gnasJyj^v&bU|LRLO@-P51i!tz%41nQ1!EDTvKI z{$M2)mXtd=wIyqq?EP)^sJ-Qw04lPZmj4^3k)-5*1vE~alf;@_X2~%#A=q*T_XP$r zYl-z@y-KkL2adrVg@%g|MBOQ<9fqN<70IR(W0qE}sNTqvrHrHKTv`2f0o5Cc=pCP) z91N`<;Z|@xI5j^h&rN1P#S%xBup&fwi6_6n8FZ%+Uw<^Tur#1nj*Zz-bh+ijx|44} zbE)w~yhPCs}@1`Mk%Xa$vk0-YFcJ0z>JvXb_MDpmC2z>a)Vm=_V3y$pia zq=49_Y#BC!W{)4CVv()sG(Z5hyi>4UG)VK&{aU=8%n11(4{Dik;OAKzkx zbIv`GGq&%jXlsw9NTGV7zgdxctDb0`(J$}_=?6L0025C(F2O{D|H-QmOdRfXP4OGK zD;c9W+P7j39ZH%b7z~M|`OVqs%>AKnKY?&vtg^GSd-~n2s=G`Pfk-3t#b@FDHa&KW zvAwSp-RYa(PX)jzq&{ZQWYkImQ6RvbFmvW^*ycYo@Gu~hQ#=K_XAw?-A+g)K^!}hm z+^(=7p2eoo`i>+u9FG+E!U{mxT~Pb;J|Dk+Z43WGfh`B9V;CR|EFFGMVs$pZ&&bTl z2`!IapX=7^tI_cv9wykFlresrxBB{8kZbXc`!n_^yX;`N`1s?%HW_rq$@_nO1Y~mw ziuK;D3c`lV56?fDWqKI$G(Pk|7+R1pW~y>S>V?4*qqFC)MXCe*IGBk`0;*?RDi)s2GvN6`C4W%XGGKXyV(HY`H1u6FX6$-U$QHZ3w{G34_aPFb z@K0|`e}+eL<))Thh!MkMr|Et3HF(pSdY3~1V5HE%Jb-T!7bnw#dG3R$GZ-XwG;=r^ zF!*R6kGjjCUVOmDTj$;K?VnS(-Dt@^PO{;lQCm9l`|_^suuqw_ALAx#90_LXJY% zyBaxq0@sW)G%(mfxk=qjbwEvygdfCX0X#jZvh)6XQ*N0Y<)36vBki;RIu|c4H`}}# z5>}KNL?Rl5olj^0RK>duc&L+@jUAlvy!fn2`1n89&@R)8`8@kqtjLE4L_0H4XMC&dr)&$TgQEv2l;8)2Yc`2S@v4cs8f zc;xMjW*0Et(6M{>CO2oBF81}W{TvJxO7|`F2j>@A0=bq7i~GSK1qDkVuzyM=)Sj|u z0JjMgPPfFt%gG7vANU7rb93*B`wb-x`s}ptcOPvm#RwHSY6vMFXgfr#NV|9V(R){u zWj~g+_dG_hp+**kA2Z{^o(r@iyCXi9mLl6>VoK?Mr)MzBN0Os96hjFDmDH}X^c7(e zfIT}d`^}rtQvTvK{;_>_uSihIYhs0+0LM!fuwv@#k)7`U)J)N)|A^m~PiaTlP?}e< zPO53Fx<^E20{b9ff;=x8R94HtYC)uF?Vq6yKC$%2B7La&Xl*K9^(QgcGz>W-{k`f= z^Uve%CypIUcMRHo0;YtXMr+;2j=*obCP#Hxq2ZTRu}tYKM-N`_k?ej!2>jpfIp)wD z&8%H=mRI|Gb8QJcKrK(MOoxXe*b4y$q{Jr!b{N0COd~kUrg;G0F}X}Nb5YoCuY7Zd z)DINaG~+;4Qas4&avY)N_&W8Q^#+mw2u{|3iu}1lhexy-Qd)7-pRDZVQQEI+(bS0s zt$_-ORj8&IdItATm7zNHH@p|w&i)sftEcqc?Q3FU#`iZ8g=5BY8y^<0mL(9I8#Ii? zpHiwj5+zm0n_`P!R=mn&vD^aP0P|x`PQXZa_wSp`3cjrh?F=;lI?iJSiYNSIO=wiH zYC5Pj;=NcG#rxKZdQ$iK5bXE5j<`K#YAw?PDvY#(*53oSu&S_plV9$}SOZEuN^Ht@ zp)!~!I-iyK<@1086?Ji(PxP)~W*bhb9<{V*@aYF3Yljm4RKSJfF97j!<3$xF*T8B~T_F zxEg?lHi12*caznO*m=oFy0A?QAXCy{Rqrym$D6Z5Iw+~F3nV9V)OAC>BNqGc^DSJ>kbvBldeVROU zeU|&-HUJ`{BBVw#nrN1QTx3iMj1NC2@r52A`oq4mi?`H+FTofMU_8w@aQrfHKVnfF zm4_@AVT>`E9f>bqG~E4f%citC=r{!f1PS9|^rn3|Wf$DI*Brg9yn&`UI|3NS9y&Cb z@|{Xo9Caz*MHNB(eGGtzt&}lkmbcmlD@*?0bfd>a_N|GYp zixM432Yv0njn9o&t?uGqXvcVM1V%$Fy=-%2^j-0t0@;FQN+=$DGCJS1_7r2>4Cu02 zj?an|jOGSg{nnHRY4ekp4)7Zo-&8@NMc|5I@}u59Ue$0;lK>7Q;Wg?J!~k0`AW1>pm;HDz90$%n*q&cpEq zN}1OYJq*|o300zo*j3Zw0LjWFwzKQ>!!bPq925XN_xL)iYR} zj5*q`+nz~M?5x#o-6)3=IfWznl5QNsJW(9g(2NO!^P<1AW>H5+x9!X1uUM)wYT=e4 z$)*Eq>VA|I8=t+jE5FJ$-F@L;i?Q}gLpwKaY7=m+<5kUd9ben(+SV^uF?sW9NU2}l zrkyeRcb(?>&h{|#X{^5U^-JGlwgoypRkm`^Ze9DK5PW2H^Xy-1jo!Myows?-^w$sF zt2UlD4qSCH@b}d8>FL*&&8VyS=1()*H@VO zQrYtG$_d*e{Ce|me!>7f%gE@Mbq%>F#!iQhrCeOLez^M%mqi_)H&uuox+lHYhf}NV zNkHy&eH(aPyw&j=Hr#}9J)M*^{J^0@-I;0K(?mf>PsO*FQI7}Xbpz}dEs7k**0EVP zJjPKmuX}%F(G|Se!Qz}4?fqKq={`e2q5Wq2EorGd_Mi zoly6CJ(De6aq&qXT6t{smzSYbkL?|=xMauA_}y%?vcld*@)<5p9Zf)zyOq31Ey8jWu?C`;-BVli+pEJjzeS#a!I!*H@pb zzj@Q{-f8wD2G&=Y-`Kq6!Y?^XzVnTk*m}mnewvvXMrpAuCt((2!}cyN=b0BhVAH8} zQ>DU+b%BY)b@(w=er#EvrwA&>0CWDtFn5x@{RvP3CoJVKoe$`3Mb9~D+_;RM?fTbO z*y~)?eQWz8KSQTvU2Ef?3JH7)J>+61CmXoUbp-b|J4Ya!*DflK$$1V(vfrlDlUaim z6xLW8nBPC86_k>{yjjzxA5lR*CSygQV|IeNS-f=V#E~O+B1H|Sv@GKJzQBO4k)^_> z-Kpj67pBm(&6{Vg?NxITA>>sw!NqTH&hQ`l(9CU=s{f&*gtM2@Wit{Ni_9_Z|OWl?|XIvW%@xeox;Ou;6#R`vSyfweX ziHE^@P5~8u#@lToN1sCW+2hA2NXDI28wDd{53m_Q74U?~_#O<#9^$Q)AMR>ytG_*} z=YuD?xhD`0gfVMU?K$jbS#p!t*Omn_ERdXc0d)I2jEyr|Gj&bPW7m>fD46N%w(i*Z z;DR)@Fb|XSFhCy}MeTF9(}x{o7=4~`^X<>FvbsC&1|>X;&Bf&MdJ0Xv7yQ4Lt_2$E zG>U&DjZ(*0X-1AV27|UtvT34|$!kiCXNGERL$ReI8A_s=Z6k(iJzFGEUWt%LHd=W^ zTc*-O(_`sHjVOzv_BYO+<2c9RZ@&Nc-TS-uyZ3kR9p7W^Za%6y2oNpK&tC@v>Ah~9 z2)=@srzeA(;E+G?<_!-SR1|F#L8ChRhRE+Me>PuT72?q!R@eV=BEwjjETM??&jpNFPK*sMr$>VS3~0$)6fgnNOkb65_UP(v> zl|z+3f(|Dwliby;#rjy) z&xMrBQkkp_K7N$7o?@F(OZRrE8i_r~flAN?h;bGOpGG;NiqRb?ggEEPvG2N-aN2fa z{jQ8aer1SX9?uDHQ@LaV&}w;Wdu6F#&%anSQ?alXXfSjQ>?CAO+_~J2#KR(qCQe2p zA*Hgzg=eMfZLPP-);12C27J1@bwPvACCM4&*>M07H@XsJ))5I zd0o(-RvI*upH=|9g-|hd=b6#e|7O!z9x9}r0U7QJ)dhLaOoI{xM;}H5M}2wT%IW=j z@ot7m=H&Pw$~o>mdsdDtX5%O626dkqq!M<=q$0#mBoeWsM$W~2dA|}TJ&5K4&O$yR zU33oBeH%XwkPqAD<>Lc+GF>GxNI%X$MAk<57g9`4XJp(NAFqW0aB-0+$2HkZ=~+O| zojKx#as2jfB0MT50zAZd>ZY>=AJr`7(6?Sxry^ zTnidj5b{bZk8;IwBHKcch^eXj(Xu>ywa^9s3vcc$jKacJaj0LHrFRzKf~&xONgtD@ zUf96Eqq?G=+WeyO)|M9W+3pL8ESPS51(&Lm*t)hgmhzjle6W@VFb%IhdZLnHIO7EC zXy-yD!WZ$x!A5^c0{QSU*h2XqijIMeU*rwAwys|z;A<}oBC(~-&Q!Un_VM6=06SdM zb!fCZ7!TKPC1J?v5ZeVDYGI~M1VjQbH9AmIZmJm{gsWu-uO$TFuCcakSA&h9 z3;M^tyo_kLyG0wvFL9yDi_6M3f~n#Q0&Py@KWYk_ypx)+*AX)S*)fx~3O5M`KIG|D zK8m2opo@cW2fjd;KI%^i3 zl`5nz6#Fs==t%;oM@7{ovP_vw^cHgxav>P2x8oNfy#qr-m=TzcAO$>uK)$vNxZ!P} zWh*`W;De!|C^@7T5P%C&YBXs&6xV0bPBRcB!V!ciM1zrX*eP)Lyu7^ubxfI|GBFnKVQzi~v{uFOH~aH2H7tEa<0%!gw&sPyPAB)U?M(P$$<`ys&}Zc$EE$lC`wc8} z5@3d9Z3JdGI{pR*L7C#-epaEmF=BmZi_zX4OBdX-BTZ9r*CL(Rm?Wr(lQv=)&}VcQ zvShPwR?x}wKW&*DDVV-R$o5p%)R^kdGluLs;K1wPe7suFyCbE~3V4RVCj`lhq3MRr ziC**52%YT!*=uv|KD2vaGKB%PLrW|8xE+I|M$2{wH0h9(_Y`}ltg9_<_4_*$*Q$`( fMH&Dz!kC&BbjRQa+3eiA_=#}sUFu5e literal 0 HcmV?d00001 diff --git a/control/mpc_lateral_controller/image/vehicle_kinematics.png b/control/mpc_lateral_controller/image/vehicle_kinematics.png new file mode 100644 index 0000000000000000000000000000000000000000..2d0eb5f26af71194dc9018688afad2d665e47bfe GIT binary patch literal 36676 zcmeFZcRbc@A3v<@vWo0oMkSRM5t*f=G$hH+N+L6a?6NAAM3j+)kd=&#q$N>ynIW4H znZNhZb^o5{|L5`H&+l=3j?d@4j`R94-2;s4xYtooP%vsA)HqH-v8IQD zVy!404SvV+s8k~UkIGq1`y?HHdDB^j;@`PkG!0x%T(WX;H@keE!rI}|#q&FyEiRuw z@8E26$z_7_wK9H4+V-S@i~8mBW-hju90X6=UObQAq@a-6B_-WooWDzMAU<@r!az)_ z;BJ|LSbtgh{?K3zC} zjoazh=WM0nv9C4lz3g^_Z}D>Zr#j2a#j%UkvBkx=btK)j2jUM))jHf-3i&*`$4 z3B`sD4aaL(M1_T4umV*-?R!JRwLI&3b4yDqMYmBut>kM)lGgLe z6d}piLv3^oB-nrU_diZerM`9R*1Pxbujz-}p|2?0UVJJ@uP-gq_@Pl6d=cTwfERJlbrN`5}P3f_rVCdrNW)~rU@ZLT4{K7)D z$HxSB?1=EXmUf-2hO~LE!7&4a(DfXW6<@!yrl+T82BuxFZoms}PEJl1mypm!?k(YjSb33gr;A6{2iS2||q+E=b4oE6d)hwy+;bnHIumXQgxsEw?w ztXzZFqg%JmoUWdNJlqpa7OI(;!-JkLl>ae=Kc#`&%rMp+VK0d8T^Yz}b&H(#%0hO3Ihm;yTq4qTK2H>m4(k_I2puu?5N51nce<# za8Nz{WEIwSz-QF;QYJpdySLrlf}bBMhBP$jy(})StgB=67i?X8VKv%W<|QsJUU7H( z=|^9OZYCr|R#a5b*XJFe+(_DI+tc*)%Gr^Y$4z!ZbZa&A0@WVp%$=_@FfkEWSX{(M zixgOy48e@?JO1~`Qbd;D*zWJ2an2ruq$IDc^R~LK!hWAHX^w;L* zU{XO(pVD1=y)5VN@1Ln&Ki#lYCMBFCqQRz_pP%2|TNC!~;Gdn9JEzK$w`)NUzhlz=aj*01QB%k8V+qc*5*;AzOszfZg zxR~q8l`GAf=Nsv+<=1oFH&|{j;kH7lw#CZqIb&iHH9Tx5&bBV|TnT$_Zmy%ND>cRZ z-@j&kZzCKX9V$ zKHl5G>a*)sxxnH`EA=JP9P$H$f`iT5UYvjFGPIG2Unw-}Y!M2YZ+v{bdGKBT<V7MI}oqMrE?w89e?E^IY1uwqyvk7=l zR?_|{rxQ*})!`OxvXLQ2*U4%3t5>fun>Mhq1$K9v#+|Ci22cL}{etOKz353TEv?C> z(}nT*7K+@8yu&|#Qt7cZw6ug^ zr5_a(Y&w*>(Z|OJ+nMmS{*H1{em#(Q^`o|KWBSH>?=3^a!`~EM?6?^d6S6or(f8%CIz9;&~*G}1vI&Pl06ty_8h6#{Im^z!y z+h5QWeGGfQ{q*xdJQHSa`O3nDrJ&< z>{@6pa_s-G>03)n-@99z4{B?x>gzX$&rL+``&OQoMoY`Y>z9$ST}oPd0|&>Rm?GIh zPcJV$jJU3qKSjSrN8fl&l!aip(^r;!As}Pa2A~7OqS|ETH;2(n$2DloD5uu)tBNPL zZIa<;EMFMj{_Weh?rLTg4M)da<`x!@kEcIbKGT>}1o-jg3k$aYW^{DG{QUft;++Ps zIzNBbjaPJM3Sr_?yL4%1r}y*`rP*&MCTC{y=R`hpV-UX6MO7TbXx(|Bd`&;2 z#<_l9NQoxAYTaS~+}wxkf-hfQpJrXK@-Gw9P)a#{61dp7e_s zFIf2bBfot4a^;j}q{_>r^mKan@$c8#?BAkOPcC*3y9@zFQ_39*XVFd-Qp`jspuRjtrw6wQSDy^-pwjntuPd%u!qVS7~ zVm9`k(j2kqJ8JuYIpx_^>^eq#XjoV^Dy#d~FZt}}&T?b|N3~f#jMLD2)0A;~D#K)X z-SYCX$DcOqT-&ct=$-R~goHw`U%!Ud0Z8XK{$0WT$E!^jnloedSZrmuDRK&(crTPw zb3V=rL z+lE=}I3{T5_)k?p1o>;7N7S4cF``f;ba;JIq(;RX`T6--s48FipIP|*=Z}`Xy@cPTcA0hlyZZOve&!JUJ`fFe zX<~I{CllY^nbvWqCn<|tw{ESR@R?Iw8rbt1ur#NRnI&ukw)+?Tt+h@b9-OC6oqC#) z5fB~CDkUY=JvfMoasF$mx@QD_$J^Umlus?;gEiKglY!>vz(DUnUEHssq1Z_!ui*G^ zN~pVZl6T6knQtGQDIMifFG>`2QV8l*GJ_W?qFM2qptq)*|T*?;%aGMg|=+5tXeb6 zEd21u68fr;k`gaK9RP@5TACQ!I_mE~e)Nou?X%5`JPh5qeyYiVd`GQ)DW91m*y^2P7NheN++Jwua1>*Dro zmXf;sAu;Wkc=yoI^~_8OyvU=>OorFXW0!zv{4VJVHrmJ>OFxOfP;`E;U#p;RfRdD! zmfpN|>sm#{{*m|s7XjzlIWi$$FL%7w1l+@hKhn<_>@V-Hy+$@M@Zj{z&dyGBiNNIK z&7?n^JsT!_p;^ac<<~Fq=f-6>uw%dx8#45>)U2#TmzI{E@akemFjYMIBKELs+7$Ho zv5--@H&@H^b9oNSx1JvXgab?~FE7V_`5j7CGcw{me*E}=1RJ$##ecL3pY-Qt1+3Yv z=;*v(li}?QC2whQd z_wEf7<%^QSn>VL<+b6OuNJ>f?6glwW_y;rckl~J{1U@*EDCU|rjOl`oNKw(z!PC&t zFgZ098_j+Thz_tpTv9SPF;SqZs;YZ@ynIXb>7@DY0A}T!IZc)gJ4Dc(FxaGG8UFtL z`!q8%U;nX#gTpTF2>hwHzRNAXJs<9FCrqSdqCD)h@s;x*Y8iE#Zyd*SVB`b6Gcqzh z&CX8TMZ4pMFdi6ng3;OOvv_gKmMuE1k}{pi&$Zb!UEJJ+_w3==%#?hs7|Wb*jR%+ zD@Mo!-2(%E-W_{kAORX{bMa!$j}rHu@$tx%lr3>uLVy_rD)C$@H7iS)T8su2ntC|l zO*>IG$8Th0#9u5jtk=>6a9FRh%DebyECBWG^E*0G5N-PT^XJTb!^WdRpL%S78Blq2 z3=H|tX~u_#UpSn*t*^8A_pgwQ%(qtt@4@fp=u|>yS|6*2kdZSpGeyEyJvOJg@pGeXd9*b(H3xNc-b~l4LlLwYyi6^ggsTQ-aGHO^Ud9FJsvl`i39XK(*zr|;jxSkB%y>qJc#m;Bk? zrsg+80s|ktdc|E(Q1Cz6u`;c)%9@an@H8u{=Aojy=`PNk%l(J2k}8fHnp!7nY0FDW z6ag!8?b}{xuQI*uS~VZPDa+rXzi~rbWglHE@Wl65uIn+`E5YVA@bf#IC_Z$mXvP}U zO;1nH;I+>_phygqkhv~j0Xzul2a}|h(5(}F7Pb=jJ1}51VjUsC9m5{DX8-x~BF{21 zvc#0Nqv|x{KGp=^#UK z(y6Gap~fS9u`3`Puo!;Y130hgy>A9Yqx;uwbT|G+j z@>rn`&HqY{+69_7)=^{uG)T98eO`H2t4Zyu&(=NyMyPe$jZ=<^XI9royMFS#Pb=Gp z^?7q+({>gfp0KZJNBhY1{`D(Izna2vdBK&ianb>@eU~qQzNuz3N&ekeaiS8(H;H=u zWhgdMwx!UO4HsMNtgmNoYHC8625Kd%VoJ)s#GDt5`8X9IutWit z+Pu;dPw68=#-`yT9Z}ZtA0;7uwNc5XrM#OrZ*H#rdAZp2rN^`BoM>6vTihXNcF=wp zDIoaJ1yq{u^8vNx-dR2`jH%Gw+bcCzf9?a3HS+f}GB614+{wIS#}42-^@9gPXLF00 zRERLrS<1y7L8Fzt;o`-MAQ0CKvw{KTO3TW!>dy_I%Xtz&h8ZVLBpN6K6BE%m@WUt} zU0q%9$)LQv>F`c>fC(rc1aYviu~9wL4G9emRVW#s65GCJ|Di+czS+q-bd^VR6kVz& zWjH(C+H$#W;v1RS{FJuh?s!p6O-=D_+ti$$r2!)`G9q8TlppC?2iki~oUOU|`_$c^ zmZ#2JTZ=tER}zd_MRvd-*SsdatGc!GGn%c9jZMWhN?OOId8g9Js`X!nTpZoqeY-ED zzNXc$VSY>c8v_Fk<)K4|a(q{P$Xvy0?26Ud6;}s{c`Ty|KYjpQ$!(%D;z*nv2If~Q z#VA3#=B}u&fz5Plp-2)|fPRbPqEzU`#m85ZdN^kCy;vdyp(_v8Vt$=Z`3z}MCp&Urh*`pkXpdPrn$pJ;OaBLNf&cDb!rndRP>BJgNeC!cB4uLzQjsfPll*0tr`6Muo~wp+&bY34Iq8A8yY0Ed+elQbKED4-(m* zcC)A`4KN6n@TK=(DN>JE*ZGBoUXzu66UV{Nd@ooeoakkz_W|`>h(2lf_ZJ>Po9X5{wU$Fu1^R8l>}Y|Qsu0OB2>lZ9+Yxa zeI)ealjBl_m;TVW-o z?*m(sX{o2?DJN<_^LcrA2#SbM8yg!#kHH`XuC4$GX!!m;{J{fmrMd6Ur1iXh{d%@L zaOjoSoTP4o(sgO;&saq&f=12F&AWg8d_&>$$-|El8$||%t7{LY_4>Vg_vV<_ti!f+ zF!boUE|1Jl`=y!y(|+mb2m|%A?D<*`}~n6bkQ*D-kK#UM^_oV9jJp*@n!0+)s?xP;o%$D5%aP30s=2) z{z@xY6qS_lp!)&7;>-o=h*kp^5`p4yjHD{md4LfG1%=jJ3)&ZXc^8#0yr}0<^!3IN zVL(%cz}e1h$bI8UZ5UCsrMEC{5P%R5iv8~MXDw))ynK9+Ek&=^>G;C>kb1PBQv<@wX~VBMaSxdElg=xUp{NeZKkK1T_!Fsp~HkU5dVwN5| zeVUe;nK=_Jzj4QjnV)2IJTbGkh=kPIGu)Kn=;Tz1Pe+ZPiR)Xnn&tuvIh}2+i4{yr zN)o-OtsocLWG4$LGS~a>Wr$Y%X-5;ae0-EwX3|t1mzVRup0C+CIoo1lvAVL1A}w(n zdk{?{AfreK6c^gKI(` zVr<+LP;uXWLl|QN{Wp|P(X9g0AE5Yvu`&NY+^?b{fRW!?aDhYaV*8AN?zyBPBI_K9 z7Ni4_DEQ-~d-SMpx81MUQuouRIX=fLP`bOjQ&Cf&iP{QlrTK z^oq3i)vExT7uV`b#&n!4wHSQ#&q`@draSeIszU^(puiczk#u&;lJO#&c;Kd5c5d#5 z+9-ho02!c*c<)EY(@mvf2kcXw3=O%_)gCq3_4M|x4XZIfF{Z0Ob;uX0rXvP1nFo6x zGGTo0;$(n`1A8zeI{Iti@>@$2^4eRU!4$c*`2b#z6bRZ5Mb4QlrL(B~S33P=7@O|swy?*d8M*|+HvyoYQRZ_oOsP^Zd_>dW=hCqq+z2B#=Ble!52Zfs3tdj z8eXt*AY+uj(o%|0Yoeh;i6KUZyZan{+LpR4ewuo3P|fWf9X$gBH1HQ_%j;Wjc#M`h z;?;;qg&CKUlG6QmFtI2>(a6}C37VO8;?+NoCyxB7Y(np17NkB0^WyjPG@8`4ckd2v z5fQ0`a;3YiTU?0m{{PJaz#M?5Vv1u1cM1cmP@&OrX)3rcr0l#@?5r!;w#QIvO!w$0 zgB}}sp;xc=Lcub}+{e_oU(-dikBRtKqPq)+l{{y}f#cFlxz zLbU>bzH_L5bBV!`=8?Za9L7W=0TM-VVAGH3>jyVB9=9nlBohMeB*t@Q;2@Vi2uAeH zKR*i#3wz=F0Q2v6agot~ZbVPPoT-yW95z6B(h%_5Fn2`9HtcJj*EDRL3dZqoZu&;y z*!PwZv{V?KuU>gE)Af{0)XhW_B7jG1rfwphOT2=`#iOzJ@6*8pQ8*jk)Y%z{j{*a%Dxi13h(tr7SZU9%2GJ=fX!5M%;^G1aD{yc%7g2hhvk@ne zY#uic&lL8@0&tpWJ@^4G$%L`O6hIgPCEs!9*Y*GrsS9CAoV77sT+`5OWTAQLHm1Lnxvf(tEjai~!QfP#va_NaVOtGyhWJ|)2_ zMv}F}%)EEcYMS?f3E?R+!Gd&fiJkwK(v5L2~WH=-rW7587uBKX8J#4s$hd#Dk@vWc+DZ<4;2pOdqq*m-dB zqshv~J(oJ)R8&wv;v<_V8(s!uz<+7U8!QIxdMy^MZ*qEV6==8NZ~PEsR>!};&l{Dv zF@hVASripjAD_96ooY>1Ru+_vYjJTr|Dhh*?(8+tNh1)Hcm%5!gcj%&ERb301IxUX zM@dG;dg!ijv<0sQl4{6`oloQ9^R(AKT=s2k_sE0{S? zpFLB99zzxaa{&DX<|LgzX`k`RSGiMDQ_;w;>81rFCU&WF1ZYPntXsdn;oG;sU%zZX z=>Y0>dvmZrPT6qU~tz!7)K6W7}}?#q%;W&Yz@UXyV*cd-oCCA z3&wWhYblV}*$>`M>9Gc}lZM+O(eIC)JQ+xy(?-U8dY}$aHr_??i%$5}henb`3Fekk zvA`VosQ6!4^UNi>-EpVB4YP*UR2K^n3jOVV7w9!LEiudPQh02HY>szU~I(esF zs`2l|0goODg6A6+QPsJKd)l{TfKkTFJFSIo3XPbPo4X1we?x0)D4{oqwtV$Gstqq3 zc`Jy%0R+E({Tl6Dxt^1JUba}BU<~p*F)@8rLF-vG|G5}&LHuHACtK$(0kebNk=zi} zJ52jWSUDV})?#O|1kd5EaVKK=H5V$OvJ+ZWR#s+E;BHn%Y;16Y@PT@`CtW-D$6FFX6$?<|bgX8t6!G*ajlWWnpQFKHP9e5Z+0c-IXsRp(Gq;qnIZY`lA2=%5ypLt}408Xj=m^AbKa2q=amQK-Sn)Q%mySuiWI!<*E|x0IY` znVCfM->ay2>pmiBp>#Gf56%>%+!?rV!OqUHr-~Y^VY9MwXS=TWYGf0=q*MeHG57 zjEu`$RfRu>c6N5hPMx9#f(Nvucdq=}rl$FCg0su!wK0?#N1;r5k*Xq$9!|!|ZB3b! zoby)iT49@Ef>l3|y^wiC{%yhL8MNx&dV-Efq>j(iJ zmN;9;0O|;ZI{&%hz2Gow?zezq#H_3_&mCVwQ%=7cn`wG)ujeTcJ1CFf^W?oxp8S?l zpBfZ_%#KZiRICp1bZP~B82}G(Oi3|{MxWuhpuIlti5d$P=KQ0_kJmt*o_fDkgJ_Zm*(ZkfwPq_W&R6`i)W9(TV?nGLZm6av>HfZte~JE^5;pm3o;TczcZ)VRK(xCrF{X!J2qBR6UIT1 z9?GC}VK@Vso#5l?`B#3}7b&#tR!72!Re@=E&W>XG|k}?Sa*yM7)JB0lP?wD7s``ec5Hqf!*5bm5uCB2$a5;xHDs>qo?N6*M(3&5x;@KgV={3ia^%U!rWYRS!YN#ugndy zroaDMnLT?f@LH{}T(^I-E2vA_E&0|kUcOvaBg|h%G@!vo!`61|bCcIhIC}6hJ-oa+ zTpm2Im(8Ck!+(Q`_29t+q$q5h*M7CkNFNmDth16vqtHV$)MFzl%7?nvdCS+?NeXZW zH%-8r7|C<~=nf}OvH?Wn!Jp)xe+SBi$qa&BlWS268Or)$_9o2@R(WlroP)xCK=n|C z9HE&Kvl$vEPAz`PwbX6%nCf+u!Rx}gAxTs!)%~WXf)FmUg#P4QX~LLc+q9_%kst_l zYL_od-P>uxL?Jh}<61EvJ3rkwu1)`=J0dL7pC?bg_t8(hC%rSBr`mmfpBun@t%@Eu*M`! zb$F*Zj;{_Le&%0$Y2p^U&3{f0Y&iTU3!o?XMM$`OO*tDo@cUQAty>#NP79R|npz`W zuOw~j^`R~Pt;T=ISOzf(up|hUh&yBaQ&Gq{XY_Fx{#{rwuX`Zt0Tuk#ojaC4jxNa` zRR4rrFQ_0uAPC<9MCV9~W@Kd1KsjdMza4>8h~i z1pe&vVh12^etx3S?GdKT$cA88uyzHWYCIomRXtb1&X7bUA$EwL2kh*g)d?{gZ76eb z)Wsf9+m7!M7pDWUs{LO?75*sF&v=&ar9JW1Kd$ zYEsaEv!bK>tf)(6z)NlPZhh=Wv|QD~Vl#^NW@2I#%xx&?Nf{X*UxgU7Pb<1T_jn-~ zf7oB}5khh3Yk>frBtU9a_dq*pza_;^Ml!$HQm9XZ(IH_70)+612ACsMIKAV?*8+1O zdJea;EpQ^aaChC^o*px!*FJn0=os7wbak2Fkq^vB{|6qE1TP*&+UN}+LoghrM+hk< zi5ni>2SEAq%Csm^5^PI<*KaYxEmBZ?Y-|M;#2_7T!BR&_HdWoe4rRA@&uC zrXg^HXf6S^@87>SD75E-XOr0xY$YwRBj#hn*RKKaML=@Raqi({!UrmfjZO%ounOI& zbc0!WzMIm^gla>B%k?BW$Z#Y#k?RqV6Ad0^R>qYDlIH{Rui$?3$py$Is%EC15fB~Y(4 z%|If~kPOHb4qzdmW{_2q*n$fy8b7UgMs{04TB*ZuD3<9~TwK`I#Xmg&xSV`^K>)OP4eatYP|o8jlcXT8tzEk7S>l1(S~>&q{J zt#P-cqNh54M8XS>ozbZ-7CBa)O9|0NOGUZ2>#_kj;UK z0?|8fcFNC>0!jfz&OS%gy){GdO<;~5u(1*U5Aw?m4K_B4u(pZBhYoB_ew$ylgksn#JeDY^cUwp^1m$mr8=?CC%38fgTCh|2{vB&y#*>~01h$4 zpG@O2Za82-^+ASNg;(kTWT6ZY9n4f%F4PNqkE55zzU*<+-lf3)yACz%v*pmqn!8;9)hCO3C>}Xl;hBJUNt> z*bB?^rTYXwiiNoUh=Z08Y6kL;sK{6Pm+zSzJibkbgoHFeOCnM1pYNmP-N$*MbwOl? z;S80;OfHT@aS(xlGG>YnAm{vz!IYa&#p$=LzL5k#%nqO8RrcnDn5Yf`6DT1U-Ic_7 z(MJo%3;4QzJ3V{6_aO_v?^O^jMCLSbQ2~Pl$2_v6`wn& zfgLS58o%V_KYa%663$I6SX9G*7YrcJSYRnV`tP0wXjorhSVw`YI9O7*&9a4r*H87; zl1n4#CTgP5X3Gn+#DOIb3_%%JwmuDut0w&=%*suOuj<%|69J^ganSZ-Vv?)}hzJQa z;5hC_N?bjLeFsqDKBxcZnh!}7hfH!+n6D#%Xq#|t||Gg8127u zxjSf%uR9Kw1z~P!-2HVm5w~}7B4SMkr_Oh&*7prgV%FK46s@DwqJ}zn>=}cIOON2y zk$^vGIx|CGiQ9uMGxi)38AYRm4u>tX^g3X;k^H>ILG>>r2u>0(P(U#Ss%mR#v&;T) zlaMzGqXLbgGv?6Dn|~h(@dwE8NY9gTtGw`Qvr(DnCIDa!@`A2C}roLsLt2m7vaKJ@hF%`2hzxUs%3nplN0`5K+v%C??4z&@gA^$VaQPtBlRxqaCO zy^ZuDat+~U|HGg*30yz47-`O$@0=?S$>F4j*`7PuW~&HQMinjT_C2 zM0qu)`EXXB9Kj761{&g)AOQv*LvCMU>VV-8nG@H&cyNaXAuC>gQBMl&!I2IyO`tQf z`XptA^!*Ed^?69IJrCVGul>6mNjv#V$c9PpE4f&kbb#BJ*9={2Wob~QuP;hP%hy*0 zd0~VrJvTQdj2Sb?3k71tB3^LJ%c!FI*eBhSa6{JsWPaLY1vS zCqXZ;NT;70RUQzy`hWFc2%E&sAt5iMooy5SH8ijR+}7T#k!VatuUzPH6{oTx`2eYG zUfyt$7lbMchX)KJXZjTD4jugb3n50a_5ZE{BKEZrk<6jhOI2&|k-JN}mL=WBzh8%% z?`6EL!JkwdJ*Xc<2waO(=|&R*nF+=m#oV4u7Febu;S>;lpSHm|Sh}@suQVlYXiy|J=b$iIvk>9&lce|g0 z(UBQP2D2m)-MuYE7^|zxU8@l|`ALXyAU*1_@^|oGycvBDEHwJL|3GnthBO}6(EpQ_ zG`tx6g>ef3VKU5QX)O>Hai#$^3hmqcNv;)m$no-JPaEn_H)3q8uxW>H0?Gka0gjVk zRCNgRW<(^AV_x-t}zk>_m=5WR9>h zasvl?h?L)Te(B&{4I7c1#z-cyAXYKm?!0bd2Eg|=>C*T+~ zY!s$j3$b8ov1^qdgPmk6;NKDS^EGjWEpE>xB&}YT?Tog0fAhLg^TB*^?bQp1Gh$$~ zKr$zMz#EbZB(`hE({b&EePv~0l^@^}DqW6b#REGP8~smMuS553=t=H<+qNnK@Sw3` z|H-{J61PFL?B>IVjJO0s!CD;`$}fPAB`gy33wM+xP0Gpcb$^PL274y?HaUmyq3256 zgrHi3G0XKF);NlAmd%Fy68mvI02GxVnz*hd8iaq|!Ys!9R5-i=?p;?oFdDEG5Au7@ zZ_=5W2k>OS_C7e0+zUW!NnH9V2E6eGZ;CMgBql>I?s$-x8I~g?@%kucXg=M+kX;8& z;IR?Wdu7mfg%;^JuneLI<3^7k(%O8`(phy?%~{2>GXHNDfP!`36$UfUlp5vJ;x~Ge zUhWyDDpy|Q-A>EvbSKg1D$h~`sU#>D=$xNOD>E|md*c+K1%pAXw%Yst;$=-7|6s~4l3rJ z&o!`L5a3{AgNCsVJ|vumV1PD2R+6H?wQ__%!S3J*B4V(l@@HH|{+VLg+Ud0yN_ z`aNQ@`T%OPo{7m(1cDICLtBu(&@2JNaLPjosw^`7HJHj(fP1!fc2y&@mqA)_IzAzg zb@0$3?wR$AderxQ-G}e*wg8rXQc!TWs!ENQDWtt|tZTT*t^giBVn4gNBS^|bE{=ZP zy55LAo!|{iw~XooayrIPis469mhT|$dIK&ld}mTmz=fvhx0tN69`wdSS{(E}VtyMB zDknxEZ6SH?)tl9ou2r6rAD@x0u(h=%I|%0hTHCeio{GWg9+$o8PoJ9N9tbi)Bo5Yn zx%gYNBeXh)qO%X36dxEyd?*HY+Q`a!CdYKm?CdNFWKMY?xV{;o$f(+Yjxl}TqK+{S zCH~nM<7_dMS5e1uX!ftpd2eN2CRz;oe%$dGob0KFOum7YwfgYRvN!lJvAZp3aHpUm z@oahW(8Mu1A)%!=lYhNE%8ZrxnNjzUF%YX>2U8x}2JXS%N6t_AM~-oJ3=}iO7w$`2 zGZ9*izRRm{IS}?71C4s3{+-tiGv{I5ze~}MHYoMjjaT|!dWAKhazC!xVczd&JeT%0 zoyGud6gjf{I!`(r3{=`*{&0Y>GykXK(%+e{M-_aQE3v7Yk)9q=P8a~+^hp$0TUS?f+cx4TVFc-C z+=Zhc`$lV#;|c&Ym?-d71mr%@CvZ{`Fs87Yq(Z8F{LJvTv9c{AsmaOe%YSXs?5n=C zw!VWedIdm?v^L$eXiQ`F9fogq`b@3I2nVpdNx|N`o0RP2Y7(g@T3TuMaU)ly{Is+X za-GO?ABHQEw-gApXdga2BCYzd$MKl4@#jE#R!&7X3H^-O+rbqVL_=X(kf*!(k6DfnUAHx3v~GGm-R4&&*6IE{=sL z6n*E8%r7^$uofTgY_%(zwfQ;g zX=`tv^7JXeBhE-NVb2d9Jt`(Cse7*D`)l!_Ms9skyRAY>h7OWBUMFCKg6jv zsDS9*7+SaP-mO7Z#;W)#V*>&bUg!GUS4;MTH!(?ePUrBS#My1h$nLLjJH+(8gwl{Ph!(Kk@J3 zju^fz0*1>Us$NiQ$2hLKrlkSJOh;gA9{vX0>MJbRVgQ}W(bGA69I*Synr^{VpH zq&jY7LjsgI^CA%UcEWI|L5;Wg1$lhZrc>!bqOB>m0RRxXpsUiizNs&5ZSODnO!Gi- z_)vOfnm7uyS06TfSvNj5mipqwZPKJcvrCP4MvERX#~lQ9eJct_n7q3se@=8Z)iW*7%hdB|bned~R+7&CXp#&R_!AMBGO1bw^rhmsqf zx;6ZnnQ?>;2Q;b+%M#rL^40^S9ZQWTp4xG(9d7*vdjIdzb-*>u=eOZp>ZYdMH_yw; zaxWTFSU?Gum zhJ=OT@oXfH6{m}hRqs{oa^hJ%J(8f<`!EufaQxIM&9i5Dm_h>Z9Ii=*t4=>d-jBJR zID7UZV&&xC=1Y$$VQkx{PdYx60RqGiM0z%O)SsV8y&y#-K8RCcW7Sc4s2RFx)!3w_ zCQU>~FtiXbA?B^9Zv49^N8;YSdxu;F)4Z_H9g~VfSKh)Ge}UT;_>LBB5Ci<1sGMA3 zy<7MpvyG|yL(qmxO5#9|E{`xsbMb7O8LXgrprG7%O6F})rfOoItszi*Bg zzX@uT2pxE~>xQpi)IDEmH6;g9r!XIM5|tl9*+6Pd+Uk=aehjwj6P;K!q>wh-F8_Fs zZogGryt}(H0MG$4jP{WuB2rSvA2wxrPi>FR##;cx;PW9wsRlO+FVc;YJU{#`9dHDP z6%Ym~{Ivmhp1$3lU8?5bFs|PAvADcEK|O?N z3WGkm$qsFDJt(@t}1M$^L6qnD0xNE&tHDy6BmZ%r->Sj$>KG4UAS)w`0^M`GX0oW+AQm*Ovy$;+)-Md`AYtdS~%%&xUFj*Aul#IHb=y3 z0pIWqEt}ycBkic0#*@#KU)xvdP~?ncF*z(iF}RXAh^#G%9_6)tx30RUm5e;fovkOH zeEIPsOL@Ri9>}%AJ+Z^lHE$s*Z+qM>xXn}jF($w9cVkdaFhY%nvhq_m$t42rH&%XAeTE6+)c2%veQYQ1H#% z!@wMmElZTTu`>E+&b)_ZNP@xu{ow*C2~K@)@m)cSE-H)lf%?sRyU_n|_XZk4o>iUs zRBw$Adc^+iEw8s&f_!5Vpl1ib97lt+xo`m&Ql7=BV1Tgl@+#T8#>U0PDY%Vt!tgqe z06D_M2y>o+qepJv;l2>>q(a0UM5&30h9?2zU3aN;iUv6q%0D*?4YTgoSs9)^yA@=6 z%x&(k;?3q9mbs&921+i6igKi4p8>ET4myc1pwYv$Kyny=5XJbkcDC!@PF#{gouh|9 z4oU_d0H!20j`*84sAk{P)B2&VtBl<)dy6mkuUnm4T`?hFG=j(Y=Gh3XLnlrSw8A3* z3KlM;@Qd5uncIP5_`|)cA4hRGm>Y!O8Y(rGLl24XKfd~(%T_Ni{+Ta}UtfVL{)?k? z{l*R4XODr650ibdG+WB+yXh`8+ZtSu`PNBm`sE^P5R3bN427E1fsUD2OkI3edOhk)DbiP^ro>5aHg$}l`QjN&H3-^}U6WE%gOJy_osghw zWAp1KtM8rhpBO}7pg0#g_jg@GSG4+cUllb6X*a4id7NwXbxm@|*Z?kPAfFZsYwZEf z1FmdwsrV=XHrVwhYDRJW%VLKr?~(ULiEm(G zA(v+%fN(MduL&~rQ%+3YSpLDI)aAgxE>feO>}}RzF9*GejDN(4;MGL$ws^bCtcnUk z)i_LDgVF!kt!Oc7 zZX00V(63(=_HwNF%Aaj9`SepSZ?mv7pMnI?-``KVE;u}h71m#5>sIY^T2u^lds2A{ zFlBK1<8{R_wM^gN7D4hUqq3)KuQVsaaC;B76CHtEAHw%AZIO@&N0WcHo_?1=X5t}C;vClzedr82cuv@7 zrN2LH2mIB#=$e+AdJ8X#grNzXmqf=gfwxd?huf|5+LXGlPO!ZJL_qH9gucEO9tXmL z42HIPbEm#1A09i`KmrjR@GHwaI-%g}=&Ry)oZ4XH=Ha`m(0s3lg;`Vu(jyu2$My^p z`xam}z<6T$*vs+Y$Pxe;PAu${9jP5B=}%?1e1u<4d;~I@v45pAjfXAIpHKf$ymDu% z4}LI2J@UO3BeTD8z4jV0S)fP|(+J@lr;;C(hZJ7@aKI(Ezfkq8Q7QW|BSoXH0}3J) zuo8J?dLUi;=3RYv*e*a<#7;>2s_=-Wkiakrlk#}0sU0r%J3&*0OoI5P8>9e z*e)s>93Rh%HG|yW10(oAc1s?VIm8+GqYms_g%?0r9Qk6K<0nsQ;E`b>lczata`i5_ z!5sa?qK*iJiR|U1&wpU^S(#3p_Fbu8eGK8COtRm*s+#1AnE+R>`aD@GT9qYYCW#B1 z7j;bi^~Uyrxvl+@w!Nzh9~3GvZgSqm@Mh?^yhA83Fo)C3p(qXn6<Ej2K?p@+=*M6iXT}OotsVD;gi8zs&>kYj zxC|X9UkiuByUX-=%w;AB5s2H7Th>Hb3>8$Fn*b})C|Ju&O0xl-#GzxljzS62Qgt`e?(Gc zHG&N(FBEk^*u%{92LNttY(%l1N2h@hz`R%CAUz|X9l&P@v%mnp&?_Z3_ZC(uo%Q7% zhnC8Oj#VW#sEl|i_MOJzAN$EI)1|2>-z{=-_i$+f>-SH3f~MV?XUz`~ph=CGu@?IJuNv)UHUhfDQp>L&^*`&bG#pocp$IY-k9RUiIOU z?8SDO*N_sIt|J$n5ez+7v%ug%X40HnyKPwxXOGk@i$qzgF56~Dyk0yM=_v<{CT>rb}_2F+2 zqt2bHBWM+0IZ2DNjBl*7glmlkn$Dv#uVVdlg!4xIpaqmMP#tpC=f+c?J-Y#NMo1S% zFEql$)ulo38Gkevp%RDXTCe3*-c6foPyu+9UV?LlgsurJ|AIASNn%HnaQGsTc3Q0( za{1jSuzDW>hLT}+w~`;joH9r{-O!NaUM{1=(e;vfxpvSHphUgv*zGS^4fCD0$#bb`u=fPP1<;?Cc3jezm3^KkE7+)^TeIDBy~{1Qxadm$*I(eE|v?1uSFig2K?UeEqdac3Hq z;}-w>XB$$Kk||TkJXFFqCn}Vp2uVmq$rOd6nNk@;i6j{dAyO)-sALF<5<)UpnvhiL zIiF?!&ztk=yg1i&_T|OicF%Kn-}hR-;XC}+=nTZU=zT1P+cb=*j%VRnSX?j(A2j=D zHu=1&Vq<50`PurG@)5vGrjZlxfEz#>Q9Sz>6wHTeW{_Cxu%zEf(MSPO&|4F@sp>-_ zBOig{5X5?Eue#u~-__OC+|u%1wU5+;FPUy+Br7ltFeJ_;*4XlZNjXRrLo>@cF{ z?v$1T6m&SV7Nxih6kRb9y1Y&Fq{NHunzmy5-rY?T_25n}Plh9T_hruyGzoi084BYt zP88=v=|xFSZ~7!ff2*uquyVRbnza~vW2w>FctNIHR2j&NFg>z0D5%g`o>d)wDBfR1 zRXNk{#rWPG>5wxCe)*37id#8yTd>jrO_3D|h$w5MxAz{u|)afu;z7=T)2A+b5+5f1f~I&DgbqPv*^e1g32$ zszKy3%%zNYDmb&P6lYHzIT;|MCLTTwh zzy@~_&s^YU08d(n*F4#%_RvPN8Eqsy{>X?&t;5Fb2n_u3J$FP1RXcV3|jLPJdwfc|b?HUrei=y$(cmYyIyqEL(~X|8gkeh;L3_|Als5V9nb zUA^|+d3%3O-10=LMFclcKXm^!0>cnMEM8&VFFqq04`2GW?1({JQS7z!)~#9zR+`Gc zqVu2Tp&_5;W&Ca|+A%8p@iNSTG}C+@+DWWk`rz`7*7`xECz}eH;}~n`rTqT>iHjby zXU#f8Jx^j3uB?2=Y`d%HOl$~aq5*~EjCeEyURi+c1UN4%FF&u+;hV#K1gvzsof?i5 zF^*KIa*Srp`gTn&%P8S_V^YqcyP+1brZLz;{HY+=nP4V!R&FlId#Wnc`q*MW5X2a5 z5#uV&=ghf9>ao0Hvu)zzVrYiWFvrn&C24R_{W0=Zb4vZwui9%@HdR2H6YISGub5L$XM&%uiX&*phyJ;;kqH^p!Xx5zFGj2W6ybSWB z+x&B$pa`kD=zTOSulUl_$iMQ^IjA{KS_85SoPQ8u6wvtB{L`M_?+*qYrUu-1$1Wvq zV_AtprCDsDfZu2v1zJE)1F3~hX^i7LrUSxt_!n13M@I+H&!I*tJlXoUlY<9+BSSo9 z9oGpL(O%Th_Z|&nJ>RR{?FdLVfb_# z0)*lwQ-i%GYZ9#};c&&AOh^c&PGy4yEa~|C?c2v__|C`1E(atjcironDv&-gy#(ab z$lP2Ru+#y?aJiG2L1adoYk$EF`Qc|x=5f75D>-@jkI)dj+NT8t)8HLQZ4aZSL?Mz8 zASPJ<4^N*5M+<48#4FIHGXx*s`1zr2Y32@4f?s9bRLdm4ro`P=?La)GU?fb5Jw-JD zZAVa zig{?Ug%1SITbWn{?|F4%uSVA|8)Bm5)Sb+7{O<(i|HlZjfJMCw93&_|mbF{mANtY9 z$0z3sI)kFj%+pf`^l1fF&DHgFl{r3Y7Q2(BZ!^GGPv;x4mDxjR1VlH^y^C&~O`{UF z@ki8{K0SKeXAt!ZYDn-Lj41rT(=S`>?T+2H5zZ0Fl{SZS&uaC*W+_JJBqp5u7CIpN zUWih+{>CxSKwjIn?#2=so+NeCd+V#3ZnThet;G(b3NXFG}}ng1nwv@o&kaxYVsBEEjq}>=-A= z$H4IFZhlY8HkH5F@MCwU@IPHv%SZ zpc=bpaZ{4J;WLH3N+pApGvA+7xv10cr@}kB0L-rs#Hd+OkYfpY$=e0`%3ht=v7zTK z4~0huv|lup*SQPgZjzf;%~F$PFN^A(wo9EZPY*wYwto+zg3=AIf3(q7&(0dWJku_F zXxAtik0lLh)Vovulq!lL7ctL(L`;O)fD*vVRnNWm;&hHeG6UO_@xF&>>`vx8TE?%CBNNhz#g2AYv#)SX z9#ZtpHPty~NtX9yT5x+!FE5LKmKC1y1$AwH-6x1aV6_jOu{@SWG{8@i$GoA zY0GspwXaOQ^Wnvv^4cwo$;&R?V3TOIbI+dMzv|Htu69;9oV(-hhq*~py6EpQ6_3z7 ztQr(pY$L`%-`&}HF!^4?Dj|qns|;l9D4C+Q{8;niWC{hMy%-Du*D`V>En&8;&$6hz zn0*n$m(~YoeHoZt9}e2$7`mTMji%S%=Xaow7}QQ1at%F1nAXyXHXSnWZtiUfl8K=H z3#;SU6rbZEY5&@E;w!Kz@C1ga z;Jf`()#OI*nkl=xQ%r(y+_H?vteXa!k$c2V(I2<`X&{lMt+EP=kKBr(w|E0l0pU zbVx;Uhqq>#;MfUNaN(NX^@mO))>&9s zh(ZDogLNU`K{W8%bei%qSywV*Tuzhv_Cj$SsB%)zt`QOG0ZlJgLZa9JodPsc${eHj zjBlZU?*;>VSv~#m%RCwx8bScG6wF9W$hcGK+L@UCA7(E^0;9qMzP(Cc==Lmr)5J)M ztqmyO#Z)mOA}!+0y61~2lo?rXcxH2zYbuS?8Rnu3D#fqEf3I%Fp1U7T-nenp?d{e; zP`WN@)@JAhnUjW_*n6sWW8mw^Ow-sxn%_)R>4H-z+eBLNg{g7;7Z&I?>{li&8132? zo#VD|QXJ1!Ads`CU=lSWWxus|gMG{U5iNH2GJCsuWH}G!DXu=S@4tM1JZR@SeVsA^ z6wJLb?a&5IEv?UrKEGT0G(L;MTDyW^`tbPV2q5$m)!$Ehgm3s%yG1BeM2`!nMpH*e z$+gL1;wb^eOB%jXY&DcYS)ZDJ57^R{+_s?tz3RAjWQLE|!Q z)25%TGrmNUP=u(8)`9mvSY6#VyJVhh?K|G#w&EFO6Kr7i!;{mhz96Ln# zLh#*~tX=!Zg(x^u5ylUAh6;bp0-YXt0#m&+Pg;+aL8!VKX-fV&Va zVu%G;N#O^X2;a@)G;O2u0>Oa5H$*wuy?e~_6&6;7^lR`s^lKn=Vy-)TU%qLrt$n}w zv><{yVMj9QTqw_p0lNxU_sQ?^pe3#tj+-D!Eld4zp{-fKD{TYWkrTl)nm$W+u%syF zL<0GP;B6cpM-e6Mhcqu26J|seNKu0Mi^_{N zP8SAlx@yguc`Il3l9v&kECVBA3+=AGBm~8vZ@y%)8yIrMD8$Ma11`Xt&@!NVqMi}u z0KBZA82=zW{)npOH0n3tVl=R^*NxB@T(!DH=}Zj_+CHbm@TD)QTQ;_E7vBhNz|PDn zofB8Wm*b0~<3zRwulRE9zhlM_1CIa_S=}&C`3Hfx`IKG*kKmd?I>|4@#z{&_$~$Uj zACMToHK3?sL9WfPIQHj>mL;;zJ|Crhol8O`bz5}s^lZeV;oXqC>?tG#{O=-aHL4?n z%RAFbHfMmvDj3@Ty2hS@U~Br=s$7NYE(e7Hr7(Kg2`(RHT~qs9vM`~TgQvA2@Uh&S z-rNvoUD)=2kq;w5!(MAPxvmSHR;%>lT{Xa8XRln@4}1XIEfgGtB{74}B?TnxBl%Z& zTx+)g1PI8NN&|h39xDU~iirA1ZO)BrEf`oL6M|#JiFp(r==tVMi46_y5^WY`>h3-f zIFa$}bPLI@l|7*dkodq%0I9-1zjsYd;cqDmK;-HIm;oJ5K!DYQXA?58moInbe$uB- z?b2l$#)PPd5E_rQV6#Y$Jvbby>{9X46lqZOr}L*DU6+%>9fNo7w8b)2 z{^FwfjHmyp*DC4+wF9~Ub?TJlajT~TW+iC%>(?#PQsDlWOeGGPv4}mJmKKRmPMF*X zHSTH02_+DYhG027O4c-@{YRkXh!uIq0`osLeK*nM44>nqm9YQOXTr=wzH2PK5h`3M z;8@TE@I7ZvoS3Rm3hPIkYT)Yn%S-CjzSb+I_I}?3$!1{HpcL_2e+Zz$04WTMLKK<6 zv5D=&lxIEISmoN|sUW=&majz0NdRHo1dX*k`53r|=9q2}+^t4H2GBoi*IvA_ z$vs1d>s7g=fMMQp)4R0k&0FYglBsZvW5N&`eUVa-`oJgGIbiDuK|fm892(~F7mew3 zICnZUkx*7gnxbj~%?8_`t40;b<*3IjTr-6I-BN;jv~_3kW>k)37|{fN6J8PaQ@4htCm7Sf$5eX0DJ-|;U!=2S&!_3 zxTq8QvP56!Jb~YfDqIYb6HF5-8NHLC;PU#)n3TEN=42Z(pVu_|aQ zKC|deQjzqA%;Km?=4)IJRj8~^I)Bo5o9>WBej3?+2dW&M^FYCeYHH%~()0un<@QA5 zvb}8w92`A2?b(oAUH6}KnE&%1JR1WeMQ$f`5wSI_Zyw>LQ=F_TDVgHgoHeT<4H1&K z3z!&)AGrP%_>YCY2)l=#$+V7!J1%=Tr`A_UliS27oeoU!zcab4vhou<>1O>~5AlH5 z*<#T_u#zoW51Xf8m|GNOt~9O3GOD_>0IaCt2=}y7P!}VeUXPcI>0AA?rR77RaW3im zgKH`$QP?82!&0MA;p;{No z7qpwqRWH;;)Dnq}&m9$P$+d&kHL7+MRoH|63zZDSBzZWOl`s=gE9SFbWz~=Ri_|r?ed5 zbHIm!4!r;PvA3P;S2jxdZx>|8D<2;dZZHX6@I8?2)D^@3(ePJsrH#1G&Z2X^*;JKE za1|6exJ^ObmEcMC3S!hLP9;Xu1ktN3Pb%KZRNxoM&ixUGDBsc#>LkjKkF*3%PM?wR z0ED3{GFr0a*ee%LWwel*q z+@W=A)|`%ySEf)YT)kko`H;E@wfd>?FV|wvJB06SJ0bDl(W4KrRNP`}4DJdw?c7G^ z3~Sy@KXnb6PZ2t^N`53) ze{gNq56ai$v&KE7j0Yb?=%C}J&0)KX!syVc(5t0)hx$`vOduVjcW2V*G58LV^Tl$Y zKHGM;!rfmfIPR{`wiuO8#pK?f-UMMyrRO&Tc;vzKQFj-Qt>0^_Ppef1F$+V zx<~jag?5hNRwvfW<=!4rR4Fx)Rj&#JE;=~lG-G-gP@5}UzIW_O$@`!1=Mp#1-Tf`q zJ$ksBpm=_9-_`&2e13Upt~j;`d@S&e-Q2-P_kLcZq_4AxShqjaMxkRz2TBVuMS$M` z2!6um^3WTL{GMt~KKlGgV@a3fC0b~dAH&W}*izFMdK^!&s2Cj{AKRJl*jwq==}!~= zmCVNW=-e&oY`!CG2ubm;%ktVwA<@BLgrs;M=!ov-(}vHitXKi$NSb#{K0=NNY|5EB z!p4fG%6hWvnP0_S^@EBJD0Ooi*tf5k00*u?l0iWxw*Plzi|0LlWxCmp?U@sJs9$fg z)88f7wQHw7eI7xg-KLo^AVT=2_nKw&yS=*VSUFqg#m3`@rpiBxnxZBFEC5A$!IQ?@ z-nV~$gEz0t!Y4Ejd0FX-Eh&enDhe31VYbMD1s_EPQ8)hVrfsgiBkl^-DJ1$BiO?W)t8iK6>JHS4?P zpro6=&M>yz4vC~Ol# zQw4j(vp(PXp1+dazx#P!XMN6bRvU*wSmEj?wFr$Y&rpIGbjz0HiHp{p3Dc^pp7HCy zlnEV$o);+AmM{9luwZ*)Ib|q4>jqQ0Htn8+BIMMMYw#;xu2WPeh{72X3>DS9f2%{@ zfZAVi%icvCeJWmqC|o)U%Cv0*VjW%hr{gN?h)zt&{2$3k#|dc^WKkoUcKrtxJh61lzlpM?9d^r zbc2vEvKnOpz~J1FnAE)}J<+BX=voL*88C{d<~y8wJSGqJTa2q-$?Q)1iX#uVxuRq@ z;Qo;-D&5G1a5^QkdYneMG9BjL%;ngT-MxGTB|12LzhfnMM{Q`l@xq3sqehOxW@x#aR@Cs z@-PNktzboCIG#Cq#@66==UZ!%CAO&iZNxJ-lY7L}sUHoZ*A}mss&g1=-D|9I^0SL& zElclH;4?HvOgGp7%_^)SVyB@h80DB;QZ4T&$6LrESO@G?c1P~Wg11d7Mrb3FJ2jvn zM9j^f`hIAVrbt!qPzKWs!WIi%8=F|vK_dByp>n(LjQXnat!Yuuf#NqjB7VbXRO~r9 z@iwEOV69c6`9a5Fo#3Dd~&i#1Yqals7pZ9a*vLiwcLtd zg^7K)RuG-P+a0%v9Y}+M_<$4MZZkYxu$N4eqJ$cw)77u7{r_Z0UC$$sJf*c;g#Tws zbD0_2;4TnmZqH?M%ha<^pQ@c%)^}tk2~k?3D7^*31D7L&{|H7|Cce50F22jtqjf$H zOo9bg=)8#QA~aXTWju!Cu3p_-{ifwvn34zzPnEm%*CO^U`}4j#c^1Yeoh{fD zq6=^gLT0LTYJ4C9N(jaSh{44I6r)P~TZgf?LE8)Z_0ID0=F5f{xg~&Hj_oC?4YD2g zAY`L(Gi+Ev$I`g3T6OZ&sa7NsUI5Ld0GfBjm zhy;-{i~x&yCg+*d#}@4`za-d*Flin4`t>rkr?k%=e|Am+=vlCTQN<@Ai6(3EwdUC7 zP`7cb0;49|0=M{Xvvp58IaHm}=kJwb{)B9t7|C7pUj9~rLcp;;_n@)kxh+wUNfiZ?XQw! z>C%e2DW>itv`XLvJcshy2OF77a{u^b z4>2PO3puz1e4|jp;ou4W+-e6|TwAWxL;HnylHX=c#pmxAv`VI=CsByG2LiQX-~(+H zL=e;4?nC{`b?8v><0u$8@5`A3)QAc-?rHxyl>T(OX{GKj!poK;@`ba%Nt2(Ioxyj4 zqdAHe^9>Nlg5J45 zgXh=zELqjI$!$!%&pzqK;#aSP8;J$?4yD6LDe@U%A(CVCDzXVdDp71|4YN+wnhd#gJ z&F!vApAML+dH=}4gF-kZ9@U*YAgjpnIEo+w<_!~_5hRFf>e4qW7v=->fR;e`3rV!7 zfo(NMpV^`GzP7)*XRyrTu?5mRza6LZzGQKdU7}T!PB&FmvG*a;SftFHSOry~t849b z2|buFhS45_W3Pk~^|Cf>Whtw!YWB3xWc`4ihE*frD0wdAPGUYzMa0RCy_cb8Vs~@P zV!n#d!a{764yOX0uu4vGwEfG2(vDMgqt@S>?0iURtcpCfoxtFnoMxQPe^=)Yj4h%n zEg{cDEJKUf1zzcGuCmV3&kX#d^gCFse#A1u30z^{i4%L$TXOF>e^zf;mhKrYG=QVR z#kf5XQC02F8vfk*0CmrHevhW6zqzbCSx%eiqWY+s$vBdS%^K9UbxL;jsywk>G;v(Aob_KqG(P zqY|hFHk{H_ZB}vcG5;Fv(*9vPFgOBjLrQ>u{dM`KN z+Ue@sbMz(^$aHK8hGVO{+OqVu@jqfNmL$F5b1wwuY`W!8m7=%0_K|31q%8*BBMb}be5Cd?=g^wqI^H8NxPSZf3t~I z#~E97IMjx+4g^V9KLExsT$v1Ki5#D!oT>QLbm=pXxnbleuWiZbAU-iuHdve91aDSs>~0tsDhu8P+2tB#1Ue>K(&}ezHCfb6 zdLKg8H+T9%XG~Lz+JdVf*~a)nBvsQ~S@1;>|fFP|MAslZRk5NO&_rhR*H)o*=zLjZO>W|+N19c&siPhlbE zgW^i>?3IW=-aGeKe{CkUSOapZ=Jm6am{{VeE5y)HQv}C~&e`>ost6tLn@sdy1p5S` za%C+G{utf=DF*fZ)(Qo<$)z|U?avK9Jaaz3K-D4t8YP5q=%GI|ha#(NQ>&7;mw2X7 z4G8c=-AKM@`ld#3(0{EdXFH|E)@trtF_wgn;B0P>h2SshkGe~w38vHmkzg=@BvsB9 zSJ>%a)7Mj1ZLryK@gQczJiE#fb@bX4n!@ivot>hY_(Bo9aifq(<}0K7Rs zfV>!;1f6LQWJZ5T$D^;OcYAVL@Y71Km}TOHQDS-{MeIJp`Xdv6xQS6-jhlp00xUd# zbw<-U78v41A+IM*fsZM)Z{Hp_;;DoHGW+Y!a-~vS(obK#dVnbfm`okzaN7>uv~gf> znz%G-ITJsj6NFZ>QzvnN;Z`NEajPjdqz%Ss+_uj}o zzzTuZ2J7e?W7n`2c*IXzb<>x1HvA{Jv5o4Ofg0Es83x`TRX7sFkfTQ@etqF24XeZi zXNez*J&f=XF^9oqAMt8O@SM~#DLTZ1bbI4(ZF=5H%KdS&YW$@o<@Wl(wJwhzB6Ho9$iS8MR& z-*@8=Xix7uZRwL~uP*knoOq^it=6hF>zX63C$_BKShsAk^m^jc&lZhldo5Ng)G2l_ zwC|{M!2X}wtV#0ilc%OUNzJPduC99e@Zbs$zk1KL`yx^&y;W=3TGia*savfmIWoil zABncVY-@>)Y$qAXEv0_)5?}RcDiVo7khVk;pkynN$j>b6bFIoDtK--viR87WL#9Nc zqW^#KG4=m1KbSm=yXtU#-I`6C<}Y8qeAkTAE-6dCyQlGoe;Jpy-rd~_USQX>eh1Gm zA*sPr7lJapr><|8JF`^k#YDj}j{SBo-O-)(83XM5l?}vC$FG~zPz{n&&cWzx{mWDP zo?s27G1jV)xh=EQTKRUF=6}`Z<#o-O?E|yiafbYCX{xTEEx+MjcR~N;eo)pENRwup zni?O>$;sKoSkuZksS9zl?8v!$H<5^8s;E49_wED)8E>$Ekao9( zojc_e6cuwn)H;F)J;=*D%-&qNGG@@AL0#l!5|Kpg-LvN_W$F#jhIk6WuCi?kN=o9$ zLX&?!F>pzJ6Rp^GYWsefmiskwjaz;;c)D|tOaj(6jtyKx1R6GCL=ldXOAIMpn!G`; zbb}@M`>Pw9V`-xnZB4VjWSwROt#xof%(GSBH%QJ5>9?otrE}+u0KoRID@NOQU%EKM zI}K&>Sw=q1B$6E0Q4OX$NMI7fr;i^8ltfrQmyvW%*6z^ZxcwsWf#D~JY#LE{gNv_^ znK0oz2R&a}nreaPWUiCb zF>K8-G1(1x^Yg**I7j3{RLtf>xeePcbDBruiwY zJei|{GB_%DSsAa6^ z6UKU-;b+VF+xe?josW)*Szuui&C!Fr#Pf-X3$Th!;V zDcL@JrRMYJBr70tijOtPWTCV3RqW>JK7SrA2^fEMjJ<(DRz`YJrEBbk3&t=M8^5Ri zW24oH&q0N6;vDxh$||dC4trZ|^JO$Di|SBhq?gIs^*8rAYUz8%l-!o@e#_I)f8?5c z$N5w{Vw($rcc01b^Qn_wrFkTw*gK}97O{WJy5j51u5n4>J@$#%Z1WtIMl{@b8Y&#C zB&Wzhg9jrzJ%zVw0e0O0Dx=M`-FNIpia#HmXZA~%)O3Hym0;+G4H@!;raggj1~{yY z`k1_I&SNz+4CHzjv4boD5#0ati}|MaGYyl9U6m!~a-DK~u!3F&!a8^5${a=B;0~<(rQr%COq#!6w&_Ab z!hF;{h18M>=v^m7n$Gm^K6Q2fOt-J-@E0sz9D!%#Q+?BH#g*2^$0{Pj!}EXtPVRRg z2)uq!Zv02r)K7Kp3pihhUcicz6b_#GIN^Qv8i~=sW1Z6t*i-Fy=o6C}aelb0^x^Pv zZ?~@~zRsi!&EOt;W}GHhw3qR-N^v>MdH~JrcQ|+RvoDgqhm=Yu6})(H4rYDnjZHJJ zTFyFnu>NLC-D`(oM*cr?KA633r!#hJG(pJkXmd`XhC{tcX0+;q%t5 zyTtQgw#3-;@Zu@*v-!4B{r03P2j7LGBqY?~%q@rZn&|rNNI+m9fiaQdS8Sd0=V$6h znX#mz1LY+@WIE+6-N|jr_)T>A7$hbsvxx7PZs5k2nCaB|(-6WN4<9e~kkt4b z>zw1`!qnDgbgzwg7C6M~GG>)kD^^V7TY+AY35SpyKGrvdHxoIvJLg=acs`$+x)?yO z06knCl!qfdso1&PhN{Q6kzKoYC;J^y9VU6WMyz+XHmmz7kn51RqKG}~y}Z(>%-7$r z-}z~!BzbbbJ&ST5J`63XUbuO4B10)op^o6&X2DDzY`M9)gQOJi@dk?<}UNY#esA%_ECYNlo?NQtHfG@7)r~!_NOlarOVFO8Q&Z e{lC9-gPQ4>eFd5${mLZ#GjqD>w9EQ7+y585_T!5H literal 0 HcmV?d00001 diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/mpc_lateral_controller/model_predictive_control_algorithm.md new file mode 100644 index 0000000000000..9dec0def2e477 --- /dev/null +++ b/control/mpc_lateral_controller/model_predictive_control_algorithm.md @@ -0,0 +1,389 @@ +# MPC Algorithm + +## Introduction + +Model Predictive Control (MPC) is a control method that solves an optimization problem during each control cycle to determine an optimal control sequence based on a given vehicle model. The calculated sequence of control inputs is used to control the system. + +In simpler terms, an MPC controller calculates a series of control inputs that optimize the state and output trajectories to achieve the desired behavior. The key characteristics of an MPC control system can be summarized as follows: + +1. Prediction of Future Trajectories: MPC computes a control sequence by predicting the future state and output trajectories. The first control input is applied to the system, and this process repeats in a receding horizon manner at each control cycle. +2. Handling of Constraints: MPC is capable of handling constraints on the state and input variables during the optimization phase. This ensures that the system operates within specified limits. +3. Handling of Complex Dynamics: MPC algorithms can handle complex dynamics, whether they are linear or nonlinear in nature. + +The choice between a linear or nonlinear model or constraint equation depends on the specific formulation of the MPC problem. If any nonlinear expressions are present in the motion equation or constraints, the optimization problem becomes nonlinear. In the following sections, we provide a step-by-step explanation of how linear and nonlinear optimization problems are solved within the MPC framework. Note that in this documentation, we utilize the linearization method to accommodate the nonlinear model. + +## Linear MPC formulation + +### Formulate as an optimization problem + +This section provides an explanation of MPC specifically for linear systems. In the following section, it also demonstrates the formulation of a vehicle path following problem as an application. + +In the linear MPC formulation, all motion and constraint expressions are linear. For the path following problem, let's assume that the system's motion can be described by a set of equations, denoted as (1). The state evolution and measurements are presented in a discrete state space format, where matrices $A$, $B$, and $C$ represent the state transition, control, and measurement matrices, respectively. + +$$ +\begin{gather} +x_{k+1}=Ax_{k}+Bu_{k}+w_{k}, y_{k}=Cx_{k} \tag{1} \\ +x_{k}\in R^{n},u_{k}\in R^{m},w_{k}\in R^{n}, y_{k}\in R^{l}, A\in R^{n\times n}, B\in R^{n\times m}, C\in R^{l \times n} +\end{gather} +$$ + +Equation (1) represents the state-space equation, where $x_k$ represents the internal states, $u_k$ denotes the input, and $w_k$ represents a known disturbance caused by linearization or problem structure. The measurements are indicated by the variable $y_k$. + +It's worth noting that another advantage of MPC is its ability to effectively handle the disturbance term $w$. While it is referred to as a disturbance here, it can take various forms as long as it adheres to the equation's structure. + +The state transition and measurement equations in (1) are iterative, moving from time $k$ to time $k+1$. By propagating the equation starting from an initial state and control pair $(x_0, u_0)$ along with a specified horizon of $N$ steps, one can predict the trajectories of states and measurements. + +For simplicity, let's assume the initial state is $x_0$ with $k=0$. + +To begin, we can compute the state $x_1$ at $k=1$ using equation (1) by substituting the initial state into the equation. Since we are seeking a solution for the input sequence, we represent the inputs as decision variables in the symbolic expressions. + +$$ +\begin{align} +x_{1} = Ax_{0} + Bu_{0} + w_{0} \tag{2} +\end{align} +$$ + +Then, when $k=2$, using also equation (2), we get + +$$ +\begin{align} +x_{2} & = Ax_{1} + Bu_{1} + w_{1} \\ +& = A(Ax_{0} + Bu_{0} + w_{0}) + Bu_{1} + w_{1} \\ +& = A^{2}x_{0} + ABu_{0} + Aw_{0} + Bu_{1} + w_{1} \\ +& = A^{2}x_{0} + \begin{bmatrix}AB & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \end{bmatrix} + \begin{bmatrix}A & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \end{bmatrix} \tag{3} +\end{align} +$$ + +When $k=3$ , from equation (3) + +$$ +\begin{align} +x_{3} & = Ax_{2} + Bu_{2} + w_{2} \\ +& = A(A^{2}x_{0} + ABu_{0} + Bu_{1} + Aw_{0} + w_{1} ) + Bu_{2} + w_{2} \\ +& = A^{3}x_{0} + A^{2}Bu_{0} + ABu_{1} + A^{2}w_{0} + Aw_{1} + Bu_{2} + w_{2} \\ +& = A^{3}x_{0} + \begin{bmatrix}A^{2}B & AB & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \\ u_{2} \end{bmatrix} + \begin{bmatrix} A^{2} & A & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \\ w_{2} \end{bmatrix} \tag{4} +\end{align} +$$ + +If $k=n$ , then + +$$ +\begin{align} +x_{n} = A^{n}x_{0} + \begin{bmatrix}A^{n-1}B & A^{n-2}B & \dots & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \\ \vdots \\ u_{n-1} \end{bmatrix} + \begin{bmatrix} A^{n-1} & A^{n-2} & \dots & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \\ \vdots \\ w_{n-1} \end{bmatrix} +\tag{5} +\end{align} +$$ + +Putting all of them together with (2) to (5) yields the following matrix equation; + +$$ +\begin{align} +\begin{bmatrix}x_{1}\\ x_{2} \\ x_{3} \\ \vdots \\ x_{n} \end{bmatrix} = \begin{bmatrix}A^{1}\\ A^{2} \\ A^{3} \\ \vdots \\ A^{n} \end{bmatrix}x_{0} + \begin{bmatrix}B & 0 & \dots & & 0 \\ AB & B & 0 & \dots & 0 \\ A^{2}B & AB & B & \dots & 0 \\ \vdots & \vdots & & & 0 \\ A^{n-1}B & A^{n-2}B & \dots & AB & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \\ u_{2} \\ \vdots \\ u_{n-1} \end{bmatrix} \\ + +\begin{bmatrix}I & 0 & \dots & & 0 \\ A & I & 0 & \dots & 0 \\ A^{2} & A & I & \dots & 0 \\ \vdots & \vdots & & & 0 \\ A^{n-1} & A^{n-2} & \dots & A & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \\ w_{2} \\ \vdots \\ w_{n-1} \end{bmatrix} +\tag{6} +\end{align} +$$ + +In this case, the measurements (outputs) become; $y_{k}=Cx_{k}$, so + +$$ +\begin{align} +\begin{bmatrix}y_{1}\\ y_{2} \\ y_{3} \\ \vdots \\ y_{n} \end{bmatrix} = \begin{bmatrix}C & 0 & \dots & & 0 \\ 0 & C & 0 & \dots & 0 \\ 0 & 0 & C & \dots & 0 \\ \vdots & & & \ddots & 0 \\ 0 & \dots & 0 & 0 & C \end{bmatrix}\begin{bmatrix}x_{1}\\ x_{2} \\ x_{3} \\ \vdots \\ x_{n} \end{bmatrix} \tag{7} +\end{align} +$$ + +We can combine equations (6) and (7) into the following form: + +$$ +\begin{align} +X = Fx_{0} + GU +SW, Y=HX \tag{8} +\end{align} +$$ + +This form is similar to the original state-space equations (1), but it introduces new matrices: the state transition matrix $F$, control matrix $G$, disturbance matrix $W$, and measurement matrix $H$. In these equations, $X$ represents the predicted states, given by $\begin{bmatrix}x_{1} & x_{2} & \dots & x_{n} \end{bmatrix}^{T}$. + +Now that $G$, $S$, $W$, and $H$ are known, we can express the output behavior $Y$ for the next $n$ steps as a function of the input $U$. This allows us to calculate the control input $U$ so that $Y(U)$ follows the target trajectory $Y_{ref}$. + +The next step is to define a cost function. The cost function generally uses the following quadratic form; + +$$ +\begin{align} +J = (Y - Y_{ref})^{T}Q(Y - Y_{ref}) + (U - U_{ref})^{T}R(U - U_{ref}) \tag{9} +\end{align} +$$ + +where $U_{ref}$ is the target or steady-state input around which the system is linearized for $U$. + +This cost function is the same as that of the LQR controller. The first term of $J$ penalizes the deviation from the reference trajectory. The second term penalizes the deviation from the reference (or steady-state) control trajectory. The $Q$ and $R$ are the cost weights Positive and Positive semi-semidefinite matrices. + +Note: in some cases, $U_{ref}=0$ is used, but this can mean the steering angle should be set to $0$ even if the vehicle is turning a curve. Thus $U_{ref}$ is used for the explanation here. This $U_{ref}$ can be pre-calculated from the curvature of the target trajectory or the steady-state analyses. + +As the resulting trajectory output is now $Y=Y(x_{0}, U)$, the cost function depends only on U and the initial state conditions which yields the cost $J=J(x_{0}, U)$. Let’s find the $U$ that minimizes this. + +Substituting equation (8) into equation (9) and tidying up the equation for $U$. + +$$ +\begin{align} +J(U) &= (H(Fx_{0}+GU+SW)-Y_{ref})^{T}Q(H(Fx_{0}+GU+SW)-Y_{ref})+(U-U_{ref})^{T}R(U-U_{ref}) \\ +& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Yref)^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10} +\end{align} +$$ + +This equation is a quadratic form of $U$ (i.e. $U^{T}AU+B^{T}U$) + +The coefficient matrix of the quadratic term of $U$, $G^{T}C^{T}QCG+R$ , is positive definite due to the positive and semi-positive definiteness requirement for $Q$ and $R$. Therefore, the cost function is a convex quadratic function in U, which can efficiently be solved by convex optimization. + +### Apply to vehicle path-following problem (nonlinear problem) + +Because the path-following problem with a kinematic vehicle model is nonlinear, we cannot directly use the linear MPC methods described in the preceding section. There are several ways to deal with a nonlinearity such as using the nonlinear optimization solver. Here, the linearization is applied to the nonlinear vehicle model along the reference trajectory, and consequently, the nonlinear model is converted into a linear time-varying model. + +For a nonlinear kinematic vehicle model, the discrete-time update equations are as follows: + +$$ +\begin{align} +x_{k+1} &= x_{k} + v\cos\theta_{k} \text{d}t \\ +y_{k+1} &= y_{k} + v\sin\theta_{k} \text{d}t \\ +\theta_{k+1} &= \theta_{k} + \frac{v\tan\delta_{k}}{L} \text{d}t \tag{11} \\ +\delta_{k+1} &= \delta_{k} - \tau^{-1}\left(\delta_{k}-\delta_{des}\right)\text{d}t +\end{align} +$$ + +![vehicle_kinematics](./image/vehicle_kinematics.png) + +The vehicle reference is the center of the rear axle and all states are measured at this point. The states, parameters, and control variables are shown in the following table. + +| Symbol | Represent | +| -------------- | ------------------------------------------------------------- | +| $v$ | Vehicle speed measured at the center of rear axle | +| $\theta$ | Yaw (heading angle) in global coordinate system | +| $\delta$ | Vehicle steering angle | +| $\delta_{des}$ | Vehicle target steering angle | +| $L$ | Vehicle wheelbase (distance between the rear and front axles) | +| $\tau$ | Time constant for the first order steering dynamics | + +We assume in this example that the MPC only generates the steering control, and the trajectory generator gives the vehicle speed along the trajectory. + +The kinematic vehicle model discrete update equations contain trigonometric functions; sin and cos, and the vehicle coordinates $x$, $y$, and yaw angles are global coordinates. In path tracking applications, it is common to reformulate the model in error dynamics to convert the control into a regulator problem in which the targets become zero (zero error). + +![vehicle_error_kinematics](./image/vehicle_error_kinematics.png) + +We make small angle assumptions for the following derivations of linear equations. Given the nonlinear dynamics and omitting the longitudinal coordinate $x$, the resulting set of equations become; + +$$ +\begin{align} +y_{k+1} &= y_{k} + v\sin\theta_{k} \text{d}t \\ +\theta_{k+1} &= \theta_{k} + \frac{v\tan\delta_{k}}{L} \text{d}t - \kappa_{r}v\cos\theta_{k}\text{d}t +\tag{12} \\ +\delta_{k+1} &= \delta_{k} - \tau^{-1}\left(\delta_{k}-\delta_{des}\right)\text{d}t +\end{align} +$$ + +Where $\kappa_{r}\left(s\right)$ is the curvature along the trajectory parametrized by the arc length. + +There are three expressions in the update equations that are subject to linear approximation: the lateral deviation (or lateral coordinate) $y$, the heading angle (or the heading angle error) $\theta$, and the steering $\delta$. We can make a small angle assumption on the heading angle $\theta$. + +In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackerman steering expression can be written as; + +$$ +\begin{align} +\delta_{r} = \arctan\left(L\kappa_{r}\right) +\end{align} +$$ + +When the vehicle is turning a path, its steer angle $\delta$ should be close to the value $\delta_{r}$. Therefore, $\delta$ can be expressed, + +$$ +\begin{align} +\delta = \delta_{r} + \Delta \delta, \Delta\delta \ll 1 +\end{align} +$$ + +Substituting this equation into equation (12), and approximate $\Delta\delta$ to be small. + +$$ +\begin{align} +\tan\delta &\simeq \tan\delta_{r} + \frac{\text{d}\tan\delta}{\text{d}\delta} \Biggm|_{\delta=\delta_{r}}\Delta\delta \\ +&= \tan \delta_{r} + \frac{1}{\cos^{2}\delta_{r}}\Delta\delta \\ +&= \tan \delta_{r} + \frac{1}{\cos^{2}\delta_{r}}\left(\delta-\delta_{r}\right) \\ +&= \tan \delta_{r} - \frac{\delta_{r}}{\cos^{2}\delta_{r}} + \frac{1}{\cos^{2}\delta_{r}}\delta +\end{align} +$$ + +Using this, $\theta_{k+1}$ can be expressed + +$$ +\begin{align} +\theta_{k+1} &= \theta_{k} + \frac{v\tan\delta_{k}}{L}\text{d}t - \kappa_{r}v\cos\delta_{k}\text{d}t \\ +&\simeq \theta_{k} + \frac{v}{L}\text{d}t\left(\tan\delta_{r} - \frac{\delta_{r}}{\cos^{2}\delta_{r}} + \frac{1}{\cos^{2}\delta_{r}}\delta_{k} \right) - \kappa_{r}v\text{d}t \\ +&= \theta_{k} + \frac{v}{L}\text{d}t\left(L\kappa_{r} - \frac{\delta_{r}}{\cos^{2}\delta_{r}} + \frac{1}{\cos^{2}\delta_{r}}\delta_{k} \right) - \kappa_{r}v\text{d}t \\ +&= \theta_{k} + \frac{v}{L}\frac{\text{d}t}{\cos^{2}\delta_{r}}\delta_{k} - \frac{v}{L}\frac{\delta_{r}\text{d}t}{\cos^{2}\delta_{r}} +\end{align} +$$ + +Finally, the linearized time-varying model equation becomes; + +$$ +\begin{align} +\begin{bmatrix} y_{k+1} \\ \theta_{k+1} \\ \delta_{k+1} \end{bmatrix} = \begin{bmatrix} 1 & v\text{d}t & 0 \\ 0 & 1 & \frac{v}{L}\frac{\text{d}t}{\cos^{2}\delta_{r}} \\ 0 & 0 & 1 - \tau^{-1}\text{d}t \end{bmatrix} \begin{bmatrix} y_{k} \\ \theta_{k} \\ \delta_{k} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ \tau^{-1}\text{d}t \end{bmatrix}\delta_{des} + \begin{bmatrix} 0 \\ -\frac{v}{L}\frac{\delta_{r}\text{d}t}{\cos^{2}\delta_{r}} \\ 0 \end{bmatrix} +\end{align} +$$ + +This equation has the same form as equation (1) of the linear MPC assumption, but the matrices $A$, $B$, and $w$ change depending on the coordinate transformation. To make this explicit, the entire equation is written as follows + +$$ +\begin{align} +x_{k+1} = A_{k}x_{k} + B_{k}u_{k}+w_{k} +\end{align} +$$ + +Comparing equation (1), $A \rightarrow A_{k}$. This means that the $A$ matrix is a linear approximation in the vicinity of the trajectory after $k$ steps (i.e., $k* \text{d}t$ seconds), and it can be obtained if the trajectory is known in advance. + +Using this equation, write down the update equation likewise (2) ~ (6) + +$$ +\begin{align} +\begin{bmatrix} + x_{1} \\ x_{2} \\ x_{3} \\ \vdots \\ x_{n} +\end{bmatrix} += \begin{bmatrix} + A_{1} \\ A_{1}A_{0} \\ A_{2}A_{1}A_{0} \\ \vdots \\ \prod_{i=0}^{n-1} A_{k} +\end{bmatrix} +x_{0} + +\begin{bmatrix} + B_{0} & 0 & \dots & & 0 \\ A_{1}B_{0} & B_{1} & 0 & \dots & 0 \\ A_{2}A_{1}B_{0} & A_{2}B_{1} & B_{2} & \dots & 0 \\ \vdots & \vdots & &\ddots & 0 \\ \prod_{i=1}^{n-1} A_{k}B_{0} & \prod_{i=2}^{n-1} A_{k}B_{1} & \dots & A_{n-1}B_{n-1} & B_{n-1} +\end{bmatrix} +\begin{bmatrix} + u_{0} \\ u_{1} \\ u_{2} \\ \vdots \\ u_{n-1} +\end{bmatrix} + +\begin{bmatrix} +I & 0 & \dots & & 0 \\ A_{1} & I & 0 & \dots & 0 \\ A_{2}A_{1} & A_{2} & I & \dots & 0 \\ \vdots & \vdots & &\ddots & 0 \\ \prod_{i=1}^{n-1} A_{k} & \prod_{i=2}^{n-1} A_{k} & \dots & A_{n-1} & I +\end{bmatrix} +\begin{bmatrix} + w_{0} \\ w_{1} \\ w_{2} \\ \vdots \\ w_{n-1} +\end{bmatrix} +\end{align} +$$ + +As it has the same form as equation (6), convex optimization is applicable for as much as the model in the former section. + +## The cost functions and constraints + +In this section, we give the details on how to set up the cost function and constraint conditions. + +### The cost function + +#### Weight for error and input + +MPC states and control weights appear in the cost function in a similar way as LQR (9). In the vehicle path following the problem described above, if C is the unit matrix, the output $y = x = \left[y, \theta, \delta\right]$. (To avoid confusion with the y-directional deviation, here $e$ is used for the lateral deviation.) + +As an example, let's determine the weight matrix $Q_{1}$ of the evaluation function for the number of prediction steps $n=2$ system as follows. + +$$ +\begin{align} +Q_{1} = \begin{bmatrix} q_{e} & 0 & 0 & 0 & 0& 0 \\ 0 & q_{\theta} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & q_{e} & 0 & 0 \\ 0 & 0 & 0 & 0 & q_{\theta} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} +\end{align} +$$ + +The first term in the cost function (9) with $n=2$, is shown as follow ($Y_{ref}$ is set to $0$) + +$$ +\begin{align} +q_{e}\left(e_{0}^{2} + e_{1}^{2} \right) + q_{\theta}\left(\theta_{0}^{2} + \theta_{1}^{2} \right) +\end{align} +$$ + +This shows that $q_{e}$ is the weight for the lateral error and $q$ is for the angular error. In this example, $q_{e}$ acts as the proportional - P gain and $q_{\theta}$ as the derivative - D gain for the lateral tracking error. The balance of these factors (including R) will be determined through actual experiments. + +#### Weight for non-diagonal term + +MPC can handle the non-diagonal term in its calculation (as long as the resulting matrix is positive definite). + +For instance, write $Q_{2}$ as follows for the $n=2$ system. + +$$ +\begin{align} +Q_{2} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & q_{d} & 0 & 0 & -q_{d} \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & -q_{d} & 0 & 0 & q_{d} \end{bmatrix} +\end{align} +$$ + +Expanding the first term of the evaluation function using $Q_{2}$ + +$$ +\begin{align} +q_{d}\left(\delta_{0}^{2} -2\delta_{0}\delta_{1} + \delta_{1}^{2} \right) = q_{d}\left( \delta_{0} - \delta_{1}\right)^{2} +\end{align} +$$ + +The value of $q_{d}$ is weighted by the amount of change in $\delta$, which will prevent the tire from moving quickly. By adding this section, the system can evaluate the balance between tracking accuracy and change of steering wheel angle. + +Since the weight matrix can be added linearly, the final weight can be set as $Q = Q_{1} + Q_{2}$. + +Furthermore, MPC optimizes over a period of time, the time-varying weight can be considered in the optimization. + +### Constraints + +#### Input constraint + +The main advantage of MPC controllers is the capability to deal with any state or input constraints. The constraints can be expressed as box constraints, such as "the tire angle must be within ±30 degrees", and can be put in the following form; + +$$ +\begin{align} +u_{min} < u < u_{max} +\end{align} +$$ + +The constraints must be linear and convex in the linear MPC applications. + +#### Constraints on the derivative of the input + +We can also put constraints on the input deviations. As the derivative of steering angle is $\dot{u}$, its box constraint is + +$$ +\begin{align} +\dot{u}_{min} < \dot{u} < \dot{u}_{max} +\end{align} +$$ + +Discretisizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex + +$$ +\begin{align} +\dot{u}_{min}\text{d}t < u_{k} - u_{k-1} < \dot{u}_{max}\text{d}t +\end{align} +$$ + +Along the prediction or control horizon, i.e for setting $n=3$ + +$$ +\begin{align} +\dot{u}_{min}\text{d}t < u_{1} - u_{0} < \dot{u}_{max}\text{d}t \\ +\dot{u}_{min}\text{d}t < u_{2} - u_{1} < \dot{u}_{max}\text{d}t +\end{align} +$$ + +and aligning the inequality signs + +$$ +\begin{align} +u_{1} - u_{0} &< \dot{u}_{max}\text{d}t \\ + +u_{1} + u_{0} &< -\dot{u}_{min}\text{d}t \\ +u_{2} - u_{1} &< \dot{u}_{max}\text{d}t \\ + +u_{2} + u_{1} &< - \dot{u}_{min}\text{d}t +\end{align} +$$ + +We can obtain a matrix expression for the resulting constraint equation in the form of + +$$ +\begin{align} +Ax \leq b +\end{align} +$$ + +Thus, putting this inequality to fit the form above, the constraints against $\dot{u}$ can be included at the first-order approximation level. + +$$ +\begin{align} +\begin{bmatrix} -1 & 1 & 0 \\ 1 & -1 & 0 \\ 0 & -1 & 1 \\ 0 & 1 & -1 \end{bmatrix}\begin{bmatrix} u_{0} \\ u_{1} \\ u_{2} \end{bmatrix} \leq \begin{bmatrix} \dot{u}_{max}\text{d}t \\ -\dot{u}_{min}\text{d}t \\ \dot{u}_{max}\text{d}t \\ -\dot{u}_{min}\text{d}t \end{bmatrix} +\end{align} +$$ From 1bf505638c35fbfb36f0675294c66d81e2dadd48 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Mon, 3 Jul 2023 05:00:30 +0900 Subject: [PATCH 11/12] style: fix typos in PlotJuggler config files (#3618) Signed-off-by: Kenji Miyake --- .../config/controller_monitor.xml | 12 ++++++------ .../config/error_rqt_multiplot.xml | 2 +- ..._pursuit_lateral_controller_plotjuggler_debug.xml | 10 +++++----- .../config/plot_juggler_trajectory_follower.xml | 8 ++++---- .../config/plot_juggler_obstacle_cruise_planner.xml | 10 +++++----- .../config/plot_juggler_adaptive_cruise.xml | 8 ++++---- .../config/plot_juggler_slow_down.xml | 8 ++++---- .../config/planning_validator_plotjugler_config.xml | 10 +++++----- .../config/processing_time_ms.xml | 8 ++++---- 9 files changed, 38 insertions(+), 38 deletions(-) diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/control_performance_analysis/config/controller_monitor.xml index c5189ccec4110..cb3f38e55e5f0 100644 --- a/control/control_performance_analysis/config/controller_monitor.xml +++ b/control/control_performance_analysis/config/controller_monitor.xml @@ -287,7 +287,7 @@ - + @@ -381,7 +381,7 @@ - + @@ -389,7 +389,7 @@ - + @@ -405,14 +405,14 @@ - + - + - + sum = 0 sum = sum + math.abs(value) diff --git a/control/control_performance_analysis/config/error_rqt_multiplot.xml b/control/control_performance_analysis/config/error_rqt_multiplot.xml index b80449fb9067e..8597cbdaa253e 100644 --- a/control/control_performance_analysis/config/error_rqt_multiplot.xml +++ b/control/control_performance_analysis/config/error_rqt_multiplot.xml @@ -726,7 +726,7 @@ true 30 - Measured and Computed Steerings Plot + Measured and Computed Steering Plot diff --git a/control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml b/control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml index 284380c654255..9e0fc652652c3 100644 --- a/control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml +++ b/control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml @@ -77,7 +77,7 @@ - + @@ -85,7 +85,7 @@ - + @@ -98,15 +98,15 @@ - + - + - + diff --git a/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml index 303ba94ddb399..6956db84aff56 100644 --- a/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml +++ b/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml @@ -243,13 +243,13 @@ - + - + @@ -263,11 +263,11 @@ - + - + diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml index b1a5275feda37..75500da8d2d49 100644 --- a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml +++ b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml @@ -145,13 +145,13 @@ - + - + @@ -167,15 +167,15 @@ - + - + - + diff --git a/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml b/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml index 5770820fbba1d..b7c37e5077c55 100644 --- a/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml +++ b/planning/obstacle_stop_planner/config/plot_juggler_adaptive_cruise.xml @@ -102,14 +102,14 @@ - + - + @@ -121,11 +121,11 @@ - + - + diff --git a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml index cbb840b12552a..92b56c2d2e6e4 100644 --- a/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml +++ b/planning/obstacle_stop_planner/config/plot_juggler_slow_down.xml @@ -57,13 +57,13 @@ - + - + @@ -81,11 +81,11 @@ - + - + diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml index d331ed9985dfc..d6afc0b017d32 100644 --- a/planning/planning_validator/config/planning_validator_plotjugler_config.xml +++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml @@ -120,13 +120,13 @@ - + - + @@ -140,15 +140,15 @@ - + - + - + diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml index 5da1ec3720fb0..569e2e34df670 100644 --- a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml +++ b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml @@ -29,7 +29,7 @@ - + @@ -37,7 +37,7 @@ - + @@ -53,11 +53,11 @@ - + - + From 679c94b1bde7a023eea8155314c5597602526e98 Mon Sep 17 00:00:00 2001 From: Ambroise Vincent Date: Sun, 2 Jul 2023 22:21:38 +0200 Subject: [PATCH 12/12] ci: add JSON Schema validation (#4122) * ci: add JSON Schema validation Leverage the json-schema-check action from the autoware-github-actions repository. Issue-Id: SCM-6366 Signed-off-by: Ambroise Vincent Change-Id: I5f284e96b2eddc652a6da8c0338f92c411277c04 * Update .github/workflows/json-schema-check.yaml Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --------- Signed-off-by: Ambroise Vincent Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .github/workflows/json-schema-check.yaml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .github/workflows/json-schema-check.yaml diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml new file mode 100644 index 0000000000000..3d0c0b83d8746 --- /dev/null +++ b/.github/workflows/json-schema-check.yaml @@ -0,0 +1,17 @@ +name: json-schema-check + +on: + pull_request: + paths: + - "**/schema/*.schema.json" + - "**/config/*.param.yaml" + +jobs: + json-schema-check: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v3 + + - name: Run json-schema-check + uses: autowarefoundation/autoware-github-actions/json-schema-check@v1