From 452e076d7659efe19a413da3716979a2c537cf28 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 26 Dec 2024 12:14:56 +0900 Subject: [PATCH 01/45] feat(autoware_overlay_rviz_plugin): add plugin to show string stamped (#9683) * feat(autoware_overlay_rviz_plugin): add plugin to show string stamped Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * better visualization Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix cpp check and typo Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 28 +++ .../README.md | 7 + .../package.xml | 30 +++ .../plugins/plugins_description.xml | 5 + .../src/jsk_overlay_utils.cpp | 225 +++++++++++++++++ .../src/jsk_overlay_utils.hpp | 143 +++++++++++ .../src/string_stamped_overlay_display.cpp | 237 ++++++++++++++++++ .../src/string_stamped_overlay_display.hpp | 110 ++++++++ 8 files changed, 785 insertions(+) create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp create mode 100644 visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..7ff4e19c69419 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_string_stamped_rviz_plugin) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugins_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md new file mode 100644 index 0000000000000..e5add261325f4 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md @@ -0,0 +1,7 @@ +# autoware_string_stamped_rviz_plugin + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +## Purpose + +This plugin displays the ROS message whose topic type is `autoware_internal_debug_msgs::msg::StringStamped` in rviz. diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000000000..c22bc75f55290 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + autoware_string_stamped_rviz_plugin + 0.39.0 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Takayuki Murooka + Satoshi Ota + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + autoware_internal_debug_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml new file mode 100644 index 0000000000000..302bcc629b892 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml @@ -0,0 +1,5 @@ + + + String stamped overlay plugin for the 3D view. + + diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..03fd8bca5aee8 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,225 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..e69abed49f371 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,143 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + /*virtual*/ void hide(); // remove "virtual" for cppcheck + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp new file mode 100644 index 0000000000000..d5746e99ff084 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp @@ -0,0 +1,237 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "string_stamped_overlay_display.hpp" + +#include "jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::string_stamped_rviz_plugin +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); + + property_last_diag_keep_time_ = new rviz_common::properties::FloatProperty( + "Time To Keep Last Diag", 1.0, "Time To Keep Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_keep_time_->setMin(0); + + property_last_diag_erase_time_ = new rviz_common::properties::FloatProperty( + "Time To Erase Last Diag", 2.0, "Time To Erase Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_erase_time_->setMin(0.001); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + { + std::lock_guard message_lock(mutex_); + if (!last_non_empty_msg_ptr_) { + return; + } + } + + // calculate text and alpha + const auto text_with_alpha = [&]() { + std::lock_guard message_lock(mutex_); + if (last_msg_text_.empty()) { + const auto current_time = context_->getRosNodeAbstraction().lock()->get_raw_node()->now(); + const auto duration = (current_time - last_non_empty_msg_ptr_->stamp).seconds(); + if ( + duration < + property_last_diag_keep_time_->getFloat() + property_last_diag_erase_time_->getFloat()) { + const int dynamic_alpha = static_cast(std::max( + (1.0 - std::max(duration - property_last_diag_keep_time_->getFloat(), 0.0) / + property_last_diag_erase_time_->getFloat()) * + 255, + 0.0)); + return std::make_pair(last_non_empty_msg_ptr_->data, dynamic_alpha); + } + } + return std::make_pair(last_msg_text_, 255); + }(); + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(text_with_alpha.second); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + text_with_alpha.first.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_text_ = msg_ptr->data; + + // keep the non empty last message for visualization + if (!msg_ptr->data.empty()) { + last_non_empty_msg_ptr_ = msg_ptr; + } + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace autoware::string_stamped_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::string_stamped_rviz_plugin::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp new file mode 100644 index 0000000000000..6f7cd84b91aaa --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp @@ -0,0 +1,110 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef STRING_STAMPED_OVERLAY_DISPLAY_HPP_ +#define STRING_STAMPED_OVERLAY_DISPLAY_HPP_ + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace autoware::string_stamped_rviz_plugin +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + rviz_common::properties::FloatProperty * property_last_diag_keep_time_; + rviz_common::properties::FloatProperty * property_last_diag_erase_time_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + std::string last_msg_text_; + autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr last_non_empty_msg_ptr_; +}; +} // namespace autoware::string_stamped_rviz_plugin + +#endif // STRING_STAMPED_OVERLAY_DISPLAY_HPP_ From ee93be1fe1ecb1721a3f4dab118d814d1263a193 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 26 Dec 2024 14:35:11 +0900 Subject: [PATCH 02/45] refactor(behavior_velocity_planner): independent of plugin packages (#9760) Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 3 +- .../behavior_velocity_planner}/node.hpp | 8 +- .../planner_manager.hpp | 6 +- .../behavior_velocity_planner/test_utils.hpp | 47 ++++++++ .../package.xml | 13 --- .../src/node.cpp | 2 +- .../src/planner_manager.cpp | 2 +- .../test_utils.cpp} | 108 ++++-------------- .../test/test_node_interface.cpp | 61 ++++++++++ 9 files changed, 144 insertions(+), 106 deletions(-) rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner/{src => include/autoware/behavior_velocity_planner}/node.hpp (96%) rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner/{src => include/autoware/behavior_velocity_planner}/planner_manager.hpp (91%) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp rename planning/behavior_velocity_planner/autoware_behavior_velocity_planner/{test/src/test_node_interface.cpp => src/test_utils.cpp} (50%) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt index 37a02b844dfe9..31900a23d00e5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -16,6 +16,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/node.cpp src/planner_manager.cpp + src/test_utils.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_lib @@ -34,7 +35,7 @@ endif() if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_node_interface.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp similarity index 96% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 4efb58e38b74f..4c1f10c355226 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "planner_manager.hpp" #include #include @@ -150,4 +150,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node }; } // namespace autoware::behavior_velocity_planner -#endif // NODE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp similarity index 91% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp index fddd658cef06e..9bd423ecfef21 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNER_MANAGER_HPP_ -#define PLANNER_MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ #include #include @@ -57,4 +57,4 @@ class BehaviorVelocityPlannerManager }; } // namespace autoware::behavior_velocity_planner -#endif // PLANNER_MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp new file mode 100644 index 0000000000000..f34182e77e6f1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_velocity_planner/node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +struct PluginInfo +{ + std::string module_name; // e.g. crosswalk + std::string plugin_name; // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin +}; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index e051374ed3dda..04b4cf0328fd1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -64,19 +64,6 @@ ament_cmake_ros ament_lint_auto - autoware_behavior_velocity_blind_spot_module - autoware_behavior_velocity_crosswalk_module - autoware_behavior_velocity_detection_area_module - autoware_behavior_velocity_intersection_module - autoware_behavior_velocity_no_drivable_lane_module - autoware_behavior_velocity_no_stopping_area_module - autoware_behavior_velocity_occlusion_spot_module - autoware_behavior_velocity_run_out_module - autoware_behavior_velocity_speed_bump_module - autoware_behavior_velocity_stop_line_module - autoware_behavior_velocity_traffic_light_module - autoware_behavior_velocity_virtual_traffic_light_module - autoware_behavior_velocity_walkway_module autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index d78bc883e6b35..5f78e4c670b49 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "node.hpp" +#include "autoware/behavior_velocity_planner/node.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 4820c340058ff..45ee83260d53a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planner_manager.hpp" +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp similarity index 50% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp index fe79450d0def8..ee1bb8f89fc46 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "node.hpp" +#include "autoware/behavior_velocity_planner/test_utils.hpp" #include #include #include -#include - -#include #include #include #include -using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - +namespace autoware::behavior_velocity_planner +{ std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); @@ -45,7 +41,8 @@ std::shared_ptr generateTestManager() return test_manager; } -std::shared_ptr generateNode() +std::shared_ptr generateNode( + const std::vector & plugin_info_vec) { auto node_options = rclcpp::NodeOptions{}; @@ -64,49 +61,29 @@ std::shared_ptr generateNode() return package_path + "/config/" + module + ".param.yaml"; }; - std::vector module_names; - module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); + std::vector plugin_names; + for (const auto & plugin_info : plugin_info_vec) { + plugin_names.emplace_back(plugin_info.plugin_name); + } std::vector params; - params.emplace_back("launch_modules", module_names); + params.emplace_back("launch_modules", plugin_names); params.emplace_back("is_simulation", false); node_options.parameter_overrides(params); - autoware::test_utils::updateNodeOptions( - node_options, - {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", - velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), - get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("no_drivable_lane")}); + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"}; + for (const auto & plugin_info : plugin_info_vec) { + yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light @@ -141,39 +118,4 @@ void publishMandatoryTopics( test_manager->publishOccupancyGrid( test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); } - -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) -{ - rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - - publishMandatoryTopics(test_manager, test_target_node); - - // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); - rclcpp::shutdown(); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) -{ - rclcpp::init(0, nullptr); - - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - publishMandatoryTopics(test_manager, test_target_node); - - // test for normal trajectory - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - - // make sure behavior_path_planner is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); - - rclcpp::shutdown(); -} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp new file mode 100644 index 0000000000000..6e232318c1711 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner/test_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner From ad16612cf12fa7122304eebb9f7c5112fa72cbaa Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 26 Dec 2024 14:45:37 +0900 Subject: [PATCH 03/45] chore(cppcheck): ignore examples/ dir (#9798) Signed-off-by: Mamoru Sobue --- .cppcheck_suppressions | 1 + 1 file changed, 1 insertion(+) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 5e1035de20c64..ab267ec3ac007 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,4 +1,5 @@ *:*/test/* +*:*/examples/* checkersReport missingInclude From 0997af503fbcb36a7d5679968eba18659030d144 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 26 Dec 2024 16:42:01 +0900 Subject: [PATCH 04/45] fix(autoware_planning_evaluator): rename lateral deviation metrics (#9801) * refactor(planning_evaluator): rename and add lateral trajectory displacement metrics Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../config/planning_evaluator.param.yaml | 3 ++- .../metrics/deviation_metrics.hpp | 2 +- .../planning_evaluator/metrics/metric.hpp | 16 ++++++++-------- .../metrics/stability_metrics.hpp | 2 +- .../src/metrics/deviation_metrics.cpp | 2 +- .../src/metrics/stability_metrics.cpp | 2 +- .../src/metrics_calculator.cpp | 10 +++++----- 7 files changed, 19 insertions(+), 18 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 73c1f3dfded09..7605ed2a5e859 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -14,7 +14,8 @@ - lateral_deviation - yaw_deviation - velocity_deviation - - trajectory_lateral_displacement + - lateral_trajectory_displacement_local + - lateral_trajectory_displacement_lookahead - stability - stability_frechet - obstacle_distance diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 0e08398ffa87e..2341ad2bb6ba3 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -45,7 +45,7 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector * @param [in] base_pose base pose * @return calculated statistics */ -Accumulator calcLateralTrajectoryDisplacement( +Accumulator calcLocalLateralTrajectoryDisplacement( const Trajectory & prev, const Trajectory & traj, const Pose & base_pose); /** diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp index 22b365881ce28..7c207bf6c8f57 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp @@ -37,10 +37,10 @@ enum class Metric { lateral_deviation, yaw_deviation, velocity_deviation, - lateral_trajectory_displacement, + lateral_trajectory_displacement_local, + lateral_trajectory_displacement_lookahead, stability, stability_frechet, - trajectory_lateral_displacement, obstacle_distance, obstacle_ttc, modified_goal_longitudinal_deviation, @@ -64,10 +64,10 @@ static const std::unordered_map str_to_metric = { {"lateral_deviation", Metric::lateral_deviation}, {"yaw_deviation", Metric::yaw_deviation}, {"velocity_deviation", Metric::velocity_deviation}, - {"lateral_trajectory_displacement", Metric::lateral_trajectory_displacement}, + {"lateral_trajectory_displacement_local", Metric::lateral_trajectory_displacement_local}, + {"lateral_trajectory_displacement_lookahead", Metric::lateral_trajectory_displacement_lookahead}, {"stability", Metric::stability}, {"stability_frechet", Metric::stability_frechet}, - {"trajectory_lateral_displacement", Metric::trajectory_lateral_displacement}, {"obstacle_distance", Metric::obstacle_distance}, {"obstacle_ttc", Metric::obstacle_ttc}, {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation}, @@ -86,10 +86,10 @@ static const std::unordered_map metric_to_str = { {Metric::lateral_deviation, "lateral_deviation"}, {Metric::yaw_deviation, "yaw_deviation"}, {Metric::velocity_deviation, "velocity_deviation"}, - {Metric::lateral_trajectory_displacement, "lateral_trajectory_displacement"}, + {Metric::lateral_trajectory_displacement_local, "lateral_trajectory_displacement_local"}, + {Metric::lateral_trajectory_displacement_lookahead, "lateral_trajectory_displacement_lookahead"}, {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, - {Metric::trajectory_lateral_displacement, "trajectory_lateral_displacement"}, {Metric::obstacle_distance, "obstacle_distance"}, {Metric::obstacle_ttc, "obstacle_ttc"}, {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"}, @@ -109,10 +109,10 @@ static const std::unordered_map metric_descriptions = { {Metric::lateral_deviation, "Lateral_deviation[m]"}, {Metric::yaw_deviation, "Yaw_deviation[rad]"}, {Metric::velocity_deviation, "Velocity_deviation[m/s]"}, - {Metric::lateral_trajectory_displacement, "Nearest Pose Lateral Deviation[m]"}, + {Metric::lateral_trajectory_displacement_local, "Nearest Pose Lateral Deviation[m]"}, + {Metric::lateral_trajectory_displacement_lookahead, "Lateral_Offset_Over_Distance_Ahead[m]"}, {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, - {Metric::trajectory_lateral_displacement, "Trajectory_lateral_displacement[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}, {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"}, diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index 69df00b26551b..1b46fbddfb297 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -56,7 +56,7 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto * @param [in] trajectory_eval_time_s time duration for trajectory evaluation in seconds * @return statistical accumulator containing the total lateral displacement */ -Accumulator calcTrajectoryLateralDisplacement( +Accumulator calcLookaheadLateralTrajectoryDisplacement( const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, const double trajectory_eval_time_s); diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index ffb56baf29f17..82ba86c65d6af 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -45,7 +45,7 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector return stat; } -Accumulator calcLateralTrajectoryDisplacement( +Accumulator calcLocalLateralTrajectoryDisplacement( const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose) { Accumulator stat; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index b99a4ebd20050..61e18a6ad0b63 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -98,7 +98,7 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto return stat; } -Accumulator calcTrajectoryLateralDisplacement( +Accumulator calcLookaheadLateralTrajectoryDisplacement( const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, const double trajectory_eval_time_s) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 201fbcba0e9f7..c30420a5632fa 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -50,8 +50,11 @@ std::optional> MetricsCalculator::calculate( return metrics::calcYawDeviation(reference_trajectory_, traj); case Metric::velocity_deviation: return metrics::calcVelocityDeviation(reference_trajectory_, traj); - case Metric::lateral_trajectory_displacement: - return metrics::calcLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + case Metric::lateral_trajectory_displacement_local: + return metrics::calcLocalLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + case Metric::lateral_trajectory_displacement_lookahead: + return metrics::calcLookaheadLateralTrajectoryDisplacement( + previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::stability_frechet: return metrics::calcFrechetDistance( metrics::utils::get_lookahead_trajectory( @@ -68,9 +71,6 @@ std::optional> MetricsCalculator::calculate( metrics::utils::get_lookahead_trajectory( traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); - case Metric::trajectory_lateral_displacement: - return metrics::calcTrajectoryLateralDisplacement( - previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::obstacle_distance: return metrics::calcDistanceToObstacle(dynamic_objects_, traj); case Metric::obstacle_ttc: From 231c341e4bac7061c477f1222eeddd526fbad18c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 26 Dec 2024 19:48:50 +0900 Subject: [PATCH 05/45] fix(behavior_path_planner): add freespace_planning_algorithms dependency (#9800) --- .../autoware_behavior_path_goal_planner_module/package.xml | 1 + .../autoware_behavior_path_start_planner_module/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 232c548b28da3..78c4ea7e4c609 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common autoware_bezier_sampler + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_test_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index c7cabb403f164..214731f96cebc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -22,6 +22,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_universe_utils From 4ed851f651777e7e241e791ff2cfd76deb3522ba Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 27 Dec 2024 10:16:22 +0900 Subject: [PATCH 06/45] refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (#9799) * split into planer_common and rtc_interface Signed-off-by: Takayuki Murooka * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp Co-authored-by: Mamoru Sobue * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp Co-authored-by: Mamoru Sobue * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Co-authored-by: Mamoru Sobue --- .../manager.hpp | 6 +- .../scene.hpp | 4 +- .../package.xml | 1 + .../src/manager.cpp | 2 +- .../src/scene.cpp | 2 +- .../package.xml | 1 + .../src/manager.cpp | 4 +- .../src/manager.hpp | 6 +- .../src/scene_crosswalk.cpp | 2 +- .../src/scene_crosswalk.hpp | 4 +- .../package.xml | 1 + .../src/manager.cpp | 9 +- .../src/manager.hpp | 6 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 4 +- .../package.xml | 1 + .../src/manager.cpp | 4 +- .../src/manager.hpp | 7 +- .../src/scene_intersection.cpp | 2 +- .../src/scene_intersection.hpp | 4 +- .../src/manager.hpp | 2 +- .../package.xml | 1 + .../src/manager.cpp | 9 +- .../src/manager.hpp | 6 +- .../src/scene_no_stopping_area.cpp | 2 +- .../src/scene_no_stopping_area.hpp | 4 +- .../src/manager.hpp | 2 +- .../README.md | 2 +- .../scene_module_interface.hpp | 229 ++++++++++------- .../package.xml | 1 - .../src/scene_module_interface.cpp | 238 +----------------- .../CMakeLists.txt | 11 + .../README.md | 3 + .../scene_module_interface_with_rtc.hpp | 154 ++++++++++++ .../package.xml | 38 +++ .../src/scene_module_interface_with_rtc.cpp | 140 +++++++++++ .../src/manager.hpp | 2 +- .../src/manager.hpp | 2 +- .../src/manager.hpp | 2 +- .../src/manager.hpp | 2 +- .../package.xml | 1 + .../src/manager.cpp | 4 +- .../src/manager.hpp | 6 +- .../src/scene.cpp | 2 +- .../src/scene.hpp | 4 +- .../src/manager.hpp | 2 +- .../src/manager.hpp | 2 +- 47 files changed, 562 insertions(+), 381 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index f296ad03cbac6..bf01986d5c28f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -42,8 +42,8 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 1bfa41d86ffbe..50ce8634ec600 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -16,8 +16,8 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #include -#include #include +#include #include #include @@ -59,7 +59,7 @@ struct Safe using BlindSpotDecision = std::variant; -class BlindSpotModule : public SceneModuleInterface +class BlindSpotModule : public SceneModuleInterfaceWithRTC { public: struct DebugData diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index a2ea4a82a884d..9739370387df7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index cc63b42df68e4..a1410d2fecfd5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -83,7 +83,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa } } -std::function &)> +std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7697786adb19d..7d1a68c5006a0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -41,7 +41,7 @@ BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), lane_id_(lane_id), planner_param_{planner_param}, turn_direction_(turn_direction), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 08bd8f91c7d71..ff01e85aa5456 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -22,6 +22,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_grid_map_utils autoware_internal_debug_msgs autoware_interpolation diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 8ba05be36ae56..cdaff7225a7d2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -216,7 +216,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) } } -std::function &)> +std::function &)> CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; @@ -233,7 +233,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } - return [crosswalk_id_set](const std::shared_ptr & scene_module) { + return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index d6f6ddfb7b6ad..091a427e14949 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -48,8 +48,8 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const PathWithLaneId & path) override; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 07b2f1deff0c7..64f7e9e14dcd1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -176,7 +176,7 @@ CrosswalkModule::CrosswalkModule( const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b3fbc2f6cfaba..d5a9e463c730b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,7 +17,7 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -112,7 +112,7 @@ double InterpolateMap( } } // namespace -class CrosswalkModule : public SceneModuleInterface +class CrosswalkModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index ff91cf40a32a6..90aaddf96feef 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -18,6 +18,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_planning_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 3f39696ff6bcc..62f5b88699a37 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -74,16 +74,17 @@ void DetectionAreaModuleManager::launchNewModules( } } -std::function &)> +std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [detection_area_id_set](const std::shared_ptr & scene_module) { - return detection_area_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [detection_area_id_set](const std::shared_ptr & scene_module) { + return detection_area_id_set.count(scene_module->getModuleId()) == 0; + }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 7aa60cbbaa18b..1fbcc16461ebc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 61b3b185999d8..6d7754624485c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule( const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..bdf2d05a35bcb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include -#include #include +#include #include #include @@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair; // front using PathIndexWithOffset = std::pair; // front index, offset using tier4_planning_msgs::msg::PathWithLaneId; -class DetectionAreaModule : public SceneModuleInterface +class DetectionAreaModule : public SceneModuleInterfaceWithRTC { public: enum class State { GO, STOP }; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 7a64d1d6638ff..208e7ed49de71 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 39ed8e80412a6..41bdbe3e43c8b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -353,14 +353,14 @@ void IntersectionModuleManager::launchNewModules( } } -std::function &)> +std::function &)> IntersectionModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [lane_set](const std::shared_ptr & scene_module) { + return [lane_set](const std::shared_ptr & scene_module) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto & associative_ids = intersection_module->getAssociativeIds(); for (const auto & lane : lane_set) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index 80c87e55c6696..c6f76d7c39640 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -47,8 +48,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -63,7 +64,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC tl_observation_pub_; }; -class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface<> { public: explicit MergeFromPrivateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 7c492a9dd42bf..e33416860c319 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -53,7 +53,7 @@ IntersectionModule::IntersectionModule( const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 164df7588bd68..6c31be2ce83b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,8 +22,8 @@ #include "object_manager.hpp" #include "result.hpp" -#include #include +#include #include #include @@ -46,7 +46,7 @@ namespace autoware::behavior_velocity_planner { -class IntersectionModule : public SceneModuleInterface +class IntersectionModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index 545af1576c6a8..bea068f5b9579 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class NoDrivableLaneModuleManager : public SceneModuleManagerInterface +class NoDrivableLaneModuleManager : public SceneModuleManagerInterface<> { public: explicit NoDrivableLaneModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 88fafeb5b90dc..4415eadef62c7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 9d66aa6672d36..dca2dde33b693 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -69,16 +69,17 @@ void NoStoppingAreaModuleManager::launchNewModules( } } -std::function &)> +std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { - return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; - }; + return + [no_stopping_area_id_set](const std::shared_ptr & scene_module) { + return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; + }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 7009e94612580..523cbba291632 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 3769aed71a1ec..5c79ec69a9d98 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -41,7 +41,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterfaceWithRTC(module_id, logger, clock), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 51f3a0d261ebd..1eafcf157623d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -17,9 +17,9 @@ #include "utils.hpp" -#include #include #include +#include #include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { -class NoStoppingAreaModule : public SceneModuleInterface +class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 32b1818214952..08c1516dea67a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -39,7 +39,7 @@ namespace autoware::behavior_velocity_planner { -class OcclusionSpotModuleManager : public SceneModuleManagerInterface +class OcclusionSpotModuleManager : public SceneModuleManagerInterface<> { public: explicit OcclusionSpotModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md index 2abbb83575af5..c4c8d97b4b390 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -1,3 +1,3 @@ # Behavior Velocity Planner Common -This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. +This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 4e898d9d28715..9dbed9009a93e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,12 +16,14 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include +#include #include #include +#include #include -#include #include #include +#include #include #include @@ -30,7 +32,6 @@ #include #include #include -#include #include #include @@ -54,14 +55,12 @@ using autoware::motion_utils::PlanningBehavior; using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; +using autoware::universe_utils::StopWatch; using autoware_internal_debug_msgs::msg::Float64Stamped; using builtin_interfaces::msg::Time; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -114,12 +113,6 @@ class SceneModuleInterface std::shared_ptr getTimeKeeper() { return time_keeper_; } - void setActivation(const bool activated) { activated_ = activated; } - void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } - bool isActivated() const { return activated_; } - bool isSafe() const { return safe_; } - double getDistance() const { return distance_; } - void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } @@ -127,10 +120,6 @@ class SceneModuleInterface protected: const int64_t module_id_; - bool activated_; - bool safe_; - bool rtc_enabled_; - double distance_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; @@ -139,16 +128,6 @@ class SceneModuleInterface std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; - void setSafe(const bool safe) - { - safe_ = safe; - if (!rtc_enabled_) { - syncActivation(); - } - } - void setDistance(const double distance) { distance_ = distance; } - void syncActivation() { setActivation(isSafe()); } - void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) @@ -160,10 +139,39 @@ class SceneModuleInterface const std::vector & points) const; }; +template class SceneModuleManagerInterface { public: - SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + pub_velocity_factor_ = node.create_publisher( + std::string("/planning/velocity_factors/") + module_name, 1); + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); + } virtual ~SceneModuleManagerInterface() = default; @@ -171,31 +179,111 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) + { + universe_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + scene_module->resetVelocityFactor(); + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path); + + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = scene_module->getVelocityFactor(); + if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } + + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + pub_velocity_factor_->publish(velocity_factor_array); + pub_infrastructure_commands_->publish(infrastructure_command_array); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + tier4_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); + } virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } + } bool isModuleRegistered(const int64_t module_id) { return registered_module_id_set_.count(module_id) != 0; } - void registerModule(const std::shared_ptr & scene_module); + void registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_DEBUG( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_module->setTimeKeeper(time_keeper_); + scene_modules_.insert(scene_module); + } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const + { + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); + } - std::set> scene_modules_; + std::set> scene_modules_; std::set registered_module_id_set_; std::shared_ptr planner_data_; @@ -220,66 +308,19 @@ class SceneModuleManagerInterface std::shared_ptr time_keeper_; }; - -class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface -{ -public: - SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - - void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; - -protected: - RTCInterface rtc_interface_; - std::unordered_map map_uuid_; - - ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; - - virtual void sendRTC(const Time & stamp); - - virtual void setActivation(); - - void updateRTCStatus( - const UUID & uuid, const bool safe, const uint8_t state, const double distance, - const Time & stamp) - { - rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); - } - - void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } - - void publishRTCStatus(const Time & stamp) - { - rtc_interface_.removeExpiredCooperateStatus(); - rtc_interface_.publishCooperateStatus(stamp); - } - - UUID getUUID(const int64_t & module_id) const; - - void generateUUID(const int64_t & module_id); - - void removeUUID(const int64_t & module_id); - - void publishObjectsOfInterestMarker(); - - void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) - { - bool enable_rtc = true; - - try { - enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") - ? false - : getOrDeclareParameter(node, param_name); - } catch (const std::exception & e) { - enable_rtc = getOrDeclareParameter(node, param_name); - } - - return enable_rtc; - } -}; - +extern template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner #endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index a0b54cb879cab..002c3362260cc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -29,7 +29,6 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler - autoware_rtc_interface autoware_universe_utils autoware_vehicle_info_utils autoware_velocity_smoother diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index cdfde1ce51205..ffd454012d13e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -13,29 +13,20 @@ // limitations under the License. #include -#include #include -#include -#include #include #include #include #include -#include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::StopWatch; - SceneModuleInterface::SceneModuleInterface( const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) : module_id_(module_id), - activated_(false), - safe_(false), - distance_(std::numeric_limits::lowest()), logger_(logger), clock_(clock), time_keeper_(std::shared_ptr()) @@ -50,222 +41,17 @@ size_t SceneModuleInterface::findEgoSegmentIndex( points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } -SceneModuleManagerInterface::SceneModuleManagerInterface( - rclcpp::Node & node, [[maybe_unused]] const char * module_name) -: node_(node), clock_(node.get_clock()), logger_(node.get_logger()) -{ - const auto ns = std::string("~/debug/") + module_name; - pub_debug_ = node.create_publisher(ns, 1); - if (!node.has_parameter("is_publish_debug_path")) { - is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); - } else { - is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); - } - if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 1); - } - pub_virtual_wall_ = node.create_publisher( - std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); - - processing_time_publisher_ = std::make_shared(&node, "~/debug"); - - pub_processing_time_detail_ = node.create_publisher( - "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); - - time_keeper_ = std::make_shared(pub_processing_time_detail_); -} - -size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const -{ - const auto & p = planner_data_; - return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); -} - -void SceneModuleManagerInterface::updateSceneModuleInstances( +template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - planner_data_ = planner_data; - - launchNewModules(path); - deleteExpiredModules(path); -} - -void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path) -{ - universe_utils::ScopedTimeTrack st( - "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); - StopWatch stop_watch; - stop_watch.tic("Total"); - visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - - for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); - scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path); - - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { - debug_marker_array.markers.push_back(marker); - } - - virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); - } - - pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); - pub_debug_->publish(debug_marker_array); - if (is_publish_debug_path_) { - tier4_planning_msgs::msg::PathWithLaneId debug_path; - debug_path.header = path->header; - debug_path.points = path->points; - pub_debug_path_->publish(debug_path); - } - pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); - processing_time_publisher_->publish( - std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); -} - -void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - -void SceneModuleManagerInterface::registerModule( - const std::shared_ptr & scene_module) -{ - RCLCPP_DEBUG( - logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); - registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); - scene_modules_.insert(scene_module); -} - -SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), - rtc_interface_(&node, module_name, enable_rtc), - objects_of_interest_marker_interface_(&node, module_name) -{ -} - -void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) -{ - setActivation(); - modifyPathVelocity(path); - sendRTC(path->header.stamp); - publishObjectsOfInterestMarker(); -} - -void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - const auto state = !scene_module->isActivated() && scene_module->isSafe() - ? State::WAITING_FOR_EXECUTION - : State::RUNNING; - updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); - } - publishRTCStatus(stamp); -} - -void SceneModuleManagerInterfaceWithRTC::setActivation() -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - scene_module->setActivation(rtc_interface_.isActivated(uuid)); - scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); - } -} - -UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const -{ - if (map_uuid_.count(module_id) == 0) { - const UUID uuid; - return uuid; - } - return map_uuid_.at(module_id); -} - -void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) -{ - map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); -} - -void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) -{ - const auto result = map_uuid_.erase(module_id); - if (result == 0) { - RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); - } -} - -void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() -{ - for (const auto & scene_module : scene_modules_) { - const auto objects = scene_module->getObjectsOfInterestData(); - for (const auto & obj : objects) { - objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); - } - scene_module->clearObjectsOfInterestData(); - } - - objects_of_interest_marker_interface_.publishMarkerArray(); -} - -void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - const UUID uuid = getUUID((*itr)->getModuleId()); - updateRTCStatus( - uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), - clock_->now()); - removeUUID((*itr)->getModuleId()); - registered_module_id_set_.erase((*itr)->getModuleId()); - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt new file mode 100644 index 0000000000000..2bd9b9b8d20f0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_rtc_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface_with_rtc.cpp +) + +ament_auto_package(INSTALL_TO_SHARE) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md new file mode 100644 index 0000000000000..79b5a4e3d7b95 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity RTC Interface + +This package provides a behavior velocity interface with RTC, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp new file mode 100644 index 0000000000000..4e30ab019aa4e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -0,0 +1,154 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// Debug +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +using autoware::rtc_interface::RTCInterface; +using autoware::universe_utils::getOrDeclareParameter; +using builtin_interfaces::msg::Time; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; +using unique_identifier_msgs::msg::UUID; + +class SceneModuleInterfaceWithRTC : public SceneModuleInterface +{ +public: + explicit SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + virtual ~SceneModuleInterfaceWithRTC() = default; + + void setActivation(const bool activated) { activated_ = activated; } + void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } + bool isActivated() const { return activated_; } + bool isSafe() const { return safe_; } + double getDistance() const { return distance_; } + +protected: + bool activated_; + bool safe_; + bool rtc_enabled_; + double distance_; + + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } + void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } +}; + +class SceneModuleManagerInterfaceWithRTC +: public SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); + + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; + +protected: + RTCInterface rtc_interface_; + std::unordered_map map_uuid_; + + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + + virtual void sendRTC(const Time & stamp); + + virtual void setActivation(); + + void updateRTCStatus( + const UUID & uuid, const bool safe, const uint8_t state, const double distance, + const Time & stamp) + { + rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); + } + + void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } + + void publishRTCStatus(const Time & stamp) + { + rtc_interface_.removeExpiredCooperateStatus(); + rtc_interface_.publishCooperateStatus(stamp); + } + + UUID getUUID(const int64_t & module_id) const; + + void generateUUID(const int64_t & module_id); + + void removeUUID(const int64_t & module_id); + + void publishObjectsOfInterestMarker(); + + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } +}; + +extern template size_t +SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void +SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml new file mode 100644 index 0000000000000..88b252106a90a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml @@ -0,0 +1,38 @@ + + + + autoware_behavior_velocity_rtc_interface + 0.40.0 + The autoware_behavior_velocity_rtc_interface package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + Fumiya Watanabe + + Apache License 2.0 + + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_motion_utils + autoware_planning_msgs + autoware_rtc_interface + autoware_universe_utils + rclcpp + rclcpp_components + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp new file mode 100644 index 0000000000000..abac509fd2b2b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + activated_(false), + safe_(false), + distance_(std::numeric_limits::lowest()) +{ +} + +SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) +{ +} + +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) +{ + setActivation(); + modifyPathVelocity(path); + sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); +} + +void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + const auto state = !scene_module->isActivated() && scene_module->isSafe() + ? State::WAITING_FOR_EXECUTION + : State::RUNNING; + updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); + } + publishRTCStatus(stamp); +} + +void SceneModuleManagerInterfaceWithRTC::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + scene_module->setActivation(rtc_interface_.isActivated(uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); + } +} + +UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const +{ + if (map_uuid_.count(module_id) == 0) { + const UUID uuid; + return uuid; + } + return map_uuid_.at(module_id); +} + +void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +{ + map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); +} + +void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) +{ + const auto result = map_uuid_.erase(module_id); + if (result == 0) { + RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); + } +} + +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + +void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + const UUID uuid = getUUID((*itr)->getModuleId()); + updateRTCStatus( + uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), + clock_->now()); + removeUUID((*itr)->getModuleId()); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } +} + +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp index 131ba32f32100..068ed81015fb1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -25,7 +25,7 @@ namespace autoware::behavior_velocity_planner { -class RunOutModuleManager : public SceneModuleManagerInterface +class RunOutModuleManager : public SceneModuleManagerInterface<> { public: explicit RunOutModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp index 950bb8471cc22..f98db8b88b7a9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class SpeedBumpModuleManager : public SceneModuleManagerInterface +class SpeedBumpModuleManager : public SceneModuleManagerInterface<> { public: explicit SpeedBumpModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index bef8a5eef4ac0..c746e2bf6a314 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; -class StopLineModuleManager : public SceneModuleManagerInterface +class StopLineModuleManager : public SceneModuleManagerInterface<> { public: explicit StopLineModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp index a7a0d83be6368..c5b9293fcdcc5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp @@ -38,7 +38,7 @@ namespace autoware::behavior_velocity_planner * @param node A reference to the ROS node. */ class TemplateModuleManager -: public autoware::behavior_velocity_planner::SceneModuleManagerInterface +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface<> { public: explicit TemplateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 082973d9431e5..ebc45d372f92d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index b6747724ba6f7..1b66824d04623 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -123,7 +123,7 @@ void TrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { @@ -131,7 +131,7 @@ TrafficLightModuleManager::getModuleExpiredFunction( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [this, lanelet_id_set]( - [[maybe_unused]] const std::shared_ptr & scene_module) { + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index eb8ef53ddb604..5ac32d1107880 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -42,8 +42,8 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 5eb0d5aa5f267..458d8e1588a5b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -44,7 +44,7 @@ TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(lane_id, logger, clock), +: SceneModuleInterfaceWithRTC(lane_id, logger, clock), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 8221bb3740273..3af13bc1927ce 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include #include -#include #include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -class TrafficLightModule : public SceneModuleInterface +class TrafficLightModule : public SceneModuleInterfaceWithRTC { public: using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index ba5a4b23a6fe0..8e0abc98c90de 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface +class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<> { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp index 85d4495914823..b323d6d201795 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -35,7 +35,7 @@ namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; -class WalkwayModuleManager : public SceneModuleManagerInterface +class WalkwayModuleManager : public SceneModuleManagerInterface<> { public: explicit WalkwayModuleManager(rclcpp::Node & node); From a313653f7b14a40dd0c938b96ea68f9ee1e2fe48 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 27 Dec 2024 10:22:15 +0900 Subject: [PATCH 07/45] feat(build_depends.repos): update version of autoware_internal_msgs (#9806) Signed-off-by: Takayuki Murooka --- build_depends.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build_depends.repos b/build_depends.repos index 7e547be7409cf..2313a9be487a6 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -32,7 +32,7 @@ repositories: core/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: 1.2.0 + version: 1.3.0 # universe universe/external/tier4_autoware_msgs: type: git From 4d1f69d793449c349d6a7befc79120ea6a9f8efa Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 27 Dec 2024 11:23:07 +0900 Subject: [PATCH 08/45] refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (#9777) Signed-off-by: kminoda --- .../universe_utils/ros/diagnostics_interface.hpp | 6 +++--- .../src/ros/diagnostics_interface.cpp | 16 ++++++++-------- .../test/src/ros/test_diagnostics_interface.cpp | 16 ++++++++-------- .../src/gyro_odometer_core.cpp | 2 +- .../src/gyro_odometer_core.hpp | 2 +- .../src/lidar_marker_localizer.cpp | 2 +- .../src/lidar_marker_localizer.hpp | 2 +- .../src/localization_error_monitor.cpp | 2 +- .../src/localization_error_monitor.hpp | 2 +- .../ndt_scan_matcher/map_update_module.hpp | 10 +++++----- .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 12 ++++++------ .../src/map_update_module.cpp | 8 ++++---- .../src/ndt_scan_matcher_core.cpp | 14 +++++++------- .../src/pose_initializer_core.cpp | 2 +- .../src/pose_initializer_core.hpp | 2 +- 15 files changed, 49 insertions(+), 49 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp index 120aed7c7548e..db370eb46ef43 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp @@ -24,10 +24,10 @@ namespace autoware::universe_utils { -class DiagnosticInterface +class DiagnosticsInterface { public: - DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template @@ -48,7 +48,7 @@ class DiagnosticInterface }; template -void DiagnosticInterface::add_key_value(const std::string & key, const T & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; diff --git a/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp index 978af27f202d4..e4d1ec8494113 100644 --- a/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp +++ b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp @@ -23,7 +23,7 @@ namespace autoware::universe_utils { -DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name) +DiagnosticsInterface::DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { diagnostics_pub_ = @@ -34,7 +34,7 @@ DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string diagnostics_status_msg_.hardware_id = node->get_name(); } -void DiagnosticInterface::clear() +void DiagnosticsInterface::clear() { diagnostics_status_msg_.values.clear(); diagnostics_status_msg_.values.shrink_to_fit(); @@ -43,7 +43,7 @@ void DiagnosticInterface::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -56,7 +56,7 @@ void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & k } } -void DiagnosticInterface::add_key_value(const std::string & key, const std::string & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -64,7 +64,7 @@ void DiagnosticInterface::add_key_value(const std::string & key, const std::stri add_key_value(key_value); } -void DiagnosticInterface::add_key_value(const std::string & key, bool value) +void DiagnosticsInterface::add_key_value(const std::string & key, bool value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -72,7 +72,7 @@ void DiagnosticInterface::add_key_value(const std::string & key, bool value) add_key_value(key_value); } -void DiagnosticInterface::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticsInterface::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -85,12 +85,12 @@ void DiagnosticInterface::update_level_and_message(const int8_t level, const std } } -void DiagnosticInterface::publish(const rclcpp::Time & publish_time_stamp) +void DiagnosticsInterface::publish(const rclcpp::Time & publish_time_stamp) { diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticInterface::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsInterface::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp index a0683694cc2b7..6ec4fccf78b43 100644 --- a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp +++ b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp @@ -25,9 +25,9 @@ #include #include -using autoware::universe_utils::DiagnosticInterface; +using autoware::universe_utils::DiagnosticsInterface; -class TestDiagnosticInterface : public ::testing::Test +class TestDiagnosticsInterface : public ::testing::Test { protected: void SetUp() override @@ -44,9 +44,9 @@ class TestDiagnosticInterface : public ::testing::Test * Test clear(): * Verify that calling clear() resets DiagnosticStatus values, level, and message. */ -TEST_F(TestDiagnosticInterface, ClearTest) +TEST_F(TestDiagnosticsInterface, ClearTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Add some key-value pairs and update level/message module.add_key_value("param1", 42); @@ -87,9 +87,9 @@ TEST_F(TestDiagnosticInterface, ClearTest) * Test add_key_value(): * Verify that adding the same key updates its value instead of adding a duplicate. */ -TEST_F(TestDiagnosticInterface, AddKeyValueTest) +TEST_F(TestDiagnosticsInterface, AddKeyValueTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Call the template version of add_key_value() with different types module.add_key_value("string_key", std::string("initial_value")); @@ -139,9 +139,9 @@ TEST_F(TestDiagnosticInterface, AddKeyValueTest) * Verify that the level is updated to the highest severity and * that messages are concatenated if level > OK. */ -TEST_F(TestDiagnosticInterface, UpdateLevelAndMessageTest) +TEST_F(TestDiagnosticsInterface, UpdateLevelAndMessageTest) { - DiagnosticInterface module(node_.get(), "test_diagnostic"); + DiagnosticsInterface module(node_.get(), "test_diagnostic"); // Initial status is level=OK(0), message="" module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index d52fb5e798b58..0a85f75b74ad7 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 70334738e9ce3..b59e6af341ec2 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index bc7dbbcfc9ca1..34fc61797231b 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -125,7 +125,7 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_listener_ = std::make_shared(*tf_buffer_, this, false); diagnostics_interface_.reset( - new autoware::universe_utils::DiagnosticInterface(this, "marker_detection_status")); + new autoware::universe_utils::DiagnosticsInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index 1b5672cff9d04..22a0c3f642563 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_interface_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 5ebcd105d57ba..1f26f6b80aa17 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -59,7 +59,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index b38958c420b19..b7d2454eb9f75 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index bf8fce2b302c0..12990259f88cd 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticInterface = autoware::universe_utils::DiagnosticInterface; +using DiagnosticsInterface = autoware::universe_utils::DiagnosticsInterface; class MapUpdateModule { @@ -63,19 +63,19 @@ class MapUpdateModule void callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); // Update the specified NDT bool update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 25b7417ffbe3c..e083e6751c3db 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -211,12 +211,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index eea0b8f3d06c4..299d596401b19 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -56,7 +56,7 @@ MapUpdateModule::MapUpdateModule( void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { // check is_activated diagnostics_ptr->add_key_value("is_activated", is_activated); @@ -87,7 +87,7 @@ void MapUpdateModule::callback_timer( bool MapUpdateModule::should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { last_update_position_mtx_.lock(); @@ -143,7 +143,7 @@ bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & positio void MapUpdateModule::update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); @@ -231,7 +231,7 @@ void MapUpdateModule::update_map( bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index cef8724423bed..d3e1fa5982867 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -51,7 +51,7 @@ using autoware::localization_util::pose_to_matrix4f; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; -using autoware::universe_utils::DiagnosticInterface; +using autoware::universe_utils::DiagnosticsInterface; tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -209,13 +209,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 67d5c447c09b6..5bde25a564f1d 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,7 +40,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( + diagnostics_pose_reliable_ = std::make_unique( this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 5305bcc3ad347..a0c1ed3a86576 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -60,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); From 8048b9ef8eb4efa1a6db0351ae6be7acdcc1e3ef Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 27 Dec 2024 12:03:00 +0900 Subject: [PATCH 09/45] fix(autoware_static_centerline_generator): fix bugprone-exception-escape (#9805) fix:bugprone-error Signed-off-by: kobayu858 --- .../src/main.cpp | 62 +++++++++++-------- 1 file changed, 35 insertions(+), 27 deletions(-) diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp index 811d57c6036ef..71a076ee86fe0 100644 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ b/planning/autoware_static_centerline_generator/src/main.cpp @@ -14,38 +14,46 @@ #include "static_centerline_generator_node.hpp" +#include #include #include int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - - // initialize node - rclcpp::NodeOptions node_options; - auto node = - std::make_shared( - node_options); - - // get ros parameter - const auto mode = node->declare_parameter("mode"); - - // process - if (mode == "AUTO") { - node->generate_centerline(); - node->validate(); - node->save_map(); - } else if (mode == "GUI") { - node->generate_centerline(); - } else if (mode == "VMB") { - // Do nothing - } else { - throw std::invalid_argument("The `mode` is invalid."); + try { + rclcpp::init(argc, argv); + + // initialize node + rclcpp::NodeOptions node_options; + auto node = + std::make_shared( + node_options); + + // get ros parameter + const auto mode = node->declare_parameter("mode"); + + // process + if (mode == "AUTO") { + node->generate_centerline(); + node->validate(); + node->save_map(); + } else if (mode == "GUI") { + node->generate_centerline(); + } else if (mode == "VMB") { + // Do nothing + } else { + throw std::invalid_argument("The `mode` is invalid."); + } + + // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local + rclcpp::spin(node); + rclcpp::shutdown(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return {}; } - - // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; } From 7e3a80c0987c507b789148d3df1073798df5b65d Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 27 Dec 2024 12:13:25 +0900 Subject: [PATCH 10/45] feat(lane_change): revise current lane objects filtering (#9785) * consider stopped front objects Signed-off-by: mohammad alqudah * simplify computation of dist to front current lane object Signed-off-by: mohammad alqudah * add flag to enable/disable keeping distance from front stopped vehicle Signed-off-by: mohammad alqudah * fix object filtering test Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../README.md | 1 + .../config/lane_change.param.yaml | 1 + .../structs/parameters.hpp | 1 + .../src/manager.cpp | 4 ++++ .../src/scene.cpp | 12 ++++------ .../src/utils/utils.cpp | 22 +++++++++---------- .../test/test_lane_change_scene.cpp | 5 ++--- 7 files changed, 23 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 86d8d1c0c1413..5a6c22a731bf9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -886,6 +886,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | +| `enable_stopped_vehicle_buffer` | [-] | bool | If true, will keep enough distance from current lane front stopped object to perform lane change when possible | true | | `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | | `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index bf892b3058a16..61e41b4ea3ea4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -5,6 +5,7 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] + enable_stopped_vehicle_buffer: true # turn signal min_length_for_turn_signal_activation: 10.0 # [m] diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 719bbbbf3057a..94020e8a08279 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -149,6 +149,7 @@ struct Parameters double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; double backward_length_from_intersection{5.0}; + bool enable_stopped_vehicle_buffer{false}; // parked vehicle double object_check_min_road_shoulder_width{0.5}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 1bb41069140e7..9f7a811770b5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -195,6 +195,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); + p.enable_stopped_vehicle_buffer = + getOrDeclareParameter(*node, parameter("enable_stopped_vehicle_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -305,6 +307,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorbackward_length_buffer_for_blocking_object); updateParam( parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); + updateParam( + parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 7f0e6d7f575f6..e2543c602cbfa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -515,7 +515,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; - if (filtered_objects_.current_lane.empty()) { + if ( + filtered_objects_.current_lane.empty() || + !lane_change_parameters_->enable_stopped_vehicle_buffer) { set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -948,8 +950,6 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - for (const auto & object : objects.objects) { auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; @@ -968,12 +968,8 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const continue; } - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - const auto is_moving = velocity_filter( - ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); - if ( - ahead_of_ego && is_moving && is_before_terminal && + ahead_of_ego && is_before_terminal && !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle filtered_objects.current_lane.push_back(ext_object); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 147ef3856ac4e..9360435891632 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -920,19 +920,17 @@ double get_min_dist_to_current_lanes_obj( continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); - - // ignore if the point is not on ego path - if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { - continue; - } - - const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); - min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + // check if object is on ego path + const auto obj_half_width = object.shape.dimensions.y / 2; + const auto obj_lat_dist_to_path = + std::abs(motion_utils::calcLateralOffset(path_points, object.initial_pose.position)) - + obj_half_width; + if (obj_lat_dist_to_path > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; } + + min_dist_to_obj = std::min(min_dist_to_obj, dist_to_obj); + break; } return min_dist_to_obj; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 114a38a77876c..5fb445788672c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -242,15 +242,14 @@ TEST_F(TestNormalLaneChange, testFilteredObjects) const auto & filtered_objects = get_filtered_objects(); - // Note: There's 1 stopping object in current lanes, however, it was filtered out. const auto filtered_size = filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() + filtered_objects.target_lane_trailing.size() + filtered_objects.others.size(); EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size()); - EXPECT_EQ(filtered_objects.current_lane.size(), 0); + EXPECT_EQ(filtered_objects.current_lane.size(), 1); EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2); EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0); - EXPECT_EQ(filtered_objects.others.size(), 2); + EXPECT_EQ(filtered_objects.others.size(), 1); } TEST_F(TestNormalLaneChange, testGetPathWhenValid) From f211728650c8abcf757fb8088c9b824780b736b6 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 27 Dec 2024 15:38:11 +0900 Subject: [PATCH 11/45] feat(lane_change): add text display for candidate path sampling metrics (#9810) * display candidate path sampling metrics on rviz Signed-off-by: mohammad alqudah * rename struct Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../structs/debug.hpp | 12 ++++++ .../src/scene.cpp | 8 +++- .../src/utils/markers.cpp | 42 +++++++++++++++++++ 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index 8738b412e18b9..90b13f86976b2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -25,10 +25,20 @@ #include #include +#include namespace autoware::behavior_path_planner::lane_change { using utils::path_safety_checker::CollisionCheckDebugMap; + +struct MetricsDebug +{ + LaneChangePhaseMetrics prep_metric; + std::vector lc_metrics; + double max_prepare_length; + double max_lane_changing_length; +}; + struct Debug { std::string module_type; @@ -41,6 +51,7 @@ struct Debug lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; + std::vector lane_change_metrics; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; @@ -64,6 +75,7 @@ struct Debug current_lanes.clear(); target_lanes.clear(); target_backward_lanes.clear(); + lane_change_metrics.clear(); collision_check_object_debug_lifetime = 0.0; distance_to_end_of_current_lane = std::numeric_limits::max(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index e2543c602cbfa..fb4f7aeca1525 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1061,14 +1061,20 @@ std::vector NormalLaneChange::get_lane_changing_metrics( }); const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; - return calculation::calc_shift_phase_metrics( + const auto lc_metrics = calculation::calc_shift_phase_metrics( common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, prep_metric.sampled_lon_accel, max_lane_changing_length); + + const auto max_prep_length = common_data_ptr_->transient_data.dist_to_terminal_start; + lane_change_debug_.lane_change_metrics.push_back( + {prep_metric, lc_metrics, max_prep_length, max_lane_changing_length}); + return lc_metrics; } bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.lane_change_metrics.clear(); if (!common_data_ptr_->is_lanes_available()) { RCLCPP_WARN(logger_, "lanes are not available. Not expected."); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 30af582175d14..da9ee52b038c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,6 +16,7 @@ #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" #include +#include #include #include @@ -185,6 +186,46 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg return marker_array; } +MarkerArray ShowLaneChangeMetricsInfo( + const Debug & debug_data, const geometry_msgs::msg::Pose & pose) +{ + MarkerArray marker_array; + if (debug_data.lane_change_metrics.empty()) { + return marker_array; + } + + auto text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); + text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); + + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<170}\n", ""); + const auto & p_m = metrics.prep_metric; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}\n", metrics.max_prepare_length); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto lc_m : metrics.lc_metrics) { + text_marker.text += + fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", lc_m.lat_accel) + + fmt::format("{:^21.3f}", lc_m.actual_lon_accel) + fmt::format("{:^12.3f}", lc_m.velocity) + + fmt::format("{:^15.3f}", lc_m.duration) + fmt::format("{:^15.3f}", lc_m.length) + + fmt::format("{:^17.3f}\n", metrics.max_lane_changing_length); + } + } + + marker_array.markers.push_back(text_marker); + return marker_array; +} + MarkerArray createDebugMarkerArray( const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) { @@ -212,6 +253,7 @@ MarkerArray createDebugMarkerArray( } add(showExecutionInfo(debug_data, ego_pose)); + add(ShowLaneChangeMetricsInfo(debug_data, ego_pose)); // lanes add(laneletsAsTriangleMarkerArray( From 5de2e4a3123c37390e83bd45a6e11e7755db56b8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 27 Dec 2024 16:35:02 +0900 Subject: [PATCH 12/45] feat(behavior_velocity_detection_area): use base class without RTC (#9802) * feat(behavior_velocity_detection_area): use base class without RTC Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/detection_area.param.yaml | 1 - .../package.xml | 1 - .../src/manager.cpp | 16 +++++----------- .../src/manager.hpp | 8 ++++---- .../src/scene.cpp | 13 ++++--------- .../src/scene.hpp | 4 ++-- 6 files changed, 15 insertions(+), 28 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml index b49b43794685c..9294795274066 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,5 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval suppress_pass_judge_when_stopping: false diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index 90aaddf96feef..ff91cf40a32a6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -18,7 +18,6 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common - autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_planning_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 62f5b88699a37..fb2c17d9e3745 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -34,8 +34,7 @@ using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) +: SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(DetectionAreaModuleManager::getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -66,25 +65,20 @@ void DetectionAreaModuleManager::launchNewModules( registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, logger_.get_child("detection_area_module"), clock_)); - generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, State::WAITING_FOR_EXECUTION, - std::numeric_limits::lowest(), path.header.stamp); } } } -std::function &)> +std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return - [detection_area_id_set](const std::shared_ptr & scene_module) { - return detection_area_id_set.count(scene_module->getModuleId()) == 0; - }; + return [detection_area_id_set](const std::shared_ptr & scene_module) { + return detection_area_id_set.count(scene_module->getModuleId()) == 0; + }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 1fbcc16461ebc..4b34ac4a45298 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC +class DetectionAreaModuleManager : public SceneModuleManagerInterface<> { public: explicit DetectionAreaModuleManager(rclcpp::Node & node); @@ -41,8 +41,8 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> - getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 6d7754624485c..031c45753022e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -37,7 +37,7 @@ DetectionAreaModule::DetectionAreaModule( const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), +: SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), @@ -105,12 +105,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) modified_stop_line_seg_idx = current_seg_idx; } - setDistance(stop_dist); - // Check state - setSafe(detection_area::can_clear_stop_state( - last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time)); - if (isActivated()) { + const bool is_safe = detection_area::can_clear_stop_state( + last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time); + if (is_safe) { last_obstacle_found_time_ = {}; if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) { state_ = State::GO; @@ -139,7 +137,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) dead_line_seg_idx); if (dist_from_ego_to_dead_line < 0.0) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); - setSafe(true); return true; } } @@ -152,7 +149,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) if ( state_ != State::STOP && dist_from_ego_to_stop < -planner_param_.distance_to_judge_over_stop_line) { - setSafe(true); return true; } @@ -169,7 +165,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) RCLCPP_WARN_THROTTLE( logger_, *clock_, std::chrono::milliseconds(1000).count(), "[detection_area] vehicle is over stop border"); - setSafe(true); return true; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index bdf2d05a35bcb..9224cf4624687 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include +#include #include -#include #include #include @@ -38,7 +38,7 @@ using PathIndexWithPoint2d = std::pair; // front using PathIndexWithOffset = std::pair; // front index, offset using tier4_planning_msgs::msg::PathWithLaneId; -class DetectionAreaModule : public SceneModuleInterfaceWithRTC +class DetectionAreaModule : public SceneModuleInterface { public: enum class State { GO, STOP }; From eaf5ad7a82a527a51775166b0f218f3804e43abd Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Fri, 27 Dec 2024 16:46:50 +0900 Subject: [PATCH 13/45] feat(autoware_crosswalk_traffic_light_estimator): overwrite invalid detection result (#9667) * add code in order to check invalid detection Signed-off-by: MasatoSaeki * style(pre-commit): autofix --------- Signed-off-by: MasatoSaeki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../node.hpp | 2 ++ .../src/node.cpp | 21 +++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp index 8efb90cc87c89..085e423db38ba 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -97,6 +97,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void removeDuplicateIds(TrafficSignalArray & signal_array) const; + bool isInvalidDetectionStatus(const TrafficSignal & signal) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp index b0ec4d0e5ffd0..d7cc6c725edfd 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -298,6 +298,14 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( if (valid_id2idx_map.count(id)) { size_t idx = valid_id2idx_map[id]; auto signal = msg.traffic_light_groups[idx]; + if (isInvalidDetectionStatus(signal)) { + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output.traffic_light_groups[idx].elements[0] = output_traffic_signal_element; + continue; + } updateFlashingState(signal); // check if it is flashing // update output msg according to flashing and current state output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal); @@ -314,6 +322,19 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( } } +bool CrosswalkTrafficLightEstimatorNode::isInvalidDetectionStatus( + const TrafficSignal & signal) const +{ + // check occlusion, backlight(shape is unknown) and no detection(shape is circle) + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence == 0.0) { + return true; + } + + return false; +} + void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) { const auto id = signal.traffic_light_group_id; From cd06f625c980e57fc21b9cbd246513f6512dae29 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 27 Dec 2024 08:01:44 +0000 Subject: [PATCH 14/45] chore: update CODEOWNERS (#9808) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions --- .github/CODEOWNERS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index cda3c8ceaca89..5bbc4d1c6177b 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -170,6 +170,7 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_m planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_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 planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp zhe.shen@tier4.jp @@ -234,6 +235,7 @@ vehicle/autoware_raw_vehicle_cmd_converter/** kosuke.takeuchi@tier4.jp kyoichi.s vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai +visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/** satoshi.ota@tier4.jp takayuki.murooka@tier4.jp visualization/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp visualization/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp visualization/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp From 75fde99c78aef0ea140154b183fcae05f1639784 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 27 Dec 2024 17:27:23 +0900 Subject: [PATCH 15/45] feat(lane_change): implement terminal lane change feature (#9592) * implement function to compute terminal lane change path Signed-off-by: mohammad alqudah * push terminal path to candidate paths if no other valid candidate path is found Signed-off-by: mohammad alqudah * use terminal path in LC interface planWaitingApproval function Signed-off-by: mohammad alqudah * set lane changing longitudinal accel to zero for terminal lc path Signed-off-by: mohammad alqudah * rename function Signed-off-by: mohammad alqudah * chore: rename codeowners file Signed-off-by: tomoya.kimura * remove unused member variable prev_approved_path_ Signed-off-by: mohammad alqudah * refactor stop point insertion for terminal lc path Signed-off-by: mohammad alqudah * add flag to enable/disable terminal path feature Signed-off-by: mohammad alqudah * update README Signed-off-by: mohammad alqudah * add parameter to configure stop point placement Signed-off-by: mohammad alqudah * compute terminal path only when near terminal start Signed-off-by: mohammad alqudah * add option to disable feature near goal Signed-off-by: mohammad alqudah * set default flag value to false Signed-off-by: mohammad alqudah * add documentation for terminal lane change path Signed-off-by: mohammad alqudah * ensure actual prepare duration is always above minimum prepare duration threshold Signed-off-by: mohammad alqudah * explicitly return std::nullopt Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * fix assignment Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix merge errors Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Signed-off-by: tomoya.kimura Co-authored-by: tomoya.kimura Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../README.md | 20 ++ .../config/lane_change.param.yaml | 6 + .../images/lane_change-no_terminal_path.png | Bin 0 -> 52805 bytes .../images/lane_change-terminal_path.png | Bin 0 -> 56568 bytes .../base_class.hpp | 7 +- .../interface.hpp | 2 - .../scene.hpp | 11 +- .../structs/data.hpp | 18 ++ .../structs/parameters.hpp | 8 + .../utils/calculation.hpp | 3 + .../utils/path.hpp | 2 +- .../src/interface.cpp | 10 +- .../src/manager.cpp | 7 + .../src/scene.cpp | 186 +++++++++++++++--- .../src/utils/calculation.cpp | 58 ++++-- .../src/utils/path.cpp | 4 +- 16 files changed, 283 insertions(+), 59 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5a6c22a731bf9..0508dc753a2e8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -874,6 +874,16 @@ endif @enduml ``` +## Terminal Lane Change Path + +Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and will be not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively: + +![no terminal path](./images/lane_change-no_terminal_path.png) + +![terminal path](./images/lane_change-terminal_path.png) + +Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. + ## Parameters ### Essential lane change parameters @@ -935,6 +945,16 @@ The following parameters are used to judge lane change completion. | `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | | `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | +### Terminal Lane Change Path + +The following parameters are used to configure terminal lane change path feature. + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | ---- | ---- | ------------------------------------------------------------------------- | ------------- | +| `terminal_path.enable` | [-] | bool | Flag to enable/disable terminal path feature | true | +| `terminal_path.disable_near_goal` | [-] | bool | Flag to disable terminal path feature if ego is near goal | true | +| `terminal_path.stop_at_boundary` | [-] | bool | If true, ego will stop at current lane boundary instead of middle of lane | false | + ### Collision checks #### Target Objects diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 61e41b4ea3ea4..91b296f8bd40f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -10,6 +10,12 @@ # turn signal min_length_for_turn_signal_activation: 10.0 # [m] + # terminal path + terminal_path: + enable: true + disable_near_goal: true + stop_at_boundary: false + # trajectory generation trajectory: max_prepare_duration: 4.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png new file mode 100644 index 0000000000000000000000000000000000000000..2c7b76e1749974c64c7422aa524ed6839e12059b GIT binary patch literal 52805 zcmeEv2S60p+BO)wF=|X=G`7TG7q^0o4VLXNvuv4}-7%JJY%lIEY_Xux#KhR6V#Thp z61$>^iHTjX_Zme|M2!XP{_pHUL}Tu~|Mz|Oe*e8cDY|p!%qj1A`-cL{XJp^#;`iUuLPINW*`jOBr~)lU^8Q*nK9HONg^eZ9K}e4-!`+= z0DlP$I;)d-9pN)8R1aDpswcqQ>oZ`d+V=>aq5;Ls;fiv^bsrvo10n!I*f#mzqm2fWKobvnEiTta76 zTqinh&M>z#X`z3h+~ajSt%ld`2n1ZJukfA9#RNVJbuR!hhyJK5oehSd(S`;}U29=< z6_?cWUX9ym@TM5tX1l>_a4TFIo!McmcouYxtxBhQwxNff8m8TuUZn}3u{)rm9*Jf% zIB+{upbizVV%e1iJW7bd6 zM{{uTW^3WqGU(rs4UaR;t*aE5x3Em8=x_7wEAAB1+<@VRnyj^Awe+!b&|> z*RC| z?0@zQ{%1(U1eZ<_7N~zjctX{|PpVW@fBTt=+94bN_iFyh6{ujb^uJ+$!K%6_O=`A> zJRmMOm|ipZJ;@ra!It9mn7v_lQ|t74op!J&HtGgfqq7>_&NPR<(#q)#shTt!xGR6O zkYF<#DM!`obU~@c;|lv_sb)WA@42B46Dmt7gH|yFo?heCL@^V>mkfa?-5AB-`oRxi zB&LW^1{vV8w1huR7j&_-LR*?9NHc3ha+;3kOiyO&nfd^mi3Hf`I=e0%QCitj1t(lD zAT}C=bdT17aKv^KO)pA_Ne*z}30;~#h@@$m5=U}S>_d17={hFq5SzIS4S_Rdb`C2= zA@+%R2}VT8XD3@^3^7mUgZnO$M>8vh0ye3Vq#|Z9j59?jw~-En`XxeX2EuPkF|#dN z5#mKuUJH!Zm#pHN^g?5dUM2A)C8D7+olrp2BwAxr6%}pMmG)B->GsEsS;vhn(0|?wp zN#q3eBAbU)vU)%}E}bAW7Ndrc0ZmYkM#c51RdT1;!s?;3;}(4)*GUpKheibVEulFx zHH4g|fq4Th=*bqoALu~QCk<7IjnJk^$E4^R8bd&A2K&i{UPH6Qs9Ew@J(3xJ&`78Y z_idA;s|k7V?fvr2Vo`XG&`&Vgg2A;F#zUVO}@G(ybDL6}E~ zwp?q9LgGb~3AAL3)q_|P(on+aMFXq=qEdSi!pKavi2ZttK%-UA{AePJhA4bKO(HE^ zZl?J)JY5=M5ik(kLIC<@hw6sL^(T(5oMj@%kz(v&?l}=z)-}xW$WdB>XiB&ZJVlX6Zkb1HoMw3@%|%h6P?bk zNJdu#(mg8d(Ero6sba|0aZ@P1s7Qwy7c~T5X%E zZBunluR5pqMy|6uZu$?2o0vUb-yi(9h?^?%kkz)S+BQ|&rfS<%ZJYih_nYW%+9vwH z#fDSGn_Sh#soFSI8>edHRBfF8GmX=mNxgp$<5ZD|tTs;7#;MvkRU4;jKwocX7soFYKTc>L4^q*;+XskDm9sXP7_24A_-#ukqeah)?oN|KGRK6G; zoV8XnFpk*44Uj6-rgM-sI5`5Rg(T@32Ii$KA4#~6;3Qb+1enVSrDQ2R6 zCo3@ABgX{8-wJbZkYp@%Nbslo=-Px zXmXp3L2^-=z$V9Vja-Ryl{A4^!qI6uA+oUy3NN7u3M>R)&eYR57LqAulMd2tuzMwh&F0ZENdysb z6O|aACKt)va2hQM7YXcWQ0La!FqTe8isg2Cq7-wvwLE7)vdR4WEz2hsZf$!1&+97b^(H7 z>>$bzNHj_v6V`&Iut}^7LybexNsdqZY41n7ZJH6YNaGyMz@C1ZRW%D2+IijYqQN(JK z>I6Il6%tekWPU_I+6*|FrWLwuu(nQw zZZc~GYM%}cz{zNw6kMsNO9g6%hR^ZnY&M~m&-crf61R@2^Ai{?sHAiK8iGxzVVxD2 z#}8*O2|VBp=xt~WNheKmOpvZnaRY=>z$FM@P@(X+Nud`dMF?G~#8nDdStZkj$}wvI zwd2V;n!uy8$dhow?@%zrY)r*9>ufG1jyc^joa<2F+!UEjLNnO$L|o)_YitsYh7dC` zCMif}y3$Ann<*E{d4x*LLUCM;VJ5no&PDVLnOkZ1r>QYc49pAU$4S%EF`r(9%C!!3 zuz_%)Bwgm#(O7;Aj)Ex(#4X269=XFMM=hjTO_!K48a%`I%hb5btstP3Z%WY;Iwqks ziIpmS3~mv~Gz>wI;Bkeho#RpRNL1(GYH&M;i3+_YoKLEh7ThNz{7wx)^AiqIt;9&0 zo~F0r47vcf=z=N^au@e=oLyUmX0#~PLl5;QLBlO6!1!=Dw|Bl*DKWyu2_jn zLJd&-q+F~>g2~LW*Owgk~ zC5{J_K{6xfXpClEcBCSY(NclPzAtD`c zYLzCpvXfAi#E%9|iMWDemeJTEB~xOS1x-mhLgEFTA=BA87DR|TNR>;C+u2DY@J;-rQlM?l6S zq|A!Z6g<*|1?5VmjZ8DB^m45d_243tLv59)byhVERq6*z2`gJobFn~PXfm3>MA+GC zTp$MCchWG7?N#ywX(XQD)>#BDDV(M@m^mJ>B{4}%xf;hzb`75&P|($EIh^2@F`Ypo zk&}e*$ryFeq|{ir46R7dMj199>5y2GX`r74E<3@usWFC;1@mT6wi2hC6e!_Mms#`# zNtgtn-z_KsGzm(0dJC>m(!3_UzzTLxl8D>&K1_&{B=`&3AmYXtbcF$v2bByzUq%S5 zm`Ea3+sGus!A;Z(a5>ILZDdfd#2FH|nn1-ehbu|Oz;P_7=b-_P6`>1~6I8?z*o}cV#K1Rfj%(H6TJ?^VpGHEc3(2hm55s4=bS zK^R~IZPs*@k1wQV!2SFG^juZEpkGJ>t#}0!X*S;vqC9LPhyhb0k{y%ViKw? z`k;;<9={&+F)Bchz>66+}6L}g0R;c|sR$O@7S z@NEcsiX1luquA3%eoS+CM_Nh`tx+wE3c1x&fXrb2~u1sO!W zU{`~3yECXzngTMF-ltRAthi9-mQg+$Q|?eRa3;yK`jcp=h_EFDpz^L}XCz#W1;7BQVLZ07rrmCRH-iNk{lH@XK%w zN*8EQJMghcpGJUQRuXKkQlJ;e3A$8I05XFBYzV=YXqh_D_Y#(p#&Kexalv14%9&y& zDzKT9e2h-u0te`F@L5f2%I8GE579C83MoNX;#OOV6hm2Bo1UjpN_=`@52haMGRD)h zbSjfgE|9DB43imbK245s?vZ6HtRsgR5K)J?J(aF7fNF5;tM91~HXDgsZrS0mB`*Lmrbv!w1V5s6)@>sxk0E zHFo)6HB+Akv4K;H<0&McgqVXQA=XgfbUjyr`RK_s4-;|VLOEUF0RK&`qnidxRdSmK z>>x(#K?j*LX<`1jO(50_y)2M_iP9lRl49U9*gR0I!z2F(? zgD6h`{+vD;2Y?0*W)dX@1>o`FbTw{uD`|RxhNpMR>^4Hq<4Tkw9G-Vs6+C>f&Y^cI zMXsRE%yH-luLiKw*QiXbe_BNc(r-Kg*x*G+9%2pvN zGN2RTTm#<;V-WkLd_DvGBDDg8wX_Qor9wJSZ9|o$ikkxb5F{-&8rX5V-U=q1&!Wo- zw!`26x#Xjyo&esm;w(7fZ;^ssQrJOXIE+52&}#-;?L|Z;narVg$q5#n;F%n7A2FyB z(zQ%^5&$s33Hs2dBwWd$Bi)#VYbNOuw?csU3^oa63w(0A zUJS7v$UDx!tZX)BC!HwaOUH2u2Ju*oj)%FmnBK1d-(Eq-*$|hcp;op?VP(0K85|d> zw8=1$Kn?t+AqjdCPUo^f$E8RKL7Lu<$_XLM4?5k1Gf}1<;yscWE>{&{q~yI>rZ`MR0&OXz4mkt^qzI6ySf# z7;1MC%_~#z0Z##Jk=`x`KSytaxYI8S>cNL3({v7m0kM}|Ddg~>9}`YHNH5+HiJU#4Z+*fJW7 zi)Lji1#+HVXmS{=Uie>-qEm?j1n3owz)Htel5~Hmf{{uy7lihKsle!UMTCds!N#2?xF% zoWw*;GP_=+wqiaVAvZ%@o-U&q!AF+Jl}s*AZwEU?=;(5Rk%eP=hL&j}G&GQ-ph*G# z7lB$OiF$!ah=VQz`>8}|R2(4>l1kJEK4Va6cRA%EJpy!R6IMB@g_H#a4n!rl5V%PQG}Y*Y5*%VsyNuGExWt_t1dYP*K)g?b z7*L*~U?%vJL$Q(%-hsVxdQT1E-fI_XziRd63|nsi|^;&_&!%FCgKC;6S_4dr){IZ{m;W&2E5Q3Rm$ND7@2u z2_y-f#RBlOD)?!finlMT9Q%%`Kp&}!NCGH`p{M>=5J~72L;hRaoK^5B|1MINrUC#< z2e6J5Ak1GuXGJlXsiA+MN@b(3gJ*d{D3(M8JR)H9y$#ngF%d9F;Z7yKD0QLNcuXO5 z&5AoAs4u%8fNou;XtUE38*K)T@kFQEGyq-sC+?|8tp9V-YnT;CFqKGql{`>|RRH2M z#1^ly%Ks*K4ZRv!tr}Ua8d2l=mdr4tj3urnRT*Hnb-07fTUE5s-S#0uO_K`aTo zoLZhMP0L{0q>vAQsK!kyofgER3jhKl{f{yQ-K&9&jLuH&cH3xD3*RSY(kQ&a3S7}w z<9dat`Nybveur3SqfjqBkhinxEPonl80j-yC-fVIxPwA8)dC(P;2_eFAU}vI`G9gs zpo2N;=3*#CsxGnlIvXvhNkvMc6FlPYx#GyvshSeIY|9p+Baz$aA#-IY3{ zGBE-0Gg+{HVjqk-jFbvlbg5Ei1Z+)MC&Vx1CBT@))OcXCRL6$)0I!n|_Y!EKew0>f z4AffCDc*oOKu06+lgo5MwLRY7=~<9sad=E8jhd3B}CHKd!wM zv2qN>AJQ<&tydd*f5k>KD4ihB6zc3NNeJO|+Qm$hNg7Bnrc`3i+WjU2>{o|zd;h2n zQk){Hf{N=1{bquW2qDPAx>3llbSaM?kScuCyn(k9;M!^g9-w3(h){e*(Ga8=wWt|l zA;qh{(0eW+I#K*=27abw!vefd;hs|RWC1yWwF5Z-+9trWzW>NKz+04T0Y6!+K>q{=;HiQv=wO|w=fiRX>&HZRYBt5=A!J%=?P2XG*#Q0obYOTk zk*f*6(?cNy)68K_<+PAI0ktCbx6%3!ZKZreqbcYWNWo5O{1g)JA1fdRO z-#nvPETm9~Y2op~{wwe#$Sxl+T0v%%2=G0H-irX=T4B{f^8)^}QZh;LCM6pzkkznm z0(p}vjUZ=$!fT;Woz);4Kw~Ha{tbr8m1-8s(*>z=C}YC@5@5fAEb@SMNq$MU}h}X;i0HaNv#f~7&HQKH}$p_tSAkhVIct@tO{s8i<%A;HUcFmj%S00 zquK-M5Q7GWT`Y3~U^VMNqky&mtwL!MDM$fG5m*mCXfrElC>;$}m3qbxnh~so`0X0v zUqwMbm4BH!-8JOCqB0(Nj%RzU_qZv&1n@Mi$!!A=tBW$GCz#kcV8 z8T5^bfkKEsYzNg~0}?>TQ1(Cu{b96_ESp)t z*VL-W!nzvpjzOQoeWM53Wdr|%Z8L)21$1iA+o)2_09`}bM&NHN6ZEqt~x&W0&4RIR}d_$mTs0@6ZH_ED@oCDIknfnLIIn2ERZ5$*8e8{lx_%^ih zx9GTV92nMd)Nh^`+LEy0{YFaCSTG(+pHO8Kg8J)aLX%BH=$-ly(z+Vzyh1=nC`3jo zKO9Cqr=ATXp$k#)_0S;rd|||S#jCtA=v>M>{gcl}<(eyaoRWAzJ_m^gJ_3FOO(CX* zZIP80vSE~@Q+`+?@UMcx$~Mx&_hsPc$iRMCL;8>67foUU%*SX{2Ppmpbal$t3jnVN zDBnp82>KZ1BT#a|lq$HM5dOCXq2$5$79{yMZTj1F9-wpoVWlZ)0_%Z*HV4a0NiSGn z&@)wM0@nzje^cHeczb~P4xS7(>yQWccVG4*aOg>nyigDR(FSO$Dy^uziG{z01kxhGm> zPe3E8b?|7R^)SYMsAZrdBh0B3KKy^PxA6F(hu3=p*#fGA{idu|#r!Jg&8OxK)D8Lh zV8Q-`$^JIp`13p&mGgX^q6oAMo@ywYNl6ve8-?2+`VNGTiW&_(s_gZ8e?qwbYmYPN z3w=!@h59s97$d&wo{T{g+LL*6+vZ=9c&F3efaaw$V=BKKS0(q(s@SikA^exo|S0am9vzfqmvsLpRx=QrN&>sRMDs5gmTWsF|SivB|xqU!uc zb$-KKo!_7$_t&xl)%gu7OH-ZSc>P<3|5%oxI==xg9aeeg^nWwI!C+V5QCGfM$YlRp zPLeBO=;H!0Iaf14=K{8Tz4N$IIW_)cfYg zmGc|($JT7HxBjLsKdpNib>swgN)XZiL!SB^me^#fYEraF)_Cn5@^Vhjpj-A|Gqg+8 zKOXT;p7MioNnLr3@)xa{XFlcZo5?X=pTqI=xgRY2kmbp`H@rP_=AA%3Gp+2_nf1cc zjpHLCYrgf5yQ=x`MQ+%$@PjcCHR`q<@s>XZ)vP<8)A+3qQ>_kd(so4ISJSvn{;G@Q zhzs|pBpLE=oiV+&{ZOBcWo^53NlGo4S0ot|arVPzJ=?u)C{T8zQ)5p=uIUO_y~VX) z>a^)gm$l3lM2sBWK%bf_2?PQ^mn`AdY%G#S=5uavZa-)-QeV%NFP`4(;b)VwYRvH* z^_;)KuCecZ)9Jgh?h|Zc+lnmq{QiU2GXG-oBSt79YnYbFZ~Y~CE=xxI%>mc#@nPE$ z>79Qa{&woZl-2-&D~Av-srT+D?eFFH91$UqTME;uc5fO2dc5xTIv7$$XP(Ez; ztgO!{DtCyFm^)mDPJClkYedCIwEE%7e0r#hton`PhwK+?-t2D+HRCS3Uxdn+w}YnQ zbo`_@n&NhXruJ)=C_=RaBO+$K6Zr;@j$8~KbeJ}8f9TPn&}!MV%%9(Ar|~!Kp&{g( zc_EHjye1;jdvijoZo=1TR`)9?@A&m(G(-d52W>}O-}ySjYCr1Oy?f?~mBfLuA9m57 z?C0F~WvvF+W@qP3FFm(;&ll~xKNvWF`12?3(^tkE7`&%dhh_7I=d?^et#P~7r%stU z^F~Ht6HD6>X#JkkC<4~~oCoCm^q}&~suFdjX~!Pl`v&;n}IP_lKT+zcZ{vilcL471E46 z{?Vht%z=xGw-)Jt8(r_-{er%;^N+9YN5pw=bzGJeTygE%wY^P*tveyJSmM$NCyzc^ zbavC~XE%OZlB8HSukVXTi`1PBFP=TV_u}cD1I^}TMECUFW(_kRo43;2X>Bpr*O;FiJ7Zb zbv>fqptdlV1LpKc?nLMY5Y)Ai12cP{QTO@ElZROh=D4N$j?dI{{ zhl%RVAJ$=MjZlY+De{fa8v9y@8*83iHK1FwIT<^OZ>cYw-C%BawL>y({k*1IZ%kv+ST-RN+n&Zq5z{We-K!?e}pMv3>~nwu;8 zWoNYUm0V#G^{)g%RV=^ z-uw$=*Y*u*i<__j%kou6AS~~qmsLSLhi6; z_9{EAyZ2jz?t5pktq0E@%kkEn-E-uyvNOW6OBosadY7_poZFn;(t9|eNrS_8IWK>1 zz`x`@T)JkhE~;au`{QV&{QFkfm z589ru@J}h>uCQbAgKEBbekbF=p;5K>{hYmH%I@)>9Jp|G4@hSJq4s(C!7P?}O0W9| znd<__mhR7aujext=lO$UhurUX>$hp@^H11gKc1gso)dlJ_uJd<%-T5d)aw4$)EABm zIVV=pTbZBUST?Wo%gw>hb;ri^jFFu*O&Gd*ox9h>2nB-GavwCR0+ijWd};VLVrEXX z``A+C5V@~;pYHVcrA9Aj{P@1lKX0)xyZ^TI?1dA0{8snIx$Jp!(x2Q&{dU5N*?o4( zGEYBxbaf(9yoUKho9E{)m66B#8AmqCxzT zE7p24`F6{=`9Y^r=h`o`6<}`af9c7_}XApBY8CsgCUw)vSzEG z&!fxlTs?gIheg*GjA=b3aaL^kLH;S`Z6c-ZN+LVv%KbJu-pN0G_0^QQb4%5&_C<@@ ze9S3Zvu)g>uMZ!3kHc&2za07a!V#g+cJ?$~ckSCfZDRXAz1rNkcl5VS4~=S=)@}q^ zW|D_BYY*__cF`w1s8Lnh4{a*qMXWAqQl4{b-TtW0voe0?-`|uWVwe0ldLhC{R1?nmH$ zEZ+Bhms4la>k|KNw7?6XA}XHAQOd>~!?m$epNxOC#d{layJWJ!yrIlJq> z5|+<$-1Hu=m(GQjxu5@7dLnS|*oZpF#GCWd=d^s3lDB2ZvW(ci(R*gQz2b1N7scQIwr1CWogSuff9d86sV|7DyT=!1WDP&Ns=Vb%WK>yJX8-n|#5!30 zTlRF!`NDtYs}ng-pO;@cfPjpGTh;k#X+RJoRZi+BB6 zu&#gWtXr*@jhH>3d3Fc(&V^ZJ*)5I=&UTmgXu0}Dk4}D0NE?o=d#n6r4RfDCIp)&o z$A)(>G%!|NQLH~ikW?|w|%*{hc{bL5O(hOsTLT)B0vpzqV!_oF3 zpJ{4S@MO1sy6C5>Vl`<{WZpZ)Q-0Gn_a8$RGN(y750&fR{RMRPWp#Jsp;=LzKm2I_ zQe^hQ1S>z1_D1Mf)yX-7QW{U20?$JiFS4;r^xW`jcKze1_t? zSLE3PdKc{X$tOiK@10$ly&pp1gX8oKcS`#de!wZZwSLLB(nj3lg>3JIlFWg@ef|k8 zdXqWVi<-vwY)&f+iB9~^E)eKtf8ME1Rq6e?^JQPe^P}H&-`_3vlI_uhgh0-_qchJv zFdx$&-#dBf-UCC*r=N4=T{!TYwVM^ja_=W_gMVC*xMbC;RcCjs?zegR;z)7YV9P-(b#)rQQ z%=IUScU{aKa$x_5U6Lb__3N%(2Y(Tr8#hyZl!qlJXU^MbJ%zRo+@f!+PBpBogf{POnOdCqe%kXQ zP~k_x^U<#C`flU{L&ny{C-a?W*6{}nEiC-z>4ql0Nbmi;}Gja&(_Z8uqW~+6>xs9d3cV;^0G83*OTu!b8*3k zMRPtzR}6SQ;l2+oeKg1Ob9(-jJ5UsRJqqe&UcI;?z0K{_E1tTX*i-s0P7yILr>|pL z?MrK(KkRk>hXuBty$(Fxc*Cv_c46=b7q_!lp$N0*l%Bh?%M~wAQCR^a6fMtE1DaZ*4cMvqcw9} zE2dz>upD#zg`(_?)wr`)sd}yZt5qvi*B43W^>HU@G#l?-=rj7qA*Y5eADB0Me4lfh z?B6eJmQlL-*y{YO-b2bBY`-R-$sCeCKKqCyk6hQ|w}K0`505dHd(LiZQGV{ky?FKS zFAi@xaO9H(C-d60_HkOY4mLr}mrMAn>)4P7g5voJ(mkb%G}+A}bH8p?C)oHKXK}`+ z4kNdm{Xw>U_E~?EZ%WoK+LN+ru40oYNtzL9?Qk`JA;a`_A$7DEh^HzS!EM$d&(X;c2LT z(Syz%_wK$`v(0f^jODF6UD`-dxYwT6@s}Q5I(1;xX5({m_+v+>?%gLAHlDiU`O~CJ zFHU&)n7?hnr8XQWr*d!BUWVv}`?YTI`YK74l9 z!TDzk#<%$9RJZxt(w@#~dVsFW{W?@Kp-nz#jz`OJ zJYa`uc*~Vv?+#~7^TTVO3BK{>{V*mur+k1qH@MN%c<8fw9YmMDta0P;+C@|xf8KX# z*u7ti=FQ&u{P4-ug-3*Krk&ixUbvcMH}2l8+xX>8%9EosV|1uH@eT~kJ#}; zzw+Ga?=L4WUq3Xva0xMR@QDFU!& zuWPe)4CF{!&xpg)Z=IIuKFwLjdl~r-6*y_LfB2fX$kBiE?EV?!A*paei>ThwIX|4u za=c?I_@vWk5J3GPZyz#UZ9yHrLw}d`n#Wh8EBK^OFBgxhJ#Xl5^#afCWMn=pS((u} z*Njj9BJBirbUo31dB50uo^{&n{-Yz}TK69CT}bJTr~~PO)@|iAskSTC;Aj#Iz>X^y za))dly3IB-d3Fqv3bwZ7sZYK%Fv7(H}I6XEC`-DIrUyRSLA{eTj~4b zF1!|Lm)89GgZP0b*DpIy*MD<-xuU~^t3Ryld?~P}6+^fC-DAY`%XObmI+hUf1KL9V zt_)fDf%6Y*ptF>u=KIt>|(u7=#~CaA==<{7`VZb~-PQTLWZ=3xJD%?EuiLYqQ#41hdh4plR>zwx?jM@> zg2rt}y!R{ZXuYaN=B|wDbXTLkQ%Am2d+)@)FFJNezxjDNI_q<_TAlT<_=4}!w4cC4 z?}wDs)xwMWzF(qVpPDtT&G9ey@87SF&l`U4anZaai1Q53e)-mYZ(oO|;BW0Th5fA_ z;DPMota(tss_Ea}^lr})1Gf9uKDcv^nBIM0^ca>iTCp^F@_Vg%22YN3Lh^1+-tD*} zcjDGRSum!}^!yHUuc;p=bSus+zxe%^pExEB9R8Spc9XS3xpZ#lF|C6gBfTkg#-)cM z+R+MF)Hzor*;R9TJ9|?8)uGw32gb*qZ|f;KIQihguD%i0INIo9D-0 z%wivy8pxmbvTOPIA=x2Eb2w;^+0N~3t>%*X7q~Z4eYDlV4BP_ zpFLjl{@k9v7zpj^7HYnY@Hd({smN<~(P+BdWw9v4dKRm$aiHQ#R% zZ)M)jEovQjv|vo~cixk?n@W2}b(rJ&a@0q0=L`_?xUqSC?)}qzxI7O8})s(^I1noe{HoMoU#AH zjL5k)-a(s&gI$QcTRo#M{Hlt2Xglc(D)NTp*AyzM%K74J$DURh+uvsF?2Kn(LNa|Kx$~+g=A&Y&vy%_TilA>)NqTn73|BTfa1BPyDVVs@CCp z2Q$*9KeO+gyL8~5Ib}n(zF07LaJj7D!sPr*ai0*zyD#qUYSf-};;uULE7OS;*!eU| zWDf7wg4$6wD6mD6DHldWm}$iXGhlY8^TX}ZgOT~ zEAg2dXM-BisW-Zp%zS9;y+51(-AB7ie^@kO?3Q2lcIlqiHRtnvOUHkL+>g$EAm2J{ z@1)$;@)`2T4Q=*L{Wh%0JJbjSPSh}UivDQdp@)n8^%L6Sk=;kxy6>+qkH2$bcJl#S zl&uD4&-tuX?C=e-C$D~g>eud<+tXHe`*^$(MXuOS3~M*7->`YdXV?1ZWpPT4kRV`@ z{S|)5Ij9VK*8Kd|ty|rvPOn|(939agxis`X(wsANi|y=&eWUL{SjuYDX+z%! zvdk`2>Ti4b?QPdzPTWxvegCi^Z&TcmRmJDCTXbv|QjoBv1}XKVtYDM)wqMn(o0rk5 zYwUu-^&56a!uq1^ik-{m4b}d5{3_$J8t!jIMcD+@?m0N{J2$)lVNCUeejI^>J#`K{suuso1Sn zvjN4{MQMF*b$}_QcFO)@?ZS0yAjUr6EI(J!wcC*;NzL{-jtN?n=4E_gKjKP1Fuhy* zK`(C}=w7&I-tZ|W`aWw53H2rio?oBs?EEq>`qboc$SGT&Eovrb{$L#^C$WzC;`Xi2 z$~Nb%jwuO1g6AHjefQr2J2$NhF*g6%rkIBF_Ka`SJ+J4JV#@*V_J=3?QQB%@OkCoV zEb;ff`qhq_(k#-;nDE(69+d$Sje}rJJ7;;Vs-{d%4tlm_@V)Z9=Z99&S2bEOx zW(`L@zj$&@|1RHmK&CXhm@yDvR{X9ZZ~Xb`*$0+gD=ZXWv2PYH5{zGw`$d}+{~?Un zH>S;S)A00tOEPDTY5rtullZ60lg@;Qwhy8-bWPuluSJ}ax}Z=}qHiBbOuv1;)6C}0 zk8aM$*jkoT^8V5EhPio9ZgzoiVAJ;A*6BkZ7iKPPWxHK-)@IYE%jUSp&KYts;J&qX z58iL-uFl9M(~KS(ZsEIkn-}yprY1c7yFKr(YwGy8xZOnMrp8R3GpVc)0!M(YEK$dUFfgI-j)t`Mp+gGtw5Yy(jC$o!nqP{PGsFVDH{0 z?KJTUWL;M8 zrt4Y?Q#MSnoZGan(|4KC4JGSy%0E+8z6a-c^ z+F#b4+*QCSS+&3afFW1Mo3_=tS7bf4YBFr+d?ArYu0aZuH_CK@!C)4Giq3=`yM1f- zw95Q(Q`7hk<7SNw`9beTLuT3K?i^y|iX8dKQT(tF?QN(`!xQV-jg=9dgl4qz&eWMid!79E$=YD zQRiDzW_NjB=iN#8YBI{u{KFc>$9p@kjHq_UotLJsOpyhux@!jW+5U ztclMJK3r@#F#RZ`Y;PQ2{yocazQ^+MQ6tdKeW!(3RaJhW2;3L}YR_ui zH?&zT>JN^_@m4*_2~}wimFh!z%De66hDh>F$M}dtBS*eoWkmdINwgi{wm;pUbfqez zP->0`Y&)U-F9jiQLQX*6X?rG2e`8RixlsPWE>VZjntTRLc?P`-N5a1*DXE=|dx}uv zbVN32(jE8YJt(qO9oG@y*G2Vy_fVeO$m7(yTv)rP@Qq5ZdHW;3fu>r1{6>|!pJOnj z<y>Y>+zha z)7RWvQloxVf4ZuIZY&x$^6Q2f?TG%KjVmY2hK43uB z-Qe=Zku{GhMtIQRgr2#AwrY4&W=fPax9;L)5k+$zwDbkX$FHlY9q?s4b;G3t{zQdJ zS-o3-MpTcV&-D(5GJ0?tekC#%{PjsS)i$)pr!CQff*R=!mX?n}0<~ zZuI4T5xGP6xC7b?P4DGT_vs&(6)t?ax?fqJGt(AteOf5F(>|A<_jE0P#j%OOj!mn! z9b)*3Kj!C*SQfeA!?;E-hi~a(ZMY=&-9@FP9Uc8{_1jsx;`>{n{k*qKaPiFe^O4?F z3p_~v@1u`QKFf`mn<`{oc>8Ea?yUPutN5aBhrXY3C~AjyulVEDX(a@*ehs|6(7eT8 zGRwmHHRDFCIUBS-FTVBU%#hVx`uFQzZ~mgz@7#LJfI=Mc=rbV7YR%d{gHm>B`(HX! zf8y#o@7!Ef=!?l~Cta2CbLIr~S76=VqA@BMDt>&89W@UpHfr8^*Q`dKU)GG=Fi*pZ z+^|S9sKZ|{!%k1+z#7G>#DNVLWjY5&OV?x^T|e{4+D*TD%=N44_p0Ip- ztpz3Tnyoh`thlP{_He<0p~a)>uI$rzazxyr38tKIs^k@OSNs~;q^>QZm3VLc1!>ba ztm$T7-NjxydoycdL>z6*M-9Un{wll2p>9Wdn~9c3#GM_~BwpC`x1zt2p5H)aG&Lf~ zU)?9E-I^1wW9q*kMccuk)V^K6G_)^;^?aLlshM%TX(ib*{{D1K2ZUdzH% z5%ozpbYNzm_lEX|-kc@$D4GLK;cu&!*Q?rljrJ)x-oBvOhx@i~^%yR04@6q~`)40p z$Zv3L>zw?#ORT5XT<`fw#gN{bu$W&vvWxfRW%bKL(}zYjT)gdi(Sh-`YVC8r+pR(& zL`p#m_byxvdk1yb@61Un(H~jJU*{+#j~(KF*KEwzZQ{~?__3u+Uo1MiIrb9uvH{ZN z#X$Kdg0PY3)O|j4ZudJUR+4>VpzZvz>aFM8ZuyRR38nG5V|V5y{A#-|;PZ#Du~D!& zyob>)?uU0o+rm`+CIL)FZPoyI{pv>4pgz-kIf>2|vh9_;vB8&)uFKN|UT zS#p!->tY(tVfzOZtU9Oa(K|4{_R*)!J{0fT>RF=UcRXY>Nll$p!Wz@Gk$m_HU|_Mz72+h$v~8w+wz`UemxlkQ$t z?>ZS=vgF%4+b6Pb+s>?WU-xf6!Frq*TX)vki)|0b&YL%@Pw$J`K<^5gf3DtnDkAuJ zLgBq%ayle8BW|faeRAo{Q~m6hx7Q79cK^=#=kDI^_V%0AA!>F~S?BC6_O;8>{3liQ zxe+6G)-NU}ZGNxy_EX3TcA}*3lx@?-T>IR1Yt_UXHxG7v+HiTyh}lO+H+<(q%1i3h zeNdx$4`;pDb$acNfd%aj-9iF;$+-Dzo-E1e@GEoC7Z=Efw>q7?6!BzVhyGJWHLTzv zIk7V{;>puCdx_*U~_y;~&+!bt*mg zMb|FTWgCxSnUlVOkJt_0@hrN7a9ywA_r1HPc3S7U*h_KyVe5jA2lT_+NAdYfpUsL{ zP2{b(-LypBp={>iFG_~>wsi`59ubk3YSs8K{@jp~_Tx59fAaB*=XWiGy8d+KDV1t2kns*m9vYcV8NeQY7vf4j8;Toc9xqds_Hb6mI_CK-Q zhK->0wQoK#)Vld3*l-v7$4xQ0w@VIsHW%cloZ8j-MW18V=jZz5jl8Tsf4XwB^L&Gv zF(W2?ASiih&6SqyZ*wE(=#lA9;4RLKl)Lq79+kC~#r|Mf8LMcK6|?Tbj5CK;H|Gr> zH|pXCH!~fdtX<|-P-E=4z-p}G*Y8Cmq**lQ>M zPcv5@4(0m((T0vVEfk@0q)pDTP4=}^qE01@Ey<9{SO;M&sg#bT6ooO9CHtT;WNe+2 zrNWFXGiESI8Dq?mGQ>1|pO-r4{J!7o{QvXET&{Vq_kHf?e(&dg?)&rkJd=RDf%7}W zEqDayPufW!rDOOR-Z!6LD0CVScZgo&;9iAE3$EJHBfV8N16fZS&c)+tvDHhm718uL zj;wDQQ729?v5(qZC~w(DE`2P7FZWhmx&M-Qq??`oXhpXwe&ZNeG;pt9xJ(M!lTOu@ zR_tZGAvX{gu!Wc)Y+$Y(=9qD%bVV z(ZYKu?U2i)bv#cim?S0@h1?ij#%Nh|iE|Yka(9#8?)7AjTZ2g>_FO7!`Z4%e6{^xu zq#M<-<~H^XafJ#O5&M1fN7Wcwyi5qA{O-g&Q&%e2S~vZR>@nns%OIhD?eJA7G;GAI zNWRFZ_wyN4JNnIDh4aX=W^ZDSb_#7rqrS=;I7~Awd9qdB)JZF^_k>p?JQP)*{v}xeFNC~{hV|1ze z>?dCPhu8emtvXuF3^caRjW8hxZ6{|kV}xV1Q3RW749Yd(7ZUI^}vb!n_W1E{0pb#r+%2zw5bJtOTBCiF)a6&K^;^ zSoK3Je&a;k2EOQxC`wc{H4^nORb`8d;r3TN=DjDM{Tn0z52IUfazr;3c~+qj#>FX)!UR4nNKS zFkfFw+^w+V_v}?p>p@X7xzRydZjLL35KOV77poeplFa+Xyc=lTz>AARyncuA(lyw{ zCfw{w{jf{8_UxV*+a~uXZ9v8 zheY5V0Z;6}C?cW zeJmU3L*1LlVd=O58b-O&>8|YI2M`tVR#D&b>os5Y&Kg^iFe!TGgWNNZwoUZi1wFb| z$N}0^Mkda!Huz=_oy^eq4OXx?n(sGMS(ye&c!F@iDI4u%GC36$Xs(^==p9}x7i>vV- zle)LX$Q2`)pZ`h59>`up&nak#1sH|Yi#PKf=Jv>#OlO(gUEf)Gm4po%OtT-*QzN>% z0Ma2zL$nHGbjifaB%VJ$TLQ||uyR+#^Py6zZ|BL+mqNC(vzXBXN7$BgKqQfotguA6 zqZINn)VA{&imHd6FY=Q7tQA9J@0rN#h(ftb;d!_a_yy-F?^^X5QEiCwpZSU+8?k%O z+v^WG>GAhci(C>63qq7XWic_E(G-9!-DiAe>j_CPf!+G9h`FhvnVSl_S5xTAe8h@3 z8pItMG8W$T8Ju_R{3ylzZ46I}59J`2hiInG$(%5lR5L&H5Q~?C8p?NVaPwO#M#-N_ zPfxdwPf}D_s;>@88VKnNq09t@oX=6R%~Nu(*CUyvbah%J46?FlA%+2z(X6q#$Xo$X zgLL|s3F`KE^T0Pey|!{>Jx1u&G<hec3|imHOi zHGzwvTdvvi&boJ|A7S5MXXP3(>aP*&XD1KZM)NwON=11Icd-BqI3*TEkiLR3q)*3h zxw$ajyOffny2;q5L>A>|`!d+DM-E!AvCHCL(1|tCf4rXN<6cH4)|9W|&8FHqk1a^8 z9=F)_57?!7{|LSFO2acvy+az*s-$FgtMTzWj}*5>aKlug3fDAc-B)t!fJIldeq@lg zQo+wrZ%!CZiB|P^o)yQB>D>=gU#)}xMwd-DgFEy*Lmt55PKxjnRq~-4o3n3(t^t zfEbtn-IST~ZYJ2#8YTRLCV!)ap4GsypI5 zhSjTFp_0L<&1xW};grk4p`@*{Bjde{k$|Adjynx2eaUr|7dkA41GsCv*>%;*T{1Y( zokd2~%Zsi(>kyN9&MU61vq(#AZp9q&BoTU;m&flqtjAj-R;%S0(@0?9%)*QhD6HH? zQMOnqC0GF^tAFE)`A5WnIf?7QUA9vz?bOb;Aa;^n)&Gti03_ zacB~<|Ia275oDGs2^YaRihg5s-Zcn&tRDRHr{)Ob} z8ED~}&7n#IIYH~!uZ0+-re}gSRWK6joDD0@Bugc$et&BJUU`w+P+shw( zFi4y3d)Y=*PM^X&iyJ_~A-wJOZ8um!N7AS9kDvu`-r+j)iLl&6Lefow23e? zNhiGP(Q9gdeb2q8KyXr?CIU4OP;=u^(>z+xB96xx_3r!D#kj3|w+WB&Rb0WWjU~UJ zgM4s`Ep0&_%BF(0pvQ{a>Qu@hAgBA+7a$qICkwJ>du+)|M9THKpdCpS>$_-bS;0In zOe*8`F%h2S9n5N9{*QAMreeUYdZZ0qMv{`2OxOF|SN4(#o@5-vb_f=MUwY_mzEPPcwXzC9D zl{hWgD$uiUJ~U!4QZ`hPeW09}8?5MUQu8w_tC%iz?b^PJ$5f-OCoR#|B*Tej>vdHw(DmwZtM<8c>rqMk^LAH z@&GENP4f0#g%yPFZ2siGCqe|9nyPa8+ShwrOJ!W|N=+_P7jug$Z_XXpod|}P2yIR` zr42rF8^aPFPUQ&=oT|0!bQBC75DFO_t zcgI0EAcmASI*3-e5p$Uzg?Q~z^!t=6Y2+O6o z;DitGcU3iu2;#O8rT(0oS=l({o*?Dm+9I9&{FU~a#ZBVNK_EX!#$<3SW4vf;b5uj=ef@ZnpJ2{W@(r+yZuWG5GTk zSN)V|73&6h`#W^ z^9+BlP8#0SE0e3AMxGHdSF@56K-=u7T!kCkL&5|r$uQwxzS*U4z?_xwrh1+*ou4`OFzk|>+*xjb z;Nl*Pl(br)ruh`)(L7uXMjhbLX9|468-|+-k7qEi(~HYVgoiHV5H!4WOc;FPa6`y| zN#*-5Ex(5J2I}0V_D-GOyP4Gr#9FS^K=cU%1Wi z1Ke~tjp&JJs@`H;@cEwQ@Jb~9u?v(Nw|>o&2Emga!~fY_{ike4eOq~Vw}R)@LUhq4 ztZJE4se4e~Cl4t+(x+tGDW@mFHd?t;#9SnNgH~GnW;-c>$?TK)4vD@ve6fipFxtW_ zwvY1`-bkV((4x8ak6>KKt>o4qL$0sA3*{#@HLGlZR(1r>uHu2QT|lN4A9b6v3t}0& z9J>Jcco==ZI@Epw_&K_VFX9*%12bCz!5m)^%qJUsS~7!i>*MCf+`&Y{b$V$NOb0Cm6?_ORp~u9VLAitlf3 zv->1Kok9OS*%rk2AYXkgSD`d{xpO;vug-#pId};ovFEpTaT^+EO4|b+to`#Znf%d< zXm}dhNo6~3a+g292sRIBW4#H8%Xmmxbmpk3-$`YS1NS-GxR#U2WvVLmj~X^U4-!4- zP1|rphm;tf_JW*3QG_4~8B;t$6o84{I{}M$jfI!+*^)u54gSFQ%>wlD z!PoIc>!Ivw>URu{c))seVW>2o7sl_x_V&y)wMZQ_&UPa0MI2x7J1zCC5Qb{@mE{!% zNIu@fpTJr(-M8+`ZIu5lyEre*u6vaG-(Y+2I#yo~m~JnSi`pgzQmOT_K|LNC>Ig9EM;$%$>hGC%BPNw-Hn)tE8w*65rh9Ly@Z}8UI~o8y&;|O&F9~Z zSzYkJw$~sOY@Zr~BVYe>X~pc_vOu>wGt#Z|mu-Jr3H`V0G1$Ie_ZFc3?RxXyWU*ub zj3fWe8-E$_-A$&3Vp0lA#|Y$cAN!+^M96H_^fy@yqDw;x=;Z$4ntu-8ZT0>46^dW3 z?IJwy-|^3@-<=7*JAOL$(VB_b_c`?M!~M@}qa;7dn46&YEBxCdJV0{PR*EG5H%0&b zNnapca=~HGfAi!OprFRK#+|1B+jL9-uO#fvzxr>UOa#|^!aj2J|HXwP1wcc9BR@#| vZqa}FI1h>id1+&p|N0eyzr6c(>le5En*3g^WSpc3_&agb){zf_g+FX9ZWYE|Fg0Uh9r0Q-&gNGlRMD9-F;>H%zHEYZ89pV_DRD}A|fKH zNyL0*L_~!J@OyjZitzWu@l__lKapOgh!?T?P~+PX5tD*GfyU zqd7%wdY6YXdWssMN2E1N(3%ksz0m(-B^0k1E}S|TJq(FtSafG*(d!nfs4dYMqoQ!# zfMCF3le#%EHiGZ@xVMr%9RZO?=Qhh+l+g|yQ-R_sF}Ov2Qo*7I4!3yTCQr#Q#iP;< zDodz%KFsjE5r!$nLK(e9?-oDkbGht3i~GYygUjhO8j7)2{GiU`arr-dY@o|tytv|L z+_3HsdZ!yy?_pZ%F~UOm%tg<3a7N3|M?c9kRJGbO&%<)FG$%>V0o;>wRZ% z@o`@sSBjGgqlO-?2ysU04{Y+DQo_XZx5@QA(Ta>9@nAhbxc>JHRl-l^QoK~s<}uoJ zK1=F{Y^p2fYty^#K9q8QCh@<|ZGUH6E{h!vlpN-l;!#;FW;BDtegCIetd~rpn6`@M|IbVUcuhwm zprk`L-IbS~!#{$>zq24FgIg>*J_h3dAFPUo`7=y0Vm=%IxV{K}|7io1bXbBAq?io; zlLp{+{b&KKV)^@@)&Iu;`sey*fyp8)qLqJyF2aw49Z+ekRab07 zr2a3`FARoI$fJdMk>>g5^~>MX93LqkP(58KS`B`vd}!%kqz#y?usV6C4Op?<|8IRz z#9RLdGyt2;jpla!Kold~m}oAym^(|v<$t~spq0-@<9?)g{%M*2-^i84OZZz{3AUV@ zR^%2(*zV?mTjR5UwVkBX8|}$1ufKpUVNHYNv1TbOxI#Y?hQ5UX*d5F2xQ; z^1B}5?G_X59{F5uXw`Y$Mc%+bOMo_Ic;Nx#OIm5erg#c`O6SwHX2lm>B0_JfsWrk2 zfc1$aB#Tjm4D#4|B9LMTx!HP=Jw+E{Saf0~!@zf?Cb1|M738pFK~Ac{VMvv!Z5+9Z zTl8E|Vls-DUcFPsl{m}{N*o`P6y(Aih7>9!OVP8W&ZLmUFXP9j8d#)LV&Ne=0%s{4 zTz0Zb;+OE_O)|BBlVnvO628I@_uV9)VNr{O98x14D6>dlp2;Gmopj3RUt*MDBm(wi z3&*M#%X~78&kFPPCuw+QN@R+mG*WM30vc{Jh=dGXf-QESs`y!_&!Hm%Ub%yls&`=T z+b#YSEfNSNsnJ*iOBvK7K6{dea+6}EOK#=)jeH+B$$@$eK}N`kkZ!%mA0r7#LNY=d zl)=5^1a62D+r6Zk-4*(A8-(Gxm~?~!V1jye8lGRPQMx2nc2|P~w^9i_7fINiIx*b0 zhS$u}5lV&*)(x;=CRqgmfCCMm6jUWKL7!#=i-vD_4nc_pxR(c`hF6KvtK_r0CL!-& zBr+6@ZI`BM31#Td#}!y4;-WReIH4pf2G=&24~+}@w}~sUu$eFq7)2N|06i4JNM=b2 z!8*dQ<=K)|QlCs6&q%V`yfSNi3QCxKXpkM0X|z5WVPYj&B>~DR)ag}>0GhyN$W(s6 zE`d?hZeav;d_#)NDnw+s+n1ykKtGB=5})TvQWv!+sfDiM{sM%90b?mrP`1AhMjI0L?A7Ir}PM_h-BaSa&#hzbH>tcti^BSEF=Ej+&-v8PaCpb{%^6ktxS ziU;nJrJ!nwNv2}^WtMmv%RvcWkRszlTQK}gK&CbUM74js4rysf!@Kbz)Iwt}J^=k8 zfFYACp{Om)y&*%alWR(eAXR_6wN&xLr3hS^k-BhNV%5)RhHN`r+OgHiWEp!u; z)wMV@P!fBJE%u}SS1TvjCi;8f@-oM%%yBZ6IZkDc6YYrpeFwG7aViM~{O@s`{-B&# z{}P^4u>vjgoXR|>vZ!8JRPUo`XPM{pZ{j&2F~uRff2puu@ort2aw=0!Wy+~cIh84= zf6FZ=CX-XVfAcS;oQf4{nQ|&qPG!oeOgWV)r+-i7#9$X6fcTeEP9+I8W!mYV)J||J z&>v%fOcgDHaV1V(kkp`dgOjww2`D)ACQa2Ln2+w8B;2Vwf(_3|Qz?RDD}A3r>^3-^ z2SI%a8%~4;;Z&cAa^OJ&Vo%ju;nbUoO`mxC`S=Rl9=@-~0x~&f zrhg}?Fx;!eg#>0*rl>}!L^bs35Rr&9;3r2CG2*uo<+)J zVqSt8!&8)Eg$GVmCgNhD0}UBG20O+!h)9Xj!Ay{2ZjYW%C6XbWrx#JUQYdsP`DTG! zj9YO>z((SzMkaDQal%aM#CD4Tk(w#7S%x#ruwGeEhuJ)ogR9U9g)Ehtd5o5usG4Hn0E}Y?W0i(({!p5)m+U7M}xmN)QD>2~mV%sT_8v%whMc>;?{%;3E}G zDx|f!1uCYHg@z;uZZ*qMp&K_KN*jr}c`~tEs#Qx<4GfZrSp^=M6ZPXvH>t#wBxbjh z1{)gGV@j6-ClQqu^=X}EvBGXhC2e-M5tneZR;gDlwyO+;JAgY$jzX=JqBbF_cAz4X z;lp(fUyKTI!^u#yLv1y92odQt2$VRzHaN#CBdr3qUV!3iyObxh+l2l?DN>g??;`kkpAY z?L33c&cW5mGaG0Ug01w18(7 z<_*9ZVge8Pf|MPNA(^CEi3w9x8eWi43wZ?L52;jM4=M7Yq*%sOt8tAAFso*{Q6**z zq7FRCzz}*3R%Iei1e_{F!of5=i^1+z6(lOQ}g6$q1Me5qE&Zw0<2TwV@cL!C(yrnx=wW6te-$oVcH2 zqBey_5Cr_NFal&0!f}EeVPH;5rM6+IC=zgy0yl};%t&I;CzosN3WI=BYn?oa8kdS? zAcHUgZXq1ZkXqmyq`=%_qsS+fIqfM5%*#|@HiW`BQY?WhO$rHPC>-aZEHugJaCv~< zB(f0BGK5H=c@s;=BDHc1m+A!pfh?pHp&FrEZIkj*hbd?jn{|dDH=uB$s22FZV3#7$ zzhAGxRR&Cm%P@0_98-#w0%am;7a|4+WyK&{m?S8cmKpECHFhZ_q!b|Q6lz4Mq689# z7DI)4fr6=Y_?ddASwuRhWK_e0zBo3$h$%AgNrhZ!CuBBBDy#vK**Mlj1mupIk}JhR zo!W{96It#gogGg=5iV1KC_Mkz)W$O4Q2H~V+6rbc!HhYZDDaA=Kj|Fnnug3A9 zI;520Oj4+32~uIcOafC%aT_ksIhBbjn=FWGlpv`nuR*{h39%B9Im`|ebb^r(Cd&n8 z2FQLnf#V+BsuapKcu_}G z8z&LfNCRldoPevi76pSNRw;X1jF52*Hg^(<2+g=v%EK5O7s?V^WDGN3#{yla z7P2X^n+>!l1^vM>Izi&I%n5`I1)Sqd9Ye@4U;#7eA+wASc$7h(*Fb=ru}M4z4aOt| zfgmohr^0+BxWy+nsJJW{CS|DzFQQ^fMPx!(;3Xkn&bLb_IF+vsdi`?TmWuOnQendw zDn4n(LQ1vTPNo<&lv1xoy|~!y)Y_z4gH6jo)zlz4VdH2SZZ_}>L%|T52?s}u3nf7N zE(V5id}_Whg~a1M2CL95hpYgjh3f@f5|hYMYH`f$&t@rs5@(uKDB(+0SSf-e%tDaw zR+Ipkgrt1RitE%2pP3TcK<`Nta0lhbL^w%;y`T@tJQ%`M88Kx@jRXV=LTJOpQn}Vn zCK66wf?kL#aRF*4LzEgvq#iASN)%3aq5{EjEJX3qAlD{i3X_yhQVuc@M;THS{vCwk zN&@6#j0$W$1C9la30@AsN39g1gh7mIzv7=TqA$hFnO>4Pr`-34{VQ zQz?;IwG5C47G;9ZDYM$Okfq^IVz?7$I z6%}9zYDL95hFPK%N!3OT@Ss&mC~!(4$E7?%$g`-_iqu3F2^N4`hFQ%bt(6KH1V!_w zKpvw))Ml_#J_U^%D~~VNNNt1zWCMfLPz+y!f#E`wB2uWe;sGNrm4cmMBN+;?O|)#4 z0A!>i;8J4tWT1bK8uNn;BvX_)PfIfGGQAy7QrMVD6eg4#Yyk(!G?RL}mLn5M1$u;3 zVGLymV+jR%fsoVzttEklDYHw3Dh(c^!i7?;1{WEHq*D#>QD96n;N6TmI7-yU4j9F# z7899yI77%L2vloyN=c>0<=257tFW7xMwp9AW0n|b+eBkdHsDH?QN#|B2-r3RGg*n7 zgK~kAq1D(^Q3fi-5G7wHpn_sVIpF{N?ZlwP4fW z8n=@I*=E3{0m>%z5O!M#(+I`5h9@&(xD$6OV-gwoASHr2DHczQfeos2C%!!MXOraC(H?4tb9wgT&?K;qd7^5o_c+RYc z_2YJ-gcAAK!2eRUQL1_K={nC zu0*ZIELS?sKD_|+7p}(~v=f80Q%UE|`NP zAQuP_u#2=R3~=cXCdfrhzSfSaNewR<=pjT}?F`W4Ov(l-T)<{3369g~1iloYBt-yi z*>E;ws#xWqmsAdr7fzF3F7jDGSNmjQvqIsd+)9GYB=}}0+?N?O@u_;2GLZnCqC?Cq z!Xahhq5z@|fz89>u|OXB)r304%>QM=0exqGV>jJ-$NlC!B1AfO5%*Nqh z4$_4Z{!|>7V&IR(82Fe+k5K^?*!C(W&H=w91+{U+DjVCAgmB%Y+OELFLM_mnjwG0g zIFrW)8J8?4gejB*RT3h00A#uuXQ3<#{5^xsoJ2T$ZoM6K%Q2>h6uT3E&cLcvauga~ zKyT^F(6P`Mz$Z#E-8vl~VOUj71*B&MC(ap_!KGuQ!x}Qm0iguVSXTM6ZIi`GA0x% z^n_2M5~Im72dV>q4*X+<2jpN#C-C6}h9Po|P)0cd3O&otQ7~X$3>!-=RPrg2*=e-- z;Qzv8gGLf0K(6S7HYTo-rjmTA+h7y&WlkYt#Ds%jJhxJ#MYM!jA`{yQ9FwZ#xRPTO z1aM`L>(@F+0u`82RRW<)EiijAG06gd&8xMVT{@f3AtRJ(lEC>%h+PUW$51eq)Iy;) z5upN<22C-FaFt3UVH%irn=&M0<90L!7xQE?KJdN8$JVLMIMD5&L>6jRI4H5!hWQPI z(gJ>Ys)Atx8(FGUvv_>U0eXrsFqJ|R8^X4MgMI4_Fd>gPy z?2xEPDndf1Orx|CxLE`+)fq%m9DGoRf|i}Q)RPnfiGp~+-)Ddis7zL|;sZ%xU&)85 z5GGFvI0l=>s}tD}rPbs|#o&vpr7prLB@&r-LWLXrFfTReJ)g?PjUkbM(_mw!fV~nj zsC+!sA+;)SsVgyrn!%Tx-q!;8*2Saj^nsD_vsv>WM+(F93qN$e|} ziRv&wb|_@cd0ajyC=xEF)R_sT82*??M_D|OK3w#O-$+-BDz1a_Uz=LTWV0a;fKCM{ zNnhw%oElIneW9ebL+R8yqjG4^2SD6AobE4J!z^8;p}3w=afZi#ZDuCx!y*DK=AR_L6hHC-_54pJ zW-^N@{%=uF2?qWZs|NgyY5@?#m(dyW4g zGmzuY(TgxTgSt5HUl>b5bXm`Lr|1!mT@H2_XcG^qc3EXsLl82kneWl4jkc9*xR4srkc{*DjHX!&WHmc%TA-~$2q>fL43IgEE${27BdEW7C zSxC!H3TNCK!1fj}VXp)7w;>;#ZwjGS6TAcal_42JrUoB40mh&!GQfQ_WD3H&L3lSn zj|0y#P`-@52eBmqQ>NC2P?g^Ydp8KmmxSbe8zK>e@3Y}vP_DMIV$a6zh?vfBz~B4QKbclUdYu76XfOto@42*~$5GqpIncOZ(zF53031y~PX;f*7I0yracUxf-ikQ{Do&JC zyNLIb4*1XNE7|jl3EL{_ch|mR8zqLO4|!3Q44*!{Z)j&2)h^&?x@yIHP6+4uJ0vW# zSssixC70B!a0JXo*e58e`|<8+;EiN)sj3>z@NX8#h;Ri8z#CmTf+^<%t4!2o!o-!I}Dz#mJ- z*K%kY4_CIJaSynoc?0MZs*Aw82|QiVUMgK-gkdR?R4yyY7t$DU{28p@Nh|e75>3S{ zt~BDJ=TBGic!vRyKP6Q?!g4oU;e&6oNJMlM6GqYeAnpw`3A`(S8V|rTY8lWyUHwA_ zbZY~wh1Ui2XQO$NrcIhR*ubkrvI+Q2t~LRmK@|@xT@^+Pya6zVHlW{7xLvJfqkKb% zZihA&#Py(d1n?psV5f#kB}KFgd~V}NKsEtRp&tnXeBuq$o*H--cnNAxfc&sA0memR z)ASv(nT&r%--semOyA)OF-ZjYbp-0B#I_jOdm8_nH2)EO|2f5SiYXQa2?Q9>($WcP z>G%PHG=-XgazHxMbyU0#G--OAZ)i zz&mVN&<|Jv$!FCv0m3GL1WoZAkZ^Q=z#S5hzz{vO#6ztY14tB*79dq04@UU zApmJ+0|{kd09NUD0w5VdOGy4)Li~3@FhKh~#riM=_@-%)maAUS5kLblV>Q$sDdJV& zL6F-}hXm*|i1HzV2XdKyM^4i%?D@lQpbr`7HGv7L%!Gi(fM&F`oB&LKRE63n09AmH zEKsBewV(syLB`PfKmp@nj4&@-*g)4bRuo0D8tSEhJcauvFZ9a+`Ul-+0=Wy-Z9s0L zYAph?hSrTh-!>M=brWD8beRqMmj%Q8AA+$$@nJa*eSq8sSrDXk49y=jHVpq=nBf0~ zthN{1NjTT7MYt)3AYH5JLzyiF|5j0y(CZBUrtP{Q?Z1YpjSsdVz%$$iHqJ+FrK%)B zr6>#UU6mwQJJ^zRwIbjYupx_V$3Lr9^d~Z|D3_;5#?im|66j0HfpaA^r?Fu^v^=5P z=n709wu#Jk9YMLMaHS?KJoCPSl8ml4Li^=KRg>s{}hb_3jvkDpt zL8v$coQ2VrEA(aHNJOArgKPv=Nn5M`=t@Lj;XAP6Wc2WFdLm=ysZi6+joJ0N_9P55nw^BQZ)w0vOS~gGCFy7uDzr_Y81EiYS#%ll~8mRy2PY z;lt5@w*cy(ziF*iyuOlk3+QzNbi;N&Xs|!jbow*e_~SZ}l68IvQ5p0MmTI`p9L-hq zXms7La2+{6?KK+tblZo|{h>nEUt63Zf4CZ5GF_^;sK!+3SeQ{4j)nbCl$~NSKF$F^ z*d_JOOGU@n#TjAdQiX((;`5pRGtsespjK&Fd;{vHmBlycN{D6gjk5SgS$yLoTe&R0 zQ5N4Qi*J<0H{SVvW$}&wo>*O3e4`}hQ5N4QU1|HzeAqwkLX^cf{vOQ!==Yb!H~z22 zHxN$oX^oOJQiSy{5#K2L>OPQ<-)u(@ylcS2#=a?^%QAlKK^3fzld@w z4oCj2#4Mweg6~7S{BTmNIM1zAqTh$#kuS={EBSsa(C^3PHW6;|4n)bA{}S|DRvWs^@-MUe%Pjvg%m3fm z@(25_IQO8WpvV8FU6b$!wLLBuls6HyX2gT>^8SCtAKMnu6)$8i%4q-Knebi8qJx4M zj^Z1|FF@%FV^PZ|7pE?dh-e-GVMleK|NV?l>0&Y!jWrcUIf&~D}0 zR-~gRS+!jM=2uqLzmeHv_!s-HMvlr}^fqc&AA{oOQ@oM5Ag;oy=H;X-tNd``h3swN z;hl@89&{oZtc>p4Ug!5}8XRS~+5PgvC*9BP+x}wPQVP6 ze#HH??M}8B9#O7x^FR5kez~Y4b1VPpE&B1y$~EI&{~}%Tmk+?(<^S^L-+fycG4I8> zyGqT=o(=!9lggt$7bmXh9D6x5u4R*!!ygR2pOW|Z2f@Tt%J8Ra24IXhQOkVdr|&wE zTYpuInGg{=kb5-mLGMe`e)xqa>Yl*_NYq>vH?0G9{n6FzqYn-Zg|UaXjO_1iJZLT4 z++61hXHV_0W9y z&Bp)O+m%Pf#gF)As5Wt2-7oS5@qg&>a)rtZ(%RmP2m^Ic?Y)QUYB&G<^y-r*f_3YD zKh>)G_>ZU|u0xfYaZ8Vxj}rjW<|m^nFHkvtz1`(Ql8M_1?d4;dvn$cWwlg8JoVf;b z*(V=U-YuZ0LkG{4r`ww=;Fa3O#pbpjbv5OansIG!?>yEyjKW$ihTmj=yz=I&p`$@J zZe0m4A`@PCy+85B$7G^z6&KO(yPAcQ=oL-g46l6KJ#~HK4=2^UJxpV&zW(hfdKz&W zm^u1sc;r%l{oWxiq6smy`b4^?D=Q)**RR>G4que8&9W9h4}Rf%b!Wwz>sf^bYwT}s zx8(*NeQEXeh##!oV8vdYJv_9{!P%UtF^==ni(6B!ovU3gcXOWC9Uf%>jguQoJf()L zDRz#ye|}TWRC$jURqJ+|+_}%jy(0=<-`F{#ys~fKxfhzqA6>$ht0l{^6>T`SQ{Pvt z)=>QX>w5d|`!BTE)5PDmZ%7!?q+V+EvGkN{){ZQfJCot9`N06uBYF;gzPFESM{9Xf zdUWrV1Je7>OFVjM@zSL^_x+dJ4}1G0eF-|R8xw0jd}909N(nS6eX}eg(wDS_Tc3tj z38l2k+_^R7F17#qK4jS2A!oCOWgT7E0b9QJ+s=LVYfbapcb0c+?_^mF3%gT~u1v`u z->7%5x?`TcsegW>W2!5w0H5BaPaE6m)zh-a)M&Klr~W&(`VQ=$l9~2;zho5|9C|_g z`Dbx^#?LMv7F56X9Ujr`=Jm!kOQZ2KBF@zP#og0~D@Sk0RaSp?=j7Da53&YC@3@B? z-SG9)9qTO@w=N#H0$+pXK4{v_C$4)Sz00D)yn1K12fh1_wZDIE!_;r5w$14>^zpcv zPbWomYjdD+RG3DiX*$#0T2N}S%_j%S<$t+L(NeU(Q82iB{Mq&E*;|$;*1LT=E9>zs zb^CpF;^r7Ox-R#c)x$DndtP47I@{@JgYdO+#(?NW&4wJ&v_1Eb7jb_R<5^1@fWOLN zJwu;2Zt%f+q*u1gVj16T9`r-Tzzr8XcUO7#%=7Q=G33$~@6sX9&zkbCjjp!NdUE;9 zMwyS^JZZgW`-A+uhM_~+Ryxs|LvFOUUphZ^_Il^Vtrr^H8T{hjcUf z-_+t=9$Z-9xtMaA%ca!kKZ_Inwi^wvuS$!!hB4IE-B9gtdLTRFEp1g49 z_ZRxkib+{sOS;i<9-lw#@ozH^47rq6gVo!fldtl67c%c`e*EJc>e1vDnPWeDFDV9n4s_kIccs!&d&HHh z_6wU86>Bu44)i!>Z17;%mB=mmvu)#N9T__PE1>b*BMbA=yJWVj-6Q?P@@bUGl=E%d z`R$(FJ^f;FgWiq{=EgQ4>UCGgG?PwmSY_1^;P}>sPjd#mJvP7;ckgd)4M7f#yan3BpX`MSf?gEIwYrUe_ALy&EI!`WKGfYxAw19eeX~%B+nG5Yzt$O}4zE79X zl=Sp2*<-3tTh(V%!;_Ysp%*=0)W2KrZeFeab?)b!Xs^0{Klp?y$bZ&f-Qb&UCq(n| zFQm+uPTH#_Gp;Xdn_j2dl8(eO{ef2L?4V~EXJN{=$b$TRqGoBWIJT2h#xxzO ziQaY(xtuSKxZkMr=S95Q4|w%KLR|UMMpg4O<+^$CY;osq<%9XP zLKTF<(_dA{o8-U59N6jZ)`O&P-}qXKgkM*C$O)XPl-MhH@8Bp90ee@h+0kZ|yw{e+ z)1r>_s#gEx7qt9aJQEbk*lJxbeqj16Ew}cQ1?lNM`!sFH+C6jMid~O>-8XeYvS!AW zM?c)Vw4+b{Men2`k9JksO&+F&eI%@V$&7vj-*AuS9BR^?Iy9GySbpc91!<{X-B;Uf zps|51)wZ)S`TYL*wy{rhTMQWTcFW@dJ)N85AMAM8t6VO>cP(@8Fdcqi$dnM{S8ZyJ ziwHfrdC>D%P_Mz=&uq8Pu9)#@qh1$OW82OT@Y?ixK2u>@MQ)&SA6-4RB(dJ%a!z@p zMn+@9OIc5UABPOR(3Mt-ikt+ zqNnDWYGJ*zAu8tKC2Z)JjEp5$kIAJD)#TptPu3x-s$(yOq4rMq){2^bXik^IPY3UO zdHHykd(Hj5n3rm==p^Z~>=tN{31<(J)P?-y z>^`n-t4@r3(WAR&ZpUM-yZ$JfXB}cu>>&mWyQn$VYJG0EGyYrG_nza}uHWiixTQzT zpdaR@JwLm9AZW#=;_6|;s^+}moSK)m-!gZ#zn$~y#n`uASDBOS`Qad%v`NiWsrD&QIN4TiCZ_&*z?BUG===Xzkdo4@}e9-w)wl{>emh z{JF!;hCUkc_{GBP(ULWncJynOP3{{0w!nDl!j|2=3vRx7a^rT^Th6k9y~gNoC*{rw zy_i0!_uDfs*Bm1HwXV_gjl02d$N7!>c1D)JK5|%LAM2^*t*^CBnw%KD<<~P4BhDRH zP2L}7<$IrKvh@Yt;KD(hzdZY3kbXmxNZ-oE-0G$EQLE*{FGsFFggzMj;^OX3H-r-! z?)%g>a9UzIZiC#=bOOAdXWk-l%yq@HbV4V>Q3IVGZ-ZuIw~ zY2w=T8Dpfe+QjS%`xj=+7%P4t@5)T3aw$4*X7$m}Ws`be zUmv@>bG2%-4kmN8rjD<7B}UvI(;=laTf`w%UN(u#;+iIY+LXPw`SBagd<`mh$+@S^ zxW4P?q6r;f`JpW@-o8$&nwGs`aL)A3Q(olXUH9eNA=(|G4Ip05SKcXi9v2P;H@d-H z*6B3^F6TryO0gVCD@=Q_@9vr%1K*@Q9a%$q+Iew*<1rEEyrB;Kh!LY|9=Q@c(eAqj zEPfvCOMUjk*`YDY>>Gm1t2oscMzUID#GZ>fa;dAb>IYP{CJw1^B=+gO_Sp}2#10&} z!F0IkmTMc&U(Yz)i*-t0z4O*K&$ir}-*Jm-X~T@~cSP5?Ff5BUDx;3{&sH`AV)=P^ z#X}8y*diQt=SA*t_ABS#A3VN_HLJ>;d9S%=M@F3cw(j{4K+^%7B~k31trtcgik=_q zU#*DWU_H55n=w$isejM2Yty#XsatpYsAELR?JrsiC$gJP$u+hZ{#-eAD|l?&sJhX0 z_8p2oKciK8g@gBQzMPjCb>wlzt-T+Z$8^}>=!D}S2XD-u+xTeft~A@F026D?aZ0geV(0)y$t%I)ZMh4|k+-u`mKXliO+EIBy&l`&oKzPH*Y702u zY^>YwtAvEyR_zB~IRInUXcy^wvuAkScOsCs9)1SvVNs*>2CRX5TVC6fvoK-I_20*J znLcLs#A)D}OjmDAxr;YI%#*hlzJ0y>fT`b{eDKsJKKSz6rg0C?Z(8%R;Kik>p=Wod zRXwt?-?mRSt!=sa<+Vxo?fBK0TLt;2KX)2T<sFFL?bT=a)A_EJqf^&k~;2Y)p!&-niYfiH*7W!zMHum#ZQNKkKl;@x_jt z<|`|8Pj2?yMm^Yn<3{xpr(B(KCT)JXtm}hTuBMmjFMfXS;#S#n$Jv{_8@a1FlU&U= zOza+du%*g^6?L88S--;w3KvGEhbuyeI%eV4v0 zPtF_?IQZ>N!8fNH_xJw5b@`h0_tFg)u5$KlJ3V*(q|?&;^{<3q4?b*f(dc!-u9q2e z2G8nbY4^&LsW?2|ls;{3jo$V*Te4r>%IUo6V%>w26BiHJmtT|D-4-`^TKa+X-U9~q zd@=Xvj9&hWt^GHyijAu2`*Ur`U@_yt6X5Sq=l`+pc#Z z^J~;^zG?K@l)Re1og}Rlg#M0d}>SIQoJ+j z>o;litg5|VZn_)t4bSM-_sZ6$)w0uv-4>jFX0AW9fOC55>OCu-=c?FaKJAhJy)ssC zdP{>Bd!u5p`mfRXT_tVgb91-O-}d_Xv~901UEA7p12dg{=SO6NqU9#R;U--!B_H*k z9TyvT>wesNUFyNv$i@9#ePV0fdO5Ktm$!auL)jT~IbRmI5475p(79o+^fZwi#(nwb z_S0Fb?>=7M^KNf#bjJa+uWjuqJH$;m*Ux+7Xw^r>8Zu>Ik%q(rbFa0@Y`I7NX7JbN zvGu#AXU^DvDpNFUgjY2*=23-Zug~e}*}i?cH)UN$PH^>Ii$}J)?bVz4Gd#bJ`+39J z-)ba2<23r=4l!uM^1S=AMT^eG`j$>5H)mz$HmZ9$jqyv$;HLvKgJTq{?;d@8bdaad z(>993jW+!TP&qvHZQm`O^Pep4O*pHr;ohj4H@o(wF{kJ5y;Tja3X>ks&W0tP?%dxy zE-^@2uTkvq}HFQ?i74y!1LtcK@J<`g~^%_=-RtYN|J6U(Nq-JKErn{N8N?*9F0FD`W~4BTq6jNGvN)2xTR zwmjInE-QBHK5qwN*%#ARsAH-JuT6;Ee`R4p&Y5mi7Npz~2PbORqA_o%SBS_Uj*vw( zc|5Ac@aRI*z7_RWs;e%zznxd%v24xh!pHve8FO1c`z~VZo1<-qM_(Qjv2{qZa=Ghz zMIE`3&Wbv+{pOih^I9E?*?#lU>}yZoT;+^8-FWG(@hO984j2@A+coWAi{YsqhWK7z z+H|3l_fjAs_1>8lDR)kMC)u*ab4qKHOlZitzVmHC-ceDreG5jEH@z`-r z4>DhCd#_tNMEla8UA<|KcH?^G?+wKFy1)K-t5vD@k@9uFKRvg}>*xE&RuimSMYcYh zJcO~e;}m0;-qUBgkN>1LPny%<$dEeg-(++YHw&HssT=(|+CDO+%0#l+s_DNsJV_N= z4*n9oOJO{Yo_%sYW`y>u?1Q2vt18dUzLB2&veon&Yq(eUF&dItw z>~iJw4SfSg1&H&_OQnrJN0c)47Fg}|3$BbjwxQ1^WZod8{MPL~_N>%2+Zq@;rusyW z-cZr!J8+M-d;*=4lbav=WzL2PiyX7ItQQJwTv4CiuYbdL0*cR1_pnQcui~2bD%WEa zBlH5C@5IARdye19Ra!1>-*R9sC$ZsO*3UiO=}O_M{!vGo#;#f1t(-ZFT2?PI>Rwgz zqaKFQdw)`V(eiEK)}<{)=PsWf*}2NRo^hX=KaV;R*`)q#dP47X%!pdeho{EY$sfN( z)GWw{s7ISVF|O(5D^(YR`>{k9+8Q>Cvn$atvu)kQqjpqTkfvLr1Rd5P()P%nnq-Pw z)92RU*edhV+E)KH5@MyyPub;48&u^w#YNm7>VAIjY|G0-pG?RdpPTmjd4n1)515bf z>4?sTW5+HugCKEiLI+{Q2&&$Zi2Pq`=RD6lYZ8Q>d^eD6*GC|a(C9KRgGWb zxX=Obu2p|UhG*vO?`XU*s>=N50T?3h%7C}}6TxSk(|Zdb27s-=pFOP~zw#c$ygd&e zUCrARdxGYblJIiAVuxHHnq(@xQj+K zxzJ*f5kjd%WZD>XWbXW)i-txlY|yPs2TjL0FYb-C`MU<(5K11&s=GcMJ1jxLSM9I( zBX(r|B=&eb)NeBe(9vJxvFrQt<~%w4;#0%x=l4B#560%)N+0Gt@#;!K4atn?emz$7 z{^|1_kAA6|H{-jO6K>A)KG;J04$Fu&{g>UmgEZ+@KK~%syI8qI3VarLb$lAp@cgF@ zyCz=>Jvq2&V6Ev7A1!~_Y1h?ht)78x^t4>VUZPhI+GfmKWRic~v&sUM?)%#b~aa4N`cuin0X*eK)s4eAxR?Cq8>N3fk2&ag&| ztW;yapkDuWy$fD0QrxmEo!2H~Kp-AGh^uFrUn##^z2xF58=)1~>G;=r$LT|7#z%B(*HYRYY+6RSPHSt;Tqyo&H0(N@a39r_ zmNDOqjO$b`m$vOr_S?1}Lha+5&Yv@@z3NBw*;C!9hMpR^mj|RiIMV_G@!vtDTylA6 zL8~2#MO}Z&_wJq0D0cs%+HLki6s^5zVq@m?3r9E`&Tn!xdp?{Fx7Thq?83{eszYD* zF}#`zA>?xAVWY>8wc?szjq){FG>Ziu1odFSVA@Qqa=Jpp#={_j|8i1f=Tf2Bh!I~_ z{q>9Ccx1KoHCfNevpWWFqMa(|mS^~uYlpfo8F+B^{D|9~a+j%o+*{c5$@s=y-Eqso zJ-s#8JMn_&yrWimQeM6BB5AKTSIP(PEL(a#L*D!I`zsDTz_ZUk+?=o+Uq$}-`PLqy z)6xCH4rXDsM!oump5MXNHDN6vGY#{Cmg%6R*}HZQq`>dxp^kO zv~Qhi3mQciEItvl<4LorU#(1HeBk>>M)9eK+gq<&b=jBw!|tz5dmqa$Tsya9(c&C2 zcZ<4bHv>`r_uI#RY>?sSu$%OtzVSUgnq7JEj&==uOlr!J_8(S|a;-i|+XwXq3|ag1 z_Hohdg5T@yzGp7@By_Y=@hJhi=BDGvH}n zII(Th8whDIw+7!l-jf55t)QM~yln(>APYYg%b)MXz6Umh|2n}qcMZbEr`#|WE((DC0hF^Txgj*k$I7-$C(6g~xyN@UHF7e0Fpkn)~UkX2bF&nH}3rPPuz(+VZ?n zwb98=VnAowm_E~M<$$dIyC*d=)o+yJJF%tcy}U3%(v!@JCVn_?e%brx!ZC|g zw+{7Z_p~<&Ti0nT=8XkJjrb&r*Q{OOw~pTHV~IlC+GSqBkcaE5Xm*QtHvIH%`l?9x z<}6!A->-ii`g~l(IeEW3!V+;uYn)a!UxJHg{Ic-vI`F$J{{8KzbT^KrDC*^@40^*r z7JI2({_q~1VVf0c+VH1DV{VeM%h*GW`)~0qNt)B`+}F7I7%qVi9BCN}9fAtQv<1#)KJ-wweFR)h|ziNc%uJBUN5nc07 zg|@eiT)$_58Ytks^556y=g>221NN80hk$sQGJk&bq#pS{z3KK#!u{=cUW2bm?Y*)K zDNHY{lNPs7ix!?-Gyd$6tZvsny%ssR;eye9E}xNf@1@;(LNldDAy!aw7Qk?0?C12> z5ZCwe=f*wU&GnyyD(LvC_nK2nQ;+rcp0}QR(E(cwQbiay44V<{xv4cxTP}UQcJSe* zX92o-;NR{)|Kw(aoK*ACjRPL<>AipQ!h|KT=dnB*!KVEfOlbckXOwjeiu(u6VRk^#|v-ZKCGvp3taKjkeq0 zjP1C8)Zwj1f8En;#?tvijxqKgIy7C>ba0aO^y>S42r$1EEzw8SWK>`W=5O-^XPrIt z-6&t(^>v%}2&ngdIwc|d=Hj_)?>(DTEpq+oA5MILZb`dGd5K;2{REmfPC0V#uxQPf zJvJYD_`{y_AYgms?`;WzyZ+109vRxKyY{YY*xOg0!h#o#SaVW>Q;)3k-a_|1ICFGn zwbjNmkFM^XQ+;UNa^~JC%NmWSQ~aV}AIhZhmKcH4o9YEcEY{nTV_v)@Hm?qzG^Bkq^) zfPD~2LO#hIc{lX@Ui)pS57=A%&VfIU&(9gK*qAe&d4Kil8aph<+nn9zUz3Y8SsQ)Y zcNh2UozXwo`qUV|uSdV-38($1mhheq-dw5n?9Yb+01?lxm0#s%*I2T7^BGN_{M)Aa z-?0w&IDVA5=5Yx5mSr)6b8JS=DF8 zo5JNMSB$)L;gj=AYtZ_j*^&X5GkWJ_Ae9fU+1O=wc67>mPE}rzl6I@v zsWmfB+D`2qT(#c*-j~xhU6*}*=j4h+a5Zmi?EYYK{tK&Gk1RSF7x0u z8S@6CX+Nr8u1x8?|4wt(>kD-p8!{j6X?Q5J_W9m$fFZ5p#?oE_=;QHMf=7Q4Ailjd z&knU$8<;k8Ud5wZ?Fq20u^9IEB(@D8Tc%{*9{J|8wYf-}ZW9~w@A~_B5)yahKQkUk zsIjj?lWyXJyvX+cpFY7e6Kxn>IQq} zjmAGenH$mV%kn($d+3DEg`8TcZK?8jjkk|2S?9QmL$p}@1AAU78RhGj3+Ej6a&OIT z$@O3EoVC6a^Rtfa%H?V#-o{a%shii$v41aGl(FFTr23|n1@`I9qmCT#pp$C@bmA+H z9rywH)~w(wxY8x_B@bV%X_4NkUk7?)E7EA>tFla+vAf+Uyl>qHsz``ulexGSt)hst5a9BGcM0Oe}Dhx%Ci@GU*BA{Sa-kM zAWQj5Deb_mt2zGQ2gc8cb>+s?XcjZwdt^-Qrf0I!&mM{FE%BZ5Zn$%D!VTq}Q>!|H zLHVt1T$Kga_8;C=tMdG~9@EeMB#t-rzMD{)Gh|{)3*W>IoA>N#IsAG3@9&fb9<7Dl zZD7a)<5txYOGh+6m|DFYW1+XEaMz-knsI}pB%Jqpt>_mY_Kus(Yc#mUd?lSQ%o((j68Uva5)ImFAC-r0NHRf>ga8)rD2eyc<^H&-mz zze=m@{6n8>N6o3wGvlyq9axJ$|0)!O+CMLhI?_JoiVp$~rE%2U40DffRO{TxxbMsD z`#Ce?;_ADgJkc~^Osq7PcWcYV)ph3Gu#Y~RdRMotPEDBli1;7JeKhrWh<{hl%RLek z)uE>Nlrg45!$4Zih0(vQ&0p8MzO7pZOhDiM4LyM#FMUlunm}cG0`)7L8$LW?gu2@N zY2UVvX*|EvvP^EIZ+Uv;(`Fw%eh3~P)HI^{q~Wzks5O_lQ!3S{{@{4{*Apz7q(s|?$N%`3vI%($C1D{uZ>yyV%uRro|x_eqW0=Up27#j97&eP=%! zD*V#U^7QGkn#;;X_KwmF%-(el_BRgCeU&w9b+_ydM3>&9zwY({HxyscaMj3*XX<>oiud+u^J!6FBy>(+^YWNt#)OFcmB$ZHZqj{Y_jBJ(__Kb)i&Baf zcy8=pmw(>XIOR;`1+Q;j=nUINQ~&(XV1H}Yp>3&>w zbNGxk+;7*!*MLKEE;v>ArtXhJqiX*A$)nlXuVCwKZJlpI6@8r=?OR(jF0K`J+8@)b zLqM>NwY_Jx@Xkvw(u7a{f6aY)IMi+XH?k&0l8`7#vXq3(5ZM#T64?tGjIGAl2^B4N zvW+qJrR-y0qR754GmLDB!C10}Qtvg?^W69I-0$!G{rURCaX4nK<$GS|`Z+(>SIcu2 zRZwRAwGjOMsGl#?E7Fr)hOQ>r^A>~D*M1Bulu?!c9vj%TXk4;dFfML=k_l~G@^$&d zz<)CR$R;+S1~4WTb}qgN6W%*8=-X%(2(Tq4IQCfJns|`xf3NEQFXN(Q>zNpiTlq0c z@GI-VSZ|kg1j==0=>JXkf1d&mneWz^F+acxa-yvH%Uo5BfkYCtS~}$5L8Xoi>AS3i zkh9aKsAzc#}br#$<~^Wg!zG#0-2<7L?oU3_rI2aVPAO-~&?KA2skJ3+1E5g&pJ z;pA$!->Q0t?PtM*zFcB`XDedA*SIFL5B>%4fhX6gU!yQ;nBhQKw|G^L zMLmPE$Bf7@rAUM?4Hssu+YPmYf3CaiSf;4x7~puzHmBoEc-s-SfqLn15)?lBsoHIB zO^-*T!jYRO`nP%amyr*qkzq(iKkbUxfd%$*lU$&P0F>p`}z0edOb}^z`R94U;s4>Yu>MMK?TMtdO>;gpFV`wkW7YsaCR=Ju(9cv78TBIhVE}D_3sh9ex|3H8%5Ad^J;E{sQ(^}8I}I}`t7sq%=DBZH<1Josaa{;<$=+N0&*N;9Men3>V3ZF&VSi2SQV&z8 z;#9|Q*CMJsGO(1?m_l>%=%0`VxWR&zm8s^FZSR8*O<_8hTSZtfV;LfbM>p#^I5<@3 zeGIg;yg8e+fBTGjb`Vm{@8;+|u@>RHj3QknVFV*+?ppH=!Bn-iBgCL~t)D+OX$Y#@ zQ?IM&-JJ0t-T-`!;6K|>W$Phr7oJvBH*J@&85^s_E_8N8%bLJaGmtgrrKx8uF`gbC zk*8Lz{kop}I&RuGBa09G_JP#8;56f=A}{5gv2m5tt7@dhqDP@S+D|wj@UOX#GaJqQ z; z{xM5!#ztX3(W|#__qBj9pfozM>e^w37GZP~6)wHuOY&}ggPLw%(aF#ex5CADq};Sg zf9a3kwJO8nQ^)XG;%KMY-ZYJm5XH71YWZE!sat&a#=LP5$&ni07DA$@_|Dg%qK@aA zK^Oqd0>Tn2eQo}7Ye)35<&_5gCwFS|?}%Ku{LVl~SmVO@6HOH7ZtwW3b+@xp*p|U& z2wwupM+bdjzx?BUM&r$pjB!}WJ+^Q${F`g7!8vBhJc*}{+G)?$?T2(dB^IVznVIFG zmWSAd;Y`*o_P%cVLdLNI0ACfGs#F$ArV?hZ5+Z zvwr3Gb0uiPPxu&3BhC^y`b=N`ynF7eZ{D17|IZ*#7QJsK8)fd%vhnhrw1DnbEPuunjsO6-JY9dR&Atly{E%3U!1Uk zxQkF^AH2dWOF^#EatVK)PjS6Hthgv!ZT;p=DCSzm1KB|IwONfdrl}_74S7`wdl{GP zhbNd)*uM}*q|c80wqtn;8Y&j~psE4cXwf`3*YGGe^u-+*^Z{&8a}~!hrBr9!&hz-h z8O_boB3+sh-iaKFc6;|Wj;OX=scg;|#}iN-9;LUaR}o(;HfNuBI5mH!88O;%O}E^7 zv^qAIbvOqBv%GQmH+HK}NyuRz*)|?QO+l3CPo5vu%9D)o#c}I5aWUiS? zOQ)LuaSUoQ{_?av`YPhVj>XGgbgmb3TzPCtRNPL%T`)99+_~#RHQNXkWUc)uqjyP6 zwWmtbN*MP{V>)am0oD=`d}!EX%%eC>H!C~97s$*?tu^xGr?_)7-KX-%;p4|KoBpa& z?b>ntX3s66OJ}Rthr3J`k1q%dv$X^=rXvHFyAvcu7&11xwP5P1T=FPvW~r&r@r#ny z+9Wsl^OFQ?aTi=6)Wpw`7GGebK(e`Z$GR4WJr;zK6C4QH^P7HkRAP+DfN-P;6Z}4` zyoYs-9J^PBu)r|lz8b>jyh&@SGKx+*4RdoEXAsuWu~nXY*;3J-Q|?*K*!m7s=S#=C zom`LLb*YttgS&a+DIT{YVqpi72_^*!M9rrNv?fLbfOHt8bBgwb3v zGi+~=w7Bna>0|tyK;F=9V?%G+yYS#*HhpsK?etJJD#%DBGOekJo&Ok(7sugfjuTc-I+tLolyesUwl5Z1JGP>zriuPD66D?qI7$tP) zC_2JQQHwFB)8h@IGtW$7=qSKnPe15+uNJZYU=~oE?DaunW`jP0=IeD-qjBvu#KI3 z(lydfw;4OBbkuh1WRA!TSIx|!2R>=In#1t$r909?B{|x!H?NPkl=@zNlL~jc;!qga zQ$=kM(Ll6!Mu(}OLH0;40_a2iE>f1D44WN4*dTBl(j)nYLNAHj;H1R4Y9*uy53=mq zZ3E!l9Ab-3c}v$bQC{;E@41$9y#N_gc#+A)FbC-xjf}yL+9~2+1kX^0>rZub zXn1tJKXj#f(F#h=Zu}crzJA|1<>|m929JRqVJ*2p;%eUU@-x5rG+z8wt+(_2bgWXr zH=j_EjLCJ~Tfz+ySLM7)_A5t!XDWZ7*&!u?y*n@xp?4;sZ)f*ut;5rvc-BuxD6OI2 zQ#f-Wok6T~9p}-fr&WvcnK%}i>}<|+7lh3l;r)WwCsJ&fkmwd|S0Z6WrN8v!fRsCpsdM7;qb`hUwfZ)Fo>anptxS#Y?mZmffq3(s zq!?3kVQ0mp_F>rc!g*76BVb};4_u=S_-@GprIP>57)O$WGD$CP{`i8+3(?$&k~5PV zz!L(k7?Vq#X4cLpzLS=xaQL>cFH1`505VckBbK84@MsUn;$}a+Q+|83)UTn*sD3$& zXoMG9UrWntbw$`Q4WRQ!hA%@$?zR_$?$T=EEpdXSPtqfqy^NDcewp<3^`(2P%|z-~yAC<3dUigv zD*+S&zNOW#0OL2ej< zbvsK=+YFIdt64i3sMVrvN(9u zX1_qjFG(5nh54x4z3v=;8?eq9H!G(bVI~&2Vs_4^&TYj@>kH{lxNlSG>+5!ay3@Mr z6^bcnovxj^8%DZ<6|EhK4>HLxYFNxymqY_Izhqf21 zms`u1V|=lE2~Frn+x(AiVxYmpU5+tBCoVgO*Zd^Fd#|JnT$?%-&kIPTpR1Bc1(moc z#<8`Fwdc*KnI2bc9W|`B*3{d*uq@GR`z+5W@x;T}3xjXe0Wa`m!F8J~rX9U)>t>wz zCVY+w%iVev+E#TeNK~kpShKcJ7%R-4N|LWeb4z%9J{vFdFg0%~ro49)T!9$%88yec zcM3i}e-scGms-=E)J1B%+nk9Y6j-=kJI3wDdmh>O>#3APlSaII}Q-I^Kq z?+L6qWqMWR#a`TBz9p_^Y{_PM^DO1HeS0wo58WrHRm*KQxl;E~_pscGvtmm2cz>{o zQQsr)`5lTyVrS0Aq>?VUb;&bEG`P`((nWI*3P4AA5yie)9td>`?rr-i&4OadBG1mE z@0|4^98%V<-LIWTR=m2j+3NCFa73nALU$Ey35K7eU}gS+X<)+=S0{CkYWJ0~^$WiQ zLdOwXvdwewVgvt+PM)tXcB9OuzhCu9lhtwX2efg9P!GV;-sz23d(fY$`RKjVW3D*< zs{u9XPx{%Rq)lI8v55IaTf&69t^Thq9TG%<)Z7@b{*Y(4rD^rSk$Ll z+KO7x#gf9C&e7ihg)ULF`(t+)v1xu%X+9!XDyvr}y=AV`)`Q5(V=VM5D+HhT^_p45 zSrYOk(Ei7nUspCInhW4H$X!_7_&u!|xVJEsjc+EIdpCY>bnF$W5=Z}J-*Z%g}F%Q&~fJ5WxvpO(+e-iPLkCl2)~X)jWgVfbO%X=qD5iJpD~aEAgTOx&>3NOtI+VIci<0(^=m;!qWTf>;UM^+H=mVWN?rBOHaG#Tc z>IQGU#Ml^b6N$4RkI%16O2vMk@Fss50NVl$=g&Rl>FBBSS<`Q_haIFc+nYBErR$}& z|2^iDl=&;0O#$#4S%EUpGTig*Za`Q|KZZs?X8QB>y!W8 zXs9|ET^dKaip^Dt4qup2_XFd+KP3_n7D9V5eS+ROq+IH_$<(5B5!>%Fvw{`y^N7u3 zZ1l`m5HG0>$z#$QI&=PPjy{wE@9t&!USceX)p%cgetuAJ2yVU$J{o?>@w%s3Xey*j zrR@G!3&mdcLQ!J#d+qZFbJ4K5;Am-@$l^3GluO@H>(932ovACDMz|P(oDzrrT!sDw zIv>?*H@9l}3VLAf6ckKsq6nrObO;dAN|`#wvZtOm{@_3pu-Xn zhy;jL53IHbbGC{>Z69i6q~jt^kIxXjd~#6*?P>;WvcI*Cn8Y zb~2{nD9mxtP+QYTbKFyA>K|;*-0~MenxVlMzq_!UtPKSWRlW&)ryx)FhP8JtHtiio z;^yYiR-Cw0*4U6P`Cdo(ES0b6)Zj-GDMS2YoK(WDa4V-giK}BrWi6>gW7~Jr-uS7> zl!I6i%f90J9|LR96&qY3>qp^fFgZwS2hf(y>M>8EB%y#xD3?e_~< z=1(pJHAuX({mdy#T)%y>gtM2E{evWIQ>i^VSSrg-tk}!Vu+r+q`wz{CG+F4-5%PMP z{#;dVu>iKZns6U((6*z39mj85)ikLhe9CEi;^X626T9n-hh^D^4uh|gwPFFN_{g5I zswq&)9k4;fg!;-Dz^-E~EVrNAyog$v(G%&QV}``nHj&qPwZD!irMMq1Xk<=tsYVnx zT;c0W5FE~50mWzxIe%`L{IZijLTccV_^~`tiIrJc$#pI0J~F@WPAUBR!1E=t0QnbF zftntjD`&$l6mvvSy$q!*QFbj+?a2Wq=7nS8%q=Y~;WXE%54#fTCHD&}_P9uJ{L=;$ zd3X;~Mx#0ss&mIyV0Q_5BNJY;G5KNs3wdP}Mil7Aj&Rzw-?{-{zs>PDK9EMs)BO11 zp6@EH50=YB>8|O$n3+`4M>uL7*ilr)lHKj%>{RpqGl1TO2I$(vvm5jI$sc7n#0Aj2#=%kz1Wa)0hfUZ?QRM0J#4K2AId%QrQd?K^Ln@>>=B$f?|sS7 zQO)ytyNM7hwHw**1O-$jc^In~H}T%O=H*1nr>=v)58Z!)+zo{Z+khTd<6Zd%7*k5= zRrCwX@*JGR8KiZDbal(pPuDUmZ4acq{jEh;s2!m6rHdF^e0sXGE3XWmPAgWWs7tB8 z^fdDL6&kW4@giv~9-{HX4Kqlc42h@jvJu3~crQ|ib=G|>2faZ_W6&wZx{_Im>KY42 zm~X+Xu$ZDvoF zdXdScSACd6N6#5LY4D9k6LN(5R39xakOn1jT=x+A(I^h>l*Qt)bQP?1mc#;n1B2wY z*@K7V=MUa3SnD-0SZJFoeP?Etlun%CtRCoU32zs63YZrrt_GN`?YzOn^jQSfxQ01_ zEe5pAaW&z};wLMR9O2H3J(6%QzS61)~|+$F7+Vf%twe z_&^>|p{opypQkEubVu!^7CW9wI(IU4c7dX;EK8h@Zrca5_^ixOrM`8c z_f2OSw7#!b{oGv`)|Qp;-Ko~Kw$7B1@+w0s7BlVpRC?;$;GF%QC`17*Dx<;~f|d`) z)%dMc#*UcIZoabOtVr`u6YaRSQjxvBXYoem2H3O?{_7p_ga|6zwR2ZpT0@EmIhcJz<07(0P)$&XTj&LM zoUbt~3gK{TK-}MT|0uu8k@G!T;7&dKdnC@Jp>r(hF1w&xoWIA3KZm-`TV<)Mj?5ll zl=v}tX@o>~#;%rANpHBye_BrqGlD#`@MH{u3CW$sZDjtYfoLX57G_Azz?z21O zH=gJT-$OIx8N+Sfh^K65-d6uX(=q<4gJTM^wmn&7$=^R&Y_hUUCRXf41OMCP2WJ5G zJQ3%&ab3Arly35vu2ZoK!wb*mYpJLOp8h)qyL}Q#quxKHU8GrYfi9jid+2m{0+6;G zDpmlCK-{G0;ODD}t3@Te>D?gwK&w8cQ!ft=!ZjgfA-bFU*(Yx{2m#l*ImlON#n^jL zI$)RAOJeTkvDz+Xdx&T9;59QXfJ%<|8seXQIX9Sh?mPKxas-p{p0zWiQn`}2_8W2I zy1FgnW?9PaaYbF8OCZvGdL1?8bRhc=gTjLRA{brA!=P#Of)@yayMPzMl4vp9q;>?+ z$ED|}V<~;~B)Fi!tab{pvXKrpOsTO-^h!}#?xBfu;(S}IF9MA z?y%Zzn|z)(1AFxk5W>EC1Df(vPPPN@%uMs{`<4N zL16S>zWyKc{69zk``u7BzkS<(?2r5>vwpI6no6$`3nWF^ z2WD&VX0abWEbHoqnEgYuabKo-=fh+4jUC@@#2hTgI@WXkPqWI`gEY3pRB#_{{UVF-0d9jioiM3au06#aBwUqJ| H?gjiG%@K1E literal 0 HcmV?d00001 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 1061e07e516a2..43a929591dad1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -104,6 +104,8 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; + virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; + virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_trailing_object) = 0; @@ -130,7 +132,7 @@ class LaneChangeBase virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, - [[maybe_unused]] PathWithLaneId & path) + [[maybe_unused]] PathWithLaneId & path, [[maybe_unused]] const bool is_waiting_approval = false) { } @@ -285,8 +287,7 @@ class LaneChangeBase FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; - - PathWithLaneId prev_approved_path_; + mutable std::optional terminal_lane_change_path_{std::nullopt}; int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 9d6fbd65acfd8..a69ae0d92647a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -129,8 +129,6 @@ class LaneChangeInterface : public SceneModuleInterface mutable MarkerArray virtual_wall_marker_; - std::unique_ptr prev_approved_path_; - void clearAbortApproval() { is_abort_path_approved_ = false; } bool is_abort_path_approved_{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 5f1da79bc7ea0..fe1d18ea6ea0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -65,13 +65,18 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, + const bool is_waiting_approval = false) override; - void insert_stop_point_on_current_lanes(PathWithLaneId & path); + void insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval = false); PathWithLaneId getReferencePath() const override; @@ -137,6 +142,8 @@ class NormalLaneChange : public LaneChangeBase bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; + std::optional compute_terminal_lane_change_path() const; + bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index 5f36be806bba4..d5bbfe25fe1e9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -128,6 +128,22 @@ struct Info terminal_lane_changing_velocity = _lc_metrics.velocity; shift_line = _shift_line; } + + void set_prepare(const PhaseMetrics & _prep_metrics) + { + longitudinal_acceleration.prepare = _prep_metrics.actual_lon_accel; + velocity.prepare = _prep_metrics.velocity; + duration.prepare = _prep_metrics.duration; + length.prepare = _prep_metrics.length; + } + + void set_lane_changing(const PhaseMetrics & _lc_metrics) + { + longitudinal_acceleration.lane_changing = _lc_metrics.actual_lon_accel; + velocity.lane_changing = _lc_metrics.velocity; + duration.lane_changing = _lc_metrics.duration; + length.lane_changing = _lc_metrics.length; + } }; struct TargetLaneLeadingObjects @@ -219,6 +235,8 @@ struct TransientData double target_lane_length{std::numeric_limits::min()}; + double dist_to_target_end{std::numeric_limits::max()}; + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 94020e8a08279..b25dbc99189e8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -137,12 +137,20 @@ struct DelayParameters double th_parked_vehicle_shift_ratio{0.6}; }; +struct TerminalPathParameters +{ + bool enable{false}; + bool disable_near_goal{false}; + bool stop_at_boundary{false}; +}; + struct Parameters { TrajectoryParameters trajectory{}; SafetyParameters safety{}; CancelParameters cancel{}; DelayParameters delay{}; + TerminalPathParameters terminal_path{}; // lane change parameters double backward_lane_length{200.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index eced227a5be32..29a9f258545a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -67,6 +67,9 @@ double calc_dist_to_last_fit_width( const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin = 0.1); + /** * @brief Calculates the maximum preparation longitudinal distance for lane change. * diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index dcc327b4793e1..77ba8fe68a653 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -48,7 +48,7 @@ using behavior_path_planner::lane_change::CommonDataPtr; */ bool get_prepare_segment( const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, - const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment); + const double prep_length, PathWithLaneId & prepare_segment); /** * @brief Generates the candidate path for a lane change maneuver. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index f80aad721a07c..9f3c6c9ef48bf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -43,8 +43,7 @@ LaneChangeInterface::LaneChangeInterface( std::unique_ptr && module_type) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{std::move(parameters)}, - module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()} + module_type_{std::move(module_type)} { module_type_->setTimeKeeper(getTimeKeeper()); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); @@ -109,7 +108,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); - *prev_approved_path_ = getPreviousModuleOutput().path; stop_pose_ = module_type_->getStopPose(); @@ -155,11 +153,9 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - BehaviorModuleOutput out = getPreviousModuleOutput(); + BehaviorModuleOutput out = module_type_->getTerminalLaneChangePath(); - *prev_approved_path_ = out.path; - - module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path, true); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 9f7a811770b5d..ec000b8fee97c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -242,6 +242,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); + // terminal lane change path + p.terminal_path.enable = getOrDeclareParameter(*node, parameter("terminal_path.enable")); + p.terminal_path.disable_near_goal = + getOrDeclareParameter(*node, parameter("terminal_path.disable_near_goal")); + p.terminal_path.stop_at_boundary = + getOrDeclareParameter(*node, parameter("terminal_path.stop_at_boundary")); + // validation of safety check parameters // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index fb4f7aeca1525..2719239baaed8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -169,6 +170,9 @@ void NormalLaneChange::update_transient_data(const bool is_approved) transient_data.dist_to_terminal_start = transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + transient_data.dist_to_target_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); transient_data.target_lane_length = @@ -246,6 +250,10 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin + if (valid_paths.empty() && terminal_lane_change_path_) { + valid_paths.push_back(terminal_lane_change_path_.value()); + } + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { @@ -380,6 +388,45 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + if ( + !lane_change_parameters_->terminal_path.enable || + !common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return prev_module_output_; + } + + const auto is_near_goal = lane_change_parameters_->terminal_path.disable_near_goal && + common_data_ptr_->lanes_ptr->target_lane_in_goal_section && + common_data_ptr_->transient_data.dist_to_target_end < + common_data_ptr_->transient_data.lane_changing_length.max; + if (is_near_goal) { + return prev_module_output_; + } + + const auto & current_lanes = get_current_lanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); + return prev_module_output_; + } + + const auto terminal_lc_path = compute_terminal_lane_change_path(); + + if (!terminal_lc_path) { + RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); + return prev_module_output_; + } + + auto output = prev_module_output_; + + output.path = *terminal_lc_path; + output.turn_signal_info = get_current_turn_signal_info(); + + extendOutputDrivableArea(output); + + return output; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -450,7 +497,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c } void NormalLaneChange::insert_stop_point( - const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, const bool is_waiting_approval) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { @@ -475,44 +522,42 @@ void NormalLaneChange::insert_stop_point( return; } - insert_stop_point_on_current_lanes(path); + insert_stop_point_on_current_lanes(path, is_waiting_approval); } -void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +void NormalLaneChange::insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval) { const auto & path_front_pose = path.points.front().point.pose; - const auto & center_line = common_data_ptr_->current_lanes_path.points; - const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { - return motion_utils::calcSignedArcLength( - center_line, path_front_pose.position, target.position); - }; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto dist_from_path_front = + motion_utils::calcSignedArcLength(path.points, path_front_pose.position, ego_pose.position); const auto & transient_data = common_data_ptr_->transient_data; const auto & lanes_ptr = common_data_ptr_->lanes_ptr; const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto dist_to_terminal = std::invoke([&]() -> double { - const auto target_pose = (lanes_ptr->current_lane_in_goal_section) - ? common_data_ptr_->route_handler_ptr->getGoalPose() - : center_line.back().point.pose; - return get_arc_length_along_lanelet(target_pose); - }); - - const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; - const auto min_dist_buffer = transient_data.current_dist_buffer.min; const auto dist_to_terminal_start = - dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); + transient_data.dist_to_terminal_start - calculation::calc_stopping_distance(lc_param_ptr); - const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto dist_to_terminal_stop = std::invoke([&]() -> double { const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { - return utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + if (!utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return dist_from_path_front + dist_to_terminal_start; } - return std::numeric_limits::max(); - }); - const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + if ( + terminal_lane_change_path_ && is_waiting_approval && + lc_param_ptr->terminal_path.stop_at_boundary) { + return calculation::calc_dist_to_last_fit_width(common_data_ptr_, path); + } + + const auto dist_to_last_fit_width = calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + + return std::min(dist_from_path_front + dist_to_terminal_start, dist_to_last_fit_width); + }); const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; if ( @@ -522,11 +567,13 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) return; } + const auto & center_line = common_data_ptr_->current_lanes_path.points; const auto dist_to_target_lane_start = std::invoke([&]() -> double { const auto & front_lane = lanes_ptr->target_neighbor.front(); const auto target_front = utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); - return get_arc_length_along_lanelet(target_front); + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target_front.position); }); const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( @@ -1129,7 +1176,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) PathWithLaneId prepare_segment; try { if (!utils::lane_change::get_prepare_segment( - common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { + common_data_ptr_, prev_module_output_.path, prep_metric.length, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1247,6 +1294,91 @@ bool NormalLaneChange::check_candidate_path_safety( return safety_check_with_normal_rss.is_safe; } +std::optional NormalLaneChange::compute_terminal_lane_change_path() const +{ + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_to_terminal_start = transient_data.dist_to_terminal_start; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; + const auto current_velocity = getEgoVelocity(); + + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, dist_to_terminal_start, prepare_segment)) { + RCLCPP_DEBUG(logger_, "failed to get valid prepare segment for terminal LC path"); + return std::nullopt; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "failed to get terminal LC path: %s", e.what()); + return std::nullopt; + } + + // t = 2 * d / (v1 + v2) + const auto duration_to_lc_start = + 2.0 * dist_to_terminal_start / (current_velocity + min_lc_velocity); + const auto lon_accel = std::invoke([&]() -> double { + if (duration_to_lc_start < calculation::eps) { + return 0.0; + } + return std::clamp( + (min_lc_velocity - current_velocity) / duration_to_lc_start, + lane_change_parameters_->trajectory.min_longitudinal_acc, + lane_change_parameters_->trajectory.max_longitudinal_acc); + }); + const auto vel_on_prep = current_velocity + lon_accel * duration_to_lc_start; + const LaneChangePhaseMetrics prep_metric( + duration_to_lc_start, dist_to_terminal_start, vel_on_prep, lon_accel, lon_accel, 0.0); + + if (terminal_lane_change_path_) { + terminal_lane_change_path_->info.set_prepare(prep_metric); + if (prepare_segment.points.empty()) { + terminal_lane_change_path_->path = terminal_lane_change_path_->shifted_path.path; + return terminal_lane_change_path_->path; + } + prepare_segment.points.pop_back(); + terminal_lane_change_path_->path = + utils::combinePath(prepare_segment, terminal_lane_change_path_->shifted_path.path); + return terminal_lane_change_path_->path; + } + + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prepare_segment.points.back().point.pose); + + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = transient_data.dist_to_terminal_end - prep_metric.length; + return std::min( + max_length, dist_lc_start_to_end_of_lanes - transient_data.next_dist_buffer.min); + }); + + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + constexpr double lane_changing_lon_accel{0.0}; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + lane_changing_lon_accel, max_lane_changing_length); + + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + + LaneChangePath candidate_path; + for (const auto & lc_metric : lane_changing_metrics) { + try { + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); + } catch (const std::exception & e) { + continue; + } + terminal_lane_change_path_ = candidate_path; + return candidate_path.path; + } + + return std::nullopt; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 550c0fa290a99..a0130fcd27041 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -58,22 +58,10 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) } double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPolygon2d & lanelet_polygon, + const universe_utils::LineString2d & line_string, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { - if (lanelets.empty()) return 0.0; - - const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); - const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); - - if (center_line.size() <= 1) return 0.0; - - universe_utils::LineString2d line_string; - line_string.reserve(center_line.size() - 1); - std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { - boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); - }); - const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; universe_utils::MultiPolygon2d center_line_polygon; namespace strategy = boost::geometry::strategy::buffer; @@ -85,7 +73,7 @@ double calc_dist_to_last_fit_width( if (center_line_polygon.empty()) return 0.0; std::vector intersection_points; - boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + boost::geometry::intersection(lanelet_polygon, center_line_polygon, intersection_points); if (intersection_points.empty()) { return utils::getDistanceToEndOfLane(src_pose, lanelets); @@ -102,6 +90,46 @@ double calc_dist_to_last_fit_width( return std::max(distance - (bpp_param.base_link2front + margin), 0.0); } +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & current_lanes_polygon = common_data_ptr->lanes_polygon_ptr->current; + const auto & bpp_param = *common_data_ptr->bpp_param_ptr; + + universe_utils::LineString2d line_string; + line_string.reserve(path.points.size() - 1); + std::for_each(path.points.begin() + 1, path.points.end(), [&line_string](const auto & point) { + const auto & position = point.point.pose.position; + boost::geometry::append(line_string, universe_utils::Point2d(position.x, position.y)); + }); + + const auto & src_pose = path.points.front().point.pose; + return calc_dist_to_last_fit_width( + current_lanes, current_lanes_polygon, line_string, src_pose, bpp_param, margin); +} + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + return calc_dist_to_last_fit_width( + lanelets, lane_polygon, line_string, src_pose, bpp_param, margin); +} + double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp index d7303d7d1df2d..44ee1624f0f51 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -154,7 +154,7 @@ using behavior_path_planner::lane_change::CommonDataPtr; bool get_prepare_segment( const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, - const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment) + const double prep_length, PathWithLaneId & prepare_segment) { const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto & target_lanes = common_data_ptr->lanes_ptr->target; @@ -168,7 +168,7 @@ bool get_prepare_segment( const size_t current_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( prepare_segment.points, common_data_ptr->get_ego_pose(), 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prep_metric.length, backward_path_length); + utils::clipPathLength(prepare_segment, current_seg_idx, prep_length, backward_path_length); if (prepare_segment.points.empty()) return false; From 203587335202d0c39260b40fae458300929d7a2b Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 27 Dec 2024 19:24:46 +0900 Subject: [PATCH 16/45] fix(autoware_scenario_selector): fix bugprone-branch-clone (#9699) fix: bugprone-error Signed-off-by: kobayu858 --- planning/autoware_scenario_selector/src/node.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 4bb64e27368d8..defbde4f6aabc 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -156,9 +156,8 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } else if (is_in_parking_lot) { return tier4_planning_msgs::msg::Scenario::PARKING; - } else { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } + return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { From 869cf4c435704f704e3d9ebc898d91ba697d2653 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 27 Dec 2024 19:25:33 +0900 Subject: [PATCH 17/45] fix(autoware_behavior_velocity_intersection_module): fix bugprone-branch-clone (#9702) fix: bugprone-error Signed-off-by: kobayu858 --- .../src/scene_intersection.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index e33416860c319..93a13a4d62737 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -354,9 +354,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < 0.0); const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { + if (over_stopline || (is_stopped_duration && approached_dist_stopline)) { state_machine.setState(StateMachine::State::GO); } return state_machine.getState() == StateMachine::State::GO; @@ -367,12 +365,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; + return over_stopline || (is_stopped && approached_dist_stopline); }; const auto occlusion_wo_tl_pass_judge_line_idx = From c8e00409c03555274c6ff26cc438e75144e1dcfd Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Sat, 28 Dec 2024 03:16:48 +0900 Subject: [PATCH 18/45] feat(pointpainting_fusion): enable cloud display on image (#9813) --- .../src/pointpainting_fusion/node.cpp | 90 ++++++++++--------- 1 file changed, 48 insertions(+), 42 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 2837bac458541..25d9064ddbfc7 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -316,54 +316,56 @@ dc | dc dc dc dc ||zc| // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); -#pragma omp parallel for - for (int i = 0; i < iterations; i++) { - int stride = p_step * i; - unsigned char * data = &painted_pointcloud_msg.data[0]; - unsigned char * output = &painted_pointcloud_msg.data[0]; - // cppcheck-suppress-begin invalidPointerCast - float p_x = *reinterpret_cast(&data[stride + x_offset]); - float p_y = *reinterpret_cast(&data[stride + y_offset]); - float p_z = *reinterpret_cast(&data[stride + z_offset]); - // cppcheck-suppress-end invalidPointerCast - point_lidar << p_x, p_y, p_z; - point_camera = lidar2cam_affine * point_lidar; - p_x = point_camera.x(); - p_y = point_camera.y(); - p_z = point_camera.z(); - - if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { - continue; - } + std::vector> local_vectors(omp_num_threads_); +#pragma omp parallel + { +#pragma omp for + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { + continue; + } - // project - Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( - cv::Point3d(p_x, p_y, p_z), projected_point)) { - // iterate 2d bbox - for (const auto & feature_object : objects) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if ( - !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + // project + Eigen::Vector2d projected_point; + if (camera_projectors_[image_id].calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && + isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } } } + if (debugger_) { + int thread_id = omp_get_thread_num(); + local_vectors[thread_id].push_back(projected_point); + } } -#if 0 - // Parallelizing loop don't support push_back - if (debugger_) { - debug_image_points.push_back(projected_point); - } -#endif } } - if (debugger_) { std::unique_ptr inner_st_ptr; if (time_keeper_) @@ -373,6 +375,10 @@ dc | dc dc dc dc ||zc| debug_image_rois.push_back(feature_object.feature.roi); } + for (const auto & local_vec : local_vectors) { + debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); + } + debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); From 58df2a112243562a49cf807fa8f64b7957a9ba44 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 31 Dec 2024 20:26:35 +0900 Subject: [PATCH 19/45] fix(goal_planner): fix usage of last_previous_module_output (#9811) --- .../examples/plot_map_case1.cpp | 10 ++-- .../examples/plot_map_case2.cpp | 10 ++-- .../fixed_goal_planner_base.hpp | 8 +-- .../goal_planner_module.hpp | 13 ++-- .../pull_over_planner/bezier_pull_over.hpp | 6 +- .../pull_over_planner/freespace_pull_over.hpp | 2 +- .../pull_over_planner/geometric_pull_over.hpp | 2 +- .../pull_over_planner_base.hpp | 2 +- .../pull_over_planner/shift_pull_over.hpp | 4 +- .../thread_data.hpp | 23 +++---- .../src/goal_planner_module.cpp | 60 ++++++++++--------- .../pull_over_planner/bezier_pull_over.cpp | 18 +++--- .../pull_over_planner/freespace_pull_over.cpp | 2 +- .../pull_over_planner/geometric_pull_over.cpp | 2 +- .../src/pull_over_planner/shift_pull_over.cpp | 10 ++-- .../src/thread_data.cpp | 5 +- 16 files changed, 87 insertions(+), 90 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 56e207c160873..46d3b97b71e58 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -288,7 +288,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -313,15 +313,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_module_output.path.points.front().point.pose.position, goal_pose.position); const auto & long_tail_reference_path = [&]() { if (prev_path_front_to_goal_dist > backward_length) { - return previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_module_output.path, planner_data, backward_length, 0.0, false); const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 102e5ef53b164..3518b5041be53 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -287,7 +287,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -312,15 +312,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_module_output.path.points.front().point.pose.position, goal_pose.position); const auto & long_tail_reference_path = [&]() { if (prev_path_front_to_goal_dist > backward_length) { - return previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_module_output.path, planner_data, backward_length, 0.0, false); const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index f670e3b05fa77..201b7c2d33bf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -38,15 +38,15 @@ class FixedGoalPlannerBase virtual BehaviorModuleOutput plan( const std::shared_ptr & planner_data) const = 0; - void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + void setPreviousModuleOutput(const BehaviorModuleOutput & upstream_module_output) { - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; } - BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + BehaviorModuleOutput getPreviousModuleOutput() const { return upstream_module_output_; } protected: - BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput upstream_module_output_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 3a1773324fe24..0765147cbeb80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -133,12 +133,12 @@ bool isOnModifiedGoal( const GoalPlannerParameters & parameters); bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output); + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output); bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output); + const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output); bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output); + const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output); bool needPathUpdate( const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, @@ -189,6 +189,7 @@ class LaneParkingPlanner void onTimer(); private: + const GoalPlannerParameters parameters_; std::mutex & mutex_; const std::optional & request_; LaneParkingResponse & response_; @@ -196,6 +197,10 @@ class LaneParkingPlanner rclcpp::Logger logger_; std::vector> pull_over_planners_; + BehaviorModuleOutput + original_upstream_module_output_; // plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector plans( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output); + const BehaviorModuleOutput & upstream_module_output); private: const LaneDepartureChecker lane_departure_checker_; @@ -50,7 +50,7 @@ class BezierPullOver : public PullOverPlannerBase std::vector generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; PathWithLaneId generateReferencePath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index e32488965f69a..37c82ea904a55 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -41,7 +41,7 @@ class FreespacePullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: const double velocity_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 89181b258fbea..d15c1e796bbe7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -45,7 +45,7 @@ class GeometricPullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 5b336a7de6acc..5af69894c0a2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -130,7 +130,7 @@ class PullOverPlannerBase virtual std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) = 0; + const BehaviorModuleOutput & upstream_module_output) = 0; protected: const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 08e0b8508c991..2c6aec919bfbb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -38,7 +38,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -49,7 +49,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index bfef4226b2661..7173b275e26fb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -32,35 +32,27 @@ class LaneParkingRequest { public: LaneParkingRequest( - const GoalPlannerParameters & parameters, const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates, const BehaviorModuleOutput & previous_module_output) - : parameters_(parameters), - vehicle_footprint_(vehicle_footprint), + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output) + : vehicle_footprint_(vehicle_footprint), goal_candidates_(goal_candidates), - previous_module_output_(previous_module_output), - last_previous_module_output_(previous_module_output) + upstream_module_output_(upstream_module_output) { } void update( const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & upstream_module_output, const std::optional & pull_over_path, const PathDecisionState & prev_data); - const GoalPlannerParameters parameters_; const autoware::universe_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; const std::shared_ptr & get_planner_data() const { return planner_data_; } const ModuleStatus & get_current_status() const { return current_status_; } - const BehaviorModuleOutput & get_previous_module_output() const - { - return previous_module_output_; - } - const BehaviorModuleOutput & get_last_previous_module_output() const + const BehaviorModuleOutput & get_upstream_module_output() const { - return last_previous_module_output_; + return upstream_module_output_; } const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } @@ -68,8 +60,7 @@ class LaneParkingRequest private: std::shared_ptr planner_data_; ModuleStatus current_status_; - BehaviorModuleOutput previous_module_output_; - BehaviorModuleOutput last_previous_module_output_; + BehaviorModuleOutput upstream_module_output_; std::optional pull_over_path_; // LATERAL_DEVIATION_THRESH) { return true; } @@ -185,19 +185,19 @@ bool hasPreviousModulePathShapeChanged( } bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) + const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output) { return std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, + last_upstream_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > 0.3; } bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output) + const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output) { constexpr double LATERAL_DEVIATION_THRESH = 0.3; return std::abs(autoware::motion_utils::calcLateralOffset( - previous_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > + upstream_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH; } @@ -282,7 +282,8 @@ LaneParkingPlanner::LaneParkingPlanner( const std::optional & request, LaneParkingResponse & response, std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, const GoalPlannerParameters & parameters) -: mutex_(lane_parking_mutex), +: parameters_(parameters), + mutex_(lane_parking_mutex), request_(request), response_(response), is_lane_parking_cb_running_(is_lane_parking_cb_running), @@ -335,12 +336,10 @@ void LaneParkingPlanner::onTimer() return; } const auto & local_request = local_request_opt.value(); - const auto & parameters = local_request.parameters_; const auto & goal_candidates = local_request.goal_candidates_; const auto & local_planner_data = local_request.get_planner_data(); const auto & current_status = local_request.get_current_status(); - const auto & previous_module_output = local_request.get_previous_module_output(); - const auto & last_previous_module_output = local_request.get_last_previous_module_output(); + const auto & upstream_module_output = local_request.get_upstream_module_output(); const auto & pull_over_path_opt = local_request.get_pull_over_path(); const auto & prev_data = local_request.get_prev_data(); @@ -371,19 +370,21 @@ void LaneParkingPlanner::onTimer() ? std::make_optional(pull_over_path_opt.value().modified_goal()) : std::nullopt; if (isOnModifiedGoal( - local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters_)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, previous_module_output)) { + if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, upstream_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } - if (hasPreviousModulePathShapeChanged(previous_module_output, last_previous_module_output)) { + if (hasPreviousModulePathShapeChanged( + upstream_module_output, original_upstream_module_output_)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if ( - hasDeviatedFromLastPreviousModulePath(*local_planner_data, last_previous_module_output) && + hasDeviatedFromLastPreviousModulePath( + *local_planner_data, original_upstream_module_output_) && current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; @@ -400,8 +401,8 @@ void LaneParkingPlanner::onTimer() // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( - local_planner_data, parameters.backward_goal_search_length, - parameters.forward_goal_search_length, + local_planner_data, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; @@ -410,7 +411,7 @@ void LaneParkingPlanner::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { const auto pull_over_path = planner->plan( - goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); + goal_candidate, path_candidates.size(), local_planner_data, upstream_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -428,13 +429,13 @@ void LaneParkingPlanner::onTimer() // todo: currently non centerline input path is supported only by shift pull over const bool is_center_line_input_path = goal_planner_utils::isReferencePath( - previous_module_output.reference_path, previous_module_output.path, 0.1); + upstream_module_output.reference_path, upstream_module_output.path, 0.1); RCLCPP_DEBUG( getLogger(), "the input path of pull over planner is center line: %d", is_center_line_input_path); // plan candidate paths and set them to the member variable - if (parameters.path_priority == "efficient_path") { + if (parameters_.path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { @@ -444,7 +445,7 @@ void LaneParkingPlanner::onTimer() planCandidatePaths(planner, goal_candidate); } } - } else if (parameters.path_priority == "close_goal") { + } else if (parameters_.path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line @@ -457,14 +458,15 @@ void LaneParkingPlanner::onTimer() } else { RCLCPP_ERROR( getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", - parameters.path_priority.c_str()); + parameters_.path_priority.c_str()); throw std::domain_error("[pull_over] invalid path_priority"); } // set response { + original_upstream_module_output_ = upstream_module_output; std::lock_guard guard(mutex_); - response_.pull_over_path_candidates = path_candidates; + response_.pull_over_path_candidates = std::move(path_candidates); response_.closest_start_pose = closest_start_pose; RCLCPP_INFO( getLogger(), "generated %lu pull over path candidates", @@ -562,7 +564,7 @@ std::pair GoalPlannerModule::sync // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then - // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to + // planWaitingApproval()/plan(), so we can copy latest current_status/upstream_module_output to // lane_parking_request/freespace_parking_request std::optional pull_over_path = @@ -578,7 +580,7 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(lane_parking_mutex_); if (!lane_parking_request_) { lane_parking_request_.emplace( - *parameters_, vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); + vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); } // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that // planner_data_ is not nullptr, so it is OK to copy as value diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index 0da5ab5dae38b..ad42ccf1582ab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -43,7 +43,7 @@ BezierPullOver::BezierPullOver( std::optional BezierPullOver::plan( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -55,7 +55,7 @@ std::optional BezierPullOver::plan( std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -67,7 +67,7 @@ std::optional BezierPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generateBezierPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path.empty()) { return pull_over_path.at(0); @@ -80,7 +80,7 @@ std::optional BezierPullOver::plan( std::vector BezierPullOver::plans( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -92,7 +92,7 @@ std::vector BezierPullOver::plans( std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -105,7 +105,7 @@ std::vector BezierPullOver::plans( std::vector paths; for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { auto pull_over_paths = generateBezierPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); std::copy( std::make_move_iterator(pull_over_paths.begin()), @@ -118,7 +118,7 @@ std::vector BezierPullOver::plans( std::vector BezierPullOver::generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -133,7 +133,7 @@ std::vector BezierPullOver::generateBezierPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path @@ -318,7 +318,7 @@ std::vector BezierPullOver::generateBezierPath( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); + expanded_lanes, upstream_module_output.drivable_area_info.drivable_lanes); return !lane_departure_checker_.checkPathWillLeaveLane( utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 5b11d1b170ce1..473f76b5904d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -60,7 +60,7 @@ FreespacePullOver::FreespacePullOver( std::optional FreespacePullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index c0bac5c5ce2a8..74b3fe149fd1d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -40,7 +40,7 @@ GeometricPullOver::GeometricPullOver( std::optional GeometricPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index edecfd733d3be..9a3e2d6b13db9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -41,7 +41,7 @@ ShiftPullOver::ShiftPullOver( std::optional ShiftPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) + const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -52,7 +52,7 @@ std::optional ShiftPullOver::plan( const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -64,7 +64,7 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; return pull_over_path; @@ -134,7 +134,7 @@ std::optional ShiftPullOver::cropPrevModulePath( std::optional ShiftPullOver::generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -151,7 +151,7 @@ std::optional ShiftPullOver::generatePullOverPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp index f12d510e23030..89458c199c130 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -21,14 +21,13 @@ namespace autoware::behavior_path_planner void LaneParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, - const BehaviorModuleOutput & previous_module_output, + const BehaviorModuleOutput & upstream_module_output, const std::optional & pull_over_path, const PathDecisionState & prev_data) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); current_status_ = current_status; - last_previous_module_output_ = previous_module_output_; - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; pull_over_path_ = pull_over_path; prev_data_ = prev_data; } From 33b9913a65cc8cfdb99a3e0d776421f430f66318 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 31 Dec 2024 23:09:05 +0900 Subject: [PATCH 20/45] ci: pass run condition to reusable workflow (#9771) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ryohsuke Mitsudome Co-authored-by: M. Fatih Cırıt --- .../actions/evaluate-job-result/action.yaml | 45 ------------ .../build-and-test-differential.yaml | 5 ++ .github/workflows/build-test-tidy-pr.yaml | 72 +++---------------- .../workflows/clang-tidy-differential.yaml | 5 ++ 4 files changed, 19 insertions(+), 108 deletions(-) delete mode 100644 .github/actions/evaluate-job-result/action.yaml diff --git a/.github/actions/evaluate-job-result/action.yaml b/.github/actions/evaluate-job-result/action.yaml deleted file mode 100644 index c5c013080fd27..0000000000000 --- a/.github/actions/evaluate-job-result/action.yaml +++ /dev/null @@ -1,45 +0,0 @@ -name: Evaluate Job Result -description: Evaluates the result of a job and updates the summary. -inputs: - job_result: - description: Result of the job to evaluate (e.g., success, failure, skipped) - required: true - type: string - job_name: - description: Name of the job to evaluate - required: true - type: string - expected_results: - description: Comma-separated list of acceptable results (e.g., success,skipped) - required: true - type: string -outputs: - failed: - description: Indicates if the job failed - value: ${{ steps.evaluate.outputs.failed }} - -runs: - using: composite - steps: - - name: Evaluate Job Result - id: evaluate - run: | - JOB_RESULT="${{ inputs.job_result }}" - IFS=',' read -ra EXPECTED <<< "${{ inputs.expected_results }}" - FAILED=false - - for RESULT in "${EXPECTED[@]}"; do - if [[ "$JOB_RESULT" == "$RESULT" ]]; then - echo "- **${{ inputs.job_name }}:** "$JOB_RESULT" ✅" >> "$GITHUB_STEP_SUMMARY" - echo "failed=false" >> "$GITHUB_OUTPUT" - exit 0 - fi - done - - # If no expected result matched - echo "::error::${{ inputs.job_name }} failed. ❌" - echo "- **${{ inputs.job_name }}:** failed ❌" >> "$GITHUB_STEP_SUMMARY" - echo "failed=true" >> "$GITHUB_OUTPUT" - - exit 1 - shell: bash diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 0925cb0f53930..c25f6b24c5e47 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -14,6 +14,10 @@ on: default: humble required: false type: string + run-condition: + default: true + required: false + type: boolean container-suffix: required: false default: "" @@ -28,6 +32,7 @@ env: jobs: build-and-test-differential: + if: ${{ inputs.run-condition }} runs-on: ${{ inputs.runner }} container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml index d1ea9b2d0f79d..c0442694b5c77 100644 --- a/.github/workflows/build-test-tidy-pr.yaml +++ b/.github/workflows/build-test-tidy-pr.yaml @@ -40,98 +40,44 @@ jobs: shell: bash build-and-test-differential: + if: ${{ always() }} needs: - require-label uses: ./.github/workflows/build-and-test-differential.yaml with: container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.require-label.outputs.result == 'true' }} secrets: codecov-token: ${{ secrets.CODECOV_TOKEN }} build-and-test-differential-cuda: + if: ${{ always() }} needs: check-if-cuda-job-is-needed - if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} uses: ./.github/workflows/build-and-test-differential.yaml with: container: ghcr.io/autowarefoundation/autoware:universe-devel container-suffix: -cuda + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} secrets: codecov-token: ${{ secrets.CODECOV_TOKEN }} - build-test-pr: - needs: - - build-and-test-differential - - build-and-test-differential-cuda - if: ${{ always() }} - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - - name: Initialize Summary - run: echo "### Build Test PR Results" > $GITHUB_STEP_SUMMARY - shell: bash - - - name: Evaluate build-and-test-differential - uses: ./.github/actions/evaluate-job-result - with: - job_result: ${{ needs.build-and-test-differential.result }} - job_name: build-and-test-differential - expected_results: success - - - name: Evaluate build-and-test-differential-cuda - if: ${{ always() }} - uses: ./.github/actions/evaluate-job-result - with: - job_result: ${{ needs.build-and-test-differential-cuda.result }} - job_name: build-and-test-differential-cuda - expected_results: success,skipped - clang-tidy-differential: + if: ${{ always() }} # always run to provide report for status check needs: - check-if-cuda-job-is-needed - build-and-test-differential - if: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' }} uses: ./.github/workflows/clang-tidy-differential.yaml with: container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' && needs.build-and-test-differential.result == 'success' }} clang-tidy-differential-cuda: + if: ${{ always() }} # always run to provide report for status check needs: + - check-if-cuda-job-is-needed - build-and-test-differential-cuda uses: ./.github/workflows/clang-tidy-differential.yaml with: container: ghcr.io/autowarefoundation/autoware:universe-devel container-suffix: -cuda - - clang-tidy-pr: - needs: - - clang-tidy-differential - - clang-tidy-differential-cuda - if: ${{ always() }} - runs-on: ubuntu-latest - steps: - - name: Initialize Summary - run: echo "### Clang Tidy PR Results" > $GITHUB_STEP_SUMMARY - shell: bash - - - name: Check clang-tidy success - if: ${{ needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success' }} - run: | - echo "✅ Either one of the following has succeeded:" >> $GITHUB_STEP_SUMMARY - - - name: Fail if conditions not met - if: ${{ !(needs.clang-tidy-differential.result == 'success' || needs.clang-tidy-differential-cuda.result == 'success') }} - run: | - echo "::error::❌ Either one of the following should have succeeded:" - echo "::error::clang-tidy-differential: ${{ needs.clang-tidy-differential.result }}" - echo "::error::clang-tidy-differential-cuda: ${{ needs.clang-tidy-differential-cuda.result }}" - - echo "❌ Either one of the following should have succeeded:" >> $GITHUB_STEP_SUMMARY - - exit 1 - - - name: Print the results - if: ${{ always() }} - run: | - echo "- **clang-tidy-differential:** ${{ needs.clang-tidy-differential.result }}" >> $GITHUB_STEP_SUMMARY - echo "- **clang-tidy-differential-cuda:** ${{ needs.clang-tidy-differential-cuda.result }}" >> $GITHUB_STEP_SUMMARY + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' && needs.build-and-test-differential-cuda.result == 'success' }} diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index 51e0a8408468c..41d722c8e07b0 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.github/workflows/clang-tidy-differential.yaml @@ -14,9 +14,14 @@ on: default: ubuntu-24.04 required: false type: string + run-condition: + default: true + required: false + type: boolean jobs: clang-tidy-differential: + if: ${{ inputs.run-condition }} runs-on: ${{ inputs.runner }} container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: From 77c1a3f02a4cc5afece9ac701193f24de7af3755 Mon Sep 17 00:00:00 2001 From: Atto Armoo <168620037+AA-T4@users.noreply.github.com> Date: Tue, 31 Dec 2024 23:26:19 +0900 Subject: [PATCH 21/45] fix(planning): corrects minor typos (#9809) Signed-off-by: Atto Armoo <168620037+AA-T4@users.noreply.github.com> --- planning/README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/planning/README.md b/planning/README.md index ccf8288df3911..3fadafe54ed4c 100644 --- a/planning/README.md +++ b/planning/README.md @@ -18,7 +18,7 @@ Enabling and disabling modules involves managing settings in key configuration a ### Key Files for Configuration -The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disable or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: +The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disabled or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. - `motion_stop_planner_type`: Set `default` to either `obstacle_stop_planner` or `obstacle_cruise_planner`. @@ -35,7 +35,7 @@ The [launch files](https://github.com/autowarefoundation/autoware.universe/tree/ corresponds to launch_avoidance_module from `default_preset.yaml`. -### Parameters configuration +### Parameter Configuration There are multiple parameters available for configuration, and users have the option to modify them in [here](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning). It's important to note that not all parameters are adjustable via `rqt_reconfigure`. To ensure the changes are effective, modify the parameters and then restart Autoware. Additionally, detailed information about each parameter is available in the corresponding documents under the planning tab. @@ -43,7 +43,7 @@ There are multiple parameters available for configuration, and users have the op This guide outlines the steps for integrating your custom module into Autoware: -- Add your modules to the `default_preset.yaml` file. For example +- Add your modules to the `default_preset.yaml` file. For example: ```yaml - arg: @@ -51,7 +51,7 @@ This guide outlines the steps for integrating your custom module into Autoware: default: "true" ``` -- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): +- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): ```xml @@ -63,14 +63,14 @@ This guide outlines the steps for integrating your custom module into Autoware: /> ``` -- If applicable, place your parameter folder within the appropriate existing parameter folder. For example [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) is in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). -- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example `behavior_velocity_planner_intersection_module_param_path` is used. +- If applicable, place your parameter folder within the appropriate existing parameter folder. For example, [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) are in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). +- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example, `behavior_velocity_planner_intersection_module_param_path` is used. ```xml ``` -- Define your parameter path variable within the corresponding launcher. For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) +- Define your parameter path variable within the corresponding launcher. For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) ```xml @@ -82,7 +82,7 @@ This guide outlines the steps for integrating your custom module into Autoware: ## Join Our Community-Driven Effort -Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome it all with open arms. +Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome all contributions with open arms. ### How to Contribute? @@ -105,7 +105,7 @@ Interested in joining our meetings? We’d love to have you! For more informatio Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. -If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. +If you use the Jerk Constrained Velocity Planning algorithm in the [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you cite the relevant paper. From a32cae1d4547290d9e4c3bb9b86480bfc373459d Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 2 Jan 2025 11:12:43 +0000 Subject: [PATCH 22/45] chore: sync files (#9825) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions --- .pre-commit-config-optional.yaml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index ff325af5e8774..f0b5bdba2d24d 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -2,6 +2,13 @@ # https://github.com/autowarefoundation/sync-file-templates # To make changes, update the source repository and follow the guidelines in its README. +# https://pre-commit.ci/#configuration +ci: + autofix_commit_msg: "style(pre-commit-optional): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit-optional): quarterly autoupdate" + repos: - repo: https://github.com/tcort/markdown-link-check rev: v3.12.2 From 176e2e31a4f1b89a6ad4b6e4af5a04c3cc4ba4c3 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 6 Jan 2025 11:25:20 +0900 Subject: [PATCH 23/45] feat(behavior_velocity_modules): add node test (#9790) * feat(behavior_velocity_crosswalk): add node test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * feat(behavior_velocity_xxx_module): add node test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * change directory tests -> test Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 10 +++ .../package.xml | 2 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../CMakeLists.txt | 1 + .../package.xml | 1 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../CMakeLists.txt | 1 + .../package.xml | 1 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../package.xml | 1 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../CMakeLists.txt | 1 + .../package.xml | 1 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../CMakeLists.txt | 17 ----- .../behavior_velocity_planner/node.hpp | 16 ++--- .../package.xml | 7 +- .../src/node.cpp | 16 ++--- .../srv/LoadPlugin.srv | 3 - .../srv/UnloadPlugin.srv | 3 - .../CMakeLists.txt | 9 +-- .../package.xml | 1 + .../{tests => test}/test_dynamic_obstacle.cpp | 0 .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../{tests => test}/test_path_utils.cpp | 0 .../{tests => test}/test_state_machine.cpp | 0 .../{tests => test}/test_utils.cpp | 0 .../CMakeLists.txt | 1 + .../package.xml | 1 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../CMakeLists.txt | 1 + .../package.xml | 1 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ .../package.xml | 1 + .../test/test_node_interface.cpp | 65 +++++++++++++++++++ .../CMakeLists.txt | 10 +++ .../package.xml | 2 + .../test/test_node_interface.cpp | 64 ++++++++++++++++++ 38 files changed, 699 insertions(+), 50 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp delete mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv delete mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv rename planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/{tests => test}/test_dynamic_obstacle.cpp (100%) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp rename planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/{tests => test}/test_path_utils.cpp (100%) rename planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/{tests => test}/test_state_machine.cpp (100%) rename planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/{tests => test}/test_utils.cpp (100%) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index b4a688d711221..b28e486c9be05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -13,4 +13,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index 9739370387df7..8c34678f439ed 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension @@ -32,6 +33,7 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0ceca4e12951 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "blind_spot", "autoware::behavior_velocity_planner::BlindSpotModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt index 10b694ce22cbd..3a429008c1b8e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt @@ -15,6 +15,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_crosswalk.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index ff01e85aa5456..1f0e9e68a65c6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -21,6 +21,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface autoware_grid_map_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..7a875327d4dde --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "crosswalk", "autoware::behavior_velocity_planner::CrosswalkModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt index 3aff4a524ffdd..53eafaffbba6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index ff91cf40a32a6..4ae1d20991078 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -17,6 +17,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..f84d22debea8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "detection_area", "autoware::behavior_velocity_planner::DetectionAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 208e7ed49de71..4f7c3aeea7618 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -19,6 +19,7 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface autoware_internal_debug_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..b515107e0ae8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "intersection", "autoware::behavior_velocity_planner::IntersectionModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt index b710924410549..e6b362271afb3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 4415eadef62c7..28677bfb80b15 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface autoware_interpolation diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..875e757cbfcec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "no_stopping_area", "autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt index 31900a23d00e5..a520b02a43c05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -2,14 +2,6 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "srv/LoadPlugin.srv" - "srv/UnloadPlugin.srv" - DEPENDENCIES -) autoware_package() @@ -24,15 +16,6 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib EXECUTABLE ${PROJECT_NAME}_node ) -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${PROJECT_NAME}_lib - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") -endif() - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_node_interface.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 4c1f10c355226..8a52ae70231d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -21,10 +21,9 @@ #include #include -#include -#include #include +#include #include #include #include @@ -45,8 +44,6 @@ namespace autoware::behavior_velocity_planner { -using autoware_behavior_velocity_planner::srv::LoadPlugin; -using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; using tier4_planning_msgs::msg::VelocityLimit; @@ -125,13 +122,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - rclcpp::Service::SharedPtr srv_load_plugin_; - rclcpp::Service::SharedPtr srv_unload_plugin_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; void onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - const UnloadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); void onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); // mutex for planner_data_ std::mutex mutex_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 04b4cf0328fd1..eca49a09c8a56 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -32,9 +32,8 @@ autoware_cmake eigen3_cmake_module - rosidl_default_generators - autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -60,15 +59,11 @@ tier4_v2x_msgs visualization_msgs - rosidl_default_runtime - ament_cmake_ros ament_lint_auto autoware_lint_common - rosidl_interface_packages - ament_cmake diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 5f78e4c670b49..1e3e691076b8a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -70,9 +70,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); - srv_load_plugin_ = create_service( + srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); - srv_unload_plugin_ = create_service( + srv_unload_plugin_ = create_service( "~/service/unload_plugin", std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); @@ -112,19 +112,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } void BehaviorVelocityPlannerNode::onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, - [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launchScenePlugin(*this, request->plugin_name); + planner_manager_.launchScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.removeScenePlugin(*this, request->plugin_name); + planner_manager_.removeScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onParam() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt index da2f4ce33ff60..a2f35ada11b5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt @@ -17,10 +17,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - tests/test_dynamic_obstacle.cpp - tests/test_path_utils.cpp - tests/test_utils.cpp - tests/test_state_machine.cpp + test/test_dynamic_obstacle.cpp + test/test_path_utils.cpp + test/test_utils.cpp + test/test_state_machine.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} autoware_behavior_velocity_run_out_module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index b3ced8b2e9b9f..18db8281356f8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -20,6 +20,7 @@ eigen3_cmake_module autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_internal_debug_msgs autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..75bf59751ed44 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "run_out", "autoware::behavior_velocity_planner::RunOutModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt index f4528f0d13cf4..a187b4cda9459 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_scene.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index fd4b9f83ae998..849ab3915c649 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -20,6 +20,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..c6d6ff638cfbb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "stop_line", "autoware::behavior_velocity_planner::StopLineModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt index 6370dd5e6c21d..02ed192459970 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt @@ -15,6 +15,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index ebc45d372f92d..4bc7a15cddc48 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -19,6 +19,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e24c2dab5dab9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "traffic_light", "autoware::behavior_velocity_planner::TrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 6d3bc68242d7c..056f84a970c58 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..9cb6bbfd40639 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt index 11504d9c8999c..8b11fdce7283e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt @@ -11,4 +11,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_walkway.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 5f1aea22855a4..857d9b524beb0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils @@ -31,6 +32,7 @@ rclcpp visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0b7fa5c31965 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "walkway", "autoware::behavior_velocity_planner::WalkwayModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner From a8a843a6cf7645a125658bdeb8785bd9d5fa71ac Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 6 Jan 2025 11:59:04 +0900 Subject: [PATCH 24/45] fix(simple_planning_simulator): fix bugprone-branch-clone (#9725) * fix: bugprone-error Signed-off-by: kobayu858 * fix: build error Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../simple_planning_simulator_core.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index d61654c32af02..8c95e144b3f8d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -585,9 +585,7 @@ void SimplePlanningSimulator::set_input(const InputCommand & cmd, const double a std::visit( [this, acc_by_slope](auto && arg) { using T = std::decay_t; - if constexpr (std::is_same_v) { - set_input(arg, acc_by_slope); - } else if constexpr (std::is_same_v) { + if constexpr (std::is_same_v || std::is_same_v) { set_input(arg, acc_by_slope); } else { throw std::invalid_argument("Invalid input command type"); @@ -640,7 +638,7 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { // NOLINT input << combined_acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || From 4a181d6a6d09071a22a99ce71a4ad2c3d18c827e Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 6 Jan 2025 12:23:41 +0900 Subject: [PATCH 25/45] fix(autoware_behavior_velocity_occlusion_spot_module): fix bugprone-macro-parentheses (#9712) * fix: bugprone-error Signed-off-by: kobayu858 * fix: fmt Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/scene_occlusion_spot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 522e83390b0f1..c5d1cf61614b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -36,7 +36,7 @@ // turn on only when debugging. #define DEBUG_PRINT(enable, n, x) \ if (enable) { \ - const std::string time_msg = n + std::to_string(x); \ + const std::string time_msg = (n) + std::to_string(x); \ RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ } From ca2d4e7774abe3a8929fecbcfd17c2f1bb7da55f Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 6 Jan 2025 16:40:09 +0900 Subject: [PATCH 26/45] fix(autoware_carla_interface): fix lidar topic name (#9645) Signed-off-by: Maxime CLEMENT --- .../src/autoware_carla_interface/carla_ros.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py index a4f8dee7af1a0..9b978b66feefb 100644 --- a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py @@ -147,7 +147,7 @@ def __init__(self): elif sensor["type"] == "sensor.lidar.ray_cast": if sensor["id"] in self.sensor_frequencies: self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( - PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10 + PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud_before_sync', 10 ) else: self.ros2_node.get_logger().info( From d86c46a35882e75051a0abcba55853da78c1d21f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 6 Jan 2025 17:42:32 +0900 Subject: [PATCH 27/45] feat(motion_velocity_planner): introduce Object/Pointcloud structure in PlannerData (#9812) * feat: new object/pointcloud struct in motion velocity planner Signed-off-by: Takayuki Murooka * update planner_data Signed-off-by: Takayuki Murooka * modify modules Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/dynamic_obstacle_stop_module.cpp | 2 +- .../src/object_filtering.cpp | 20 +++--- .../src/object_filtering.hpp | 4 +- .../test/test_object_filtering.cpp | 6 +- .../src/obstacle_velocity_limiter.cpp | 2 +- .../src/obstacle_velocity_limiter.hpp | 2 +- .../src/obstacle_velocity_limiter_module.cpp | 6 +- .../src/obstacles.cpp | 14 ++-- .../src/obstacles.hpp | 4 +- .../test/test_collision_distance.cpp | 9 ++- .../src/filter_predicted_objects.cpp | 25 +++---- .../planner_data.hpp | 66 ++++++++++++++++++- .../src/node.cpp | 5 +- 13 files changed, 121 insertions(+), 44 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 40aa985ee42e2..7fd5834ba2cfa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -131,7 +131,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( ? 0.0 : params_.hysteresis; const auto dynamic_obstacles = dynamic_obstacle_stop::filter_predicted_objects( - planner_data->predicted_objects, ego_data, params_, hysteresis); + planner_data->objects, ego_data, params_, hysteresis); const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 2ab3c09e64edd..0726fdba02de0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -96,20 +96,22 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; - for (const auto & object : objects.objects) { - const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity; + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; + const auto is_not_too_slow = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && is_not_too_slow && - is_in_range(object, ego_data.trajectory, params, hysteresis) && - is_not_too_close(object, ego_data, params.ego_longitudinal_offset) && + is_vehicle(predicted_object) && is_not_too_slow && + is_in_range(predicted_object, ego_data.trajectory, params, hysteresis) && + is_not_too_close(predicted_object, ego_data, params.ego_longitudinal_offset) && (!params.ignore_unavoidable_collisions || - !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params))) - filtered_objects.push_back(object); + !is_unavoidable(predicted_object, ego_data.pose, ego_data.earliest_stop_pose, params))) + filtered_objects.push_back(predicted_object); } return filtered_objects; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index d20d6354066c6..6024cde019489 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,6 +17,8 @@ #include "types.hpp" +#include + #include #include @@ -74,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 3fa179903c23f..5993f81c7a8f1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -14,6 +14,7 @@ #include "../src/object_filtering.hpp" +#include #include #include @@ -26,6 +27,8 @@ #include #include +#include + TEST(TestObjectFiltering, isVehicle) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_vehicle; @@ -202,8 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - autoware_perception_msgs::msg::PredictedObjects objects; - autoware_perception_msgs::msg::PredictedObject object; + std::vector objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index e501b756af6a1..9be7f52bca99a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index d7d2de63b33f8..b0cdb1f802e0f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 532cc834c19ac..1923f023552e3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -170,7 +170,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("obstacles"); obstacle_masks.negative_masks = obstacle_velocity_limiter::createPolygonMasks( - planner_data->predicted_objects, obstacle_params_.dynamic_obstacles_buffer, + planner_data->objects, obstacle_params_.dynamic_obstacles_buffer, obstacle_params_.dynamic_obstacles_min_vel); if (obstacle_params_.ignore_on_path) obstacle_masks.negative_masks.push_back(obstacle_velocity_limiter::createTrajectoryFootprint( @@ -189,8 +189,8 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_masks.positive_mask = obstacle_velocity_limiter::createEnvelopePolygon(footprint_polygons); obstacle_velocity_limiter::addSensorObstacles( - obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud, obstacle_masks, - obstacle_params_); + obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud.pointcloud, + obstacle_masks, obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); autoware::motion_utils::VirtualWalls virtual_walls; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 47215ee5cb0c4..187f028dc5969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { @@ -58,17 +59,18 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity) + const std::vector & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; - for (const auto & object : objects.objects) { + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + predicted_object.kinematics.initial_pose_with_covariance.pose, + predicted_object.shape.dimensions, buffer)); } } return polygons; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index 7e4704ba6818c..a98d92994f497 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -18,6 +18,7 @@ #include "parameters.hpp" #include "types.hpp" +#include #include #include @@ -163,8 +164,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity); + const std::vector & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index ac07c62f62cf7..023ae83774917 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -15,6 +15,7 @@ #include "../src/distance.hpp" #include "../src/obstacles.hpp" #include "../src/types.hpp" +#include "autoware/motion_velocity_planner_common/planner_data.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -27,6 +28,7 @@ #include #include +#include const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { @@ -387,11 +389,12 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { + using autoware::motion_velocity_planner::PlannerData; using autoware::motion_velocity_planner::obstacle_velocity_limiter::createObjectPolygons; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - PredictedObjects objects; + std::vector objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -404,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.objects.push_back(object1); + objects.push_back(PlannerData::Object(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -431,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.objects.push_back(object2); + objects.push_back(PlannerData::Object(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index f9ba7f4af9877..a6c8368c20571 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -106,23 +106,26 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data.predicted_objects.header; - for (const auto & object : planner_data.predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects_header; + for (const auto & object : planner_data.objects) { + const auto & predicted_object = object.predicted_object; const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); + std::find_if( + predicted_object.classification.begin(), predicted_object.classification.end(), + [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != predicted_object.classification.end(); if (is_pedestrian) continue; const auto is_coming_from_behind = motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0UL, - object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + predicted_object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; if (params.objects_ignore_behind_ego && is_coming_from_behind) { continue; } - auto filtered_object = object; + auto filtered_object = predicted_object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); @@ -130,10 +133,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const auto lat_offset_to_current_ego = std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); + lat_offset_to_current_ego <= predicted_object.shape.dimensions.y / 2.0 + + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; auto & predicted_paths = filtered_object.kinematics.predicted_paths; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index 9ef3bf5c7f756..07c8e1580e4f0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace autoware::motion_velocity_planner { @@ -54,11 +55,72 @@ struct PlannerData { } + struct Object + { + public: + Object() = default; + explicit Object(const autoware_perception_msgs::msg::PredictedObject & arg_predicted_object) + : predicted_object(arg_predicted_object) + { + } + + autoware_perception_msgs::msg::PredictedObject predicted_object; + // double get_lon_vel_relative_to_traj() + // { + // if (!lon_vel_relative_to_traj) { + // lon_vel_relative_to_traj = 0.0; + // } + // return *lon_vel_relative_to_traj; + // } + // double get_lat_vel_relative_to_traj() + // { + // if (!lat_vel_relative_to_traj) { + // lat_vel_relative_to_traj = 0.0; + // } + // return *lat_vel_relative_to_traj; + // } + + private: + // TODO(murooka) implement the following variables and their functions. + // std::optional dist_to_traj_poly{std::nullopt}; + // std::optional dist_to_traj_lateral{std::nullopt}; + // std::optional dist_from_ego_longitudinal{std::nullopt}; + // std::optional lon_vel_relative_to_traj{std::nullopt}; + // std::optional lat_vel_relative_to_traj{std::nullopt}; + }; + + struct Pointcloud + { + public: + Pointcloud() = default; + explicit Pointcloud(const pcl::PointCloud & arg_pointcloud) + : pointcloud(arg_pointcloud) + { + } + + pcl::PointCloud pointcloud; + + private: + // NOTE: clustered result will be added. + }; + + void process_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) + { + predicted_objects_header = predicted_objects.header; + + objects.clear(); + for (const auto & predicted_object : predicted_objects.objects) { + objects.push_back(Object(predicted_object)); + } + } + // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; - autoware_perception_msgs::msg::PredictedObjects predicted_objects; - pcl::PointCloud no_ground_pointcloud; + std_msgs::msg::Header predicted_objects_header; + std::vector objects; + Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index cefc84836beda..a78ab1489e080 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -161,13 +161,14 @@ bool MotionVelocityPlannerNode::update_planner_data( const auto predicted_objects_ptr = sub_predicted_objects_.takeData(); if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects")) - planner_data_.predicted_objects = *predicted_objects_ptr; + planner_data_.process_predicted_objects(*predicted_objects_ptr); processing_times["update_planner_data.pred_obj"] = sw.toc(true); const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData(); if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) { const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr); - if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud; + if (no_ground_pointcloud) + planner_data_.no_ground_pointcloud = PlannerData::Pointcloud(*no_ground_pointcloud); } processing_times["update_planner_data.pcd"] = sw.toc(true); From 7fb4c1bd29c657847f366eb8853c016729f753a2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 6 Jan 2025 22:01:40 +0900 Subject: [PATCH 28/45] feat(behavior_velocity_planner): remove virtual traffic light dependency from behavior_velocity_planner and behavior_velocity_planner_common (#9746) * feat: remove-virtual-traffic-light-dependency-from-plugin-manager Signed-off-by: Takayuki Murooka * build passed Signed-off-by: Takayuki Murooka * fix test Signed-off-by: Takayuki Murooka * fix test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/node.hpp | 4 -- .../package.xml | 1 - .../src/node.cpp | 3 -- .../src/test_utils.cpp | 2 - .../planner_data.hpp | 2 - .../scene_module_interface.hpp | 26 ------------ .../package.xml | 1 - .../src/manager.cpp | 40 ++++++++++++++++++- .../src/manager.hpp | 18 ++++++++- .../src/scene.cpp | 25 ++++++++++-- .../src/scene.hpp | 14 +++++++ .../test/test_node_interface.cpp | 11 ++++- 12 files changed, 100 insertions(+), 47 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 8a52ae70231d0..472538406c382 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -81,10 +81,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> - sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index eca49a09c8a56..c40d80c2bf998 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -56,7 +56,6 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 1e3e691076b8a..443bfebe2a3eb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -271,9 +271,6 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) planner_data_.route_handler_ = std::make_shared(*map_data); } - // optional data - getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_); - // planner_data_.external_velocity_limit is std::optional type variable. const auto external_velocity_limit = sub_external_velocity_limit_.takeData(); if (external_velocity_limit) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp index ee1bb8f89fc46..8b9f39e97d22b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -113,8 +113,6 @@ void publishMandatoryTopics( test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); test_manager->publishMaxVelocity( test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); test_manager->publishOccupancyGrid( test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 983aaec2acf4f..f8b37999cb6bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -30,7 +30,6 @@ #include #include #include -#include #include #include @@ -64,7 +63,6 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; bool is_simulation = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 9dbed9009a93e..d8d640a078267 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -32,7 +32,6 @@ #include #include #include -#include #include #include @@ -95,17 +94,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - std::optional getInfrastructureCommand() - { - return infrastructure_command_; - } - - void setInfrastructureCommand( - const std::optional & command) - { - infrastructure_command_ = command; - } - void setTimeKeeper(const std::shared_ptr & time_keeper) { time_keeper_ = time_keeper; @@ -123,7 +111,6 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - std::optional infrastructure_command_; autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; @@ -161,9 +148,6 @@ class SceneModuleManagerInterface std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); processing_time_publisher_ = std::make_shared(&node, "~/debug"); @@ -201,9 +185,6 @@ class SceneModuleManagerInterface velocity_factor_array.header.frame_id = "map"; velocity_factor_array.header.stamp = clock_->now(); - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - for (const auto & scene_module : scene_modules_) { scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); @@ -215,10 +196,6 @@ class SceneModuleManagerInterface velocity_factor_array.factors.emplace_back(velocity_factor); } - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); } @@ -227,7 +204,6 @@ class SceneModuleManagerInterface } pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { tier4_planning_msgs::msg::PathWithLaneId debug_path; @@ -299,8 +275,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; - rclcpp::Publisher::SharedPtr - pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 002c3362260cc..9f920dff8f166 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -45,7 +45,6 @@ tf2_geometry_msgs tf2_ros tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index a51cf9211c21f..3815c83d3b6ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include #include @@ -53,6 +55,14 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } + + sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>:: + create_subscription(&node, "~/input/virtual_traffic_light_states"); + + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); } void VirtualTrafficLightModuleManager::launchNewModules( @@ -89,17 +99,43 @@ void VirtualTrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [id_set](const std::shared_ptr & scene_module) { + return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; }; } + +void VirtualTrafficLightModuleManager::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path) +{ + // NOTE: virtual traffic light specific implementation + // Since the argument of modifyPathVelocity cannot be changed, the specific information + // of virtual traffic light states is set here. + const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData(); + for (const auto & scene_module : scene_modules_) { + scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states); + } + + SceneModuleManagerInterface::modifyPathVelocity(path); + + // NOTE: virtual traffic light specific implementation + // publish infrastructure_command_array + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + } + pub_infrastructure_commands_->publish(infrastructure_command_array); +} } // namespace autoware::behavior_velocity_planner #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index 8e0abc98c90de..aecef0851d8ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -20,16 +20,20 @@ #include #include #include +#include #include #include +#include +#include #include #include namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<> +class VirtualTrafficLightModuleManager +: public SceneModuleManagerInterface { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); @@ -38,10 +42,20 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<> private: VirtualTrafficLightModule::PlannerParam planner_param_; + + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( + std::function &)> getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr + sub_virtual_traffic_light_states_; + + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 206fb14b6d41c..86239816ed6f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -333,12 +333,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) std::optional VirtualTrafficLightModule::findCorrespondingState() { - // No message - if (!planner_data_->virtual_traffic_light_states) { + // Note: This variable is set by virtual traffic light's manager. + if (!virtual_traffic_light_states_) { return {}; } - for (const auto & state : planner_data_->virtual_traffic_light_states->states) { + for (const auto & state : virtual_traffic_light_states_->states) { if (state.id == map_data_.instrument_id) { return state; } @@ -457,4 +457,23 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } + +std::optional +VirtualTrafficLightModule::getInfrastructureCommand() const +{ + return infrastructure_command_; +} + +void VirtualTrafficLightModule::setInfrastructureCommand( + const std::optional & command) +{ + infrastructure_command_ = command; +} + +void VirtualTrafficLightModule::setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states) +{ + virtual_traffic_light_states_ = virtual_traffic_light_states; +} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index b031ba5b2bb2b..7f37e7078a431 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,6 +25,9 @@ #include #include +#include +#include + #include #include @@ -84,13 +88,23 @@ class VirtualTrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; + std::optional getInfrastructureCommand() const; + void setInfrastructureCommand( + const std::optional & command); + + void setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states); + private: const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states_; State state_{State::NONE}; tier4_v2x_msgs::msg::InfrastructureCommand command_; + std::optional infrastructure_command_; MapData map_data_; ModuleData module_data_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp index 9cb6bbfd40639..ab2fd5847c5be 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -26,10 +26,17 @@ namespace autoware::behavior_velocity_planner TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) { rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); - auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); // test with nominal path_with_lane_id ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); @@ -51,6 +58,8 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + test_manager->publishVirtualTrafficLightState( + test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); From ab845fb7371ca60ef35889c390236abfc3fb4763 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 7 Jan 2025 13:35:44 +0900 Subject: [PATCH 29/45] fix(autoware_lidar_centerpoint): fixed rounding errors that caused illegal memory access (#9795) fix: fixed rounding errors that caused illegal memory address Signed-off-by: Kenzo Lobos-Tsunekawa --- .../lib/preprocess/preprocess_kernel.cu | 2 ++ 1 file changed, 2 insertions(+) diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index f300411a44aad..95ac7b4353e90 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -148,6 +148,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((point.x - min_x_range) / pillar_x_size); int voxel_idy = floorf((point.y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = (grid_x_size - 1 - voxel_idx) * grid_y_size + voxel_idy; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); From f242fbf91e5d5e55da8e04dc3f8fdecbbf20a6e6 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Tue, 7 Jan 2025 13:55:52 +0900 Subject: [PATCH 30/45] fix(autoware_lidar_transfusion): fixed rounding errors that caused illegal memory access (#9796) fix: fixed rounding errors that caused illegal memory address Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: Amadeusz Szymko --- .../lib/preprocess/preprocess_kernel.cu | 2 ++ 1 file changed, 2 insertions(+) diff --git a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu index 49131218ff698..a89268e646cfc 100644 --- a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -176,6 +176,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((x - min_x_range) / pillar_x_size); int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); From 515a21f86808bb15e5fb0601576a966f2abe9614 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Tue, 7 Jan 2025 14:27:14 +0900 Subject: [PATCH 31/45] refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners (#9827) * refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../freespace_pull_out.hpp | 5 +---- .../geometric_pull_out.hpp | 3 ++- .../pull_out_planner_base.hpp | 13 +++++++------ .../shift_pull_out.hpp | 3 ++- .../src/freespace_pull_out.cpp | 10 +++------- .../src/start_planner_module.cpp | 3 +-- .../test/test_geometric_pull_out.cpp | 3 +-- .../test/test_shift_pull_out.cpp | 4 +--- 8 files changed, 18 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index e358e35ed7794..364d2d31a2577 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -35,10 +35,7 @@ using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOut : public PullOutPlannerBase { public: - FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper); + FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 18d1f3c3b9b81..78eb72183cdf5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -35,7 +35,8 @@ class GeometricPullOut : public PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index f606679e7f2da..dfd972aff9be0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -40,12 +40,13 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper) - : time_keeper_(time_keeper) + std::shared_ptr time_keeper = + std::make_shared()) + : parameters_(parameters), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()), + vehicle_footprint_(vehicle_info_.createFootprint()), + time_keeper_(time_keeper) { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info_.createFootprint(); - parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; @@ -68,9 +69,9 @@ class PullOutPlannerBase double collision_check_distance_from_end) const; std::shared_ptr planner_data_; + StartPlannerParameters parameters_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - StartPlannerParameters parameters_; double collision_check_margin_; mutable std::shared_ptr time_keeper_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 5b4f78b252d22..8da104940d327 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -36,7 +36,8 @@ class ShiftPullOut : public PullOutPlannerBase explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr & lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 42698f799c6b3..a089f6a8a83fc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,15 +28,11 @@ namespace autoware::behavior_path_planner { -FreespacePullOut::FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, - velocity_{parameters.freespace_planner_velocity} +FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index c223390e454d1..f45924b9542dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -90,8 +90,7 @@ StartPlannerModule::StartPlannerModule( } if (parameters_->enable_freespace_planner) { - freespace_planner_ = - std::make_unique(node, *parameters, vehicle_info_, time_keeper_); + freespace_planner_ = std::make_unique(node, *parameters); const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); freespace_planner_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index fb39e186afd4e..dd8bb02c97dea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -156,9 +156,8 @@ class TestGeometricPullOut : public ::testing::Test parameters->th_moving_object_velocity = th_moving_object_velocity_; parameters->divide_pull_out_path = divide_pull_out_path_; - auto time_keeper = std::make_shared(); geometric_pull_out_ = - std::make_shared(*node_, *parameters, lane_departure_checker_, time_keeper); + std::make_shared(*node_, *parameters, lane_departure_checker_); } void initialize_planner_data() diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp index 156cf62f9ac4a..474da055b4de7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -139,9 +139,7 @@ class TestShiftPullOut : public ::testing::Test { auto parameters = StartPlannerParameters::init(*node_); - auto time_keeper = std::make_shared(); - shift_pull_out_ = - std::make_shared(*node_, parameters, lane_departure_checker_, time_keeper); + shift_pull_out_ = std::make_shared(*node_, parameters, lane_departure_checker_); } }; From db78285c34a6bdf6b1db0d3666180f680ead15fe Mon Sep 17 00:00:00 2001 From: Atto Armoo <168620037+AA-T4@users.noreply.github.com> Date: Tue, 7 Jan 2025 15:19:04 +0900 Subject: [PATCH 32/45] fix(planning): corrects typos (#9840) * fix(planning): corrects typos Signed-off-by: Atto Armoo <168620037+AA-T4@users.noreply.github.com> * style(pre-commit): autofix --------- Signed-off-by: Atto Armoo <168620037+AA-T4@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md index f373afc2351bf..efe4a9a353ecc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -1,33 +1,33 @@ -## Stop Line +# Stop Line -### Role +## Role -This module plans velocity so that the vehicle can stop right before stop lines and restart driving after stopped. +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. ![stop line](docs/stop_line.svg) -### Activation Timing +## Activation Timing This module is activated when there is a stop line in a target lane. -### Module Parameters +## Module Parameters | Parameter | Type | Description | | -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_duration_sec` | double | [s] time parameter for the ego vehicle to stop in front of a stop line | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | -| `use_initialization_stop_state` | bool | A flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -| `show_stop_line_collision_check` | bool | A flag to determine whether to show the debug information of collision check with a stop line | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | +| `show_stop_line_collision_check` | bool | Flag to determine whether to show the debug information of collision check with a stop line | -### Inner-workings / Algorithms +## Inner-workings / Algorithms - Gets a stop line from map information. -- insert a stop point on the path from the stop line defined in the map and the ego vehicle length. +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. -#### Flowchart +### Flowchart ```plantuml @startuml @@ -85,15 +85,15 @@ Then, we can get `offset segment` and `offset from segment start`. ![find_offset_segment](docs/./find_offset_segment.drawio.svg) -After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`. +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. ![calculate_stop_pose](docs/./calculate_stop_pose.drawio.svg) -#### Restart prevention +### Restart Prevention -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors.
![example](docs/restart_prevention.svg){width=1000} From 95e6b19b69dc59244d4e70361e0f95ea9628b937 Mon Sep 17 00:00:00 2001 From: Atto Armoo <168620037+AA-T4@users.noreply.github.com> Date: Tue, 7 Jan 2025 15:19:17 +0900 Subject: [PATCH 33/45] fix(planning): corrects typo in svg (#9814) Signed-off-by: Atto Armoo <168620037+AA-T4@users.noreply.github.com> --- .../docs/node_and_segment.drawio.svg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg index 182c941907f57..6cd47ffc14a56 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg @@ -233,7 +233,7 @@
= node(i) + node(i+1)
- = all segment has two points + = all segments have two points From e5070d18e4c37c02bd60a4228c2faf430d602698 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 7 Jan 2025 15:20:44 +0900 Subject: [PATCH 34/45] fix(autoware_motion_velocity_obstacle_velocity_limiter_module): remove cppcheck suppressions (#9843) Signed-off-by: veqcc --- .../benchmarks/collision_checker_benchmark.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp index 1d980e220ccef..c8aa9895a0451 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp @@ -76,12 +76,10 @@ int main() const auto naive_constr_end = std::chrono::system_clock::now(); const auto rtt_check_start = std::chrono::system_clock::now(); for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable const auto rtree_result = rtree_collision_checker.intersections(polygon); const auto rtt_check_end = std::chrono::system_clock::now(); const auto naive_check_start = std::chrono::system_clock::now(); for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable const auto naive_result = naive_collision_checker.intersections(polygon); const auto naive_check_end = std::chrono::system_clock::now(); const auto rtt_constr_time = From 9d5c246baf29ceed9c8e1ad264d559415591631b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 7 Jan 2025 15:42:29 +0900 Subject: [PATCH 35/45] feat(autoware_test_utils): add visualization and yaml dumper for PathWithLaneId (#9841) Signed-off-by: Mamoru Sobue --- .../config/sample_topic_snapshot.yaml | 9 +- .../autoware_test_utils/mock_data_parser.hpp | 13 + .../autoware_test_utils/visualization.hpp | 117 ++- common/autoware_test_utils/package.xml | 1 + .../src/mock_data_parser.cpp | 50 ++ .../src/topic_snapshot_saver.cpp | 11 +- .../test_map/intersection/lanelet2_map.osm | 839 ++++++------------ 7 files changed, 479 insertions(+), 561 deletions(-) diff --git a/common/autoware_test_utils/config/sample_topic_snapshot.yaml b/common/autoware_test_utils/config/sample_topic_snapshot.yaml index d9eff5d579540..2ff746b84c3ac 100644 --- a/common/autoware_test_utils/config/sample_topic_snapshot.yaml +++ b/common/autoware_test_utils/config/sample_topic_snapshot.yaml @@ -24,6 +24,9 @@ fields: - name: dynamic_object type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects topic: /perception/object_recognition/objects -# - name: tracked_object -# type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects -# topic: /perception/object_recognition/tracking/objects + # - name: tracked_object + # type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects + # topic: /perception/object_recognition/tracking/objects + # - name: path_with_lane_id + # type: PathWithLaneId # tier4_planning_msgs::msg::PathWithLaneId + # topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 5e07237e2c495..8375d3e731683 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -59,6 +59,7 @@ using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PathPoint; using builtin_interfaces::msg::Duration; using builtin_interfaces::msg::Time; using geometry_msgs::msg::Accel; @@ -97,6 +98,9 @@ Duration parse(const YAML::Node & node); template <> Time parse(const YAML::Node & node); +template <> +Point parse(const YAML::Node & node); + template <> std::vector parse(const YAML::Node & node); @@ -145,6 +149,15 @@ std::vector parse(const YAML::Node & node); template <> UUID parse(const YAML::Node & node); +template <> +PathPoint parse(const YAML::Node & node); + +template <> +PathPointWithLaneId parse(const YAML::Node & node); + +template <> +PathWithLaneId parse(const YAML::Node & node); + template <> PredictedPath parse(const YAML::Node & node); diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp index c2185e65422e8..dbd3dd6638c95 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -19,6 +19,9 @@ #include #include +#include + +#include #include #include @@ -150,7 +153,8 @@ inline void plot_lanelet2_object( const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() + right.front().basicPoint2d() + right.back().basicPoint2d()) / 4.0; - axes.text(Args(center.x(), center.y(), std::to_string(lanelet.id()))); + axes.text( + Args(center.x(), center.y(), std::to_string(lanelet.id())), Kwargs("clip_on"_a = true)); } if (config_opt && config_opt.value().label) { @@ -214,16 +218,111 @@ inline void plot_lanelet2_object( axes.add_patch(Args(poly.unwrap())); } +struct DrivableAreaConfig +{ + static DrivableAreaConfig defaults() { return {"turquoise", 2.0}; } + std::optional color{}; + std::optional linewidth{}; +}; + +struct PathWithLaneIdConfig +{ + static PathWithLaneIdConfig defaults() + { + return {std::nullopt, "k", 1.0, std::nullopt, false, 1.0}; + } + std::optional label{}; + std::optional color{}; + std::optional linewidth{}; + std::optional da{}; + bool lane_id{}; // & config_opt = std::nullopt); -*/ +inline void plot_autoware_object( + const tier4_planning_msgs::msg::PathWithLaneId & path, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.label) { + kwargs["label"] = config.label.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + std::vector xs; + std::vector ys; + std::vector yaw_cos; + std::vector yaw_sin; + std::vector> ids; + const bool plot_lane_id = config_opt ? config_opt.value().lane_id : false; + for (const auto & point : path.points) { + xs.push_back(point.point.pose.position.x); + ys.push_back(point.point.pose.position.y); + const auto th = autoware::universe_utils::getRPY(point.point.pose.orientation).z; + yaw_cos.push_back(std::cos(th)); + yaw_sin.push_back(std::sin(th)); + if (plot_lane_id) { + ids.emplace_back(); + for (const auto & id : point.lane_ids) { + ids.back().push_back(id); + } + } + } + // plot centerline + axes.plot(Args(xs, ys), kwargs); + const auto quiver_scale = + config_opt ? config_opt.value().quiver_size : PathWithLaneIdConfig::defaults().quiver_size; + const auto quiver_color = + config_opt ? (config_opt.value().color ? config_opt.value().color.value() : "k") : "k"; + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), Kwargs( + "angles"_a = "xy", "scale_units"_a = "xy", + "scale"_a = quiver_scale, "color"_a = quiver_color)); + if (plot_lane_id) { + for (size_t i = 0; i < xs.size(); ++i) { + std::stringstream ss; + const char * delimiter = ""; + for (const auto id : ids[i]) { + ss << std::exchange(delimiter, ",") << id; + } + axes.text(Args(xs[i], ys[i], ss.str()), Kwargs("clip_on"_a = true)); + } + } + // plot drivable area + if (config_opt && config_opt.value().da) { + auto plot_boundary = [&](const decltype(path.left_bound) & points) { + std::vector xs; + std::vector ys; + for (const auto & point : points) { + xs.push_back(point.x); + ys.push_back(point.y); + } + const auto & cfg = config_opt.value().da.value(); + py::dict kwargs{}; + if (cfg.color) { + kwargs["color"] = cfg.color.value(); + } + if (cfg.linewidth) { + kwargs["linewidth"] = cfg.linewidth.value(); + } + axes.plot(Args(xs, ys), kwargs); + }; + plot_boundary(path.left_bound); + plot_boundary(path.right_bound); + } +} + } // namespace autoware::test_utils #endif // AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index c328f37ba357d..740e7f840141d 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -8,6 +8,7 @@ Takamasa Horibe Zulfaqar Azmi Mamoru Sobue + Yukinari Hisaki Apache License 2.0 Kyoichi Sugahara diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index 6e7002501bf30..29aed75357441 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -65,6 +65,16 @@ std::array parse(const YAML::Node & node) return msg; } +template <> +Point parse(const YAML::Node & node) +{ + Point geom_point; + geom_point.x = node["x"].as(); + geom_point.y = node["y"].as(); + geom_point.z = node["z"].as(); + return geom_point; +} + template <> std::vector parse(const YAML::Node & node) { @@ -285,6 +295,46 @@ Shape parse(const YAML::Node & node) return msg; } +template <> +PathPoint parse(const YAML::Node & node) +{ + PathPoint point; + point.pose = parse(node["pose"]); + point.longitudinal_velocity_mps = node["longitudinal_velocity_mps"].as(); + point.lateral_velocity_mps = node["lateral_velocity_mps"].as(); + point.heading_rate_rps = node["heading_rate_rps"].as(); + point.is_final = node["is_final"].as(); + return point; +} + +template <> +PathPointWithLaneId parse(const YAML::Node & node) +{ + PathPointWithLaneId point; + point.point = parse(node["point"]); + for (const auto & lane_id_node : node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + return point; +} + +template <> +PathWithLaneId parse(const YAML::Node & node) +{ + PathWithLaneId path; + path.header = parse
(node["header"]); + for (const auto & point_node : node["points"]) { + path.points.push_back(parse(point_node)); + } + for (const auto & left_bound_node : node["left_bound"]) { + path.left_bound.push_back(parse(left_bound_node)); + } + for (const auto & right_bound_node : node["right_bound"]) { + path.right_bound.push_back(parse(right_bound_node)); + } + return path; +} + template <> PredictedPath parse(const YAML::Node & node) { diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index b2cb2a17c9621..bd9af2d5672b8 100644 --- a/common/autoware_test_utils/src/topic_snapshot_saver.cpp +++ b/common/autoware_test_utils/src/topic_snapshot_saver.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -46,7 +47,8 @@ using MessageType = std::variant< autoware_adapi_v1_msgs::msg::OperationModeState, // 3 autoware_planning_msgs::msg::LaneletRoute, // 4 autoware_perception_msgs::msg::TrafficLightGroupArray, // 5 - autoware_perception_msgs::msg::TrackedObjects // 6 + autoware_perception_msgs::msg::TrackedObjects, // 6 + tier4_planning_msgs::msg::PathWithLaneId // 7 >; std::optional get_topic_index(const std::string & name) @@ -72,6 +74,9 @@ std::optional get_topic_index(const std::string & name) if (name == "TrackedObjects") { return 6; } + if (name == "PathWithLaneId") { + return 7; + } return std::nullopt; } @@ -196,6 +201,7 @@ class TopicSnapShotSaver REGISTER_CALLBACK(4); REGISTER_CALLBACK(5); REGISTER_CALLBACK(6); + REGISTER_CALLBACK(7); } } @@ -243,6 +249,7 @@ class TopicSnapShotSaver REGISTER_WRITE_TYPE(4); REGISTER_WRITE_TYPE(5); REGISTER_WRITE_TYPE(6); + REGISTER_WRITE_TYPE(7); } const std::string desc = std::string(R"(# @@ -253,7 +260,7 @@ class TopicSnapShotSaver # map_path_uri: package:/// # fields(this is array) # - name: -# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} # topic: # )"); diff --git a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm index c5c5b7b1fc657..9f28ed79a91ae 100644 --- a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm +++ b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -8002,391 +8002,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -10004,16 +9619,6 @@ - - - - - - - - - - @@ -15586,6 +15191,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -18669,142 +18449,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -19364,12 +19008,6 @@ - - - - - - @@ -21131,6 +20769,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22269,15 +21969,6 @@ - - - - - - - - - @@ -22853,6 +22544,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From e2accc57e9d0b1328641bd88a48e446c74879af3 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 7 Jan 2025 16:21:57 +0900 Subject: [PATCH 36/45] fix(tier4_camera_view_rviz_plugin): fix bugprone-parent-virtual-call (#9815) * fix:bugprone-error Signed-off-by: kobayu858 * fix:fmt Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/third_person_view_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index b7d754b02d4bd..386c8cdfb0b13 100644 --- a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -184,7 +184,7 @@ void ThirdPersonViewController::handleMouseEvent(rviz_common::ViewportMouseEvent void ThirdPersonViewController::mimic(rviz_common::ViewController * source_view) { - FramePositionTrackingViewController::mimic(source_view); + FramePositionTrackingViewController::mimic(source_view); // NOLINT target_frame_property_->setValue(TARGET_FRAME_START); getNewTransform(); From 3aa84a0fb2a9796a8e54f99e44e89ae3a125131e Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 7 Jan 2025 16:23:13 +0900 Subject: [PATCH 37/45] fix(system_monitor): fix bugprone-exception-escape (#9781) * fix: bugprone-error Signed-off-by: kobayu858 * fix: clippy Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../reader/hdd_reader/hdd_reader.cpp | 83 +++++----- .../reader/msr_reader/msr_reader.cpp | 146 +++++++++--------- 2 files changed, 123 insertions(+), 106 deletions(-) diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 740f841382f47..098f4240782ae 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -655,48 +656,56 @@ void run(int port) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, nullptr, 'h'}, - {"port", required_argument, nullptr, 'p'}, - {nullptr, 0, nullptr, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; - - default: - break; + try { + static struct option long_options[] = { + {"help", no_argument, nullptr, 'h'}, + {"port", required_argument, nullptr, 'p'}, + {nullptr, 0, nullptr, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } } - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port); + run(port); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/system_monitor/reader/msr_reader/msr_reader.cpp index fc7bcab795be0..89f95d05c282e 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/system_monitor/reader/msr_reader/msr_reader.cpp @@ -212,90 +212,98 @@ void run(int port, const std::vector & list) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; + try { + static struct option long_options[] = { + {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } + } - default: - break; + if (!fs::exists("/dev/cpu")) { + printf("Failed to access /dev/cpu.\n"); + return EXIT_FAILURE; } - } - if (!fs::exists("/dev/cpu")) { - printf("Failed to access /dev/cpu.\n"); - return EXIT_FAILURE; - } + std::vector list; + const fs::path root("/dev/cpu"); - std::vector list; - const fs::path root("/dev/cpu"); + for (const fs::path & path : boost::make_iterator_range( + fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { + if (fs::is_directory(path)) { + continue; + } - for (const fs::path & path : boost::make_iterator_range( - fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { - if (fs::is_directory(path)) { - continue; - } + std::cmatch match; + const char * msr = path.generic_string().c_str(); - std::cmatch match; - const char * msr = path.generic_string().c_str(); + // /dev/cpu/[0-9]/msr ? + if (!std::regex_match(msr, match, std::regex(".*msr"))) { + continue; + } - // /dev/cpu/[0-9]/msr ? - if (!std::regex_match(msr, match, std::regex(".*msr"))) { - continue; + list.push_back(path.generic_string()); } - list.push_back(path.generic_string()); - } + std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { + std::cmatch match; + const std::regex filter(".*/(\\d+)/msr"); + int n1 = 0; + int n2 = 0; + if (std::regex_match(c1.c_str(), match, filter)) { + n1 = std::stoi(match[1].str()); + } + if (std::regex_match(c2.c_str(), match, filter)) { + n2 = std::stoi(match[1].str()); + } + return n1 < n2; + }); // NOLINT - std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { - std::cmatch match; - const std::regex filter(".*/(\\d+)/msr"); - int n1 = 0; - int n2 = 0; - if (std::regex_match(c1.c_str(), match, filter)) { - n1 = std::stoi(match[1].str()); - } - if (std::regex_match(c2.c_str(), match, filter)) { - n2 = std::stoi(match[1].str()); + if (list.empty()) { + printf("No msr found in /dev/cpu.\n"); + return EXIT_FAILURE; } - return n1 < n2; - }); // NOLINT - - if (list.empty()) { - printf("No msr found in /dev/cpu.\n"); - return EXIT_FAILURE; - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port, list); + run(port, list); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } From 19f7f95b8a25fc7deb748db8671a33d0d835bed7 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 7 Jan 2025 18:42:38 +0900 Subject: [PATCH 38/45] feat(ekf_localizer): check whether the initialpose has been set (#9787) * check set intialpose Signed-off-by: Yamato Ando * update png Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/autoware_ekf_localizer/README.md | 1 + .../autoware/ekf_localizer/diagnostics.hpp | 1 + .../autoware/ekf_localizer/ekf_localizer.hpp | 1 + .../media/ekf_diagnostics.png | Bin 142232 -> 145097 bytes .../src/diagnostics.cpp | 19 ++++++++++++++++++ .../src/ekf_localizer.cpp | 16 +++++++++++++-- .../test/test_diagnostics.cpp | 13 ++++++++++++ 7 files changed, 49 insertions(+), 2 deletions(-) diff --git a/localization/autoware_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md index aad65da2516d1..802bf7dbb16c3 100644 --- a/localization/autoware_ekf_localizer/README.md +++ b/localization/autoware_ekf_localizer/README.md @@ -191,6 +191,7 @@ Note that, although the dimension gets larger since the analytical expansion can ### The conditions that result in a WARN state - The node is not in the activate state. +- The initial pose is not set. - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. - The timestamp of the Pose/Twist topic is beyond the delay compensation range. - The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp index 20a77354c0996..b194c3e956341 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp @@ -24,6 +24,7 @@ namespace autoware::ekf_localizer { diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose); diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp index 1b437f26e9c7d..0561e250298ac 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -119,6 +119,7 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; bool is_activated_; + bool is_set_initialpose_; EKFDiagnosticInfo pose_diag_info_; EKFDiagnosticInfo twist_diag_info_; diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png index 234b2f1e19b6dd701dd1253b853580ab759f725d..a1561c3f5270709791a19557b874dfae1d6f56ba 100644 GIT binary patch literal 145097 zcmdqIV|Qi2yD!?^LC3aj+qTuQosMnWwv&!Jwv&!+yW^}lE9N@sv+o}FzwbEb3*35B z6IC@ao>}$7Gb5E0B;jFjV7`3$0xvBkrt;;>*Rn5Pz8ynBejcGWh_L#sz@0^;)u29$ z50q)d=RUTJxTcG$y}66Kk(1dM3p;yTGkRweCo?lUXG?pRE3jUH&ql0PYML&hPG&|f zR`zy8YF4&pp9jBuVPRrnJ8W-aW;x1FCk*)Jjx7WV>^rpW9B$6OEPgcpZfBJ z=!>+Nu$o8C`Icvn{5|N;%}V?0`RmUxBe>xD+A)PRI#I16C9(U|h?JrtvUeFNX_*F! z21aTV1Kd}J*H!GWzN*LV+mCl<6GFHO3g5i=5nsO3Y5wC|?ql6!U%~vDS091(BJpT& zab+~|F;TfM@HlKwWoLy{QAJcMSRVkGe;eb~Y7Q!B;>tig^qkKsg-;TK^0PhpjB=m! zzi!TI4lk5?S`*jDI;FlstIfi-_<+? z1KXjSx(82A|Jz3iU_ zB;1$X^SfgzB&Ag1jgNEsA5sE!QJ%674u_?Ws+#kxIK3#onwcMz(H@!yEZ!P#;~fL( z?F*?+WRttq!^|NbnoONfs+U`??ue#dDA~tyly+A`yqC8|9rxb*d|X))eT8hVmr(vQ zoYKl4UhaqrR^d;snw(ib7~~4V(HobdeTFI*8je@0_#X*B?tLp$0DSvbihV5f&p%go zfE&++buZ#dY#mUA1nc>msuY!iKNTtwF_K*tw&p_IJ&=o{Lw3#HlEvTj?o47X)&u8` zPp@xc4y*K8aua7#N4Q)8irch;*xihGRP*Vc5& z$=>R$w`xd=y{{}kKxs4|xLl4rAlp9efy!wjzsg&UG@}xqtM)3uyGGhgg4tlBjV;BJ zBtn6S?~eb_)Hc$55Ti!ZQ(0vp72=O+dP?b|mDapt&>kbna(~lXFSiYsL)LBN$ZGeg zg+#1Zbg7>f$Yud?diR|pmHK2qXo)`q+O1_Xt(Z-yW!7sF8 z4L};!vye58yuDgQ5HiRoosa!}tgB_}bx?pnRIGrKqY*-*H%QH9E0|}R&#D> zJVO}??KW_PAyrS=akE8#e>ko{@n|OkP(rj}&<{bgpVsKiA;Q#}Z6tws->S>7eZug` zi8B~>zCmzgbyl|cX0wFtr}>?GAOn(vO}-OgMza{r?)IlA4e_TKa>`Z^0^?3pd|FO> z1unI-86pf;V@o6dGym~mUs!8#J-UB*tOjn1Q{KISYl$1Ozpm|NNSfVnw%`T%{W`Ri zsqjvi8eJA5At$VP<#%H8?Ca$!(X|~!N^glp($QWRLfh%~R2oc9mZMb2lBlt85Xw%ZfBPokMSU)T_czNf-=Yxs%Hm6qgJ%SNvBI&n} zd)h@>jEL+h^!)1vmn)YfARE%UWdWVUwWRC1-Q1JGJX|aITTds&+NsZ z&MxWN!aY_ufG(t+?4 z6!;;QYEXjpUw85fDHj;qHIq&S_@5uPraIbKK^eJ&SbHj4cMn?C~QR zw2^ibU}mrr&s^0=C)MKzS1l4-WF_Hx| zHa?F$dfzV*VB((^n=U4X=i~rT%WO1lt}?)-MD^A{HE{lQr=9FNJ#W#BifFC({1U=I zLW14vg5qz@j_o!|O@biJ(;(}1zDmsNcAoGQnM*dlM}huQn<`5}GL70`K1}y)yTqf_ zYLn$NOO>Ay#50;>ik#?@3F@lVr0JsXwVhe_TSOYZ3#*zcZ}L62Xuw0Wg-cbAjSNwf?D0{F61GNJ)q2%lpr z<=!_c0$P4@sAFQ!Ss`&zkEJTF6>E0ztYXYbp_hPYGF3295U(RAb6({QF2cN;aVKY9 zT#!{O?|~I6>jzG(Jer=}mc*c^UFIEwEjoPg6F;wN5@ESb;-s%=NryZtobdeN3LNrS zimPwYV1%$wwhtw=8*)8iV(n>rbg9;_m+OJOE{vQQm8aO|Bx(h2_MY}9ETpOCcDW9! zmEdG%6|d^{h3Af~YR=5b=I)X@ZneC8Z{@p6!VWx2P&AWlPdcV-R)lHI8L9G-lBdx& ze13a!%n(o|;>T|UejmeJI>jTaP!SL7KhzMV2|Hyib)KiA29%ThSQJjf#J*yRWhlyAL4Lf*+cJ67~ z+kxl9SLQvK=}H06cnTCX zSfcxyfcMgsrRmE%y1DqB&rkdr_F%KR%u37mV$C)O4?y|+{z%Hjq;dc_t~j*#l5Xtc zQ-fn3LW{dxf7_yu2@jI_L1p}HE?K&#s`&XwkxUA**m3_eEVnr>_S?_vQ`l6d;^Z#s zJV6%1Up7pVYK6w-U++w(bL7F^(Qx-r9g33KK;keN>}<~9TYEJ`dt|$YA_#hpkl$!D zh{vJ!qz)#Z!&3=MoV^w(;&5nHYD-t{>k%G*FM3ag4o+oDIh^Kfw*`Wmh@UQBclh#& zDVpA2V|o_J5sUbA-!R^NV@-0IUtn>{7pJhucwdK&QLvrj+;J~-cqsoxW^aCO9b18w ziYRDr0OTYbFtXsW0<;raJL9eXIWi^uy+j{>ahjb)*nFBgROe5sb2$qt_mo6N2S1+9 z>6GNin$E1&;m&N2?n~XOU$a@Y;n=M79VW~y<8nMbT|!H;=#r`2wXk}=Ak$~C>pauH z(y)tLs29wW$Mp5+vg5X6+?t*#QTs*QubPqOU}yWtMMh1iFl@nt&A z>NPWNS8xk;e5SvTnvxyM(A`Ez(*YW(Z`=suRv5HeKirDWzO64Y6*`ZIj>dW~5hCe* zZ#om#2f>z6x)!eKbe3~^aIv0>?_Wq*6D^YpW2oyH@XqRQICN%K ze)jSmm7;byF3h`qJca3f?5cwwwew5n_e^$gR5v!==v1VRagKvxxlF^D%J{^b za@B@?UOv>rC<#alM9MuBvmI%BG$=n*$qgujiLJsXt zA>3v4E=%Njw&$7mBPsWEdih}n-3@3=I7}%t;&sj#vg#BIH8-*D2H#&fY{|(R@a-Qg zB_L1@d*!}zUU)govk6IJ5|;%OL8*WN0eWodpX zqld8{s9kAsb#xU@VsvU9>5>bbP#wPgW}RkBTr7N8MM#}AI9`%HN7|{f{yK9sj19%` zBg)Vhhc^ylwza_kp?c!H1MTNj26gqdUlDn(b_i0J3)50JL6?vGL`U4W3}?q23Rx?e z4n4#BdHP=~wLYq6WA)ak8+c~Vb^aPS5?BFnD8S4)g>5G)_ZRYm&6$H<*`nu4>ug(oY9j78$7%4y1t)JH*!h;`}G3?QI{CJ zw(I3o8$M2I$77jRLYVJN`Xb`vh+sKe_g)fmx{8Z;CB%n6P~T(+D1aW^4+Tp-R@tj3 zCI?})Q%&8I^-P>+F_%Mx%1oNo`q%-NXe0+$pjkngGoXhLp;QYv(1iza?btyj2Nc!w@ zM8jdbHNJcSt=rQA%Q`fk+TUP}2iBW0(NJ=m$z_IaVb|^M#kLo#f5`nI?!E30WR+E? zGxph8?&4UGO73A)<g3rS(xbivxkN3&dfG|xQ^)c`sPDb zGaKHx!ivZvms^;OM!Ec6ePMN4?aX)9&PKe>9*@j!t7qpbtbD%PaKs`-+0Gt)$!2Idk~CV~uW zs0gndBe+ma#o(g@v}oYhXyb5On4iRC)WJ+Tzmn~``g)&@mH-j_4Wy^(V5^? z^9SL-tf3(^+!T}DwSOdveAF1TWDPAbij1>)X!yy_i`&UYAUP7?P%P0w9nk;*ALsK5 z9~59ZDTzvmjaXBcD7p>BaxxCv>^h2M;vDfdO8UUZ4pC5 zibX$N6P0@@w^H5;3|6Uj+$tH#gSLi@8zL10qvpNm$P*_ASUT-dsnPcs%!8Hb?0QhN z+qztG9LQ)B|MVD8xYbZScU6v#c;j(WikZIA=mo)PNMhIa+9;02Yl!8PrH@Kh(@Oa< zMq|aAJKiIt1(g_^4L+O?1~+X1MUO|wddtM+I0PeqgDPyAcCxA|Nv0bcey-lCR43G~ z(%GS?brs&_G^y6;^0(okkdQg&-bFUZM1Eus`qkQWpWm{U>VQQ;-y=nj(On9QmmKLt zlS$85fjV-YpS8v;5E3m)b1~inQNFoIYu^;I(dARuJd;U4X=@PwbewPzS036=eaABnMj^@x5&M>R&wAD_n_qN+jm=qkI|}qTSyoO3 z!tkuqQB($Ew7aC}g(o{4q%aq(K}AJT4Bp$cX)xLrV$D}etMCRP6<B{cMy~h7WWP~zDG7zY--S@Scve9X z>oa6bF+7E&7fRPspSRcF@A3`&PGnRTq(D=w#E#BpG@47bT=k_1Z>%IrZ(;tpA5MXD z9Qk=m;s0EA!g>1zuy)^hCgL{z?z7d5*}KYfy>QigW0rsH*`Y-@)5Vt{U73ChFe!U_ zDB;Y^cJcOb!pk0-P;4?$KN0fF+zJO?ws(nq#s)~7ThSnef_1|yB23svRCG{$VUX9Y z>yz2!d=Ha$#gvaBHbOBLW70@c{BVefU^w>o6fdI7DrlE7+_fCLGDCrv=d*@S??yo8 zcWmH0EQZdK_92;&J_OF8?CBUtnOvMScyOrzI!oMmdyC8s8JS-z?U9C<1AZ<>A4Rz(DJA+E4J?BBcYM_Z8uF_h zw(^K1oTV{voN+ERcu-uFO;y;_YEFFaikUsm_v}?=+%$FFfMCi|uka3?|5w47d{$hq zksgf>EOxJLlnd1!O`&33LbZm|s?w*p*)BcZCoM=`oL>i%QL@LIKXLmESEMyQ7S78s z@p}a}TG=@fM^qraB`?d6j;veVB8uxH0aR&>EbpB7ThvXaBjUuuKgl8pE~uidt8tDY z4xdm;%XzuW3#_%oJoT{7Vv0MxUANYk+tBTue`F z-t;U0Xe@#Hh>_fQq~9662&f)fQq$blbPtDsV3bcg;z&mGrPV*z_~ea3B0-aTDnSJY1+;DSrpCmLklVYb+n&;mS1w=Igtk%=L_yX&_4* z*lbY(GtvCcFyzG!IqtH%au3eehE(0-%B+`^`1rt}8L`Gpu4>3dg=%yu#Q)S=ljbE15gfMNgZ$)4zw4h7HYKZC<5U7$ zXZs!my3e0sX3q@x6tVda-;iqj5`UTEv$5(6_@!9g(KLs5PudImk2uPqc^31^zuDN_ zFAB}Kc`$>yP%3<7(~xQPjHO8AoLv5`MTKn(ZVj=m(~wneln*?7Y<%{;WYX8-%Kaf| zXKFk_l!sNHb!Zj_Tq}pBTV;yv!#`p&*%#q}gSVKJ-NM4Or!*LFKiTOaTYf_B0uFiQ zNU4A8Zx)Ag){bC|UdTlEjWN3Ss1P;tv3ts4{)O}PQL2hlQXF+mm%~;dSfBj7I6Ew&3RN~ahZx|Kqz;Z794KqXR#sFoJEMICJM)UY_ zQ;M;|kaKdv^W(!oz+x3>S}MUl+dLNJ!N;pJPcA>5$Cch4BOG;3pdw#|V@$WIs;;L+ z#~6wQJ|(+8J&F@7lfR_E$2O>IUSQ)M!4>aA%x~ZRYUjyUKbQK@Umk#Y6o3UXyu7Y;e9FLs2}@~V z_@}A~j7u6h`9^&{rn{B0`IUmq>t!w7zOoEjDUaO}Ewek-eB7(c(Vah6LZ>tn$+Ta(M^UOmjeEuur>`tSU6W6ty-cqI zkXlYPWAS>9w9+K##Wd>Y|~(V(cXW8H_LKUV9blau13=wEO5S8K#F`aO&$)waDm_@0|g z1{w7xfxZB6i5~@S)hKKCH)Uhx=yY#gyw#^Gza+Mf_%wQn#JS@!tKkrrr6o zRX8?L+g;*Jx1u8&m_x8?OZ^REt+*R8K!lB)bVtdQpK*4BGczb4KGI5AS1Uw0T%$F@ zSx@Yq={6uFwU8+HT0rLy=NZT8C#_N$PZL#i@E18NRkzLMjS27(3f1&=x^ePMI}>Av z($JPAE~FhFbk}h6&)3@7pUX_uWnd}Oq0R#>$tl$#meQ7SC#Hm94OlHSQ~S{{QAZWM zS@ZFre#)L{*W)R0%}HKITo&_N)SB(&_Mj(R1CIOd-aGqGtaC_sf6|l#hurk+1a12P z@cL(*`L7o*IQ?_Z2(UeAxujWMHKt5}oW+_Be+Ai=p5K$iFdm}G-LAFI5pDA=H1CYO zd3-;0G&7WFT5IeNA8W{3A4|yss&P^>dkD%`909wzoa2v20oET59jxt>+vvRwm)D29 zrA53bN7w56SR#CM8G3KjkCz>t-&y!0r<_YsTOap!lC~KtI<68V-?-MEJ$*^uwwQ3?u;9#f`M@Hmek7~&OI}X|_Bxsgzhjwnx*6WJ^Z4+Ya95_w zSsXZ%1Mj{RAK{3Z&K04-IQzRA-F;6xM$IPRrZyUeA>5T zHgQBG`|af|py$WY={-DMBh7E6*U5(baj7}Q(RG`8)C`d>#DFAtD*hAWVPI%a#vyb2 z&F%STS&~9OCVM?Lo1FNzX*d3CJnQc`_~2#?()iPW+@)H~{$JP*womkB7rfX@zxfh{ z^E9V0bl+iA+7G=a6-Vt}0g@TFcHXDy@noX|VQZ(-4mS^SJbBHvos20N-lZ(>TJ9W? zvefBZEtlRMpQc61md{QzZ?d9UbHimvIdMXV1&uvhr4X?or|vtfYUNq0O}k4tLhGcj z&k7w{Jd1rDn=}z?ESn?`tp4>tft6(z>=dOz&(*%rbmK>5%_75IeC1T`?EI9+ZCUr` zB4k%X@>qAa63-%7b~GmOQ1+KhUl$Gu94z3D+?Gj-|?qDC}? z2FUnMH^&V$QcE>1ctqTk6O&P!Lq^Z(Zz=g?gC)0)f58$rp|1sdhsoEI zCu_%%!O|G7%O7)VVYQ zH&xqNk*lo@6at}_Mv;T|T!E^rsy#;h-(p2xGJ|#5UQv;$a}=eB{Dz70xC#F}kZ(~R ztd0cPm(@MR7`ogHuPg`yJz0A`aHONXlqlWUUN1Plk;3jKdIX@Gc+Z{`DER0iMhQqc zkNlPCp!DFzD3PlReI(Z$kxLJEx=+g7UR}? z3Ap(@lIe5$XLxRz0NRT&b{Ggn@D1RTzE&+_Hv}QaZhZmE+qHYtdX{tZ!I2GqR8(yR zNtyq+`McV$2w3Sskj%l&SZ5%FH&|ITSX84jVKj! zVozy%oTe=4K>d8+LftoAefF4@*VY2rzi#GkP(umz>1~vlK>nIl}PMEMSo{q|4|Bt;U zLH#zYJ%)zNPbxpp+A``*F8_@p((4&h+9hiAFmrxd5*6-a9*_=){3_vD)+RmMb(o2r zPVQ^P3C&4jnDVxjE;~F_0c;AV$e5Ur>};gjci5 z8C}wD(%5ejk>}kKmvw4J9O36UYWh{r28nGc)|g@lzWVZ2J@}?}r%PuhsR0Vfrn|U@ zgRH;mW7b^@=!%NzNr3T9OtRQ zu_4Z~a-T@$T;9cWRkUf6E0OpgCJXZ1j1TQ6m7RY%zUy4p>a7#gr!`LwpzKY2cSD|2 z9Ob;<8f;xsdhe2paYap+FgdLH2>ATUWb&Ni(~`r?nGD+;XGL6G-op&uUaa;6 zd=l*jFdY9T-FoQ=$?gJEb95lHhQjWF3@sP#LBuIu)u(^$ztE{yablR$h%ZpW0>>^z z-+H`IH#RjKWV&wddj>1mE7$H{p_j{`N;4?f;#f#9S8j)7^`0%46{a`rsWv=9Cn~-% zw5|-9Huj}f!jhYt)2FOH4v&9UQqO`5ql;tuGB6hk(eir9({c2jV|+ci!#z7Yb+F$v zA!#WYpuoYP##r@Benj!wnltVm?zIRi_ik__*cduDATFJz9k1uH3;tZ9jW)WJXwh}q z>vaT!9**iV7&v=yq~}juGU%H6LzvcZYl-5ff!IWr`(}j=bzp}lH_i0gjER-Io>lSb zg498eZcZKHTj|Q33zf@`oDFfa6dpJ#oFQE!#Th84_%HKcwn4&lwdD8?{5FDYXt*C& z@|>-)&X>|!j7K13_#306l?ST&ejluO>)h)%$wkvHC*6{q=~m+n za-Q9k@Un3wKR&`UeiFO$n5_&%o$zooUQ*@cEry!gUleWs-JLAQjFdd3fowh>3)IyR zIzCXWowLn?)jN(acy8Xu5Q_S!gc@GKrPxdt8!3lyrT8Z@ur6#1u+R>9)JPq3m5QHpIb4l)SR_BW zmesem$fSMm89J{IS!eL~nmS?>WLPH6GTmsg_Sp4$dVKB_Kzu;0Z-e0wuO1A`Y!=nO zSB>{TB@%p%ry%1-JE>OAZc^IR{~DR*@)KF)dcIHX(d&AXPacflTinQb7npL>^(V_r zXVg*&n>K)~-U(oSH(HbOz=5Ls8?A3zi%u(3u5*|(E`)ov)ryun@xr4~l9eXo%c~ho zn^h|WrQQtpu9)CGD!)N~wBqBCkM7Lt3pXozu}w^*qt2tSDQ?jeqEFi=bTRU0(U}o4 zTyqM+?&~pQx}R7n`bcJEvGZpj2w=fD9`c((vO}4k^o>Lj?mJ+7!V{#}0(x($M5@!n z7|-H!jY<|%9A4L~8%sI++b;xuCq4{yo|8PW-- zyK5uc&2zK2@|uD9Lcu%?Y)!@Q)lMG0`3j_-uCfFG^=mZsz^LX<GvGaZv@z z+Vh9E8t-FLPVOM5Hq+-hTUhhu^#+0HvKgF?;QK3j9o6h6P(MeH8+!_-#|oRSn8R5S zU%;#TzWWQt4*n2i^*;8@A&B)T*6@t;pUZp5f16A~kH*?*Wv`uLoBwq+k-{EWvR-*t zlh+@DY2xX7N4KX=XEAq2sM$nFB8xHLb!`zAr~PMrN@uOZjFWPqMnmC0T?9vGy@PMP zcmB~@(=T0{{n@1dbpB%nw@U$uhO~H&Zr%j{Vi9K^$Q8e|OepETLLMDYnQg7y#g?Yb;VoLUFlzAH51)Z3>D1w?u{~I?IB4V*v(ep9Lg`l=akAMh z5{YE_t^paTi_3+>a#cHLZ;=pDH+Xxp&^NuIU&L8`WDI%2C|V!To)1%V4R0s0huo%* z*E}dE8h|+3OZ~O?;=4$C%H3@a=1jezUL|x-tJAqElkO~}o4ulQvkO=5+uO#Z89b61 zIbQk~Fmmvdq^3PlE44wzp!JzFHDu&tEiLwQ&kFTX4I2zjXqs&$FNJk#4ZF?Leu?K6 z9banXa|VoLc^??HlP6xs0yj2qe#SL4$n%FxJBz}iPBdXDl>%ELzkT;BBh zq&`tWFbW`Kx8?iDewT?DSPcUSkOJN0TiMxXE2rEnoh!jUIl#(s17Ix%Z()EvnJ`ps zIr~$Jj`-KDrf`S~|3z^{^GC`p?P#&d~fH37p3@CJN)CX-R*sXqW5X12SnC`Q@e01oenN z$LOMVr!Q;r5U9yTtWCi2spB|gteNkI${ZV6>$W01J{W#pCE0d)!54$>b&Idr&8cW;c5i%zK>jeByX9lz)x zEF6d;a_l>aa=mvh;VBy>B2 zsrnX#aDw+*kDE(zwBODBUSS8xM*A$24FYH%S+$=nH;%BDUKxfVUJhtjHx~IG^=aMN zs)sQ#;;L&9iWJezc><9yx3bgv*5glAdHM<6qp_pCiIkT%ZUMkXPMgZ)wg*}L8bZ;C zaP;VC!o(#nR>2#uf`o%ew#HAY_Vuxabxi1ZFee@$=t6ac@X1+pw#!nqL|N0#|8Phy zOHAYYC@lw>*8!hqgSk*jXAkR1LOj|HIXeHM>W5Kum)23xM~7c;?k7HV zXgVU&$XsAbHau*?TM)ToCVR0JcqHIrVERXOU!zij#>~q;*LhUf!V+txN_G05OTVQ{ zMDnm=0tXk%Q}6YsMj{$)jsL$)ZanlxzP_DrYSEgMO5x+_7fXu|$OO2m6ifzA;G5RO-DEduLvoWo9L0>yKoc=;(w7*!_XDg;G zOeAiz#$hNVuxw3J>7te|f^0OIA@I}j33-&k3=84%+qn3m3X#PT_vfHJZLmAivS(Ke zQg?54_ks@V;~VtloXvB=rrtP862^Uj!nx(chAp@RMnCDz7uw;6f;r*br#1pKb~2W; z!!SNU02hyX7Z`lbntuxIZ{kua7@=YPJ@cBrMS!<;87iLV>YoTCC z4(9G9B|i-PFYvcf*LE+2-nUlPM$r?}#ISe6k-vPyfuz_GM>11^y$uhKQ|G+CG0;<6 z&h_Qs9R=ch>6i#?3{q_D(bQ&_C%QjMxJ~f#P zx09i`RS`4T(aS;#gr&+o(CdsbzcZL|KeD?0)kIjeReSdUlYG3|9sL+!r*B-UaiYR4h;|`JJ+JZak|`ys%sn#;eK>x8gXNGj zaR}$0Ty0B$A2|E#m@co1abG+T4$HN#o^~vFCj9<);cc^)kTnkGTLJGJgwTNHsq?n_#nK1xMyicUEbCAs4Il%SF%PKB;7^ef(-JENlI?u; z%3}xV{{{XXk=89=U<63ISrJ#+GZprdIh>K<^GPKk zl$MvBY&DY3@GZnReQ>G`RHK;{%NOLE^kq=vNlkcdI8tY#eGN`W)Q3!H>bBX*|B*3p zIoi%;!ggU$6-iq`hyTVrG5@F1$YT9~Gk`VlJ)9MJD)&@x`>4ciX0}D0*zTOW%S6kr zgbLI4+t^>5pIFr|K7R^?gR$=X?0MV)3TJy1a?I#dwhIY&hdz(@<_v-)^0uEVpEg6F zum|^Jqc;fOuC&xRC6?LI=}aIZhJxqirP4BF=Ry<@q=JZ%!=gT4>jUN2mzYAOnp&Da zoJ@_g93jJujna_)SaV+bQ zz?j#k$nug9S0SeNGaRR{C-ZCStuhf`KmcShSC$FuxR7RcdgT3zgQ@9XrjWU#!y|CZ z0O~AY$=2)v$^vhC@z3rsHtf0-T-5tZ%{28OhN~u$&0o$x?cB#9Uy#tw!#lGHhYT{O zJim%s>TCmWVpC?x>kvP4a6dyI4 zwS2-6H}X9K?RcUO9<+%hbT=r?iaAanHY=1s;^Cz>R1K? z=+l(EH}rTR>_5J*boksYDCJe+#f9H~qfBNC+Jx8b5S*3WmfgP0ss#d;*fh?m?s`fP_cdlt~bm4Ep|+AfD38D85wU6wlpA(`XbA7JX2`a7clj~?5adz_O*LnwBFs8mig=doUtB1R3zyD!aZGxAo z1MK7Yx~q7>QAes7^`MD(h4k`ImxXeZz(Fim!dBeYTAt>MPa4@R)$4VB?wCYZ|Kqt0rW;`=buc~!&vOVjr;jTGOdwiqw z&pOg_0^>=cHwQm~+juQwx-T@LZ$MO4cce2G(7^|~xF;Lne;~Lt&#U*h@WbDPV@NmO z270B9fq_u#Zq!cs6D5K+3OUo?+itVR7jCn6N6&9lE;f0r<;%XWQY--eP;;k%h+Z|& z4(Cjv3Z9Rnsr`h%yTrY(#wwuR-fZW5sBwa2rTUaiJ+4pqYhQNUpWnm)cr$i`U$2+e zt1R<99WZ>eQ;8=h!j_i86Ut>=@_Y{RWXpn+RUHZEN}a%a7%+6Zf~Yi)sCzaYo6${= zpZ19P9zDo<7}Pg|qf}mp607*#C-QQ?KfgJ`W|bPzT;tGQuEnQ~CmGpakJTZbfCdoG zp`;xNuhn8%#6ltVE`IjyVMw`brDHczb6aOVY_zm1BvH;!mKIt;Zf;DIk%rHlLaUwt zHIYTSbeXdCuAv!4ll4_sle0`9zRt#f$jwNkQe#}kcP_;Y(Fc_Oyuf1e7PR6_^Y{Fo zyd6lNYVp&eTKAkzSVRWeYwuMwqWQoOb0;*S)+telQyJwW&rsM93Y}hvfJ5 zxkaUvPYA`bUmdc?J5n(({|KmBZU99oL}jjqjF4L*YYLP%r!f6KSXsSeb|g+J5Gtd* z&YM~suFCX^`<2*A8S!*5&b)U^!qAkM4+XK#*ai^?e=#!tOn9!6cb@Jk43q2+1l(t) zFSotbk? z5+<$(mIhCGeFLwr%>4o?jjr5-Qy)LvOM|-VSYtDF-H$+`(*g<%d+%D1s z6L1ORS710w{K59s4IOd#K}QLialND}6V{3&|nD#1nHGkbrf3?!n!I zVuksFLtf6z`TgSwqFemo*@gF7r#qrfm_Q(T+06e1eAauv_t*3FVS2|qpPmc*`Sl=4 zyu-l?;!S7CNj=|#u)5y9?P-TTuH^2$J*!p$G`?n&)w^6PrIkXDg(EsDd^a(b9Huvr ze2Xj2(b`DIA|1`Ng2U+vMKf{qqUWA02P12b@1$@~WHj{f$&ZI~2ekGhk1H zWUo~1bVQ|rJlsp8D|-Tfh8=6hj^*C#@SAA`6&klquvVFJcS;%d-bh)t-~Z2uH79yG zn)x^Kl(FvIIWCq`{g)CG@0+QP!K%wYO;c4n=8|Nw&AujeRybjQIOOLc8xIQqs@L3Go($Ti!%!yz%PdNCo|eKG=(|A=Kf!WTP0F;N+mLv#y+y)e%%{C9 zG30B$ZVwk{YZ)YeSEpuw=2>QiX>2ia{{?^VH$pJ*5bN2B0#Y?xncv2-F-)Hdl}E#8 zPlZj9rzqU2I_B_910;W0U-hlyW>G4}XifoHM4xRI!7*$yhW{csXVg0q>NZ}A?nS#pPlqPlZ30u zLkE+DAJ36cu?hjrpTQbTF>)=WMrC&b>e8LIlllN_x%lX{p zA^HExb$`)vyWk5(h#1JbZMgRTPl)VVhv1D+!i&1<&xJa-S+RX+tN-&6&ej@z@8}JW zl!8=7s`R?A9nH8nXF+4a{{Yu1=b#ii6TZHV#jmZ24%dhD$ zt|{FMBz}UBR4J1Ck>!V@SZb+BN&?*}x#(ttE7Sev2aZ7}0T{mr5V|Alu_7aDeJQR7 z*W0!dZ<9V=+`LSIzayqvMReyBjOu0Ktz=x%4fJaEVUmJz+=@=F6~g-Hv(5Aa1jZfo zWXdd-xRoQHXJadI6GHTZL0K|I6}NH}%gv=QUW8GkXKc6Q{WxR)H5O2^nNn^MMMAM2 zMS)#)@*~cChe8)o3ER5-T1)h7upQq@zd97PpQ4M0Dp=-?HKi`$-e(VXqx;>2C=|xacycAJwG(ZK22<-s-xc@_ zj&yz%S*eJ;9>e=IBQR1*1#(QInX*E039#>YG`-8=N~4c98LIhp_6R8|uzq(b^pPuS zx%L1?L8mz!Wd@tc^ApaIk}y$#pMZ%=8U{Qi=EMJCnztkBr}f`n*4DDe>=1BKXlQm} zp(qISuU{dQk#Qr6H0uhw9<`K&p?Z!*ePL)g=%|9AsGy?QguU!Zppl^2W07P;#;0=M z-yMU&;SMNBE~f0OtA=pQr`H-E+b!}Nj%|Wu9uiwTVP8uys_py}BljVZG{htAG?4DI zx#k+(ug{L)Wcrg%I8Ib4J`7mQ*^0b;#~E2wPMqcWc9)HTz&ImEjv`xr|0r-JqfKTn z5>amo)I%x?&ReEsOTjzQ<54OBe5~ht+|3Oh41aflS45f|KO9K~y|M-4Hqd}{myZtY z&kFu3ZKm|lji|^7`!UxQ4hqe$+O`B9k0u0H2|^k2$fq$}Mo6k$u4g<{qk>`VERj0; zIpcTfI>umiY{s*95I#>H7C52QFfeGyu{^g=;jkG3<;WNzCOysVe9M!4xjL&&?@%A` zyw}&fs@R_0_-aV_+i#iN_V5KJ$y9hXq6g?yBKtatNI%;sz=CgOgfR8`dTN3R0Mnfkfyg=gJ-uQxvPjD@wJrSrAM66fP- zN3G?z#XRHSDu|h>4U6ezD$9|lJNGbIV(VA0|9AfFb(Lp= zglbazDlAdXk+o{{0-LEymmYh*R~p+P=vqYro1$l&zZD-7@HEOqTxwEkg&y!M!chQ z?RP9bH){RkS7$;}PG8b&cf!h-?P!$3yK8+voz7UWVPZl^NDwR5G{9J5XP^^b)N$E< zC>ZroH>rX_29r-=Yj&LtkkAjDzom~26lPBZqx0P-s42$}7~l*F-dKUFGU|^&-Ouj?9(4B4J?W*{?D9 zbU7&aa^eYxs2w!5B?;-jwgGcLJCQaRrRvEqCnEZXZ1}Wm_Fk zTDj52b@TfsbORKIH&zT)VXU)3byM&!m(*~l^I2i_Mo(*Wn!W>(z5k23cMg&)TDN_x zy36dcyKLL+vTfV8ZFJdImu=g&ZDiTF)%)Ci&U>qIk4$NRZRYApdiCO(Gh4w$Qg@_7M#7{zd-B%RQ5_| zt~Z%odsyjCgHR_=c6LzkcAEMN#VUI+qN$<4IR~fBhe@iHYs3UA#uYFg-bjBjpj}^y ziATFbT;pxDcwpbyg>WdRGk$qgWfzCyKPs9y8yhD3ztwL6~ z=xPCa-LSb`ap*Yqocq=O{_Mc?`cC-qsAji1J$1{RFiOrZ8@pdK1QGA4Zq(bK$HDvK` zm0l9^V;@9^ZIB(s@_Ntr24L zBlb@Or<{yne?yq$EHe@Ns#baan%y8Epi#TmG=PMOh;@g>6t#3?S3kbEQHrM7-6Xcr zF+h>sg{UHggoKQnZZ2+bs*qgwAQ7!HjVCig>^cUU#aoq;gqq1{8VU5eEZwTJ4Dp<& z7O1!PcnM-B?jMrj@cK>Oiq=?Jnf2D2Ps~uQiJKIBa~_T;GndZDN5CwY`mI1Y07Ohw zG-2Q5P*iNe=Bvk$W^dSX{QM}=G`f$?(!t@ei?>MRN;ZxCBXWpo)ttfmoNiy*QjR7F zpk-W6&g`oKxD1tO8hWy4#5A_Xo{to?H)IK*^hO!5#5>@{Y}EZM@*A1xB=2*x`Gw)G z%)to_GCqO9WCcmNXldj#N%b>QetUDWS!J-M&!~8#OS#VDRhgktCZ|ck&Tu3oa%=lw z9E`3BeYBSQb!3$(yzCt6@gvod%OFAwwX{p-*JsiIC5&c z-1x5A+0gM+o55i=b+(FzggYHqxP4$&o{h-@Cc7X}fDe3gJw?iS5qJ^Sre_4Bc)VV6 zMbvxIS(!Ftj8=j-U#1+A+Xn%9u|>MI>SP;tB`!_;v}(?kN!w)MSWj||7R2S%m%43M zbnTuppYsWyee3pvwXMu^%HUSp;5ddov;k7)tvMo~MQtBv)KGj&^s0ApVPP(1zQfR% z=5VFRxW6G2U}DMRhI+CTP?Ih%Cbcp8!pfmvp$!WLc7*NXOtOL~nK0+Qn}^D;7)Zd& zxR&y+!VmkHan96igSXMgTwn=ZwurL5TA#5HXs0ge+pQMn4}qmLQ3H38YQl6rm$Ep(4NotxF~C&&jyz z?&>s!`uBIb6?P%n`lJIa2?Xj4ns;|Z17L}ZwUae+2q|&ZF3*jvIbAAxN1r!o#PRLT zreJ0diIo~V=%8;&sN5|bemK+BE7Tdt+?6KuHiGbo0&fseosk?-0OPlxtfKfL(390< zIVQF43pDG;*cs1w$!f@>h(q0OprOqxiJfKP+hGi|NmPU);1XrvuB`?MLuTr4S513~ z6&Ce|JED17wX{%~8RhGBoU(rhwLgEJU8K`c(v|#tD?M8CMtR2A&1lyaYvV&^ex>r|(3p3yhhr(rRXDrW_5&rE|JB4*viV+A$qxbg295~*e{LH|>u%=teHHxrRT;Z=3M{4nSQla|f zPK39C6HdG^7<8c|ySExV2s8eTXdgW=eU{xPCMJe&bgcWrdk5me!~wkp&8a?3GU7iA zf3;__Rl5XQxuNjnY0cAG8#m9 zj+*kCIu3HWyTOrm`BS;aFpamzj|AD;*=n;_ z57-fnrbNizo`@u{uZ2Sj;NgcrPF_&Z-XH%CekIz(k+HInUb5AW@d2f2(YgO#=$4h5 zgzWHS8|KnjnUp0RgNiPh116s^T6B?BIL@HLoD2&KGx=RzQe##7Q2(HP0Gqn&@WTrj z+L@zyw{%}2=?eBku;Cj4U&4S6fdS#b);S~*M6-3hCFPfk^taL1Dlz%JBrU%bzNHC1 zVAHm3WWinqD~ALQ5R2FCgV=E-?;)gL6grfj?xp6Mx@d{-f|5 z5gUgP!P{6a(dsV0{~U{kUXEX131XCHgtj6{A0c+viJ>;Cixh~@LDaMY=>Qu(luY}r zex4bhEJTpBAvrp8RqP!|N*1ogK)fcjnb_;D;lK}488W<*T8_WrV27PpzVU+l1Xwua0UqzM{-CtvEt7 zY}}hplPMwaF8yRiVMo)gzEO-X>=22j!JwDuxV6bB8I2z2tG|llwK-27iFUF1ji5qEmgs~kmV`lp|5W}WA6NP$Gav0(pim8 z>65YH;d_1YOTZ!TMa-0pF(JLaP4)_1=)Qo;^F|_de}MDe$lw=KMce@M8twm-7>pEF ztde;)5{B}s3?p4vm(<>*rz(=}%`8;Zh7`=Kb8Uu1i_=p_5w2gbq>v8kOc z@r1VNZhX>m!tEmj)bmPEB~3_!e0XyI2Y)|{`B>BvD+!<9#i*Vqn&vZR9f+rC4_ z6Y5&Q8(2mh>hz8e-v)U$6ARRNns08|j3e-QREzma8=V^N97#Mm=x)wfo12`dJ}x~U za246!nWE9{n!(VxNKUzLQ079K^p7;Wj@`$2Q!ln+uD3qZAH0t=8liESJsvDi)?04H z&^{BA3moCTo4f4fXYjpD4)5QlM=s1V$~8Jeh*@bLuqvqVZ1Bc=;}7Dyfq^A+2=f&G z)i+vP;9W_Rd$?BFy2g4&zl<5LW26t;iZPXHlI2mXXSccne%e^&&T7GGh-|9T2U|yX zDULjg+@!p1JGHmh-^jv9l;OYKx#8wT-TwM3W{}Wg9OClNw~~a_3-RZT$HBv=M$wjt z-7qXAB>I$AeEuq-I0y{Nz3EM>fLhDwi35mCz)@COfGGDbKk5?amg}I7ZF= zepx<;&U-~-iYe1yMf9OgeuzrAvWw~FEcn7HD-HIhIPQB(9r9O?n9-qH|5Z~kG;EV> z4(j!$Gc9p_fAvgU_c7}foQb0}G#U+;i2gss4kr5h;mS_r!o>)>* z3PUh71d*w8paGLvRX_I*wc+Vp&*g7AxG#IXBG>PtEH}N*Z4xiv@DqM+j@ei=+2yE< z_-0(PXTIz3_DppA+&*zZjdzijd1KRXIy3yF^B2kTPF*6k*}gIxC11Pw>E*FL;A=8e zSJPbNgn^!c3v=X1pAXpCh|Xm@YufDz^C%s*_OO$0-L^XoUMvy|K~Bo1q1;pRob28% zw?DVAb}`K=NFDmbZlW)_dV)1N&*V|RzWjR1LO(YW+&`C zBKB(qYnWdJGkbltKt&)DW2;#9>$8gF*0t}(priqBcN9K`X5-4ny5k)--1-v@Nq8WR zUJrtH-I?>jD8jYz@D(gB{M8NBV~Zf9x9!oMQ3ohlo7cUhF{O zxMI8>#^mX|f(1S=UqM>%y?W=TbWM%rOGA^|rT@wuzsG1F2R$}y_kv|9tZsaUUt7-s zWcB9IRG6V-1!qtAFR*4jm0cPyyR4Cy$|{~rp1`yar>cLjSn@la(6ihpd{>cBWvN9H zv(tN0qgH(<2<>Wx&qqDXh+|(GC_O@?rwppqmL9FjvQwFPFl%?!kT3^dt$7uhD;SIS zY`hMAoGdrRn$tcPI^)29QmuFrVa-3{Oy^=yHnOO?V@m8{ZOS*>P!9xMu33HyvtEtr ziI3_RB>=^})3b6gCA84U(30bS z4*AS>%2DC-C{r~8>C`TXEWU0)tk)Y$2foA_6Gi9~JAYQ0`9f=q4+zCx35L!IY%lu5SK5-gBaZg+HZQ&yI7R%|N*F^W@nk&g4SM&NewU&ky5 zF-qEYPnI}C4MRkk72DjZiU_6oTF#8(TABE3gr1j4SoJfOBZF*ypcbPpqsZP3lWZP6 zC(Mh&M?}5;wwd z{T#k3rP{^)Ngd0tEF-KoLC)$^tFTT$Ac8hB7nXIFRo>;NKbIhjk5m~dq0GIc`mImD zKsx{oQA0Z#)xFI#F+ZZ&Wn^th6*PgZ_e@kkz@Lcek{n<~iS(ESIZL@pB%az&+v>*@ z`hXNKz;tBApqGtiW;o_NEV4l$NZC}ZlcFHt`BpFoo|@;OcXoO@Ld&I2@O|uF&x~xU zlI)tPgZ(>^{I3LZK67T#-+_UGV&2Bl35xh1C)u3%;GS0Gyf!xL zXT)oH&e$0$WFk|L^Ae=|Gk11vuIow9Hx43xDPg>YO15zdGBJg@yH+Agq`qdCENn zjD+>I2g^{ z-=_3HEHN&Fcfg|<_BeHLt>u(=ElbocMO$x&56M;q+5J5IIg)dNdH*JN?A>>8w(582 z%g{x#4VGwxJw~>&*S*E)Na+m;V`&bBmxX}>liR+~CRKH)5aXzw_31!4xw?w4#hkq|8r0}!R;L$Ibw0x84v+ghJ--?U zrOH9xfJ?3i5^jP8bDz4FuzZZkldfCk(U5X|GzykhD00nt3(0b#K*aGxfhEp*+mn~d zeG>$=os}s{g`aoIc?JQQ`q)iWVJNC}vNv~Fjc?(@#oYyRAPI4~PUqy=LEb4O;;+Xc z08tOjrI&?jT~h)MR+B85uGvid8rCu}Z2Cs87($Ob>aFk{9)5q!^zqoAxhLyEfL#Ex zef?~^@=*4e;eDxA^9(!NEk8{iSpspi-!0S+yE;lQ6%?#WqXKF*;b^!dW%w({A+ngc z`llV6Axe!U)25+V!8Su3WDa(<7892oTQKSR`277t<<9DYa%W4|Tonfg>rQew=%8_o z6j9ByO_R;pE4O4($bdC^pDTmxXc0K`?>bRJ1Qd@@UhV2*B)fEzB%8_S3+bD}*g#3xE% zgnvQ_WUW+jnbr8lhW&$QiO^2~+2->;r}E?zZOq3NYy@fLPK31e`ZqG%ICtI=se1~T z=Y+(fFy>*qvhrn1s2_Ln-@qxLh!x977nqm|`NGL*u0QyV>I{^CI9h!X%TXhm$Ohp@h@#axaOq;307$rD`g&0G$Lt$^z!0jg{cf>!a-Z2F=g0qbSA{S?LWI~ zdx1DUJ?PkJ;S6TxS=nnFk>A?v$6)h=L z4Id@oCX&@FE_WMT=2DhUyCl^#C8yE}LhXBHQddw`|D

YbpvNBYf`F%rAI);i#Bl z;O*09jjl_wrtF!{PQy#ovD9D5iCz;7#>nKOXRbeIGZOZ}9&*%MB1BVgiYl8CxWIpg zA9NMp>`e6hsr4FvvIZ^eoM`5nfsq%OtU-5wcVHv`LJ=@HlAj@2+tI=%_|?-h{w!js z3EsZAyst?eG`1I6y+;nWGl_`>izAB9nR)DS#}KeY$qN#5r731DWYnN!hI{j=1xbFW z>!E`U<^4^fBz)hO(yrSdCA5ig{!I4VIl`UlMt3Fl1YGV-a<)0Rse;CveSmX~5r1XVIwAcMri|+N^y5E?8=P9ySJ>In%RhFW8o4>55Lf;El4eTGPnm$jH6;*7F z1p8{aHD$$;oPo*Ba2Zo{b}F`$9}N*UL`W6=LvJ1T80xWp=iq zso7G5OPkS^k9T<*wSylvJVp<45XZ|ASOF-PUYt+wDZYj5UfQl19)R|EEUZ@y1Me!b zrMMOZl}1PsK=N}r@5gs?wV^lS=fFm5k={>tDyRP!m02l({r{25h_i%gl^r|>8w01W z->vY^2qk}K#Eol3RrfE81r<@m& z99{E;fd5+i;N)mEeoNbm9Q8;2j}z|aoMQrlkj_L8LMDE*T^BXVmQDo` zGo>{esQ7-KkGcfq2M?+t$~K+Aw~{SKK+?%420p;&HlIqhna|k!x^#Vxif_E;aV)ak zp0mjx%WAHtxR<`E(~^bf z=RM!96i{b94t+@Wrl;@2{daOs`9NZh)Spw*yGOLIKg#^pPz2ver0>pg-dfoM;;M;S zVP6u$K9eIcqW*kEqsJnVUo??qnj2Eu+YMARqD(cWvh(T~mQ?<8jI>ZzXZW8|q=b;r zBm<~oYb|il=b>t061%Yoh13swiK+AP#f&2#)oySZ$3nIGME`S1I0Qk<{Kj=xFN1H$ z%az!JtOmG;Fa1E#Z;dH#w@bf`q6$Y6S!X$vzcZcka7AASaL&=KOX@kZ480N9Hbc$q z;u@{)inULd!8!>rLBY9NE|xDCEqBNj<)i#Eq18R#(-JE^2%zMRF=#~J4RC@gFxOC# zkTK~k{!ZBC9pIrcvBBQ z;1}(mS_K*0Cri{IQY)q#`IK^LVPM?uv*X{?_bcwsFwZd_!JUsL$Uk#s+BlZ&m+EXU zr!0Ap`7-y*PNvt*JU8I+MA)mHfB{?!=~y<$<2W)tlI{VdDNFz@YdG?zy#rc0s~ZK) zMssu*$CHVxhI@1_BZ?_vH=G-Xkv!w6Eb6Z%1bI$&vv1-OCiVC(Xgm{;M7J>H8EYa^(DnVmuv=yThHcA!KH=ZL4vYcfSHbVA zPR7L3e!c zyhmq!bocEsNR9(s*r8n->=)S>-d-B`lsO*3|GTANf#ci0tZ{48jU0&6{s6h}ezo1+@W)UE>eH_j%Y<_z`LrXqgN@Pih@9{QmOrE8WX{GGbUBp_$yloC@ z85A)%InqjQaSJpJ700`TvDyGK;$K+@H+;TRW6l%wFNF^r$|&Z;-QhCFaHE6(yZ@vd zU0Ezby zcFR`|B(@s?2CB~8>G$wE+wD8+?otfb^avo&sx>ddzn6Cqcr-nl1;DijJD>B9mpXy< zcvw7pF{eAX$PNLwH_KzaV(W*pWDZ_nv<|==7&bZqQsm`UAAKyW$8E)QeU8g6Nk2;4FdjCX!zqxJS{*okS&J1Vb-`-LYPm;BaUWbwASOkf3(9Mvdn-dLcR}n{ zUH1J6VlOtFbgDUR5gZrKEl-ZWDKlADs`tXA-CT@Y^>aS|m|;4(AGDrtopHWb5YbR^@yKM#ft@zqIBKTHP(9c6ydFMrCxW;^;B8e_A}K zIB-ylu-rV)ZJ1&ngiQPtyg+SYFkFS>GDe9m#~w4_t}3#Vq~lSB=D|;%v0i2nuRXfW z{$QOU!B4{rK`GN=yJn3tjz}+E3(xAgOMaOy5#rAHaQ*^n1X%YwV;yL(HQNaA2C-G^WDd8tReU?iIBV-@w!!O+-W_DzKMzv)RaC2S~;1$dk%T5PQqR z5)Clrr6(73*d#%EP)Zb5799QGV`^81->6gng-JunO+e;z9g4M;YUburm5WH z$epLwevD*uRwFXvPlC<|kmoLx%F_E&>Ex!WYlI*~4OjpA_PmyU5y4R{h3Kq;fVrg$ zDXa>jvGOd!xtIP>F?{hAqGb%44^$bC8xEw(Ggi0P7u^|IeYkS!u=~ti*-(b1*boz3 z#ExAXNwt1jDouYc>{;h|XY%L1q_pL4y+l;gG~TQ}G;XbERtGVXYvPj8x{&a#9r(~yw+-Zry zGU++lLTHBv!3j8=t?nz@Izok9s}(lkpoy*W5W)XGU#re|Bl*9=ce3l0HOg?H6e^2?7o@J~0*K7{IaykriEAn0Lw?Ryn2y4jk zuQRg8XRH%qZ`}XPJl`Di?#W}4#`=9SNx}%8A%}pHR4pke7);2XYItT1XK1WoY>$Xz zw+ly25C<_KiK>#RYZ={=dPBBe*Ip6|@;%=Diw2>e<##H~Y9k(*!t_~R*)D5a;v2G-&ssn%c* z6$+G!+TGscJ7I_^o_3ML;J7XDF{aP4X zZnS3xcd`XMXK3^?C)oeyvZSU7jEme_QAy=bfb}RO1VwUgD;%^Mux$F2@jTjHLFHZ$ zJsto2)Ia^X~r8CHt?w|=-pyN~S3 zxhjfpG)+!4Wm$dT9B4es^ucSRE1y|y;JL*=E0cc{4*2}I96*` zUt!a=S9Du1#~`^-VcqmqrD*XFG8wbPO^w0|ivrT11!E!n<5n!-1o^C1!K$pf(n$xD zwXjOU;)&w(AZ;ZeoGtC%-hEcr_m1mvb?#J^S80>eG+fp9#RlzpuMEsaIr74SS;Y9@Bd z7mWgtCt|b68~1D7&D2pCsnH4xfr2bEIO69Srv0&Inv)WrP)9&ax78W59=%H}-7Ek5 z79pjAI#z`)H^w)V=0(quSBYCLMr!iIs~tpur4}`J=1*$66h7DjvP7vh?hfnJvQwkJ zG#-~jfsPpq`WALnbsna0von|49UT?D{6HMWPLq6A>Ao=*a~2P(KPH!kD)cGo^+Y3; zzjuQ?xtwuC$~Kxiq0i3MM$jI`rK@MmHjG7MezhZsm0H=cw^To3{WV(w#cbSI>Jf=wX~lz z0T)8kf*b!b!l^5|*$#zG7)xw~7%l?Sek0gVq4KQFN(DKhWAnKi>Dy@z3PDt_U zBPO8`Er;S?A$~4gkf$P$Z(mZAUBi^j?uvn(0{Vusfq+ZkEJS(_penIMOQ znN<>C;xw7&`Dn=aoU_d&Eu{g+j0bj`)sbsoh`AB0WdfzMh9^=tvRMM}$<|pia4kK=@gUMu4Ih6Jkvkt8n zt?(!ok7nIi5b9om43$OAJ6#awOpqy&fvLE3j{PHrSTyqZggpYHU=GZG1} z79^e9TM>JEGs4wpwr%tOv(+D#)e)e>Y9fo6B-q5C&2ar0N1uLKkIJ9W>P!8&ZF9UorNRKe5b+)x z`p!8sZB|6b8PP|PvMRM|XAnXHqBQ@9JfqbK2YDa24>Pr&npWEAWsI7;j#0xQ>r)RH zmun``ct#njmXTfXM@`hK{oZ{Pq^xe6J^EIpSIQaEcea}~{lf{JUrEeKBr2PtXR5H% ze_pTH2H`Iy`)nD5R#vcYBf57e1S5PB%pypy1*%x)A!o?gwrkYtl#}%yg;S%IVKO^> zYhql8OOoI#-?*Yz^h4nn$$ytx{p|)Iv*9A&USKb+a3Z=mv){o{q-%8u~Nve@W#&F3v#&@$Mq1MRlEn*dh-Tzt_+`Wra)L9iTw-cSZ`+lw>k zr;~{fM^p4@jtntnztrhwW6g~V%u^2s$JlrK;J4=AG2k1Lx645L!@zK7e*Mfg0tqEW z;kQK^^2Q39RQCtTFOW=$cbA`TBxSe6?KcC}v z)4%ev9^8;!{lyvP59cehEe=QVG#})&Vkg*O^OwCM?0$Edu_H&U7$J_CVYLmvM9pt0 zx>8%eHe^V>gKp{9i2Z^!u~Q|%8PYarGJKLJTpu$JkjALWm}|e@^5dT`ON5oq3l)RX z?;Zg;LmC=?99?=BV_B6WbJO=|mFz!axkz|jacF5PxRCp1%^`TbHbah~ofJL(iKJrf zuGvW0M?b903Y0JftOh-Ep31TJ6ylP58ta)THrg6l508nj_4qMD9;^weD{kmO%NV=t zk34&9|JlO-0=52c1X(aghW|#&^uCA?VrIt{7vJ7*UBp+JUs;{@3;w0m39wWVKG$XP#H})i*9)IM5Uit01*_q*Q)1(st1#G_oTe)W z_2qG8!*iYW6HJp;hj^UevLiG8m(HUjTQx7%{IxaNQSq~-S=&9a_?hKA*#~P+=jt~I z^>?7LO$@$}eh5!Xp1?(mp^d6Zy{cqV%cLToF^7%#Fvxm?TSyX$52zQJVLlO)tvjFV zT`Zd``B7wsb)#}`9)t+_tolQgkMm4kB8`t$sxMr*AY}4c;B&RVG;sH%`%Q-V&4L~2 zW9iEJaDQ`b6xYXCD_R%Y7I8;K)cW)rkDD_+uT5rexZOsW?2syZITC9@dHxUR5ByGK zfcC^bLQ{?78i=ygvDAD|mD^i%a*Fe?pzVe+5h#wqwU9tj^C+bek`;F@Pm%bDPG zVSv{KJYtYrDA_Sn={AcX7i#U)qbtwmwNrBC2+RU+5<&+G{+!TB3AL66yhDY$N?T>? z73EQDEtx!Z8%rr>Y+2@1#^AWin|IMEzJ+r(T|TE~|1jEz z?WR1Ta&$Y;2S%uCP5`eh?dNkLgoW$(G~yRqHUoH}hiY*V|f+AlxjJWy0hi1F1c(AV_RFF7RB#*K!iwB-o-|4@=ccHzg?K-*EoD@_Ru@BS0B+nZH^8S3%jVF|=eTb^ z)Z`Gb|5(v=`wIWPqza|(c`xR<@Mav8)b?N5JtB07S<}s4#^Ei+q==oyEVWI|M9_i& z2cL$!SX$SgW&gMsFok*rm=zfReZxg@9j*FMcodCbxaxx@(z$r9(4Db-BiyCdTj!XU zaqQiDxq1sUtMTchcZP;OHM0jZcl<(`^N#S*_BeRUc%Ez$8`vZzU;jkqot7d#Bv7Rk zrK$p%>;smKCW^0ocsCrNx)0XU*t0GJE_2KVATwHiCci4vXUPLQdU`-Y z$8T-lKMpcl+#70NaAuZRcy z?Pj65++JUdaJ{|r7<)GP&u0Ct@{ixa{!f8}OH3~NU4@5}5x>$ICC_|@hy)~;dV<($ zk%{GlF$|UGE9OAi3WqdlN=gD+NiCrk`$^XHoUJZR9T!hXPrcH&qnTOs*wLMN%|j+W zypkG0NCOtnA?#T2eE?xLnbxGM4P&O5qVf8GsgC?GdEcHb2U>>Phj_=>7I#EfMvTg? zc=44qd}N|#`X*OS(&HGVv2!u?aEey%ON*_rovQ0gw`kmW?`OXcRX5DXP=0dz#V&!p z)2lw6{k@IRGYtM#x{7OUcLXq4ESJNeiFrqUNlX2Qh(MG@wB0aWQ6?df#~aI%K@1U) zMZJcHMGPqdVG28E*LH3b(zMY2{a?l$%F2NecVPY*bWr(>Px0AZt~DvFS-5Q{rW4mE z@Ej=rK_}7cP}WaOa2Z)MOW*AfvqCGNpBFIj1$aRvwg3@Af(4dq9<${2Z(hu8Anb%h z!+NOH1y$o@hzK(W-3e{|CHQ1gp1u12m}(>Y|62m#_?ZDPB3MJtb@LPy#yGE2o+ec3 zN-Z^7>Ylory`L)@@x*>6o?k#zf@b~|+sQr~9F}g!Q?#sLz3u&Gm6q2y~qfYK_J;e%i0@_&fihbsn16ja|0w3xlSo2>=$8cK|%1#@3M0!>;O@K1dhl9+%7myrcC4 z@n2pjtkXbKWbCVUo4=D_T0}ks_cKn<$HniO3cMh27o=^|0;-url`SRQ*Uq36p>Q@I z;~7n0(=}qqa7*|%P{_I06`~Lo?n~lO`0Zu{{KTJAn{FX0a-KGC8*4W107%H+0;Xbk zOeeYG0HJW?6FdS6DMdm4=h?&}e@8*S(gaz7s#$&k2{L@5uLt+zWoH+!wLh=`;t#W~ zcdn^}nTaIEx;h8v!;DN;u-GG-ZMY4DRc~zNk6HLlne_%q%`VLEgutR^p&giE!=iHl zdq)EMD}ZI}*WtDTvDRp1us0ZmyYduBBQ^Z8#4A~_hBRC(Mo z0`VyqV(Tsc=WU@TGIHFAx$!5DjVczC#dr=5K=tfbk29WmAnl#_I!qul>%3mt_N zgb<6^STed_+Ohs^pd5M_7#OLaxX4rNW|(?CS_se|1_n?G*_%gtzJhRpK;XPY7>U<&KBV0Gl1OV{m`lYWmi)j9Y}9XBhrHv^fori2kkF z>qE~#N(Cm#p2Y93w&%Ol%YpYIb+c2Q+QQQ3W`fvUL1Qa^u2mLF%c#Gk2yeyNl&#qP zDdm+spX3PH#-OmjKIb#8$#kD+=K;_mw>XIkyrHr!Y`1T|#R3CPUm|7Uh;;|LsO6Of zszto6P7guNaW^>R0H5cI8Hl+1@rL=G2Q%igf+s40wRAH|G@>Ii9?d|{b)o3%(sK1` z>}N!>b$Kz;5EJj+99ppvKC&G!A7Qua)4;4%wa3x&65|UmMc%Li)CBCienN<3zTR<*#)7_=kA~oH_ z$QB4jkb0R6o*vEC^^39f3Gy+=0ebS@eYty>d-4*; zWk*^A68tl@p`lHD=Fd2>LM~BEDn$jxgT>HUohW#+o55xXfHsF>S32a@mVk{haj0-Q zWGj1?*iw}CNANZ@19qg!@*<1BQ`MWBj^pfT)KPY7`hcdoabipvNQa1%9aw@^sDEel zz*V&3{h4mFuOz;6Fwyb=F5Cd)AJPD)&ZyD+qRgLtFf&XkMOmouPcMMgVWJcIQYAxs zxzn^-Yp&*;-qZbffDdZ35n}YjNd!mXMeb5c5Am_Vxu{p;CJU+g_rXRu|F%L@_4GJX z=10WHn2SDelj^5@V={m_aK$8;cH*l3d21P2Ic6aiYqSg$;uO!*iRICJa1R|NCpc$1cN3mf?ha?FC(0^*x-ObGgSuYUf_g_akIK9r zJ)!~ZNbCO|f_$>g; z8pR^tdZYD^MiX3VLcPDhNuYhhSz6;l37pP`PRhGBx8hZ8NUHF6#t;VdpILqd1JpTl zO?{I<4C6h|+_a_a0c8Fp{x5G#HOjE^y7V;@dvmLh-gP{cp)frY+paQZpjfxOu%1}9F zT7;8rgs}-J%}#h;%-`^jPZT+PwHGO%ewZq`fYG9mFTFH#P^ETu;BKJgCWVUqUX-ZUN4!sq-b_seG<-E9Lhfq)!O^cp)N-r2xao3 znM;@{(S=`O^`PW>wQYJ+GSMRcyNAsLP3!Lxk7qgG5>1i=JqvS`Z@{v*lZx6%Vwg1c z#$Fl}c(O(p1HQA9^UygtH+r8|BMDG{T;lfMi7kx`eyP~#yd>Kfr3&DIz>ax+puq+c zzV)k!L11<=USJBv4UTQqm0}_@F|*y5ss?`H*vhjWM2b0foHvR(aW@x*H3gNu1WFRZ z+Qx_6^ajQb@z=SfiP-|=^1EH%E2cQG59UfLB_I^kN~#NhjL+xIc>{%?e5_GYtXo_$ z`K3O`GuMX9IgfYoliB!|}Cxlizxx zR2*Ww3tVpYW)$oY&Cw+xc(a~3XVUy?-NG(9>iu22_1Bn@HN$=&mTJOa?=8VW$;Yo1 zhD3`_zM<8BnDi~T+8`d6hCZHPuL&+E8!C|Ex4|dFw5#>EfCaqy+_zK7;FERX05Otv z^btUr85xB8Vv1*$8d2fX*7AxP|DSK{hk?1F1U#E!;_Zx8zzsl3oxhqX)kZrs<&NmD zr`ZoiDv#E8S05$hA!Sc)@gEN!-i7l;zfwLfHp168i#t?h-$1=6X(Wul{{Layvb4nb zDRna$rtyw-hB-ZVB0oR+lf}tQm)=Y^>3%Z_-VTAGB7?wMOoZl{vJG4xD1j*6;Z!F= z5{%3{otsl4udx$Xn9EoIBbWEGy#plUz_e3TQ%^l7$*tyI9p!z+r?ND&@nXmKPiQIS zVEs_7l5RWikdFijqlClQORn2doT5UyD)}pb%*~~boaocRc_@U4K~8%QuNuj&C*OqU zA-^#zk)XX3=yOd~{hCM4F$~kjLZ^-0y+0{0ahqqCW11zyNb#l(o>`K)4l!<2dPDljNBg1@m!2zQp2hdE+pDAo#5k{q{)kS9oP zWGL=Dp^fzETRn=1HS-CD^X3N6FUvoB)cC{6EWR0 zgs~H>e|#aVX1nM1k|P)mIm8a9}A8|+( z{m2JjzvDP|wKK@Dw4KA-Wzb4gB9m#~sn&vD(00E0JaG!$SBgBAYG$hh{F9APE zXZz!#Ru0WX&Cy>?ZPoZ3KvG!|2@nDGz@Tu#{<@Wab|>QxxB;H}n24TrpnpO|n%34q9Sbvj627F@>Auob&o z)6kHcGhaW!_V>AT6m^E%^w?Ki)i-~WrKz{h<^#Vxy56;s7FtNLQC!KuT<-9z&^3MP zQgImjdc@PCFLiL$4;kxkmlOWr|KZyzui{9~ilURX62m;E((P`8nNt!CXC;AFVJt~d zH2^7dJrJoAw$ovvZu^vEtf?%SQ@pnvQg^(pf`JwB( zQ-U!BQW9tqL%jSZHXy=b$A1Ko%_;=f4v}?DAFn%1{|zthp-RZL9O6&ATxBHaJW6S9 zTq=<9#XuZ!?67}epjJc;y*h{@&mYYZ1pjNE%R`Wyvp^I!&v!)B`syIv$ezJ_k>3W| z!1%xvzAK?qS9~EdB)Yr(VJS}oL&vv0vW_sG&b*@uZLZrhi1|J%0hCLN|AV)846?21 z(zQ#wY}>YN+qTVJwr$(CZSSgGwr$(^>Uq0Q#~X3FBff}p;^eQzoNLXTGcw1O_dP~I zX)2jz{4Il>xKwJ!y72AFSg8T|-K~*P$f|@OV5KxUw$X+6^&P=;EjN5@7 zdT=tDhIp7-;T@P#-IX%3tXu^IO@o8;K4})hCIa#S5J^}P2*jahO#dXF4J>WyBE`#)@|h7&5< z)A)I?qR$LMGu4h}%oJAf*S{nIt+cGj0nlr628X2q;0@@>BUDU(q6x_EM5d}?sy4Sn ziY*Bh(EdZ^Hpe$cc}f64!0(Tb?#q1l_MTB^Pja!>IU zR?Ly5e}^j?FCr3o+Ncp8VPVN~fvA;~T<;w;<+&_WFh4mDhrO9;{Cysrkt}=}qia!U zB@7D?xWH)MbSzV(XwG*FV>1xBIW~rRBLUVjOi?#b5Nnj9QPIHP%;s#owla%d?F4@m zs)nTQ5Fn6@#mq_acxK(4LD8pn=bD+>k=1Zk4-^y^o;HR=Vv>=XOWE3uH$^Ru`T5Bd z&ioe!GSln>wnb_g2n0x;%T1mWV*|+ww>0?pSG3+5O9RO!_$1~=$CAS2ABQA`j~Dr< zQ^scCX*u&^a{Be?MxuKpRED~uAAfQ*U1cbXXRQ&t+^MO+oGI2S_A5Hf2!)^V8+B)3AWNXoS{W4*S+59WclBm>z1|vm-IP+wrX$$< zTKb|cW+?RStZj3EFq93ztSRow6PFc?Ne2_`^a_>fg$b!e z1ej3xJjYq#t!6t*Y6-a&AYHH(Fn?U7ywF&}$${i&UdeOfFcWuBbNJF`Zor`|SiKQZ zf3<*DmhWpGB)nK>N7%G_A5cgP9vXr!%LZ2HP9kBt z6!OJaHO%(tJj@q8hd?5A7N3#?7%e5zgxiXWHKG=RDw*_Yj)59$9L+U8+SHUK9Y zXlo*0-OojCqw40KeClWP3b0~4ihsB9Ya^daa6NW#T<@)43y|C!%y9Ed1${mr)9V6R zywj}W9x|UK?=LX`AgztB;Lsby$(yXjO4zdl884LPu_AcQI`krUgo9OcKlC=pEvra0BbDa9yIG9_t#YdI_;rGT|$ZWexqi2Q5EWLHo%pRfpnt2)TQDn10-GH z%=Shb`6~^|75J%5qpb8^VLe#Q12kK)33kdl@05e!o9E4f`#DQVYu)p7EZ_LE?jjQo zJnv{Xpm+sX5>T&TKmt_kOxGC-@GVf-fN_V32*%J1+2F>`c2bxEF6WtQsd#t-# zbIBThcqzUpW;AhQ?Qb6@Pf8G&z+TSDy+ehJIK)DC76v=y#KDu`>XSSGl$dKr#}Qxs zhA3$SZtnwPC3LyURyMQXu&iu!4R3PRQnUxtynySz#h+}n`m_B`0Q!Wg&()!~U{JEa z@}V)qDF)voE6aX{Ne=IOSN5L2`n_+juN4}PpoQ-N_YxcP$!F1!LRH<+S6Fghe(KKB z3P=k;WI%#By}6@#;GRbG$;tK44P&T^k7RR={Rsi}zkU|+{`^ZF{STe#&)m;Si{MHn zLP%Gwb5z{?>k&c>MwDQuCu(f$;_ti2PR3I&IgmMet|cmT;cZ+t=fEfNt951y{d*p| zE0c~S2E(BZ6(by|7gxS$$F?I38VXMtrY%~E&B%dE3z(~S&3wg(xd-DF)_9wk=>ABF z`?$y==$ESq^-F9t1+ooRm3VU9o=BFIy=d>H#);aHEgJN8S6F2?W`xFwnt#m~a#nce z-9m4r)wFPvgDHZUMu>N<(e6Kl1^f;T>mLfkSc?-Jll649`QC65_YbL}ihG|#6Uv{W zASB!)N&gq+alwDiqze(uo=4O3y8X|O75*g6rTc+92R`>0W0gNHM zGA{c+vCe4bPYwx3g1xQb?iF7`#AhC@0yUmzLpLLY)~DLOkk8b(i*2mJ^<>Je3Z#E^ zLKoHQb&wRnC&nVQo-;^qGMWuUOaMuU^j`WnI8(oEa2x+(h-*83r_FhQdWzZORVPE0o*|a)yGgt)R8X`+;lA2oMTT9r?p6RAujnbT1v+jI7?D6dkP~ zS+U=6%@cU*Ix_=twE14S6zJIckyVwM3?>uiP16+qe^hV`=m5kc`^r#x>W6CZdcLbM zs-~v;%?dRON*DE3R!TvTV=n2BW9zZy;u0!^2Z=N<`*G~(kJ%Orv5Zsek4`AhzkA*|C8MP$fINka%& zP$g$y4vhdbz9qrNIX2>mqC13)3aX$v3FK z6XU$R+4(O?R)HbHe5rbECw^ zqT^{}D0xM?zn!QdVl~YA_Xuv_lDrK4KJ_n1bPI`KW0|b^kfOJtABBCw zAqis;=@<9kxzFm4KCS7yLRhBH_+I0?iD%~%Rd;4r8|-n!j*P6Ivz*IvH}}Bl1o@ie zdCfOGo&}7pF!!TxCPUGPC~(U#9$`A;fLPG%oS7U;IMW{1%KZs^$>NPUW4&+%@4Y13 zID`4YB|BVohtcDc5K|hftNFnx*HR!279|0lnyrT1U5OJAky>GkYDMuuc(NtIX8vl5 z00LE?P3@5EBbo9FOr_gtEN&E8S2WlcMOMd10>-kF-7fuhu~$-rPinHiFU-ySl?+s7 zZ##O(KnUd`YrM3?>(uB{qGsXy)}f(D;wSIn!0(0`tyWT(ff+>UR~UmKE*Yh_5@iZh zcK1!)YJeh`2vC+%k7s_pOUcgx1LDpXKxk~TelObR7rldN zrH_kUN9Q${%K5=vHvd2`b%zZF)M_YdA|fT^*yqf>M~kX*2BSR!CFbS%X>KgSgJJ8lLP$K^d}#6^#Y4L^jr&>MgVIw4C=lc%d21 zIoBFmjgKFZUCC)`MKSZtpmp$7RENpZo0E_U(CQeKv=!c_Sl0SMq73A`FPt3V5tf++u z>eS~4(-V&gq_(K&(JR8fA%d7z8|SgomMVMyq6EL9HFnO^%Axfolvy+fcLgke+cNtH z;}cFAFu=USvn?cu85{fYdHZCRj{Y$FM!09w^IkXK&E|$x8M7g_=E`AatJG%XEwEa_ zLF@hO^z)bn`+hOc3YJx{-^eF}`>Tua$14#hgw<^ADSp+o%($T@>p~4H`m^># z?#c^@m~>;eg-ehEgm0d|2pN0f_^|>L7H9A4sFx%U1;4O`-o*q{6o$2Io3~Sm4*xhR z9^|eyI5&&4MH0whEQLr6Z&7ZMH;0GA87fGGZf~$psx8YER_&?CZz9oTtA_DyQ6#R0 zx+qwc=NEKv00!U$UWEz$6Qo{|5lR3-o5k)3a{}&*NLO3@5`fpoc>=D9@Jm7tE+giE zk{3O47SMUO62{()f67~2V#?GtN)GLB7SXn^8RWh!@6)Mz z?_`74<4FeU*vUG0@`>@?1N7VdD+oLoR5Ka?K8{%FSd~B9|Mi{6#w2Rn8K7srbSE_U z+`(zz3HsyS^+L}bND3<)9)h1o7Y zU5)C^q~V7o^feEX=lq3&;~(RPT7y8TpUoXbx4A(hDVLLwjD0wx=XB>6n^T&Lu+%)t zO;j98v_TdKcNQ`MkC+YTXjLQ1Y;#Wa8S^{@0P%#eTd9Jv(+aQI-)>?_(|l>l$%0|- z0@=V;9wQ2M2M4tep&Bb`ssujO0**B`1`v=ILiIELp?hyGHSd zFXY2XJ|>$&1P;)>c}5rZ4rAPNI;{h&S>___PEVh*kHGiInQE3j<($ zvV*6WGIh3>dM`Dq)o6EuUr|#M0R*Gq{DDQERtUIMK$Ln(R7QZ!YAni(?+{vyPljU& z;!PMAkqiHToV6yoL&+HUsAq&sB)=LY)tzn!wp+({{D*Di!07W>{8E`;K_F#yyH2co zOx!cOiT-8g)d0{EVG+N&x+CaSc0=Oy*~p*8_Rr2ZR+xB0V@YNk0@)*;N`&z-_ruGP$BUdaM7kl1u9Mf`_28zJQ$2< zRaW7ZXCPOSv;B~N;wSqDE_-Fby+HGg@u#5HL#XzD7OcF;xQhziL4Z^)S7Mf_rgb3r zy4%s43LeTBSjt_n+WCF}z|f!o@a79d_`eKkhTjPwj)p4O)x;!W(6MexW~(UM(PWf36|je z5d9eLxCuq5f|8sl-8!NHkbl9+=VY2mh5?Y5(VXs`3Q8qY-1b#$QR}iM7-Pak*NA)f z1c7%8;^*Z_Ws-DJWCXmMFne&ifz=7d@nt^(5tM28d{AJL-%Xv>JWTM^=G7M@HsVY! z0y$sP8Vk;r(=h91PnW>9yI9(sob3u)Mc;@E2HoCavo=g`isctZHzL_QP*Pra*c*{b zV7k3ULbgNTX?8qmj~*QGtxQS?fJ15~ft!4a!eE>g7Xpy*gkHF(F~=h?M3E$y9DoR@ zCC=BNGD(w@*qeBIQUX9yN_Duk&rkK>s1+fnz#II<&1?`iW%BZ)LV-75v;l4uH@!X) zL;I=a=6XCrouG*53JaB|3IOPD2?fTvuBBKg-#G3XPc2##{xe7j+!!ltcW1FM=7luF zPfA}qH8xNjRT(tRUxOI&l8RlECOACiNG}Mk#R(G}{lWX+qr+G(^oG~8wz_XLohglZ zYo&axB*!j(a4x^aEnqG!y@vE!FZVjG*BgqKVq}i=e2FOtoNdrFtt@YRhfe}?#KYqD z+Dj_2RGyk&SI>mFonKsiRR6|wH00jOhm>ohsY&k~1pC^*=YU^r+(7Z!sbqNA;PY|B z-2$CW4mA#Efv{*xxeLT+1z|J61-ZP!X8K@3>yZGY6}~NV)p={${e<{1`+?GBn*lRt zjVddRRb1RCe&%%or@yD-_GGmY;$PNuk2S zG32?x3Op#Jj8TcG3#b?2dare2rq@CWg4|gV4Ur7$>_dIcftg_$+`Wpg`K#7@A&oed z17ov+ES{xJ%HcJEI;4T*^9RC$(;j_flA09yH?|;5muMJlCZA7q2f@~Yl)}Kb10Bay zZy+jy>-3L9?GWEGgc!;yP=P>Jm~s4UNg_461AsZFG{foEK>Ag3gM(I(%{Qp=NqX}h zDL}A-WYz<5nBMXYIMhyH#>AzO^sliXecuTU-G8FZ>2Sni(5k)=i2SgOS)A*ZFOYTW z?a>68M1|3TCNPV)P=K^200y&VeZy^5bMz%-9Z}km%ZOY^$>&cBfVLj zUJN45-bma=+k!JTthxu_TrVO_1U5Py(UK{!;>}#5YJPj+b$2cL+Cao}(BOQ_Pk|NV zS3p(U48JoEfIt0#oyYa3onPkbWq@0kte};N75n!v>o=2JZ@wC4uM;aRHdt6}wjX~+ z$ebj8yX57nDSaT@MdM-_)wjLQn@WA>5=Ou$OYU58K2)qBNEvm}6Mt}_CD0U}=w?1?NQ?sjOS0obbVPFs6ziFFaxrA46%!qhpV{qb5ck%o{QEC`3c-J` zIZ0+k4ud2pd?GYmnEx927)knHT7Vkiot54=rYDoNe*_};rbPcQ1Ss#?VQsDsbWgcq zL*COVCo0q3=v(ST)t^*#2Hzya@6RJAF2%b26-ajQUVfEe&wDGuSanrw@H8K~hBn=K zR!pzltlvF2zB(*Ig}1;8gI#&uIi)l|hfhxHhtIg{Zv8%f2#NdR;^boQBP-+a z>AEo|%XBfM8YonFgU z4Pz?*)vb*1Oe9(u(F`ezXFbYiHrD_6SU6kIMMeFS7UHHd67C3Fs6Sxkl%1m)Fe}NU zNjK-s-fATIF>U4%C#iugaA^afc>6Sj!c*_!Z`VbZE|hSXVZYr4s}0%iaxL#b>AcU# zpNjAD9Zv%I0`l&HW+)TmEoEY1nv)Pzqu4&989MdvbvAIRw_lrDF~eUE0WQ7%!J8So ziGd{ahL3>Qua1xdh(`rR_x0>)Tm}TXpMcq#UcrtiyrUgLqxDukXynT<{P(dfd;LbG zQhW>$)m(IgI-iAzei-kgc^b#LadXzBF4(J)#ev!NW<#AP^(RM0QzT&VP@IK}4}awk zpOK}9S|D=uRYYl`vU)+!QDa0Wo1)P-eaL-$;~Tk#5wVzccc!mH8HSdlTTFjY1gaB- zQf_xI-0@_V7g)r9M@vk3xe=~|O->V4Zo+4UZr1v~oB0&TU5>qN7L%H+?p&S$tWpW@WLvGuB{$>^N`*sp=tr*iX`gJ2Yn`}k#N$Af? zMwXLn4l0^kxS+V73$dEt({9vyg-KVo`gVGBEjmtNe9Q~8_1CN!xZ%>aKHL5o5qftM zgP)0QC5=2R`H3&jLWc_BWAD5sbz0uCJS|z-G)~sd7Xm#ECQQD|x$V_-*($o&nDM^n zj!Rm!(bY^Xy ztS|IQ(;wdHXFH3WDU$vg@0eKr`b5ar*Y_>eW{@NGo`Vcr+1%#jqt50;!};zPW_m4j zK!7F{7^nKV`K%!WQgOkh%G%SrT3}!f%He}(=~6NMBE-Vo&5k; z9SxTi+AXx}-#&#OQ>TEbtxHNuuB>*vUdL$eCX&vU)SQkswr2R7I-^%xTNiJh(8E>E z7k6!r9w6Zky)Ajg8ah)Elvn(Fr;jP4OtR;zdGihCLqo&+A6+T-sL)}$tblx9 z({5{MkoRydHb2=;=AmhRYFwk&8mzZS!Nr#RL|9z_Z#(}H3D)BI$9+A+Q6d5^R#F% zl6E!-@$ks*?WpXb8WIaOQk}sPP*#^6NRjlAUW6cF*u{l4ykE^$s``Tkg!MIQlqC*W zq1`;aG^1Bg^1Rl7C;$M00w3JSaciehf9s@J?)|sj_IO6z3G1Q(=vc) zH2VueK$^mle0WAINy^zKO=RyraJ|o5i52NYCy>C5_63y=zv}J9%vf>h-y*& zuU(Ktp+DT=L}t+VOb+JC948=vkfpNfC}ewD9CYfSsws?u>LB6 z1{45upTfive!CY4S>j~+U18-c)%f5*Rf9f#DhP221@J2N#1F6VRq&eK)XD%wo(^ql zd97-KrAlanw0-41R0~ZCeQt+h%ulp|qQIp_%a^5HW!w%nM1*j7A_X92RIUQG@KWFb zD3s3^LHH>6B-n$qR=+lG_x_n%(4s|*q&2@#D0R@I<;8FoFrnO*-+?+4LD)Ok(>c$x zgoQ;$1obX?oNCX3it_}|(wo(>abbu6Mr!-ZYH14}lj`MOi3K6H1$uW%uPCd`&w-_y zF*!%|owS?~60)JP7WmI{AYQD^P^4cu#tb8#J0aD~B(0Lb_Cc*s3e=6+Z61D;#z{_?OIsU5o6Vyb7G#AOMU7X8L# zZELazAq^(u;chVwwJqcLqIb#F2;ENE8i;X&sF<^v&Hv4yofxO)h*)$i?mBDEQZg4_ z68I|`6cf;oI32r}I3ufO9E~Q_RrvJ-7(j(0C=G4#6cSD*DS}zvPqoq?z%L=F^uYZf zr^>uDi8iqQs}9UmEED> zHyjPW;v3Hefbq`Mkf1b7rEH%}CUiUAmcep~sg*pbCW$2yBYKur$5 zUU;q`zuNiTcTg1(iIc85K(oCk@$s>46(D9jJ^90-%anmY#Kof`Mu1gAo&Q~AG&vjw zEH-L%Cv*%#&drstcRyWk_9!8Zg%j?i5DlrbEmXV*ovFN3*DMbZe%ofx3#%vt2QCUM zycmh$SUw(-7ogf#a{*GM&N3WYxb0sE36Cedui8S^MH9*HKY-Y>Bnkd!B-#-zZh*0_ z5D>KDd$FvJ-G$+h7T2w5=tNI*e_rBzmaU1$ryIln5h46!yYLTHU#}+l1u|Nky&;JY z&nnbD6#^M*?T7_Yo_$T!jH2XD!}vpfvm4g39Xdd#;Xk)Bp}d>2*dron)k-^FxlTqu zqMw^Ln~T_~f{FFad@havlolMG`Zm<4m|l&L%Ou4KP8CpG^q=|T`2AU+9`{1Ren7i? z)jBlhGNw~<+8D%d_w>K3;qi-Ngy)r?Y0KTv%m8U!NJQ%~L&G6b2)B6bqyWPcOP=U% zpud_)bhvfQ%Nc(~R-aAonGi{o+|HatEF4RP%}{{=L3T803}hBi+-QpymIk^4P=UP8 zYtX?5WE_s>7WflhS6xX?Z`?y-QgL#u&uY`iKVJ!wa_6im0*qANuPzWQ%T~ka@MxP? zBj7?9V$n?-5WR1BBXh2?bIpp@77WyYv6&_m`Iq*V#bK#0&ov)&l9GM*+sZ-;%Fh%C zlb^F${*kA{`9*%pVLUxp6do03KwZvELI(b`mFd!))-OQePqn^&40`#4D-UJ4-d(ig z71afUXgsaQ3DYNU0Be-_^Hf@4CJ58J3-NT2^Yk~fu^PoCB{n#y{WJkVKq_dkPps`E zq6H@@w?!IGw2|VMO&2F+TxMrRCL?QDbuW5}IsrWYfc@1}0Fys`hk!#%a(JoR5LqQ* zwDh2{Y-)LUpqkEWxN4FI51LB^b~QykUz=#}vv97$BNbj*O+O|TR&ShYf|?+p*!+&h zv5~oywa|%9l*NizOffdTyEoR~{!_`vnit)Iye{_|*x5pJP-oe)@CIilOet~8n(F+J zC$l5Akt~^jh_}K0w-6z-{ovidN>5O((eL?HYhiFfWrWsB3alAy`_qF4E{9_jY5lm~ zE{>%6aP(GKApG~-J_G`B%W`*}^Kz ze$!{E+nMs#HS_fLPmvhZy~gFtaI;7b2+OvVy8uEC2o6(R@XI?K)=yUSlD0OE3g2$v z_0mVt?71a+-akQWG~m^|TERSjI|h{i!$7f;Xpf;Ot|+H+BSl+;M3}L9z^V9P0w5vV zT-1ZSvB(NMZS>eJ8{*Q^0Gl20D^eSTO&LrLlZxMVpS%CwYOKpA(NBvCS`$K}Ufsai z_>B;haA^JC;fPB9|Gzn++}i)h5%;_lEBVL1diBRfqZLLJf*;!0jn&x&?8Luaw|P-4 z(BciQg5l**uEccuR;l3hHAta9ebpoF&dwvac>wrv!gjG-jQZn7z@6aZQb_wK>KH<( zSE6ACf%8G}S6YUXND61Wx}Y8kaI!X%hApKS8G{Rj%K?ATlOTD4sncj@HD5pL-{*Sc zdEkh+sZ0e;#V1J@Ji1JQn<-s~zV?waWdaaoJm)

al zc{iPD7#_IEDsurMEI9#Ya_GJHLdgml#J*e(k(KF z3!>HcI=aE71^lE3h)wsl0;(2dP(a#p9`mQFmA|uEWMkgBufZUK*LDxf78X0M6)jq^ z?gw&IX}$i5zSq+v_eIBR!{2!SOQXzund$r&+x5$UVKR5T-Q1vou@S6EU-s_-)Q%5V z^mtTH3^KZr9@3{je?GXj2#<}2UN18pC@3&e+3W@}ULlfSYvF4Ro{#3m#oNPmd~t>D zsPs0+!kF|_1F}2$ETc9elGJQtp^MNbAm#<(g93ad;h?vOpfpfNw3w?)1y0)01$6za-eso zb{KtxIFuEG;+ya4D(FhL(?_uVMx@(I)rFBCo5pcD!Ype}v4?>c%BA3Pp!xOS5pQb9xYL2i4ox zSB)nNbaI3VVXKZ{%z>?&smNz=4YHf8dHMZ`VtjOI%pJ>tLz$n{h|**UGjZW9-~8!j z-4m}y5uD0)yoQIq$Ow+R#A!Zhy?dth%R5DkPq}`gs{wX@O(i6?8J1IuEy;=#x;Mu? zPR^DaV)W=iBzx{@#%f{*@qxjm2(>pf!M0{N?1)9aMpzdkTfSr#-iz&!s0sYZFIg4L zYFg5q%>eA`VA2jnc|e~^E^ojD)J;*a<$GH3$5(4o5j!I)$2u}Uv!>n z7R>aHmtp(f4rnF1eTfRN4^|6N&&$}Po$7we!CnDC!62J!}TxF{va!3RXN6bq_1D+1_TVmfT$gcl7BRRf<^B7h6XmCgY^G^i^0@mF>*xs{8ztHvuZ zStpXIaVvaprexB3-{0b)N4ya&k$|AcI>OE>NDAxN{utQmBa&jt+j1B?fC(l|n zG(Z+fK(2MLq_J9Sf1jD&&V1rb%&2hR)aX)Gb|1FN(pZlI0D$t3grd*^ zG;#V}+?_t)#LX{YBO>O>L84c(=qk@9fO#Y(l#h0*2qNH-Sb%I1 z5kMG02u`^}76EA}P={X@MlPx1aV_F;%@fTph=16<`TiLX04NA)xV_E60rqscmEP`Y zc6Bwe*`9D~We4#D9ygnqiAevOQ9rj~-U`z8X0cgau=VCcs5aN_EV*U)5DBO|nNZ zR&*rQymKCF92CZ1m%T8q^I8bm^N;<6Ibtc0`V@b`b1EeC+5xy`p6))8f%u_FVdLOmrAnUB!HnK`@cw41s zCRCO#TZ z^e)uP82?R z@6oebC#hDeGfb(GjyS)}eeT=vN-pCGYlVDsA4BsM1AdgZ{Ii|Ia8lHmXqK*S>~Nts};fVH4&|Iyq9=aZCMW zRfzW1&`=G^t?NB8J#*IFBx&6=U_r6NVTS%hZFh;}Pq~$l^h#=X(u_JVPKJ;5%_4F? z7yoK4+5Cd|t~@xB3CIYJD6^Def`Z>?D-;V4-WQw&0R(=boUB930P0ohFwZ>zhLpP5 zmcvo_r$r^k1SN*bm6!p5yKBRd+9;6L_t~xe*_JGS4S|}9+!C5R;{fx9>#d!v3c7S| zA!r5MR%@jB3IntcC384>pkH|81I3BLh~i%&F^lZX_G_qzcElQUY$!}Y9nHZy*lvgL z(=S$Du+rNf4H^wWq=z-<^JADI*xB2T^>HPLd_$`(j#umVxhX{@TTi8(Tf1Z%9QTgV zERc`(#rPUKVcyaK0IK*b1#*+Ki^U=R_U4Q8akD&D1lMa@ho+Bh&erAXW{yi)5tviDL z3hl^B&+eT3zqA0T`-2X6H>*`F)bCF8e|^;_Ml(_G7I?gXi_~q@kpW<2u&r^{6H`rb zM$|iajC3u<#wLopUN@AV1fCAmeJ(HI`S;MQmr>CEIvne#1G`_ElVV%1IMA-N zXk`^sY8>2^iEqMuAV2&K2ANJ+c5%lRb~G~<9MEsh-yic?BUKj=7QeSy+NductOe>Y zd?fawcjLZ~CxEb>#eAXhqjYU@VU?mQ(}yHt9YJw?#Z-1sjf+~8&h-g{W=2evGuo0s zk=|9V-V5CmC#9vzs3|n9yL}NTX?hTJ$THf>k}+;iD*fFAL7W(5HGINujBGR#WiVky zH`95|k4vNE#79ZtP=snVOr=+K@TY$D0i2~kM|(I+1uZg;Wm#4gwNJ%mQnyb4483by zo^_}{oTyZu0e-;=U&zBX-!W7Z`L7N7YjY|zBGQENC09)D&!)?Yf}l22Hz0b*URq~{xI+O^XG;ppI?{q|3ApGtwtRxUA18j>mORQRQchr2PR zyLm$xj7Dxoq`Xh=rCloyUrS2v)%FhX{6al~KEu4w?Q7qeyD2l}fpAS;`tH~z7$ZSb2Dg_(5huwU zR}Ap$4(W4h3fBw?=LR>6YZ8EyQjD@%cK<&$c*!2L&igsB*a99KIk z!_Jxj80+wzM+7)Cd8MLW9XVMk*CkJJla%?KImf2`f@1s0w8p?K8Q%+th+L~4m1fk2 zKnl6a00Egvv+Vss9`lu)Jw|rF-W;zB82IM53rX3Ty{ZY&2;&3BR{9<(fzMWA5)3#o z^M^x^)E7~y1*Gl0cf3IaG3#0cVR{v=cDibAc7Lm)Nd!iJ&%Q5nxneG9?1RBsJfZ9( zCei7qCEUOyrdiLqAO)YE7g6W^pnE^|R<^;W)X^C{CK<1VgqwpeP-V|R1#(^T>2%!` zfxA=2&s3?3i%p-lcBQ}lj1Y_L+D}r1qvYuAWlgD(9QX*an?LH&@iQf!H2=x;2)o@-XOuUC;iK^T4LtGT&LQ^x9}m_eRc;!@*F?yr_4b;VG5@GDnLs*@NZ%WK6q%84Ag zasDj$0g{iIN`)(Ux8BbEHa&EV(D@H(znaRL3k(9!QQ`eS#k?lU}e?Y+}W6DD9xzLU#QC$kBl&gb)0N?f=%EPe(0Eolne znzvuE*E7|s>`Oy9Rhk_MJgJ)ywd$#&$w2e~C6aVZMv{EE)Q2$TXyOxh1Djbf6>f7U zpUKqeGqEG}0_>}FjtnyWWDS@Kyd73=T68@g{t|(#4MR=o_~NxE==%x?f%#qhD`y1J`)ZsMXKc$1&zSIN5v=Rc;(Uj3PB+ z*?Q~mF$<0gAVx*dt7(s^sxqZF8$6nFWPDd8t-e#R2pWIg>W}zQwI|QV`h<? zAJ?Y%Nx26>eb2wpHQnOz6I`(V-IZ}l2A_+`I^56zlLf42w%rPiFU-xq;42M1JK0c! zG!##Z`8L;B|vsq=@PTbsRx`p6t0M#cWQvM z`fK#Jhsdtmejr%0uQ4p9HMU#XP*(0GUt&Jop1(Qpr&?~60^qm;kLb<2XL$0Vk6nu` zFtL1}n0z=Y0tp(NMVMbX22C&KbieE@X^o4sOunB6i9aKsnipng+qpU%=L90PrUzh- zC8F1d2Aa*jUzj1!7*wfOBn-zbQob9416UQOG#UI7c=Lx?F$YJwS|Jx^Up)yWs(h~* zDndK<5BB7w=h0qo&9}572+&p^P8*vl5JOTEQOvd#7fxAYR3KVQ`v0(h@C0TG?;r04 zd7c0}VfF4VB?&IW-hD<+QlK3~EFMXiH;Z9(ziec$C!1s&ZJN=|HygMMdUuz7J_#uE z*Iewbr*bJaCecl)l66Tc7k-AlW_8>mmpj*ety=k7|0t#isL{lUwBvefRwB$=9CxlHJN)Y&37;y z-*3Y933{vb?LS%hJAwC!xTz`7x8z#x`To#jIP9ZRsD16gjdUq8GW#nE>j@I4LDJs* zsPKiHe~mJv<)Yu|+NbN@+i}Cyci^v(*6g4+*S$w-Umr?J&rH-6)cD=o!67r9OY$Th zoT2=?n8Q|22wFjdXDe~FzVQ0!%iLC4k6cvI@rk`4k)-jncV%3PMu2Q#>3vDBy~|=K zG}d~suD_?on>0EoGBw7j`Z@Qgn2_XNLPu(PVJK5wVE7yuZ^1ZB3<&qx5EeTk4mk;(5 zmWyRJ?+~x9NmUaithW#pufrMcuBdQ|Js|!+-tICuj^;rVyd{glVzQW-nVFdxEoQKo znVHosW@ctt%*@QpvRGQZb2A$gF*|Yp-HY4&R@E8VU6GZY-M@G$6QNd1hqT6;+5KiH zKf%3ol)O8ckCljYdV`ECQI;oCHSTcAm-@BZ^wWpfIiy$B>wTl&(jnfDN!ED#o;N{F z1*>(=ZuxO|2$xD)(wDt9ZG3khmuz7zHtw0c-uC!yY*V(tLk1f9-UTgkgbHtw-50~xMsjFBhvm2GPYQS(#-=KO|hg&`s$ZWc3+Omlz2R-1ACPt4*<&=w#v)NHGpw7SBhs`oQD{*ejb0~Q5?5_yk=0tYp0#{AIR7j^+6TDwBClA*0A1JhO!1ao zN39NMch_E*u=frx3MNh|=u%d1KWg(HBijqdDUA5YMJw?9kirJqooNcB*V zTB!hHD(I=eJzKskBW8*KqolY=<^v#A9;pxYMF*-NW4OAAiF=Mz1DHE|y!@2a7cC9w zze*Q~BB7Gs(4*1j;*lvT^ml%Macblst8y~tR%r;JCe}ZJJ%^V|wlbz+>3R<=Es(&3 z?dphc@&kz(+}j%d-Y4bMNQ6f~A({meQJ{z8*6N-ojMY*^Xso}J_RM9_{!!7=)5un2 z!D`J+^pM#sKO@{E+)?79lJT;Z`kSNen}?%px6nGkOOQI5S-98^>E!r{T5*T%San7^ zP2TQ9vHN!$jB`JZ2biea1W!+G3a$tYByI9=ro_LbF3i#;1vkS`I_OU_-L(-=fZIJs zq2QZb@#t=M8tv{lo0Md1la@H42#<(Ji3m)UC<-%vTh$KW1tn=vNfLo*50?~V2pLtD zp1Z9HC%9?C5&>aEKcj_J77<}-?YIRGcOG2~?>!_1@wwbZ{Qy>symYda-s$iDbx`L0 z=$fzPkY2;0WS6hAi_e`gh4!#K(S`ItLYk=>X`N6mkPZSQ;kVHp334c-KiptArSH^j z_}tA)>o+5lOM5?GH&EqeCB^>aij6ulj;VMl;R=pMSy+lc0136V)8itp|sgqaG#!<7AovKoSaB~kSQL}Uw+wMH{>$} z4epo?{PIBR|6>od0PuR|IO8O^T55R zZ8-O*Nqod0Zx{(88pCC}a>CI+D;pjep(0c>yd89BemHQjjI<>Xn91QJM(10Fs|wOM z4~{uJs14nkB#=4U)ORTT!+XcM+KyRrF!aQ(yF#JDXspDdyK>oaf4YAp)RBpNarc(a zr0~%yspc}bN)$m#8_IqVRq*VyEGORVNhmc@p49AavD0LDO0M>~`{ve4rjAH20f?@t zWI#rxetRdPyy#zCCMI$Ic=>m#@Uk$SXUR5?gU7=pUw3w3qBwiHt7nz;e*<){xNL+8 z&QH9hR?1+)e)qaS$~?Tp8)ut51H{im{Lw7xjIa_%R#sV@?lm&!59xJ2TT>exqWD8Z zcMo?u_JXV=gj3D_#TRVJ&Pd?6zDH!YBocVw6PufMlitX)pma*Lmd3#B_AgFAIkoB6 zw;0GFrn`w%6K~Pgrx2GoSEP37;H}7Kj8ZSXQkxuq(jJ^D=bfT=Eth(VPvX(o>IJvS zS@mAwuOQWq47n5(_Wf0pxoOPWpQwTkiB!TVTZ~qDr1GhYRbq;$qA~+(qh^qov`7~l zz&TzBk^hfIX#VZLBSLvJ=XAZ^W)FBNfItJPQk6Lc-$E&p1=vi>?;GTw_BK~6FAFe( z6C2sPJ4o|5wf-_7lI?vL&pW8#_GJPS5p)#hB%~y0nuE^!w`624m0{fga{t_&BwIFr z?;nV?(bP+~EfP059+h=z-)ISAM52XG-ik=Rs{ybWw&!w)WdowNIYu_WTe#0pN=>j$ zK(oNyFmo$2<>B#e0sq9`2cU=g{Z~wU*7dADSmC_6i)4Yh>zSYrs!2%3thq-1DYTm0 zz=L5nVaQs7ZvBlm_$lajOH**;8WU0=JuT!t>7ptK97lVrLB|zaX}&VUp+~-EqdRT4 z*E`n(eN>*qT=R-MZ-|f|PuswRu+VDjA7BT%*d6&Q@)-=oWR2r$thF?fz+T@92TLLpJss}@5*gs)G z4d9ahl`l^_82y=Kms;^_wfroN*A=yVsZ+{C8srwFCNWucnIPnCEK)V;+$BQnvV~Og zDMql9&%ikY!5JdzU_>hm=gpD9jh}hE3Pp*Y9iYHe-MQ z+o{&s74m|Jo3m&T*+mAcPV)oSp2(r55*SfIA}P1B!4oTmd)tF1F7iz657L2wwGjg* zQK|(cCd)-Vym%-h_ys{ATH1mP%S0J|(DL#Fc8rzEfGehU2ov2Uqh+V8j9=e)2#?S4x>% ztDY_(Yzv8a4$5zc#?B;PNf+q;Utz^NNHFxWVh%?lSVf)EwMVq9^VM{^2Wh2({^j6! zLnagP-V!rTpV1%24!gsYDsW(|xWjRqy2tCe=mQZPi)d0ut~$xRGj?Q*{EEVi$+=pj zOFSg77B?eqY;G~JJ}Xlrs`Q9f$g?h!CO5U9Sy4c0rN$^|QE8*zf)kt(3K{8J`l;=m z;7|~fcY^j<z(uM~so7%JR zzSa=42A5Cz`hRt<6$5IyrQS8Z7%HCLG0b?Hb6qN|iG1 z+054q4xRW#buQ27^9hY5SjGap-2mS3$)pG>7vj@Jl?pP}Kf%gF@_RiZZmmo$#vg-& zTUFgL7X4TJJ)%ySg%L-rP?QuAAS&E04|E8?hyX#Pb4_sHMh%UK#deb5^vpmFtmD1~ z*c%-OiWYXa483Goia~KKzU%L5Q5yq!{s-5-6W~LIsI#o!{cS>Texsqp;9lJ*RNjz4 zq!KJEhw=Z$l4V3fR|L(krST4R-oLBbaxmQ#pn695Gd0$~urFN_-Mu{?Kz>a-TcVt{ zD6U)>06QA2;&ZHwApUVRxsh4HZ8l4~}er zFAd+5eaa;q<~W;Haw)PDp(!+u*!gW+#w#vKH7#U`>G;sN$)tYc3T`opUB1Dv1-r|O z-t@z`3JJeEv^fTRr^4*$qr_71GmSDiX6|6lc76$&yUQxD?tuc;t(TBV>CnG}QOqh8 zA^r&#ySII^<@WB6$oGh@L@Zuo2P9(x-=WzR@iC2bgaiv z#!l_7$99J4abW7ROLQXtIp!_@^a9L3KN?gGDhJ~2-}O$-P!Gy|I;N(*zG%3FJQz7( z>!V)aY`K*7`gP)_cRqXn)M{pS9;b>@9aZj|lUO~O78XbVK38<_Lqqzt0~}9_aX3^mya`6tU6=f0%xZdgqKzlR|pZVs5FqGu!mFqRxu- z68$9)E@AR8bX7an{?jCIW9L>;&G(s%S;@$))_dUf(OYQ-lxQ3oM^`Oiqr%9Uo^GM7 zgSZ^Uv2vVq*JR>J@7AE8nj^h(SFa+W!N)xXXBfJ4a``Eyp#q0&Q;v)gvvOCnBag-R z1A7JFl25>SGX|Kg)U&v8Z48?+Mwv1)DeUdR-~a7zb>m9sQ(nxU^pbM^EvoqkhseD0 ztsrltpJrL?M2uT~wjM=wV>K0NhqaLkoRQ;gQqHVu;YbetflO8}TzXweY&NDsfBO;W z2t^PaQadn%Uq&kl7Fm5sd_NLQmgGHW+$*;=3Bds)QV=O{MYAJvKqC5(*yMzicIx_a z-cBnLz8{lQgiQ|tKrL{zyG)(^U`~-N1s-0UC>dpnaYL=E-q!hC4(#ZX^Yqj_rA+@? ziW(VZpA0=vaGcIZsm+$%-E#n(QYXhe1V^7it8Fm`l^>%XQ&w&lNkfW?^_)=oYq^ma zzcA3TITH6hF!wiqg`1j;{{3@uMOywO-XE&SiyhP(mf~Tt^^$NAlGc|WN_9rwC(i>u z4*P~EMq5ZpmdZeg%Cib6zQTEkY1k(&J|N*|Xg z(mn6AagdNC`8kBMtWYU8*~(kc%c*oXb*aN!1JIaarjpgyRO5H?)ELus`{~QN?w{Qf z_ag=p?*G*94~UOS`9+FaZx$Bx&dnLiPjUcjZSsCsVH5k?9-iM2?Sfc5-Zs~fKX??} zU_Cz%h!;q;#B!v|7(GCxEQh`fds0u1bOu3f&=Lq~Hkilryj}lk=BVJlO)#d-)i-D6 zlfKS*?SPfBa%f&r!u?0Q$UOLpzw+rGuSYT&X0&?)8Q(SI%$@+@!yol&cRK0(KCXeE z95FVZdpme@oT*E*iM^?E+12~eTB2Hwb2DFj73dq|lt2yS_X-oXaOWmhRvPr)Sdt{S z+t=b5`hiO{MPVrn3FX@;{0!P z=(7KM*x>PS%3qH6Z)^zj?`){mXt|KB0{_o7_(Hz4xcd$-ah?#y|5GPe7=g5#z3S}3 z<(`%Ns{(FjH}03QeZBw8^I9(Vf1LURe(gq1Rh4Oab{c)oWtpRu9iw7T)dtntkqXXF zWBRISmJxL>Pn-IxKhchnH9GT9zEXFEY774|S9h;p6B<%rZ@lrd>6|~%ET7y$PikxE zI}6>V{=lkDs056KV71u5XFvJCt7_9|dLwgM`(J|8e2*Kc*RT#(^Hz_DiE{S3*OK&~ z@=e9SuGe%q!s#s_Opk*B6R);&(N$daa=edSJATPf*RSI_`rAQBSPtMb`Bjd=RzElH z>b1sa{F~cy{3GYT3XfNrfV?gLxMata_cesX``9(VKEc)CKW|N+eKr>zgS$Hu7Hhu< z?C811vH?xkKFOJrtBxK7;&~cn2fExg}*dCvNqd>|4KuCN>&(Jr8s9W?9X8W!XpZ9d{ zWc&dGHOy!MpmM-5w%t+a`^oPYsihvc5LklC}$LPsv@rC`eh>$ie#7CICHIyHb>h-#|1_+&ibL;}ar=LFNe z!2|lHE`8welHL%l>SzK9X(WMJdNX0jG8vT~Agc2wE&aZUV&ZLNh=J>PWDNJ5c#>3>Fxl=0AtSxwJ+}y(P@Y_op(knr zJN^`Sydp7u8ZW|U`mVHxtHe?g`fpYlF=`IvyP;X|JNg4@A|{=Vb|b=*5nTbxJVJ#Y zsm&&;9Ug0(>`vm6t9a>bIkid9_DJ;unjOM<1%J1}6OyRO$2aUm>#RRon&%_0(E%%x zphkJ5{drIj`X>)oMeARC?fKm@rQRnBd0Za}7hHRAh3u`8MLifVqrQ^k56&M^$){?r7O%1i z=6RBD>a_g}TEd&0zu)!rQ(8B^4m>ILQ)t+U%CMD$Ef6aNe35nq0fi~FuTfRM z)z=l8%!Q5aCOEB63@KYfh$=)O&$<@@l|>u+?Futfi73c!B1&iwRKimTuE=gr+-1LD z@X5+Iyn!NMm*k@`k97VI2~0%GQZ@l3uB*x&n=M(2vdrWz%077&*}F)A)j8#WJ|a>oA^{+LM2-fDOTSC4G~G}hg^|N!>Nk0-COBmacSCQn z8RLByxl=~>>Z(w;&>-oP5{8>v%9TLd_i#sVd`|q^1%Rm3uywrV;6H6zs zK;_F@MUxZBP00=&<-&}9BVP&VJ209q_0~UQtVmMXt#gY;$?5GLosqG9k{cU(^gfJs zSEJ>f_bsdGO-f3~Ww)WBrG9C45-;z1@c$=0PCu>{K4CX8{K z(4};N{DaWM>Iq%O&t<`?7188l!5w&DjbP8`hUAnpo+&tdu9{4C+~WUM${qD7OXN*YN`NYQ>H6>F4kMGK%{glqEKt%=(UD0x2n6*PCVVa;W!*EF*E?79dOKc z-Zob5I)anqv4thGw9{pFYcne}+Y+Mr`NcI8?QQNH+4Ndfk21S?2U=U^#!t{96su}& zE{P`1QKgR96?nduy?0BXBe8#3`bc7fR$ufTo!`2hEe}){m)}MF{!vL@W3*@Lf*X6( z~+yVgL86B{byutA5V%Y+S?hP-b&1W7krm|a_TvQ932^t$$K%)2>-Kg( zi@1>Lk)QA?q{X<7g$?f6|4wWDOR3Ra6|}TrQ!CmBB{F38<@23~15X zl^u))@r@!qckndgWAan&>5g1VZZN_y=mS8l*{d0!Q1+|sPlR~V9g|yHP?P)&Kpmq$ zCPDmLq71xNk7%@=9-XB>emcK6M0P#ooTZ#|6RmTUmbH zn7N&vaA7ryZ9~irx6A}F-qz2w@UNHA)}2%oO^_#>yfD}hZ$BS$oLi7^hL$@>k%Wzz zwFbEImz!@r8j)4ZdDBL)QR=BPKCl5UD@c-Nf2K98BffGRD9n4T3i1v-Q(!{Rx!c;E z5QVY?S}>IAEU4M1^ID9;f0h|)^q3hzu>>_ z7^vUA`z%-?T7Wr_B*Xz-A6Zwd(_j?ofBipm>O_<6E=Nt|yJmnURz-%MWA7ez@wp39*J;T5X_sZIVvSGlTB3^6gm{{+G#x?^1? zjLTZd@ncI!oIW`C-khNmcY|}EsrVYN8*E*+d3i879Ll+K^$lUe4<&^f6d2wJv3ls# zE7>aMC6-M%k)j}AvU38weFfy%hNtMo-$Ho^i8sgEoZvzv96&j$8>dZUDN5ExW{s1Y+em7@3e#jS6Exu}i-Efu!MSvcCSA z{gFhv`pToRHoM$7QT?_Z*v9}=s?T$VyF;;p_VbBN5vAbyOJ()F8t zlW{K3G>mxVfzo%4;^NALr4!Htf@T!H8{=p53B z8N{CtGwK5jyk{5oru8LXN=Sz&!WZ!a;@3ZextFMm(_*nVDBbS#^NM+mt&}d6EN&Z zDSu#&B0&bd6fxs4O!1Wi{gbw&m*cwS&!csS07auB@31d|p4Xz(d_~Q(^0jWvX)2A| zX;}%AOwl6>}Zi;+|!iuVl zRi^4VAzT-So*iWtc=U9f`VQ220Iek>v^mm2$EV)5>1`!a9ZOSaAVHVOn2Z~~tmEjfyDe9Q1_McxR3-@cj5w9r~?G)FN1Us`eZ5*fUko4dg` z9P!5*eq4b8Tl(<=u@7gu8olR-Mj=#1jx{C&CQ1r(9Ug}S6%b`>RjOrFl3!@q@-NZU zpDPlg;IU2FbylBxE?4U#lbPHrt-m;lEj6w!U!eg$UKz*OhrfD8*psdixg+&)$E23j zc;;9;paSbbjrn`9A{Givp}7I6l?N}UejZ-n)N&k(opPe%X{9>FT#NbC8V4cm(A{TEH7efD&pr*793i~V`KI0?7%+FfR(ia4-KZgR?!5_*=3g#SID zFNS)JBE?wwb!pLpV zyCN?X`5a*uh_J76J9XRAE%=or(8|kTv7Xf^3tkoJr5J#$NUGqm7CP5HNXAU`Q`PcH zrpYLAylj_4PC!94_CN!E}U)}7Ek+=(zrL?nfVKhaY zCcw+5cdCIn&T71lB)+Mb#Mz{$(0O}h6-?32&_wF8@~riY<5?%A5SzVUmqJ7hIS{5Q zs1UWgt1ctt1sr(7k6CQ@chMFI3M&~IG8q~~yS+QgFEES#R_mq`kAXh46-F|#&LKqQ z4mnDk7!xKe!qn_VN4yE3yGyA`P?a_vE5P_E8MdNo*;!9dE-^|d&hI0^Q;9TdxIR_q zHIsU44NqSy2A`m+?if@=mk3fwv#Wdd6%w6sd_Ku^QJOual_kf=U@gwta{Ynqbp7Xc zb-b|dz8&dSAxBdPFPj#c6K!nyBSgi)CHc0KFCKjCXl#i0*T6twThvOrdwwPO2!({5 zsHcNZ^6K1{kB+dmF5q0#k4hK|V{6mFzcoqm0ThgswtL@Z;klrIbuu*QN0c9-m8 zX{Or#I;{7Q!Unyy%uk8nEPz3xQ|IXPWrs|p5t%<3)aI8f$afW#QiQU8#410G%Covq zaw{7F$`AMaL=-H1nJ87HK}e)7;W-lPcz*Zf`4DhepO*&8t3lbb#+`0PJoCjKXE{@xSo94H;hPO)zFbuqe06{Y>RxDqqkjtv3ZLOY z=~@R~;eBPKEur@f;whW;RL{*=i`Jbg`lZ%TyX>1?)h7X`mB@LJOE%@k; zk+vw*%Qu+&@cgmZ%n}w)@hx$(|m+8NN9I6Logpc7$=H#yklaNKH^N;S29x20&6s(Ec@3HVQi-ybf=lak_J z<;X7d@;^?!eLc_qE}xsw;LB>&j7!w&MxAAjn8z>r)xujb#eG__35k>Am0V2mTbA7> z>*U9DWZkBoh%2DCRLJ;83;)5#TuUFlFkr4~s6I;Du}eXdSa4X1bL`_HRPv>{OsZ3C z_bUy?>3vaM75)|2#h2Lu&Mn{{pPxy)4gB!xsJ<>Oz3T7X9DwC;J7EgNpfOSx92vgU zZvFJBQ-jMXkf*4P9;~TlfcE^bFgJT@sYL)OESouMXYO`VxBCiSLrWUHd&VmA&f}6} z_!?z&4abTz!*v4bifAIRE^`o0`Ain^6RAYwOcJ`i_vpr2v~*be|oT zQwDC9xG?kln)BaBO|^%Co}a1_O-Z?Ud+)30sG_|pyP9hHnxlz?VyJHTkQewTS!z{!QA2lg-OGx&QJMKzyPic5PcCgj&JC?vBC?m6 z!+s4ewZKc38W50&X@7BxvNC1vGD(L!w%15}?NYb<6Hzpf55JDE^cqX^!YqZMX(&+S zx!s_)WgCAZY{sg7ZqO75FlLpBN3cEy{S^c@0q{%|$YMAZ5yO@io8+6TR};k#yxsh6 zGH|#dZ<|q;S;=w)2-nq{n6r?H3f8Kaj_bYSN*&oncvEM0b%SazAv{@{i{shOq#_i- z7rxz0Ve%j4edMTq`%*pgUT&6%k`f9jaj@-kmB6A0UPcLsHTFva*kodCwuBrD5}VB) zLo_B=Ya{U#gKOWgB2`+~u#TpCG%mP|(Y8I!r{qcr8G`;oDz~&d%H|$pRhO8`^r6wM z7<)Rj^>-e|BA~$^cKW%dsU_gjx2oFb2Qr@fagr1~iNS44;=Ow{(`ehNKzXlhr^zc( ziH+_AQS84CF9{Ll-Sl!xa`oPm?e|_aoL85VS-$mGKA~)tA&gFASXCLk_U$f7zrve; zv_ZHg_XmbkYz~A|>?xG;9P_SCG_rb;I-lQ)}k>O zFsQ@j4;|EeZ*V5Ke1>y{qArjhnZ3^%4VLd&aazG_Hzy0S<$8~Q3377>+CHo@dn~HS znXTG0lQ9Yqa>3#AHl|H(p)DYL`P_ar!C~VIg!#c55Kf^Zeu*HryNSRAw})r+bFHqoz#z~;JKq+&KDD_4!EDGDHDbVXmL-PQRy{- zt(RSF*o&TMN;g(uikzMe8(GcYFjovSsQ=~4pJ=mC4n|Tkb zv(FXoyOOd|TEEZ9R^~gNKq8{a1k)nUNFh0nwStu7B2%^wh_Ng+H8pwcd9M@`tqhBo zig_R!B=87Y6qOtT2^AIWlK05dHzzp69!a_OPCK$u<8jmJ@5kSjqqVJ?sTH6MkBH+g z&C83_zV%Z?x=GXA;AD7yTC%lh)eA>`jB!GGN^fwzm0M}bXcPUWpIDfLx>ZZK8ZHj^tdab?FjVufAgWhxIcW7vmbN!w; z`%0~Z!Hw{QEU7m8p{-_(A@bMlD`E#eR$QB-(%)2RximYQU9hw`B-Wv9$iCLN{>HYVpC3qR@i(^Ye#;8D%L&VMtb2CpE@N4=?jNO)3^@s%zo`=5Rt(n4-|ukvQR5 z0p*bX#C~d&LSmfVH7=e=IxOYP&)L~TNQaDO7woo;*3aV*l9#W3kxZcdq~M^XcqMivk>v&wv)4(67!=9L3F zaYTbeyR)v>S2R8#0J~s?GF%3WC+bAI5^S(zq{Qce!R8%EsBbMMsKXhm^80759>AlD ziUKqa$9(z`oGTO!U?CxPxl>xnbzj(P6vAxHOh* z5JIl`@kSaQKsbf0EjZ)6Z=BcthbB`Kfa__Z1+^mjt`MgJ3o3Z zXlqEex@Rw}&Z+&(vj8jl2J(T=tU&t z8r)n+0`WbA#I@D)pPGQ@r!nPlyN;lqD3Ozf^{YL#eDGGMf~fXv^kJLTU1Q6&zp%Jp z(6W47IJ_-W8TsPBhgolj{s<<%E2`TcPyVKrChH>j4S`1i92X`Wwu_ifId}|u9M}j# zjT{3HP!QLb<%aELHzJB5hLGWh|6vow`D_Sg$XFM( zkMU1vuAih{?xGq`+3@ppPWsWrK2*kzYv zyiR2M^=~#P=pbVY03E5Jj9^9GYD@?f7Bjv%joxD*^-WH%3ty#=vT%tIR}uO!sOf z)T^mn0)}_jQ~b;Z@lEr7Vtht4V-NI$(9CVb{#qtEw>yC}Sw_E0zy6{kaTE6gJi+V3 z_DIYnds;BmEsn4`W(E1#6Ubk(!)={%+(7b)qxYsmrUNxN@rlZaIQz7Rs!V*K&mWxn_)31!GEJF1kSpfZs zoh@E$B#KO--Pin(AfL5;f?$14my_RYu8<<23A$D-{6_`!;4ayv)1o_-vYgAG@KffbfVi) zaSf*?4pUvRo!;twexLuPlFrOFKD662Klokj-2NJ$LZ?fWm!Dt#tWYlPSgs)X(ZPyD zUnS1+9QKZ_pC6KAi-`EKf1RPYY564^Dkcr8~CQ+149 zwXZK5T3^zy!oj;Ed{}2MVngL!@m$=XEG(R_q(~WO- z(CT)#qG08)pp5_L_-}4u!WNGDg$wU$R^nGneEi}sL0{U z5cBXA1wva-E?xz*%n$4S9{*=1+QFLF9`uM!_}Gzqi`Kc^xZG@o&ljZpF{{7&vHiYU z|CUxatqstK?dnjF!xqcG7=(=qJw4rGRntDDd{-~A*$w!$g66L8H+tg!y>xF!Zo;RY z({Sij+b(*2bWk*zhPI&LUyC1?9DS{x3o250SdyO0u1wWH$kmo4 zkDyFQ8>owc1pL|hETc7mha+@VSVWXuh!Y8wy z1l2Mt!xL(>Gt(`Z%NRh%qnof)jNJInG_M>Y_S)$8G4y!*4^VM*S`&Hb!pIN2)ft8lxAL7uxk%5(ZX<6l#0Vs2n!)93o#cdDzV>f9*I86nNYZvt$F^X% zgxFhG%WcAwl1VQluuER|{T2aV$YbI~jSDKOXm`r%8d`7}jFUSPnf^S{MSa?+ptyqlj+r9)0w5jBsv%u_zz9F;ApwNtfjSZ`^*T zU1C$>WdxW37JIf90(uR4c{vg$>8B2-ovC$*UOn3~NJ%6IPiDYTzZo3dM>z1@C}85D zqsVn3HMpA+-5i})N1E_90v*06&oym7da@M%>kKk7y2x&smr6qNOL*|I**8k_ z`gP823^~EL=zYGy=xD-lN4b|HeY^!?L%f+C!}0v|Tv_%owfS`>R}r&o(iDyi{o7Je8 zWLZ;C>!2T_l3@(_HLAdEmRC)ku!HHZIqO_mVnr=mEu~A*Il0KY{V}Ii!oazmH!q+Q)W!_561KqDR zk%7z$ku@C1r7@Hj1|?k=SW00vrV{PgupJVFC{b=2%<8V~h-UK^Lo zy_wl5(i?BQGH$eWjQ}f)hgn;8fv!mfQC8f#E24zNgx^Pq(`u-yP>aq@Zp`Fo%+@EZ zxNs$pgbbpIw1XizhzW`u4y>L*4c^OS{_Aytj#n>HhkV#k4<)P zI^l*N^{)K!A0w}yasR0(KUJhZmes-TicJnX&M~Hmqs16(sL0sS{uR=4;i^d(ID3ps z%8J~afHh-HBrY#zYFMLw%Tj)8Mn=C<_}ULRtV9uQipgwi!1{P!{viyCh{zK0T+Mn$ zphTz5d|+f^_#kQHjp>pR@AckbW;WsiDzZCkA|i8WLHmf(!;*0owv%!(c#I!XZEFM zZyk(nG%qsWM~k8OG6hG?mAke2L&xJziR06gg@P{1rjIx2FtB<=MpK>?n36FUzB_kT zz5W{KPaO#N#@1|Q%2K^z84w{9U#!?uxzQVo z!zM?DN5tuX?(I$L$mTUL&a`In>!@FGT&jsk-5aPW&NL!3w%xbF+MyzCvW9V0!yqcE zf99T&Il8a&VoN5{N_cR&UGMNoTo3%C@0~x>pDjgbb4AX=E=@rhsnOETGuwNvsJ5y{Zi`Oz@5IY-@Bqs5 zUwguqhF^VIzh~zrYwtG1mfIx?%8_J$x8PNHhHYhu_x@e59!4UdtF_qpdhFAd*ryup z{=tod3k-|*F?Qq6bAw1zrEc$6Y?b# zC$>xCYB~=oWO4*6Qo?X7#n}3|Wads%%X8GAbij{17m%(4AAbt(U&U zgC#fTd@dQ|(ksAKmgLLDRyYqg3fStX)rh}B)!j^5S^UcYL_%HGln9Rvcg3`H3vP+!DsCLiLk~ zI=+~&ICQ2yWf!hd_Br5YhTnFn57gH7(lEzxNiN9i1*>>z+@S0PN~tr|{p{n){UOvu z=7TZux2k;fq;5%m=sKz0%xC970r%!m;wP3MWH*bC&l&AuHtGTX_WMEG7O`&QxrD)< z__O(~p&HvqQufl(TxUJPM^_CEYv(d=xjh^0_mLs|Y~QCp(SKR>|EC1eGZoZmM*ig@ z;pcj^_78@od9ek-IyC+5)xn-gWTJE^Yjz@PZ?OeP!{&J)|38Hw2j$~y_-j#e|BD_m zw$z8~u>Us(s^d-iuME^~nl9$zJfsTY!0F!^E_zg>X|t`79m4uR0K);m5V=y{GC5za zjlzA0k>}?I@->cOW)Fjiuw4s>_Qks^YdRiExvKx~F;ET#sZOI2t<#ju;KBwPRt@gP zQ193`7&WT_G42;G_)@zBfeC`O14FSWtbYW`{ixB*%~ZqpCwJ=3n#*v_l(_S;^EY=O zPgYDs*0orC%do))Un2dI$!hD;Z`wgJ-tfx2KTVIGrnc&(vt88R+eLRQHu|IRe;Ce* zPv@(Dzb`Q+C5bI@cQB$!!75KnvG2eqExzBKQ z62cN0k$abBRquZ9ygEZB0J)eMWq~Y_Y+%=s-@RX;5^DB8sC&z>xS}mvG=u;_g1fuB zyF+kycXxLJ!JWcAxI=J<;1ure?(X_3r{B3>_w7FYzJB-pdF$t{U90x4wPnsR#~fqh zRQ9(X+1cLJ-bqSiRgcUbpc?(GHFOGg9EleA^f@BpcS%t2__GT|TQt|8U* z)^>y_)2(z!kkDk7#7y1YL64qOk85ja*tk@urH1!wy11clSpD@h&SAtaDb#sVWsUI8sVdPzJ@vZ|kn zU-05Qcc|YOOk;|Xa^l%|k=0iU6SH-Wuh`Uv=zmXNrX0zQ~`@^WUAlT@^N9bpVPIDLsT z4@@>5+PLTS@9vl{feE`}IShU#E)No~bWbJ)X2x(1!W{~%4%RB>U4B*yxVQ6)roDD!Zh2}gu@ z@8)pd*Xoc0rl}gM^tzWxo*siXW?_IM?aaE?HgWJ%FZyF)2|?r+_Ht#5}teOolK=t(^GS$VZ{4dZ&!{8!5mE> zSRY(^*^q*T@X}M;1vOz01(}X`R+k#LoT)@Kl(IkKm~|q{(%JL193Aa)JsH&OCv(A0 zZ9H(-bzzH^ALGX*NQvk2(1(nmc~2@j<*x;?(SFzeb$wfmk)1QAxoQmBE3~V7Osda_(#XlcuM2vzd6c-tRy zBxzF}R+U}3M+n1bOkg4V=4;CEET`SeGe-UB;<_dEKIg(2DJ2s;gTik@5Bq(#XnHVx zczt}h>H}}Haa8~t8?8VIk|I7uZhEUXF%^Q&JVDLMuAxJi9^kag7%D9jJeMz!;xEtJ zt#`DdxpdbBzMA~FnLM2h3hM6q|J|W&YlIt85at zGugZ!FH*W5l34ETOz;G$nqk}MNb&WI|14eT zOj~J9i}iNhxgz@5KUmR4c~Jv4p6!=!^?JO$ebgTBM(dfDJ6YW;wBF(m(I~S!)u~Fe zvf5GHLlm!i@^0^mS(q@86O;_e+8BS-ijih^{fQToUQ(Xtd9_bl_0H*;n@}EO32HqX zs4cx#Z+fRWrXp{8=xg#a&VIU5;aaD(`%lmv8!GNcn)wMQ%KLHAiB!rWXNL>x+VRVd ziDOTf5W>1;WjO7_DSFG4H;f;NB*0x!)UuL zXBV$`3?8c*RlDJ)q+C*_Eg}*CX>RrPXGaWmMoxuLQl4p3aYfgO|1*%U&PwZl2=cuj zi;x)@AFQ}CNg(gXY@pK>qS2ej<-kkc8~n-pEEv~d#9D(eS6CJcYxWZfYC-|q-9PE# zVHfqp0&Q9?c4vxQ)qy3$0miEPMZ3zjz{`kzg1s`Js~qBIcc8ZeX_kKWSni2hv*`VU z3!AhAAZjKbmXcc2;{O!n)35wvRh*^PDIJ>}9|ZNZxo^;ScqlUFHgPo>tnQ?I{j)WKMJZ)*5SPPrti3w^W=y%3%MH zFOtvblQo7ZL;pB;K8R4PsxW~hV2OBU^}K1|^oOMaE7tdS=8QVU)BPg|ec3AVt-pL^ zXrH442%4jiH(B+`Nm80S_A~qJhqx&YEinh@u4yY!41tUX!JuNPS#eNelWKh+W^Yw} zUR})bqrUzbGwM6smGwAa;8dWp#waToeZpXPO_V>K)r=}}ed{!$r|C@XJO7pI!{^Fb zGqr|!HMalh#-mfd6V3JP+~{xp zq{{qkxxr{OmC4`F;pqERZ{M6zwZiHR4kb`F)pr z4&x6-(N2>4<)G%*mzkdL;zREf5rrvej<0p-Jzsk!cS#%<$wkj=g@_fzC2H%whT9Nh zFMLDKj^03du_*+;MGh}GDvlnTp%X$cosz$PZV$?r0~H}-t2V+}A`pdlBgX7GP-Bok z0|_(v)`B#XzX0UE@G~C_+e`IQyG50e+!D$l_>(ZJ5Q%m(UsY7o#;uBdY+MH;`Zuai z^L(B7RajP+5K@Kh>yZ=>uTMu%LSeke#}8S`VzY2P zFmBL}OWWkUS=F!OSAI-h*6U+Szz=zzqrhzR$hu&}VdyJQ>HJTU8to!tqM{%Y@QkjH zXF&zzE0w<6HO3qXv8bph-_>j;T&~_6xa1Hy$JWUXEga~7B2JIuA+*U$t&$Kncf?rN z8}B*2N!0LplK+U8$tJEhE4gHq{0yYUj#?(Bk7V{)!C$(6Jk+HEWawkjEJUgFj3 zpuRc^c0RsY+8b89G%lK=ZNDPa^mH!@`y{^}h$#ce!$PBC@(waHyV@k1f|1Y2gD63q z1o(X*!9#pp+@J0_`zehJ(eju|2Y|{rsAi;SjeRnY+XuObJdfhv7#H+NXHv?1Zp$-$ z$^an)Dp}ymt6030Bh? zN%UD~z^^e<2lz8J{Dsq-J|Q6zN?D}XVgJDW$?2hTZD*PTTlN)*$}CsCy$4Ay>c&gp z+n$OGqrqXyyyDb7dq~G#RFIIKHkx?9Q0dt!F!1WEnYq{$pG32xc_*hVgX&~5#w~|N zAjs9vY-;}*8ai~5t&>ljEZ%^ST`f8uQk|-4#$$V$$r+&u zRNF0!WjT=`PqW0*;cN>@u`0EB*~UXJDmk>~o2XSz%aq<23!>yGSjF>UKlk6$^P2p7 ztfi>a_kP08bwPmIu;7eHd9vGC?>%>TQqs8J{$L_KXA@2=^_U|?BG9Cyr+X`%dj4wW z$*UgQ2dvZ6a#tfWM2CH*Zf}iI3H@i(Emx1LD&uCm4p)6C^%I@j0YGi&xUQ#8e01B4$Hz)p-7Fl z&c`1Ow}LFPlW9-2E`?ycy(=_hxDP(Rj{3NzE1%SN*1g^rgBp+<&JAZ%j zZKoxbSwCrHf4cVWA3H>2{zsDi9X=Ro(ndGue6-E)wo(^9OEl!C=cw5r< zMQ%0QrtRJHkWL4vlkM0VM-k$PUE!6gN=N!FkDSIDR`-M&GCA^AD>`ntr$p6ad)Moq zQSJ#)&-P2;jljn>-2ZCgGAS0movNEk_W*6cpj-`Grd{Qo01)BW{W zRzbT2J*%A-6yL9G=>zfdY_B@38h4fzXNX03=FS#a0O=MM*nH z9|{U{#C%e%1OF|GCzPHA7)tE!P3@eMG8oN>I#x%RnV-PA?l(j2OLCSFD@HnQ$fx@M z3dN($m->G~@u(f^dia&O3!jDsV6y%bO_nuVN3BBD`Y~H)X;K2|^TvB| zbU<)sow1JQPJuNGYKPS~kua=7xBV^U=W@RE85Tq1cI`9s=-A<-hdsC0aKUKC_;lpv zbSDQBhsUwJNpU>NY*PG21rd`aKe(_LHE~%&=E{i!nIYCmC zD$66ACYUef1IIPJ@sGS-jz1V#UmC%2mUQNON#=zsK2yqU`@^~$i1p}{;~R#2!d}Pt z0gvxKjZi>A$ho_F!xA*0q96hVotRcYI z7khs8C*`J%9%6^SZ%U2vAhX*o4e#5j!}Bb?u@5m6;?f@kJecqMvZlZ^nl)WR_9X}~ z-b+C=7fMq4$CzdrGKW;7rPz>Od8kJ}`XNM=O1)Y+g*?!ujgkmw7(HalN>}4%vZ>1T&S)S^I*sl=v z@)OMEE`aSiUVCs;d-dRmZ4>`=eUa(ZaYvCBK)Xu=#GPFwUFS3^q` z`WbgS-orqu;NfwPT31)?ll2^Zc&JjJYagCYEJwrcc=_`s80<@J=pheBxrMTVP7dN< z?pGtCctYFPDn)tGd1zF*TGB~~NFo}B6sm^-9oewSw;Hfg+!qyA4q`@LN`A?DXd?RX z$M|nSg^Ol&V)Rn!mnZznnY7L`8ehpgBP=eQ*y6jte}QP$U;6!iA}#=+rNs{*p6sP1 zg(f;G4<{O7_^SHVkcV59_mm7)Py!Tb5bRRuL{H~HBMnnssYcbs#RaX?>9E>j(B)x2 zLTBBYZ-)=*5`_dEMzAm9wTnr_r8rpMv~cE95}TeI6Km)fm`_2rggUTF^sHX34Ai&W=_pNW}QF^a&W=<6Azo2rd z1Hhwt=m zWx$95p`k!zx@-&@{Sqn}c|bx!6Hs)-oyvv%?lreFNdZ*t&Doz`CJ7)D%i#6j4#z<# zM&a0=!V*czg@NMP3y<%tP7cU-P<~YCmCv#=ezjE&EOozt9XLP%scw8v|q!7WwS(6q-* z;lSeiI{8D{1a3(d;YWs4v-jGbjE`0dV8*4;OR*p{UVAKO8(GGLt8|~^)`X#*M<`AJ zLF%GV;yztL&}X{wpL7=q_xs1T@9?8bw>1RCt;p4i1&Oi0BdtwBX~J)pR_|Y4WJiBZ zl&f*QgPF$%4pO}OE!S5m-%?|Ty0w4Icw}23N7XrHC(=UeQ>5V%?QD2+pZMWPcO`Fm z;M@R5M|(H5Gl~qybsCMoR#!76LO5>RXvGewI4S~x?O~_dDE7MWN)rBl8a$y3#w5^; zyN-&EASpOOa$4lV$ZH`b& z_2EaR3Ny@_Ba;sO;Lmn4!j>t!9_(a)_ON@`w;BzGI8VtQmUbqjY#7t$M%~4&e5bGA z6!JSyZb%+!q`bbqak39gPC%<7_zQ`*Pb4YR$}K-|x*IZ(XBy$@P0uw|R?Ve&%kes| z<$Gr@Wzk3Rgpw+a4{sdw*Vm8+Xapt>q9F(J2qZdhvs=T$5%i4)HpgB3&Y`Z`WXz^g z)~-4SAF}_)P@3L<1*N5KDK`WB+3?2mkd?P?gAme2dXO4QZ)=Kbo9b#${GpoZ9%4hp zAxaawC3g!GTpk(8%<6}i3X;;bZL8xV3vjtcx1`D3y`G(){H4pDY>WTX*MO7P&H&q- zj}Bp(wKQKvQ@3C_Vr5X77DjxPIO`qWPLk(>FiH1==Z|fRKZr3m)R9SE{TEA9l%#Uq zN5#N!Z@ZlYjb@6;=P_-O(r!yog4!hc4K$*?!7De~*%Glc>k}pZZg`ng0@}xvyS3rl zPDqM?#!i(8tlVFQ9JV{2mc#Tv<~f-R{3l0lty^+zg4pxJF2+=S_sB76<_1z}us19H z&CIzc=AmgFyXAqAI1;UzePFPXI*a10I4BV3?_^2vFM-PEy!cnw#H_U#2?#0$>45)0 zGt=ZdqD$~b$S1AwnzCcAm9Mn|s?l38>3|L>fAevIS9>t+Qk}yRT!N?O=1vSos?=2I ztY4x{kp=Uh5vJiz!AQpR?OhGn5jw@O-CvqRgl^`vG2?swv~Xx`p+XKqmeA#&eoW+WOUOPP!IRe(`_-Z#1qu2cu~XDh1%lw zjnAf_BJB^0qU)tQyCEa@Ccef`aYcEivmf7P6MUwIgGjL~ucr)A|1|E<9b=M`I#YCx zv?8OfLuzLa#d> zRhhSua3h)^c0cLk(jp+wEJs_`G5j{3p@)I0-?I1zjmakbAPiaTv1;s;8g-~sv2WEr z2yY>q*}h-GtAjHOH50muAiSC`*cilPf_%)!UcGY~$;A6wo7^dwvH0OsfCx9f{^*77 zKz~QcGc#UylsL@NaBAs*uRS0-V|Xfe9hI$8x&M9qYV4JhI{9bH&f@Dj_`R6>3K6-R zeXn#m17RDaGGFr#l(zT5P+#|i77GmJJY8uzXom5(z8l|lvu+5y^SR@hKXkDPgU(DI zNxP?zyuwz9j#ZSj=w?}jTqFcUJSeEUAu2+;AQI@Q82RCnJ{P-$cuqW@^F*o)BMb#_CLWy!}k6j=V z2FF9aVt zRy(-OVs7ut6PkY~;&d<2PNYdxxR?F!lc&NwN=o5R;-f`4{rXs0@PZ0)`U*}$+^SSs znzNxZcm4dcG}(W05%P2Mzi{g+CaGHULvTzgDn@ipLWnUn`=hKzek*i&AG@Nr4l`)Y z@vmMMd@RmkJX#61dAd^$p-mfnCTP0xWf(+;A2NR!eDm-Gf|TRmWir?Tz4o;WBL8xf zZU1y`7P$AXNrsGqGMSv1W!|qqGqEY0N$GziGb7h~e~ASVE#PgHt2MU7tL zyIKY%uT3y&S^w?`{+CD1xFI6T8FHn>=Px=Nr~jzRYpBKvHIDQ~VLlwD69@6T=U30wh|9mH6#D_DNI&1pS)XYbcHtFjl@yWSZrp};w(C~M%U9Wvd!T>hd4-7|8bxWD&F;?ea;#QOX1CDc*bw#n`$Ze1gsUrkHD zCC~eAqg%{^-YKXliXO;jMw>SJ^lDA1II(Ha_J=shv^`EMId=+zI+D`#4{L| zY!gIv=@Xdg*TI?Rnoh(#4h2e&ger|Nq?S$PI5uk>S#ez|iu2}OVv>FZZ2KjMZf-ao%uX^O7Wzy<@I81ofXEZ&x;6$tC<^YU|UPV z<36>oX99A%wiQZ8f{1V7hq?v0mYbbeGpGwK+6`s?_iLR{ef3FfzgNJhG;;P|!XcaE`{ODcPq z>(^Jm?@Tgs&G*vPpS{gL-AIuMPzUD*y;lhIBVSM8l(>YpuY|u4(wjTHUK+ydKS^_Y zV=4aQNzQ5qH9cJ@E#~5K%SN!C;3^4|!pQgwc4oqx(cucKGn{GNGWsv;pXeOucf;;L z(w20=RowRFpMjqgwEnQ(ZB}E5dped~n;u7~@U;I6;>!}4)TknBgZb-zxJxTAO5|^B zH7rpGCr8YXRnQyCrD)_ZOitJ2L?@4CLhihKxdqs0F|FzqqwCNHJ)Q`}L3@TI=Ooy? zwEI9|@m!yA1>1k%CsF^1S++m>by>+%8T$8rU;BBl^7qCEbtil;WhriMZeA(;#|L}? z^Y-i5Qs3mP7pgjBt^RR8;D9Q?mASK<1%;x2H!OxGOZhqLG_2fNc>n8w?t7unxMVN9 zll*ToI~4C&Y!3&A3F{Hx#o(TnpN-JOtq2o-+Oe>r=~Hc-s@=?~rzf0?G7R8Jk-3 zRFO5fN=E5vLV9Mn0c_`~3=au`yodY`l~0KzEu5DWI^wNYh!|R};q2PHfw5s#fJTP} z2i6jWW`jw&nwL(#%YSHDkn*BQ4iRrrn7B0MK>?5^y%xh7EvEPIRPO0UXN`ElL^#oS zd61lmj~QgHP|wbbgi|?}9+B@I<6LdeP9Vavv|JqVdssu z1uyAVFcKy6MdAM_>NKsI+57h)p4f<2%S#$pJGkp>CSyA86X32yj|o}#F7SoOf0t}dzaio71xM#op={NavcqpVW1{H?IAG$+ruriU-frGSv5(qTt>t;zL?|2`}Ls@8=`1dUc z>UOnb*x2HqvFi(5Pv-0G)*_(FS_n^kN>*xJr_3wUk?q#I`vzAO1o}{*@{ypbwc4AB zV{cem4H|DvLMffG=`o~shxX@N%n}LR1CUU9<34%IK>eg zeG^}FJfpf%iz--%oRREK+kB3HTvp;tCfTz3Auz9Am7;x~78i|2z~94?fM8JfMS+6( zE7uO@5j%y>FmFmy!Cj-!$#e3iV}(m<@m7VQ>c)`3L?+QA<^*#K>>9US^YBKLS#Nfw zRUUO90ua4Ie}INd9K2Fe9^o(I2x9a<>cJ)LYJxQ##DRpL1A~b%|4e#c1fg4D6A=;F zx*z!XlUn>pgfQ;5_nWBWMaW3=9`Eoz$kZNR6>;m4RFZucRY9d6?62{4M15m``t3hv zL5(^&M@AUvpwEIya#_czY|gi-dH@gP8a^h=V1$1IT)ZUrX2yqja&GdTBrYHkoiMeJWzE>wA8Nofk`7kX061AgjBO)LpDSg?vl39OKlJ*HDRBu0|fNHt%?dI{Q`p} zn<66KM$8y4BYr7}2fa6rUasmb#>IjYVG|H7KD`Ks7pY?TOgTyihTa1j|;bh08{vXtxx}9;H z8DWd#Ls5cS&F%OIaHQvBWkx zKho;mRlm&1w~j@jWQ6Htt>{(|h79kRNc(}gwTa*^St9%2*W-S8M$NJkM@YM;Wl$5x zaHh@o&>8^5m182_nx?Z#>tf{an-<45eveF1MfaWZ3nrUjJaXSzADJ~sJ+!F9`jkoz ztLyd|osr$J*tKsUUk6nHk&w;;edgC?eO`QMQW{R0bd#&w+QLV`S;!O<+9L9n&k(pZ zAadMvEzipg1Zu;_is$RhiodxN*hjUrX>>iIX0Q)zEAU#bRg&QcuM#Y_@{F~sp4PA& zA}VIxgLnq%H~yrDd5pBIM^kAAKjSWK{i#)Kx=HL`1#RZDY{aQNX8i&2mAVR~k-97Q z#)AV#7dgeUK^XX8rr{+DvBk;yLLx&F6M_**`ocISXJ#k-IK-MZex>MYVnW|e2b-X= zIk!1M&jmjdf>39O{^h83z8=QU;^h$eb#pWHj++L^*pOvzN3y;M5U8-|kxspAJdJ}} zf+VI(#)&Lclto{~E5`- zNGpbJ1VhAkFWo`S8Z}y#8oI)mt+XgFg*Sa4(#iLk+Vjd)I8@)xiXv#h@Q&6;gVDNd zjQiWNZ94ZQL#d1l@iNO?jS_?Kj#&(3GdiDs)Y0>OAF(EUcI zVS{+GiL)86-C85dS-gXL4G;cmlGQVyKP4fEzj5S(LCI)NG;^lYOrfQ}e0mWTNCEb-N+#LpKRdrsW!@KDsI z+%aaWzrk)V))FTJ5q=*B#HN5jKsiFe$CLjs>m?tb)%;%U;s_1bGlDd-wMg8aP{Vy^ zh1Jshz^PIp-eA?`Unc(OxMFL{uvrxq4*7nznrx^EV*_$KE)tdiz-X-ZLTx4w z4R?*kkw82zr!NH{C)P@TgBBXa|<7G(VvV7g6UeM|_Wx0|bA!(&M9BS0ZQC zXdjgd;G;l#xr#09(0^wuQ*HnGxqqdfvh2d(iAq|ukK9|ol7t*Ws74}#N!zs=&$QTG;C z8X(IkkhV+}S)Ee4$>@J?_$L=&pjb*l9{=bxvP`47$(v%h1hUNKLJ|+9q`SkTAOS&a zG-JpGw%9KC@hj zEsm|O!(oA`GDL%}*C&#_DH4v{@~Jfm1XCuI5P@$3{mWzedaa?t%OBIW4C+^b zE-`;^n7$}py1Sha8AS{|vkGo^p6iWAjk?Hi%U+zz5i|Qkt%~hpGW;F5BACb?g+}d` z_P~RsjHiB5)Gp0#Ylu>@AyGihF`xn z-~Q~tRc!7Crftio#4glqzfIj*`U}u4-x?UaP-BAi+jLi8mOP#6m125L=iuZT1W8Ks-XJ78g#u-7qSEFwfAF>qioNUg|{<26& z7TkqD*XZ4lYV@^efA)L}%x&jwxL>wK{Uy2Fk^pe zPM+;&DXO%sB|&v9yLU>45;-?3EV{)X%XcyO?cEiPclhc$X-bzqht|JA#kHj$WJH-Q zHS~z9ic<^J#1UkdUU8h}e1)!SRovNIqfC=5Fo6l+(>v&Y(Y^UN-`p+sJ>tJp*m{Xd z26bkeB2(obUk*{D2zB#gvTV1+0fI4m4mt8@{GN{pxw+lR+zXunqhI9kdBk;c;mM*O zg`+RCQd}2!C?pTIuZ(eNZYy<226j)oj_SdoFaRvS_jQP%{jDDSvgP>+BrF2ZIxmD=AyfwdineFDhMhUGc?-%PLt{+9{%&Od#z1!CH+`HFdjr^D8ov(G{& zw${u@vuL;+@dJ~G#`~o;+CFHzeaV@c$3JrR|2pA3@i=Uh7Us%T( zu%%@63Cg}bhj3Di+tcB-{IEr_zdP;zL!oa9gr=A=S$w#DI#g_b94M~fdQmS-LUEcI zclKCt1i-3kDuv-M#0g$}ZS`^aeTjnd5A{JZ&V$S#I@s&b;(t-K9~+u*cRX3tmEd{hQ^voL z+MzbRg!T%`amu6-nMPo|=;voG-Hk2phsbgI=6RV^*oS6RjR$N?eOS(SX)pd9z(42d zr+oA+BTJ{)Y11JwgpGB^Y}=;Dqgu%Ni4;#-W=}!O>4F&74OdAbEBb+Pzpiu$A*Ga~ zf&uI~*Ae7fXl+SgQ$Ak zNNJMoeiUR}MM2P~arBi%t8F(Wp?69%cJeKFO~uQCGk={xI%)E2S^Bj}qDqJn60_6r zl>IgC`d8HPL7$;VDsW6}pSVLsq$d_^BHux%+mknadRDI>!-)w4;X=k?b%y>*gkmK# zlhG8t`3&Ti;MnBU@~)vUL{$Zy!Vz^~Ubw_~Iz?ecnN(W*6o8Ic>-D4Hrz#TMp6O^v zMF!woM3|dqql~{wy81N=MuOPtTW^6`hGZ>MIcCfFZsnws$x4*rbY5PyhAcg@N=JGk z8%^LbgNXk)u!~ZC{OvlKceOt7QDbZyT|-O@K7&33aE;=&R{_`(D*N>~v{|@N2Wo*S zDsfXP&dg~}i!yS8jguOK9A4=n4-=7&f9mKE6+)`+ge7C3$Rh_jw}xi~VhZQP&5@Co zj69hJeJO`HsR+~l=m+RZ9K(NKb-%VbAXj2pm0ukdXKI0zXmHCn8D23ikT%i0V$|+q zsqeh?@=enD{XtK1^JtVdd4dmpQy%l5FI&qQB-7~t}{`y4@97>W#%epN{Uykkx(s$Z7b`bEYkEs zwUINrAiDOv4Ux3U>xz3}_#Ci^-9xESN-v8h)C1M4wK2IqL$$oo;c!(|<>C`>%V^CZ zR{L#_N-}}e;ISd$PLEx3`+83CJ_X?qw|AV7`qPn&jf=eqZ?0j_B4-znqxuPB;uD5F zOL%@vj9k3M0Y}|aRQSM|)@IsAR<4q@R?39Y$pBx|DGrZohf$A;fm0sYV8Wq8zEL5w zCkwWu>C#J%Lto-B0(kk(#o2d{j`Q~V-7j5q@X0FTV5A&r;E@75FD$vYhwDFh>na@m z-rGhyO!imJv#2f(L>+zwN|7L2-KBQid+b{mPxndf{1%ZOe;q z_obn*xvKcMV|4rD=K`LA;>^|Ri9?khA%7yr+R@JlYgsHGUlEYIjcC?yWB3wKM{462 zt`)MqoBXTTSD=nRid>9E$#d6f8<`Npf2C#BR`-)J&6`1N;3~bEAVP z28pfXfr+%brV=&wX+kRk^=;!NB$TmdS$!tT(I0N6c-HPKSN0oOW>qj+N~|MId&1{( zc?*TVO8s08r*IdDr8*!U&V6b>D8I+z8%}tiHP2OI{8^1T`c9AN??6qbi>A-4H>x1R}0hJ2;@m>kYI|2n3#dyU@> z6}xRzf3C-LtF|;ROpCy8SNC+!@;g&n2`e&n@6}#W^KbKOs7g za|u?(XA@C&uVGCNfcHBFpb4B2-u$7?o8VD+V-5_L}F&p-%sB`{ufvS=X+VIC`gu==lit#l~8p|dp2_rL}YgU{N0rGJsEugrD#2l z@CJKy3C>a!n7!|xuxs;2w{>!-EGqC%B(6Dc@s$~VhTS|J@gJ`FbFe4P)IXB@`lVYi ztzC>Ys$VP>n)oXTG6uBYco!eWi0@MX?oM_OQZ06~Z1ou-^HPG}TREH!`x#a=uBs?9 zsy(L-hc%EXItN<&FPvS~={;QXzH2j3k``v=UQ_xlA`gdN$n5yTF4R5G9B9;8ps_U4 zA6&)d?Qw@judWh*re=kL2SZgqzV}Y2M}w7_LuF|wM|;PgZAe1FQcAyM>7EiP^>X3svnYJb?siVvhdZE)(J`W~vfxfDrNb(_V$-%UYOHN#NqO zG<{=~PenjdI9!E{$&T9W47{@Sv`eyldkz@B*{Ip%bX8nYKpTt!IV&%w8mUuDVzPc_ zotQX-&#LMqGM5V@*f+#BI{W9MFEe}H`^|B~wbjXSCbm+=p)ZqMN_*RRniVoo6K=kT zhsuxQ@&6iiKo7Mh{bq%!SZ|W-%N)9RDqQ7QF7jq29*2v}B67rHhpY$!06Msn4ST=O zG=(5B8nAvcak_W;jEzl2ArHt4sBvN#h`|#6cwZ~I(Ep8v=F=MW%Rd!tFs<6-srPLW zZtE6*HbOhl8Aj;tx?36o$UXeN*sf~XR+$(={NEuB{vu^=pjeE;$^s=lv98AU4s!aY z%Q;hPWO;_3)e1fQNUmyu9;B3J|DN&wH8KTlTZMM6R2M?dZbfeT&e3yR5kBk3Z>hZ- z9X(0Y)xf~KlFi?S&R;9MEh$Isd{Oa!h|;BUt!WK^1JQ5^r0Izgye`S`bSNSWCG0nj>`4fB69wd zI-}?W(B(?n#Ma4Z?x4`d@Vf7~0(lQTnmh#^0pMVqEk`~<2n~CMRIbFRH9>@d!B1-V zv_Sd4;{YIVb!z!9Y<)W5$-T9)MiM>9j)uLZ4W$34Ml&u05ntB zkXGkQ_5pT6{(N+Xm`t$=AY9 z#7NywI7^M-69V+@68OCDU+XLeUxsh9uMC^Xwt?X=b}Jz)Oi)tyTGC%!Y5}+T_tP2M@+*>sSZgdhJASoFViU8 z3w7K^M0&~P*u_W53*jXf_&}fXsB^9z$( zCM%DSkBB^|CV%2cz>K2bM$?fzZontbA!x?`o<>rv-cIyWOC?^=y$&-|#IzcxlHgSILXWv>ugb`z8n?G0STt#tv)0sO$LBBuQ zT05$cWBfirW^*)wOhO`^vgu~W*0w2A#1xxgncJHJ{G#+AS9q8St{&yi3u`jWdzBGu zYTq1#FfmU%aaX}%_bw=XmC6;LkEc9&U(r9OI-y4|&rz{_sG#M{76^0AOsReeig3SU zs^tbPQa_P-@dksLolgfOqVZjd>7Z-fz69Xg1pw^wzw9-NW5Bkeso841O4Nlj3} z#^rEwD`Y7#J7ss=krNjaj=J<@V7g*WCFb#VXGLUVa!}vjTJ8J61}OKuhyp?XRlLC2 z&Kp^7xU}Bj9^nxc|7YJTnre;?PqBw;!i2(DH6xAstkqo;W?Xz$|0{vp_Vd(TW>^C2 zet-5obg!n;tuJb0_@ZI>Nxwkp@JD9bG?%2=XNp+7{7DOYwrG&M-c0;=EndkZ^!izo zQ$1DB%`1dQBbmF;(KZx!uZ}TzG_N*v&XY+>-roiP%GulPSGmX)q0ev{=iIxOJJMJ| zg!ucXZ;8F>LL2p!*@(X;aUX%Q>eb*wSc$)uu-qS<5m{W7+D+%x(;|N-&=0WVkPbD< zQkIb!)h`H%0V&mYfv?#m!}oJj@R2Hkb{XWlRk-Ha@oHO(>O3g;w=`Y^RykR%^A+;^xq!4ojwD>w%|~&|dC=;kZ=cPo ztc@Gnv%qQ)G6K&#)bMyj^#018cKF=+UYhv|ii!r8`*aE@eYS%wHkZ?KgE5Cc-&0v8 z;PMsPdCkk6y}ip_{$(HLOnmYEXb8e4!=v0B0h0fTT3U`XO=+StW#W76c0UKd>DW>i z^|ze347cpwwwMGT5w$1Oyg@e7NuEG@0=#yz{u}-8NT8h6vD>2c9&5Ocz?QNhlYU%L ztCkH}O=kbf{G+!G_5q-#^1qJeAakZc?DpZI*vG`)4ELD?>U5E-*$V4>b%B%5lGJ^Y ziJ`RY`I^Z2a&gHfhFm4Pq79YDmz*6^>y$Gxsxw+^HPA}hbLW;yy!|~nF~HL8dM_)? zLtj5#+%aCxK^saifj%UmdgIkp{)1xoAVj%2<(-$@Zt)BR0c&*M-38NbC(L8Bh68Mc z_==)_Ekg(6Pr`z}b{P@|l7L=yw4uO^kx~M^BI`5eW^LW8>S@3hf&GD0F8dyK+lICD zukK*{8xYNfo#h^ioX>>-?M_6HRDf+b+9h}U@>yg8FPSanr8~9oR4T% zRkic4aT?mC_MdDy-~RmGb!_E*VNtiIK3^VhI-K&SzoO^BjvW2AbF}*5U*MWOz@WWw zbyq^@=UavH(>$vK*s|+%dh_n`vB^0Qm@Q0S=QPdPIc76{ljA!GV9+np9FT8c^=6F( zNKdhICQVQociz^nr@r(e@@pAYy%UoNs|YzJ(d&~%k`r@TXD8N{QJad3OE-pDTzw?K ztBEltrZC~u+6^C&5-*!BF~2EV>H+0-WY8TS5E9aD$$J#oyDti7Dbz@7Wy?^fNz|(DpL=A+*=q4RG_EZ{}Zg0`2h6<9sm(_mDh7eV8eC$rzF{(~` zS=w04_W8uREDWZ0S1H@VAZEvNGdoT)O~?%3K((7rNJ9w&f--M;P)@=ST2Gog@R9oC zVDawQ#&GoFbAZvF+ODiqq94Z`O`mx3D1RnaBu)}ue3OyxIdZ)p;w6@r(pd=hr|I@G z;b)XMPJK0;jK(I;ljwct3IW~rvARs(b{F0~?a=nlwn33A7jM~BMo-Pe%X$gF@lp2KI*9;qt@J?MfFs{8+qW{Q7Qc8eusfykq z4IHAgo6oV^4-a0np!&VcF3vDxJ8XS{Jxa>r2~Iu_TyO z^3V*HL@ke(vAQU*OzhxNMZCR6S1@$Tr}B1D*;0qNu|&nOl1h~B>S4Za0)i|ublAJVYFou{bhsMk?9Gxs zkz%@9^(E_Z1Dm?B$mx-`$$Hgcq^s7MEs<60;@08yj9V!2Y1T>ZVDgQH^Vdbiuh^d} z&PlJHurR)72%0Whz0LBd)Vk3lb{X5@xzY7!e(Ih~EuV^TBZ4CvW-T^%f+uW{q9~le zrJuy^>^OC)Bp&>x0x%W4mKP#QckIhwnNr~kQZcIrC}Pa2e(u>9V&Xae|0$%TEh3MM zE|}Q&8W$dEa&zK({9&oRKEy1rb=o-?UDBK-NREY;e{Mk`9EnOXows=k6VQWhpgB=% zUy1U0K4*1Mo{Y;=@1n!BcR}f-{~~jnFBHAsf+2rQ*Wz*z|0)J>zxnF1ZZTMC-T7hP zSsDU}eh5%BJ1XY9WPA9vx?2CbX$>!$e=vjKoCAU%vJ|6b(S43XZKc*j8aptwX9m^k&Tt5m-9Jc^ zPXH8M*D{Z!xVQp0#ao2^AAXUS_--jeQK6RD?kRlY0P|p%v*w%qMB$fx&sd{!o~#q+ zaZelh3IG7U{?!jpJ9Cz;%MWXmwb6H(ht0idBVaV=7+|a#2ww~e%QtI7r@le@bZ)1Xd-N!^Y zIk8Jq;iJyX8yG2t3$um9{VKRSlJrGSb6yMQW!wA9ft2>}+jO~FGXq*gVEp`=# zt6ydN%%~=ifI9#YdCu&;AEpBE4iiB5|JHH*bg3;kH1C(bgY>VZ8f&QG%zyKD$xQx5 z9xN4%7x}2cQ%>PhTvpBP{bP?(_xi_vhJ&vb39ryE|8|y~NSLslPc+`k0{ec-!r3l4 zPcB7rH21wDv}6%_k)~MYA3)pY8ua=Ig?a&XC0|pM863v!Lh)C2Y;$7kPML;N(a;n> z=c||yZ4;dv;uT_h7>Aik_kBliaEyzqgX6XVBa6 zT7bRIMWYyu& z2nSk?DRnK>^p@J9n)35!n0@S%)sYN7XV~NcB$g5*Mrfe;Qx|YFiZ1{^rXTEjbh~ro zYVt{D4GvX5=PI7%af66t9&H#p)0E|D*rkP5JG15(diBr18#4S%LB`LCKYq3L7XGnp>1xU$L(ZmrP(s?C zcV+zY{ybRE>P)LjuSF_zJ76@TI7?6yOnwiYpJ%rnkId8FNCuDCDMlf;8ES&{((Bbg zz?LP8-<^9KR{coDhbY$kvul_^x&A=En5L_pqRwo^r^KSPw=dhO_}wEkLvFZdjw5hl zg>}1IPbQRguf}_vBSUihx7r{0zAMSUSr)-!0hHZNfI&XXK&~XuD_-9y7l|{ z=LvLs3$GdJ0PM~SJ5>Rx-N~NMtUphiIw~q98MO&4qu@(vpUq(<=8GKNTaYqIl=yXmH zvWS%N5`{y^82v6zGuLdj@n_HvCYA)g9dcxJbdlL|18jCrRxb;zk5HmeKgmtxC~|UR z?XO6waIk3Z^-oiN+<-SCpWeQt%l8zIe0>VWw#|49OXgpV9{Vl}7O13jByvD4OGeV6 z7N3{D8{DT0T(T-e6Rl1nV`j#J64ZzjJ^Qhfiia+uG)u$7Yxz0agZ%Fk?@c?i8uMAsx$^bm(jsdmY8S9{axHEY zHeF4BGK}Sz8?umV>@2|o_Cniyu;SB4W+1MF`129wgh;{Z>o`~fyF zg`|3x8BVQW2QxG;RK{7Rf=cxJKhvTFYAxZr+VXShr;t1iAUEZ&4(*7%C&Z*|$q-Un zoo+$mHB0W@y<@j0$46?B20>!PYQ;RArE){p8i~~aYVpW6r^38!bu=_PtJ~ZRXUQgV z&`6Nf7YH~tOs(UkeTyO1%d#u-qfLc?rhA`r26B9d3+JjI3B|So zt|-X6cVBl_s6Q6+tSySsDi4ZFw}o(ykF0j(ZfozTtH;w5=Yi(xpN%v}nNzRtAZ3vm z;Vmj2!WbD{1S-X#!q(3I4K(UZa)u3B)Tv6|a6b=ndsRYf4-p}_>giQR> z+quuwTW8{1`ugliIbpLFOQ{37)Hw=86SVX1XuuqdL|8@>hevzzq8uF7(nEoy59XXg zCZ0WtR%P2Z@)z)M{d-*tYBwNp4b|)2=&Vu>RU%m<#+fze%3?%;FQ%<%`cnx z-ih3@uSK>%=RW{Nez{Z4a+u(=9$UCq?)fUJi}S}V@8L;tU{hLmdWs4G3Z3vhh>(J9 z_hweW@t7$pkszAU+lH+w7X?UqbGOz24d<+~2-)Y%Cai3F zTSBrqr;yy>zHAsO+UGB(K^dm0A$KXHh8kkmF|kMus7k;)y)3?xTabgOckl2JD-d&I z$^iCrL$_w6raTQJq;w2{;TSCWokYcmWxIOz$oZl8r1+oM2<_!33eoe#4hY3+S&AWP ziC1O!|5$El{@ZdB78?2(K0Eo(72}e#GB%y7cJSXsP0nBfNpBM^o96VX`~Svg+Wk+W zCgJ}@)U;cLu}+bo|8iQ-cxS z#ACSJTaDG;**dT5L+Ja(+9mP$z|iuzX65jL1&>h=U#+=(Zgpk*Tu2}$meH1rh- zNCf=oMZ)wlU9IbpSaqkl&a>}PVT{z%1TND+l$+n5nr*lC@!-wd>#aLadvk{!g50Ef zG5BgUTOF9KR~jE2KjMyv7d2gTVZ1DzF@^?aGo{NeFbI{BaiLuJk02NJVw>vT_k@x| z__gYqB3G1Wcy>Y|#f8}aV_$hfO2y@bm@Oyk0qV=~3!voVjVpf|jweFho?AueOwQ)XzKSP20%dWhiqxd_mbH`WTc~E?&*q zLctgcm3UapZpl^tn{Vs58_6Cd9km)fl#1o3I~dud3rk$@MQ`o{%J;qIb`&kfFj;R; zL<^XI8(M6z)(hZ_ILPK-#%0t|D3aVHnllH9nxTT{~LGWYNebcOK*+tpKxG*X;rj5>Y zsg8_Jb6qYUE~_Nyr+K4k>iycQOo`th@j00^^$-SgBpALtG5gRzZqn$C0fyK##_weGf~aMBm~eFzL(CG#LiNI84|Oi zV&hO}br__O=Z;B?1Ydw-G1<4xckp$5`~btdi9XVP8u4Bl<;3A11o}$f5w@;e9fQPS zufnaMrgpC;NmEo_JJj?T~MjEJeF=E4NF%c%YJlQgPNQPR=k&Qyi_?G)B6&dIurMx+wU zkhe|7o_SX_WRv3&kwO0i=ULBI0Th(O9}r8nU1AZK0KGjVr;3_bO$b2$@ z8SI~L`05x|SQ1f1>xS5B{A zdl&bJ&9QC+>n1qA8xmp~*Y-qJx{X47Mm`$fsL3%L4d$Ulr}dupy8F_);thg~{O&U1 zbMttbOLG7V|IZMYQ`g73rbIc0{h=&X^H5~v7lJpPs`a=c?fCHQCUp3a+AGC6a<++w zMXM&1-K74paVjZ{dk54n%Kp%_3BtjUX|pA}iT6PI$n@@U6-QxUF)^j}^w#yu_-0MG zc0H(Z7v6q}@M(m*U!~#&W%;kS^t!brhKOBkks_ibWIPcHU*Ho%jb$

??p*h8I}VrmECFrBqvrs$pT4H_@s{?J%QscWNa=Kk%7}XeC-9HNsMv?H;P|? z#7Lvk!%{#k%S;_#8fli9i$5#i z@`55_lvBTqT*i5aPmA>b0F`GYP>X&xU~@BTKT}a}Wn$)9C4wx;+YXCc0O1%py9bDO z%- znidBR5fLU;ZHg$Tg;u&ey9(@I67F{vHEwXi6k(FOE-eOg<=@HejAtnJ+#C=SYkS!`O0?iPlI2>a@jCV0BDP~liR?24-|w-<>2VyxT#4xM z&$xg7*gPxd>cbPb?ZD{QpBu7B_T<0CM_8Wu6RV#tHxY8P%lf?YSP^$ze5I5U4s_>f ze1XFe6m^GI__rO1{&&hBC(IqmGiL@3<-GCDIWoi_qd|uScCD7_>6SMr!xCtK)L62i zb&IlAS$>#mv44cq2zBZz75-vdDitrzfyNq#8b9Sb8c>ft#rhd(;%D!$Y5|BQ9fTYc z;TR@4LRPAo`u2;|k6`_CaB}f5lp??oW(vZ$rQIb5hKtHr=znhLca3A^9&EDQse>+p zhH`D_f(hG}2>9_D!s?&YkZDB$Uj^|yRFFe-DP_XtWG3HN1ej>URq7G7=qhbuM!QH| zQDu_tb#LjE=4@69Z}{ij%+oOpGz<8rdsqffA+>Om+ud`eTc<&W0=zv7zWxjc#2AYL z6SR8HVr~7wJ5D^)0$Ps=4cnZtP+hxmCJF#QNu-7^NFX~IdkAK zO7Emm>LNoX#hY$j!-3Y%KlZPn=}GId7L>`@^D!;8%Mo^!juX(nMV4a!}LSe(ReJ{0E?dqk?6eo@|n@Om+mcB3ASWZUIP|+sJ z`w`DZMAuH%+s^9y;ol#=+mdVEax;bcesnJXltO6E@@&YY{N1j%AL(5Fh7?Hxb`kzBZ4!~HW~*X*j#JBrnR=6N}sU>+7>iewUBmpxW)`bnLz zD73*(ajEGNUPGSv{JfAb_rT)d$jwVgLh@s55Z4EKKLJ;OJ^)WsIG9PLSci@T^PV6$ z?0s`>B4NZAiUMc?pFdOi zfB(vMvmXE591a%N@3X==@AWJ46AoUx#^CV`-gXv`yU}x{0)5~H-w%Ov zo*S)ajMu6Y2hQp^?OrL%Bx1JVtMS6ljs8Aax9DL z(eKG?nJ!Yrg}alXJ~+4~UhE6_8)H6eeMc;mu}PO6&MM z)7FDK%=g(lMS@;1A8QbCbt`A0Qh|z-2U_bm(!^YWKsb8?VRPtFrp$8<P%#6^t8LDk}_3;;*wqxilR&j z7{)mI>EtJOwLBw$wMWg5gw!`TA6S;(bh%=Ug|EYAt{{oj3PUkfrlP3Ph(FYE_`6P3&^u#+q{wW{5Na?K{E>Nq$yh^xvC<7pB7gZX)DS#D>d zPpO*atkWRndfL{wdUa@j40p+%XM#zfl4z=IDc zjR5E*8wSH`LaT3?%;;%VFD@%SxO?F1PFMW{4NQMya}XrUq}Z&e>FMN7?RYH6Msn!c zHr-u$0%bhgEA#x*92!eJl6yS%@Pt#vd^|=j`4TdECf>uqz2y>p*GOpW?g>N{<4n}g zv&`r7tZ{_qT@B-2P_2Qd3{NJDbes!hj{dfN-Kaagq6#yPpW1dK^u+pgAj30e=vz9# zeJpgdW`MPNE3)d>BicIj;l&wKbS1w`!GyuyC99F$?XgV6dSa>We5~dtN?F!dQ`D1U zKCx@bEXpHTQe!Mzq%zSR=V~VAXLC_nBwZ^&%>^;iup6PJs{QDq6!FMHBtX zmZ>Y);7@H)hW;jVVJ(Kb`!`cN{o(vhZQWye3^1rlO8Jxo4cU!>jf%Q?V&&vK^BpV` z4Q0L`e@Pol!=>OxK}PPulNFG*4>?N%gMK~~nU6|li$QH4xqgbJ{?M)4oSRp=VvR{n z=$C@)D%zn}|4{ z3rezTV=*qLyG@(>D~6P_xoixL>*E=ZliY1=7(L%$`ix-XjVu63&-MH1i=4a`w{mF6 z3GGY>CfM*_#WRu3rhg#PPyc1D zHpBE~wyne+J~enfcf=9g>cGw6*DME|o&X zpVJv#w%T#2H(uA=Uy7#`-A29b2hikCXTG|>l0BK4@+_Nb;zHV%SXl`Ga9;Mybw(q) z|J7*J{b|fP2_cZHcxY&-`#;r`kv=eKYqZ#KPO^}u;n(v2R9ZfW)fdCB@<0z!jBSG|1?E5dHP+up#-wLUcN zP~i)w#2S?&G$b16u6!%m17q-~Ybd}iEPN;EyPP(wDSj$5dlW?E)*T{~I-2VnMJGaA zq8dmKlTW5TKcG~-G=tkYr2fcfm-b_#6h8vd>{w!$SJ#dw6C?_V-Ad*s`Et)Nl{@%B z)Uz$Jy!+%rH?g1*6^m9!-wBi8C3^pb#Ig@3FW0fs=t`C|Y`#|-!u3B`fFG8jl z%M&kW*XZ9O^J37PJzrS2{A3rGJ>*NrENCOCNckn0hD|abvtlc$x6N$A6@P0#dDg|3 zE7Um+MGC#xTo@!c)%T58;>iSg17q7!_%&M^tZ6gLA&A=(Uz^*ihc)h=tGBSLAQXpHDTg4yKX9R@UBsMYIPf4_^!JmhP$(@))6_kQ+^DW+R4r4V@fS>7Rf*ksK@5}QGc z2|Ggn1cP*IZ9P`mg?`jttINGA@sBDs9iKI#K645Xkdx%-;c{EOkmUkj4eEI$DG*Yl zvV#QdPw|KvpL|G6Dv9-FlJHqzB0hP^?&ywtIkT7~cgXip zc1={p-K^-U%gQF%*2gDaY+MW5PJ_K3+s!##q$VRflF7?fp+LKy(Uq zWRh1anPM=Rd)O=ox5Tp>Z1-~G1H*DyzKp@+ZiTWiv*B(1x;}!Nc3z#es+~n}hei*j zcfh>FKA~0f3XfJpmVPfCmY0z zL&8;H51zomoIiUbSdPfkp-g;Sj$1s&4XMf@%hapji4OV9=<;EWb z7v=z=B_6XdJ-YhhI+J_4spm%8W4)MA^Tn7@EDbG8e2>D zbM>PH7YKw^T!LYM5Od`KyAJPIO?Cp8N`VtRPby3lL2>afpR^Ei>Pcv*k}O1#{D+?m zT&{UMl4J^8-j_0zKc0}~sn{4!l)@KioL<_p*j-P1C&?Uw6j@|9bfSi~C*K?MLG{@+ z#}`I*maDcZ3Y^&%(Z%QF?SqX)b0hBt*8ucvWmD>nBEdk+&g<`d&xZ%fq?!^{6 z3gn#=1M*;&^k#deZZeKW!~IezQc}yEu`*~$E_7eAjmbh+ ze7~-G-_MP_zSFA8RFG=1!{1VXA`(SxLQ1dSy}A6MQ1>k{(?K0FFp7LRR5+6dSgI)YP?ii6R)tV)DN6>p`>x4hNq#)2`jzb~hl|*ECz5vt?ZeIs zo@7*p8vUO{?yaD%#FLg1{{q^bP7rr%6v?;2b0R*r(1HfmZR6%XJ~LRc zOP$Yn7&l6X^Vvn`Ts4)#-6lAC!b6L=z}oh-**}r$AMr}2I(gB*+feG_E*aGWR{ zr#!^l0tvkO>(4~w_MPk$K{4)Z{wBU3qQ5hOQ1ll|?pG6#2`8bK@jx!R z#EfbF=|G6oNRl7Ib+vv({pG#3hM6qGuEKR(qLoBRq;rs(D$D9{b_k=+rT#7--Ezb+ z@Ago&)TU!R$w*PXnI8s^u-F0q;8Xd0GT}g2iIFtUp>H(oRz{^zYTR z(UoNfh?v_Sh{-l&bY%_!s;3IuP^mnU8B1-TaH3ZGzx`}gEG^rQx6uRdzID{@Vqy2J zr91##Jnxt4|E#<9(*KhGQor=)Q;0c`+Sh!{0rI@^+PNb=cEW)*JagH|6oDNNW;v!Z zU$($)Vm%iHpM@vKi3%!ok-@U1_S5*6M|0c{1fZ+y)DEr;!b*g7pB(L~D4-mLVRo&z zp_KH66jx)q|GT8TX<|71Shml0qGZ8=)f`$9;=>#gjrL?r*uW9YY80;JT-Q0Q3_GVu zKli|$!7DTrDeHCS#BHHx>&V(Yobm$(ilw5DR@vtQ?Tq-B{G$eI-)zm70@#rjt*DMd zwHW4No4Zz}9Z#8`z8uR}i`EfeQ7fdZqKJ^Ou_q~^BkZ+b9bb6goUg^E5g;uL&yO%= zbW;7U5XhclxRM61H^4fzSn%mF?pCUn!P;d^e5yDIjT#X^gTX;^5C0)|yALWdg42C! z!^|Yy+A5qfAaj58Y|>KRny12`U6rMWyJI_fGcLI{2sIh200XQ(cIwiZKVsT;CMlsf zP+!e@Y#=+`p*e83s1xtfFSyI0qUFaL z%tgY{BTrAsRkbv6!p<;L67#Y)xfQHL+rKA63f>#23{*CfB#_ix+ zE?cR&N=$`#SB6K(Nt~+Bek1O8jd*k8FSCZznOU2OcBrYGcEmdySVMpHyhwE`% z@U%;xW*IU^&LdY^>gz0d+e2mSn*MOWwcA8sYOjlriEgDBu-8(Z%TC@oHHn<|XcT?C z^>QLp2sUsfDYFZSj0H6xIQ1qDeT|~jV?}6&5i-J=ci20g|BAS-o<7R=v;U5D^@N?M zpdiYbQl%j)jW)ckJseS(i8D+tr@N)qB_3=Y98dzQE2#-1@)LOlBYT9bTtrs7j862s zqxG}}DATmbBY&;}O-v(eTrtQXcvC^=n8SvSuIxEPM<7CPXD_5MC#1!ab1B-x)aL41PR0evvr6=&$oEjHeP zi9qe(-`4cmx+)@d3nN4}nK>Icx5*8?iyXoZt5a!`9LmzdBgupLHedjwA z8yAmSST`Z(g96Y%G7W@D*X|+;gLLWaf)+ncx%KX%K^xfoXTZnzIe(lMvpfp$;HGZ=eEbV_?|HpT|HgvO0MjPsTljh`2A}Aqk8!G2E53e8qff_dokNT{ zlriWyMEOenom$_9kRBDuDEVjmp8Tj0FOvJXTK^)w-ovtSY(`8JXyW6o#yVu+buRes zY<-Bv;>6tuR4pzRET>@gFayd0lDa`^~1%Vn!%F_5rHYZV-Utg+C}chJAI!yl!Kx zSg;z`ziNxe@5%Sa7(KJaTOSvf@T*81a8R;BwU z#3WRH7A@a@wene|1#T<-ZImE)RjCY-x-*g`%?gyvAyvgQgPpKt8}Rb zx0rdEt~@%;1MWi&Zg}Y|Mz@|%i&e&k2TeDbc7AQ(>fLmKp628p z$s9yrI)Q@<%XKJ17r}JaolrMfd7nu|*4&qg6@ziB3~4#6Ydox?|AhV>QQB-Lbk z1Et$%k~$xTVMuS+zm8=WuhKm-2u@yBo7{%)^Xn z30lTCjeNse+p}qGh*s$u5Dnr!ll!U#`LKuTi)csLfx#k&apqUB%1(FW_SojFI>KA~ zE##X#sOHiA2OI#=3elc+8;nMhx+RhR^|t$b`EQ*uHa6BL_8+YV=N*x?U7Z>2IHMuv zpB5Rpof{bU&UfCh(rF6L_;3A^^`PJ%bmwv?7eE1d&9_LgD4$)v;_mQ_{yt%A{-2I2 zBJTX!g!db^wYNJCZ0-ocVt56?A;cr_${mVq}O(Q{lb#f@w&jxeq zMnJj?M6D%qP~Kmq91od%k+ZFLj=@|WDr9D7pM#=YlcSUHh&y*Ds_D@FSFN(re`Utr z95HjGWmDaOlfO8@F}+;t<@^iR4c6cCY%r0GcQg+?HmnJ!E%8q0cod^+hM!Tw=j zx{FVCdpJ_N%l^^9hLM@XUJE@v8R-g6dx$(k$H2o;IUxQu=h-!&F@JfgabraKk*q6) zN{RKY9Bl$%^!C#R3CX{&?Rx2wpV^fbPqRBs(QJXU-W@+UypEoXTs6PN@bo$L+rhL1 zpe`)Z$1m5D(+vYLx;!x5=j%6}=hSYmt}Dh8%*TpC?)AN6M%yWo28Ypii?5}zHpu6e ze=R1Tp6zqh74Pb8SL+)}m7y6HBs-e&zwS_)oxXLexUMuBcpvjZ2`DUHUe5f1;nK4J zixjd~*Rt-P8w}Kqdrl0gpkFpL1F350z4(?ub1$KiHh9}U0fhM#>G;mpd#21cG*O1T z)jhq1P6kI>W0)|SFrMWuMlTGPF1IInCzKib^e5`I8dAL|DW-3C3)0S^KdqiVS)I## zc`OFaQPR_E^Z)kN!oCBz{B~XAf4^7_PJ}3>RHFVHV&8krmBU8~DlWmK4jV9z=dX03 zP4`s>@Ej0qY4!R0;NHa>&;7DV1Qq5_+d`PseLNIr?~%bB3Pz6^GV5c_wQHBemez?$ zJ!w{%K`+oB>IF5nt#40`pTV7kQ;P&cA%;r9-n6#3qrn@^eMdvI>v(gq-m=e+N3GL` zyKp1HO$Lia!yQZx*x!t36m@VV+i6`8$4>sVe*y%P&on*{t{%UAgfISVYrTaf>T(h_ zHcr=}KKB(^k}n>XXn7&4&*%Mo2&Ef0bkaTan?EyWQR4ZLDlbgjxgUPJdL|Sc^DT~y z*nIoFpfQs8Fu>@cN%NlKRIyreHo~OP4&YMDmImx0`6-y*O|fEwPRvnUa-*zXXJL-y z)?w^+>C}Xt-$Og@w>5o|8ExdbF`zI$CMxYrpzwB?do{Neq3cs`pf6RbvaEd?3C*{gqgsxA_^4`wD>*_PFX6v zyiy(SrA+F8seD&!oY%-(#?tivQ&2@qHUA|nTsKh_GnH8uo((b1ju(yt{=n~*46R=x zGCufFI`@tXus}z)s5?yoh^Ga*hE;-8R)=SkDK>N&FvDli=C7GwnP#-TunoSY+Rnx? zAm{01@bCcF4KXO*8INw}qftD#UDDa?@?SQ5ofVq>t2tM&fTrLdd&@G(21)vd%8OVu zL6*qKw@=8A`*Tq!`(R4r`uK-nrv%k*0P(^c%q=c5ayY55CsAh~6mZEB#p9h?0TNgK z9TOw(5hknhh_oXhaXs6g$eiPuK`4~cTn{R7aym0%lDV}%+@Azv9Htkza?NLc``F^4?D(%cqA|uQ7bvk z^HWIawCPeZjeckr@$H!Gw6{)gwKLsYc+ya+}q%u1j zl>tBR8lUvUdd((-WdRAp2S4u+kBqURYPJ-G=-i2XNM1_MKD;f!g?WoQuig$FQu)wv zTC{7#+30kQW}>LWt}L|IY-ke&DIqw;MyDPpC@ioHghScXNBJUru&&+Jo+s%&=&%3X z1BuYCer_-E@{EG!x+B`~^(Es0;34(9^*f{~juQHd03oFihv85_MtlK%j4VCrLLj2mZZg>6W`j zqth%N%sUHS%DiSfbQ`%~o>#PbCER+kx?OLQd~$|_s;aOYto}wznNrR2yoa{NxTVW7 zG*BcRbKbUd4hXA$K3`efJE#jwGdk-Z?p;VKvNuugwUV;9&eEZTEpkzbYtPb=+v zD)LsB z+MKn*s&tFXHN%CnYtNhcj|~`q2CsMa_pK#g#=jvS3AS zm-w<{TdVmbX!d&*%7Vd6GLbJ{OWvd55&9ET@T6yWBIsU_yXDZ5yj>ud;31wr$(CjjnIseNNxTz7e-0dj8ILMrKAvzB8XO<`@FeX-z@W;8!}| ztij#Weghy#@UpqFk^X57N!iYaiHyx@{Xq><85@m%Vb`0PDl4hEEO|89GCl2}Apa9y z{?;^9lrqU`6A|iT+p4e-aL8AGoR_CLsB;-|RT(J9pz0iX3s;qp;2Ky!qoYWDPi1=@ zZeNOLWIvv-rzFvlAUMn9T8Wvgr9Z0SJojE)7*N||0)b)0=*#ctM*`Z$^bw72wqC^z$z!Gfw-?v*-A=tjjO+=B+k#8wekak_Q61lis~w50vJ3{l&VO`^2=^7w!Gk= ze>_j;vWhOaD=K;0%#jR4Mi-HLE85Z)eM1!ha0@3}Sz%yu^D=fHcc0o~x?hl7Dz(C+ zo8v89g{52OB#wD;;j^tY12}v#MZUEv=xwSsfKDma5h!O%Fsvstd=SK`A50ZQKdG3E zCcB7p;Wt4J*67SxQ2pbxpxHfoR6LB4`)CNP0od%&W+H>BH({)iUt)qMT3aRD*m?dC_kb?ccrEz z5+x{G6CO6UA>+f|?$C-?9c@O`+N3gSL}T{BYFZRU7;@wLBhdta#T}mWFdD*EE}7kf z8WWYf(3(qeQ9e)@X8mh-y^BBZ%35qjho$(alMpo_b(M!~V?@G4SR)22gU47akhKGq|SF8P!h zuT!V)nVt0y1^)wx&pDYvWl?5aKH3buO!zi}%Jy#b-{kk|ySMbwThH_IoZoDp`FgT! z9v$tX=Q{1>cgQ<@xD*NYnh>u?t1-DoB74*xt@E;6<-TN0;7%-i4S=S($lS3NSCJYEm5feDp}$wEF$-wit6`JVEqR6 z&81Mqq$GM%bUm&|1p9GsE5Rvf{ArqO8P>z;jm^aGdtZeMZv)(wnl zdcyXWT;zt zCm}HgELs z@^ueVw>r&{&+L)`*)i^x6cz$Grc42s?nQWK7_fxpO@-$MdmaOYB)@5!K7V#lH7^?0Dv z$I*Dx&;hEBqn`nchrvJJASW2q8bE}PXFEkdvP5`_&kh*T^6K*=5}dlgu}(Ud@uMC? z4tR~u-o>ld%&UBTFups@08Q#(+4y&hwz&@i=rLVAr{^AqI2`WYOs+5v7e+7l=9W@RfkZ-@s?@XLc~QBCK$=j7+c2-w0JAVs;{s`T8xq z7*z8$S=6e{Vs z7A7mrc*v8jKo-iilKzFO#$u%`4A}^XH&ZUBtXzXL(s}4t|H+0*3v4|lAoo{2Uo3Px zcaG4?N|~lOzp40X9>%LcKbmPBvJ@#pEmhvc6Ro=McVso6$%DNdw%#)#bhsM+^&(|E zKdVXjd{Eo~b<@+|&PB{#MBKE9@nnL|bJy%MB^De4ws)-_Os6|&u;?~tGC|G4vZ-5t zcr)Ap;WSL$g0y(vw09Y~EQbm#Xe_0;uwrlV%vBxCE%c;c#0K9}u4IlCv99@Yw0cn) zQF<8OE~_rok2Rw|hUXyCi=&jJ;EwAjosMXrR5LVHAT0pk$A`Dzc8!u~q1s!Z#>wB# z&5G5$_g5#o=9=&_cdDD+7LFoXVlLD*AMIy5*o!O2zPv3_NJsWTr66I-A(rNrbwqbL zIUkt6ng{DKEV0haKCDmwnc9B)-+)HcnexbC_MXAuSZi0?z#Lp)}jqN8_ zJ{EWW#>U{wTbHZr%dFiTUmc1mRyU*nQBw{)OkrF;P*VPH_YTN-_;a(DJI#i>8GT;v zY#Zaw8|adO@$>Lchb#Q(tyUCNDwTr0>TCzsb?Osspb~@&KTKp;a5yrWd+;p`uJA~3 zI0VLX(5PC_N~IAcKS<`VH8{2j9NJ15cb`pK2?mHStz#3?BNmbcuo`+ zU&KsaIk}rNHvccc_t%{7bI(q1WPTa9Bvmxyd8+{e3oHTME_&kjwR#&T{vn_4*vRnV zInxJvjwg&!=3cIztR#Ba{I#IzpG27Biq(cRbz$)P*Fr29k5WwMpY>%Pr4Hp_Ko&C^ zW0Mp{SCcvkd4b`bqd;epsWa;T(!34kXE)CmlLiQ;upi|}^2QaFn8tiCZ``pxQS|HQ zd2QY0@P6@xn55#e{~Tar6T2XuMG;60wl2Yzn;@q+j{2?rM(BkFVkpD+>mYa%r}Id} zPR%fF`9=&pB|Yje{nb%7F`eM`n(L6>05m;I+mAJy_~%8UOpHLIW}R;wK)-J;U|3SmwD|{ozp0-$}3JmTY?x_jkO5jUBJw z0gh@$#A;x~OH#FXQ+}B#8zVP+o>AKsC~%21`MP_U#(NU}Dld+x>mQv6;#!^n9g~ zLOm|U<*`83nb3V{`m6Dpo`%ZpAloyRDl^K)_%h)@Cx?mf|Ep8`du5BfkfJjT!*H-y zEp?Zgo$+~aN$E*$WJK|Z`s7Df;f&x` zUssH6WCz7V0`40w^>(@NMgw2NRWd1Z# zzE#mTc98rdJ-~;Uu>0V{TQvI|^j0T%#hY!~0wzPJ)tvF1JPw)+vA^8QvcJd@tEw~I z*l%sh-_#}4PGKzhY=U=xk36WI$75lUj`PoPj!u#o$YvjT`UMkY4gl>}vr3{tFVki2 zKAie*6Q*>`e+Bj=ilbM`T9_r!eE&K&^0eYQrkVe?Vf|XC*n-i?9^C2Hj1~v^_W`k@ z=>L*9e2{|pIIUOf4u6ti|LNJ*^)q3Y3Kq&#G%`B)NrwHWskR&Ve;B#RX-O1*4#{^l z%RT6h=Du{MU*rC_P69Xi|1fRufNSki(4m5}xof z$FS|LPGAG{N zj1^51*B2F?O~x|=sp&ya)#w&k$S7J2I~t`qmLn%V){XbLgTl-?J8`aqmEX!ZzXz4} z3i&^Y5cky*Snc1{s9VmLDcSDNfpn((_b;cYD2L6_t)nn>8;FL|1O^K97T_HEhUb!Y zoyDmgl)0|vykVpHtxJ#OyK|4gN5hHw*^c1B%ZqTkANP#2AD7js7f60<51?x);a`K- zqW_#F9+kagc5Z))Sz6S49HeqptwkU9!zsj;2Y;SDpw!FRqhiIYCz4??&Cv-fVaa7c zS=U6JptYN2bJ!%Xf}GLSyYEzIH|gxt77!=N#Ya-~u&mxwk<`XQbK7-uoPFl+<*JFd zSKPC0JQ=PKg_kn`8l23z#HsjtHDnfY)W?H2{Vu%OsIo4&6l*Prip506al;Y!(D25Lk0kqRuf7ml z&w)25z`9l5MsDf8vAunRkWsU6DFF=Y5HA^}J6q=1j1_#$MaBq)DYleLty^0BDM#q( z&oq%6l?R8dE0uHsQ*0}o&yu@=@dXvSFe0B8q2hF zUIA@=J{YfmYmISI6O>-0arzFAdFYwe_0BTKP|=drWO}!(p;Vw{oV!13 z8u*f3b341M8ZNF|fUG9jG7;^exjZfo(@1bwQhRNwt#5e+mkNp@ux}py>0JF6^(Jba zWhzfhKbeu*si!8Tb0|<^qs+TMrP|i2*(yBFe2;7Ou6a2}5Iv^w)BjT_lL%m6%`6zD z5oSfX%1)*f0hPD%Dv@|&SBiE)c&HPVkO8#DXnKxOs!A#H5B@Ij@8;O-svd%}{iPH| zrRw9YGnS4bSc_WifJ!>EN&pqc!J0`{9QIRfzwdq3nEqc>+B5k}Gk1fRsVq<4GsGt5 z!kL$b1LOOiffZD99{Kyu^IR{^F#$^+>wrGyy zoN;mm`mO2jpVD}P?hbJY#vSZ# zl&Y;YCJ?9(Ds=Q-(dr=;EsbYFir}D+XJAVr#)3~aOIE+c7F%b_#`if;2>Bg1LptzN zgOO&E3mK0&FukHmY8x5UrfTh`u&O?HpSQWjI$DD7ldEHDj=k_4{WXXA_CE)2OP0b1 zGRDPFxYHtZr>8c-IpSSoK{IiEwVoz0hVNW^8#i&HviT=Z?}t-nHvn7f!dCgUc6N_< zTQ4#hvruoTf+gwezV?(A;@}t?gPuOkup@^f3*ND9T|< z<9)9wj%p$ys>SA3VQZ7S^%5B$EsUVFdzjd=BxeJZKLWzU93kpVc#fmxSOMvNNpbkm z_^mEdyrpme{F4LW8_97V`rNqY?%Pu<4M#y|{oKpGT0GJ(BvsGOdt;m>x83u?n4}mH zKie#nP2jrV?V};IEiZ3GW3owoyH|hruEZAl;M=J}zKP6*3%}F`RBe5Qz~75_-R1YE z3bXL&#|jJQ?3pFp!NY2qLys36on}~4GTOsbXiRkCj)dAV7c>r%6nLd-Ql_^A&6GXlgIE;$gBb-RcsV!&v?P zlt0DQnu1!=Y0kPi@02FVi2iD}GkMlgP_EJWNSn&yLK#e|v40;G9pQ6H8YgW+%HL7D z2MuPa7md}t*!BVm&jrI>1J@^s*9}%CDY2m+qTvy011f>M*mT6BrYbE5S6C~_!3e-; zVgyE{J~o`@|9}=^&Dw0A))$lwviUSWs{&q^eDEkj6bp>#l*gg&amETblOL7^x8Rac zweiE6?hUwAn^nwpwC%&i?_xdppRLQ^(?8p{#g#p6KRR7B**dR^kbm}XBWX~8i69jG zq9>A!1@I4L0R;sN%JDAz3qi#n8x{IMgewHAOe8Al8<>PAB=l>&^ZE0$77&QWNFHV2 z-7V`y&*6DuqLtb4YJ-!T-e1!*NYTXC0nX+~!##(p1idSB11sydxI-DtH@m5UyZCho z3OacLxj^IK{CIam~(ek|CP*AezLOAJ6 zIN>nFjIACyi=*kK>9m}iw0Uc%_z)J?0abnNPax&SO(A< zCr)VB3Q2&yn9{}9qE0I)zq1+vu;J+YO?j8}24i*YT`kOiApwNmlq@5EV!flCGZ1N= zXSBOrA_} z(*u4>_E@-+@RePD4ZSUimYDDA-=pLj2J9d}Fa{a0sQLRQZBYE(IDClEd`YLtsQiNs8~gbSK3LJ6am3 zNt&P7nT5pg8B7`;P0XwhQpOe0@DXK#k`{`XxM6Q6Ph@gLGq83b6|;uHP<>)p*NR}$ z5d3b7Z@ArC3*i;v3EDg5Yhlel1izdoZ?fWGC37C&Ee1&Sz4YQ>O6{5EjXK)gV0Vn= zRLXI0@#ZudGG+mFmbi}afiyYljUqX^e96f zeVNK_AOxhT>St)Bs$*=r`ynHWIy54_3nY@r3k!ZAJdv*WvdrVIk8D6_!(%_f>!f+g zXL`P4AT?B!tY~$tX3T4hK}uqT)i>LV*_6zHwwhm-AJquvtQxMhb->G3or6ME|6^hA z%h57_3<58X@`8xW1(sEpo^c_uZf_powcf39-~FPw+Kb@IX4jD%EGtzRAQ}PdGJYmM zM)Xgmv`8d&)JRvr4Cw)X;y~FAJK@{1aHj$p&PEeBd$>2b_J$wst$Uz&pt=|C=P#Dr z7rlFl46a*l14BbZZ0vAi$F^Q^P26i+4&Tx0lij&5?vJlw2^{)2kBiz%3`JGw<{vAW z-Ya*>3wb-+HP6Y`ywC|Ru>9I|;3U3}UuKjwLyhJs`ST}9Iug;r`34h1iUwdHfx@vt z&TPM*Gpg844rRzYKU86tnoyt)3Pk9GO}B6I^~HRutAP+~I4neiEyreEsSUK7IA?AY zF{m$sQiBWo+_<`UVi`@yb@Rp2;WfqxfO72RU4JX{HIgaYk|q-50-hBsw;w3HbSMC* zpt!28kNc`7610dRIh{np$j9Wxd1Y>XEsU^`u=Y_H=|?_(Sn`M>E=4 z-=hAHlDM}lqmaJ%_|+bD0-Ey-1O6rwkrcPPdW{9hoSa94Um}WKa}|h|O#%jj=Twy@DP&>ZX zq@2|8;p?0V=NID+%6)2$3E`x-1D(vtCL_gAH!*UWUbJVp=JlM41H^K)$MI92Qn1^gpneQzUd+?eo~c!Bbr&q=sLi%-vDb@# z@~ip~#I{K&wGZ>?zMNn&$1a~ugLqj8_jD@AL{P+=vJmXCEg=ydj^Mm`oxyz8YBFcJ z*>8`{ZR@Lw!|*DPw^FAI&(YmzOu$t?8L~RhuuA`wj_#%_O3KvL_3286`tAf5>0S;D zNyM6IjRIJ2{n4Vg6QR@K!brGqHb0p+8T`nAl86%vHpo}=Q+ z4B{gDVg@wpaRl$$ECK3z2I7hV8y$KAbOAipr#B^%bP>h``@XlXdz@1q4Y38(QKk}T z*~^;2YY+AJLHJSzf4`XX6M+P%><+lbVOk;LObfkmQ}m#~G;d}Lf6*e@Ut4Fi9HqAs zn2h!XqFUuKSjQoxR~3}81{_V4esH8o|7P)EujnA`j{a$AVN#PJH|vrwBtfC_ijcsg}(x|%pG;-y@qVfF=!gg{vk1Zx)g?%_4K`3vY`I*kZ&EepF1Eey zv#6v?z8OoMfc^0lGWix)i^JM%{=Y+#6iCe`cx*kxjT*#LQh4`~o(M@r1#dg^Q| zdAE@;xSw3&F*xJ2wk0Y`7S(sA+x_SrLpSed^7%wh9Ur~^MHe{SG5~;?MfA|o>*nse zEsxnLW%DEsH!AI!Y_BpqJ_gRjNJ~Bx1^`i*y|pqlL;+24_O zz)AX#J27$3Hz+75S`Rarl`W@$Qo4vYEr`g3V?-a38sA}Wk^oQV92s!E$x z*tWvIsPawkBG9j-LT0$jbo9F8?|`GJJ=?ImokH<3zhX7t&vaipKhq&aoHTRde`ZPU zMx8WK!Eb)i-ODyw`*v+XRPZtE5&C$w{qX?M_MmB{G#s6;3i0>vWA5#~HBR;a8GVjZ z#Hupw&X;e+XSq2!IX7B46X~+S;$X!;X|_HkSWrJ>8hotd)yUPz4vXDpgI^gQ6cjYb zhxO+-LQYN%OtySv&7aQvV_EV4nvXdIDdXqFey)@=VW;3{#+;jp+lTJshrp5W^{_&0 zI0BiffL`JB-Vr4n^`DfEDBWO}<6E_bQZ0;R;vl@yI>XFWf z{&s_#cQ?ewcPBBk(cwFOM`M15r3#B9BSM>N8=SX&D6RC4NR{XWVR<>%m}8?u65_*U z$P<1i8Z!ED(Il82obc5EFoT1E>zdCxQH5U5Baa5^eW+?I;Tp!zRSsK$wi{$V&tor7 zE-&6GR37Xux+ZvN6d=r4d+qkS(|6OOg5_iuIL=v;ggM;)Uy>kTbR@6S}F!Q(te-9TRY z$~Kemb=^G^zsQ1;FT-UUZWRkN8l1tqc25~fc~SDPX|TMy@^xYG;bhZ)fK1|RzTIz! zfk?P-sq{kmfN{RVEPYmkpDkVmyP4kOd)Z$B{>FZzrBl6(K9y3{za3zkxqO4s-hRV_ zY*sDdoPDYJBRkQ)3X}cafyMvuJhTPBo^u?X90sCnJTUEzTq%z4`_1iG^F}AFK=EH* zfaAYrah%|r>k&g)nrv%&R4PxlL4M~>u3ke>6)$bfO6BkQi|v+IAT@m}^fS5hC|V_q zP~ocdeIdD~x5mRVDV}?fe12;*sf61*wQj=nPfwJ@F(sc9BpB`7_t6WQ7J2b9($n)6h#(LO*io2AI0+{0diQ=o+ z@I*KsGFc_*c+|_JImpzVtkv$rJg^Y242UI^2DelB?Jl3n#N6aP_5>@WJ~bUq=MKyI zq*F2TV(sm7FvNSf#q|j-?o!o1ca`+SiS4h(ng{&dybM;HE@Nj>j74K;m#0#hyTu#2 zi#u$mEkrmfl=XE++7_Bal|!ytJW5iJ$=g80>i=sxsot&E>ac4BGvf7)mW|?Au=6Jp zdm2E+hAGQKY6go17uWhWzKD)|FjXQAgS!fA(Vv+6!{}7GS*25u9S~)6d#5b@bpw4d zVNud;Mlio(v| zQVO0NcTq$9P04&zpZ}-881^}VlfO1#vlA$~tVb|{j>frLRNNgaqhy|I24gNF04+mL zX0~&Ziy<6%O_X6^I?+8c&AnM*3A;Huh{hZzW`eW#^_tB%bk+~6l6Uiy>Ly74Je6); zp(#lGnV0%Gf#mRTX!W&28aS{Ywt~yBy~J8b_<1YuKwL(iyr6|popKNvld$v zLa803&VZBn5s0Eh>i$xiz2XPl4(&K;PD|vN9gE@X%5s&^oRz$l5w~!s7dZLNW3MUt z3S_E+Er<9TP;z>JHL0o27tLKl3p-_(<9K1Aiz4w>Qfs6Ddg}|V%AGf*y zGQ5|Xp9O@IMRO=--QN(tir?t6K1r}}CcfoMd3G1Gf298Ii;%?eYV`9uaWg<@H-TxPIudol_6QYg-)SKhxUkKf7Ew!C!5v?^ zK0K}~b8_>G(Kjz1E7&Qc6#nk_3=rp2D-!$qchZ*!hSY1A%O-iyrv)_>UJRdmxCF!x zysXlm&}k&pUGjFE>Q^^WsD2j*AWNv%MKEJ0T3?fN8xyx%B!$hOjTSEItKqjA3Gkk@Ajszp?U^K)3 zT~>^VOa1$Dt6eku3bQn^x z3LOQ9f-~q}Vj0QfPHXgmj3Q0&v7Ap%!N`Op%mGizmhCxh0U3xWhiv2o4Da6d#rAoH z4GU$uw&^LfEd9mBf(V=G*AdUmXm3trbP6>^j^gXjtMlQ+{Dl7jdmLA-C&(=hK!)@7 zhFtNRWJAL%S}}QQ@1ijNrR^_qN*mnvrvqkmHn3jbZZ-elnF!{Y~+wo>i+Y!e_cU#?XqF@L$P*%Bxu^*th}0h<_$U?vjhhMz zIghZB6r1drsL%{8(f|7z_HW>0$f|8*;;)XSJ@V9F}L0U{)0-(C2paPZN17#Imm4EL#^lxdQF=R>4 zI!cbzs7t{KX`vd+h%e!@UTqKnTpZj+2Hfc0UCltfXCn895&-Vd8;dz)YpHU8dr%yn z1Tfdmu;uhtfta`&^G*E&9F0iWWs*>x%Gt_NN65l-`>P#&tz^K{G0%p3Bti)HKraUr zG6YaaboyNB>8g;Np0wLpkLp9G2=qs~VF>2Q;jfnB{B!eHpRVzv9=KKZ>E4jf6h(IEVu>8&k z)AqR}U=QAiDjUM}uRxYGY%M6QlPEKaF(yLBtkav0NQiXrfjV@}?hTXYk|C@L&MRt2 zaZ)e|pf5zUJMV0Dt!=FteP5x?j_w!-<&|(<;9+h~kPJBhfsq85;D{(b;+Ik}c?X{Qf`YT`KCKEh0fS*z? zbP>Y+Y-VRo08%nGgA>_Rgv$3oe6J&@y523o-7>cWu{Z+nLYfT^$o7`=W)CSffRu2H zvp4jGz1jM?@4P`2Z>pf&PuDiJeH-W_rel z0TL7Z3zRS^j9Ud{q{Y%I{ASmI^Tq)1iEB713s?FR0v8TgAi(f3vgFb-rA-$8H!*Wk z$@N?Zd>dF9!QganF+Z7rf~rXLNHE2#hp$CKwLGj05P_+Y0QM}tVO|~rDl1_A)?bnD z5itfRUk&Tjm4gCTtub%_RGRLw!NZpXq-^KOAhHpFf`UJ4we0XH3ngleL<=M08f9RO zZ!&#jhPR=gJV0D1J0EL*UbOFo)l%4%2Um`*ec-5ZOloH-ZP}Cu^mAAJ^sxu}$6>MBSXQ^*)YHQFfxex1~Rs zme5T23NZC?v6tIm_>-%DTcD|DnB_n@iLuJ_nI*FxGShPm9bF(mJs525aRO(KP997w zMNaB(&o^{qL}<^PEW#D?BF@q2cG_S{#$9@D2zh54EwLAFK0@LY7wPOAQ2;qni)A$4 z@Ft78OK7adISBp}y6(r?8tQuPQLBd+u0&u3;lw@f*7j{dikm~hli7-;a0=T9i}^8M zA>&JE(OAXbaJ zieRv?t=K_D3$P9tXgX=krjRJ)H}AI~1f%!i0{nr{DLD_0*j%^2a1M6wwO#3AN5=be zmfsi8b=^}G2=U6*3|&M{Nf2Z6_OX!k9iyMQhgIui~A7`t_eesh<} z%Ze=fyF|Yh(C-PhSd(qhn(&%}e8IlKss@T`tsVRHup(blIjR3yvkCp)1;)s?Y{dC> zcO%v1FEjReIGZQ$iiI-}G+r74Ae4VpRoB-;T{8(<=#Y$797XJF`s%W#u(%;+aA0s< zMMx45$XYi`z%7zFAA9&Dk4QM|Y*dvPu=b-x*cER}Qy>gWxTTK?I=fV!-&i3Xe>AIn z8%y?OWReJ&{I#=-q6Yq0^waUoSy96&1AkdT7x`-L&4Ce{9op3>R{h6H9D@GQ=(L4A zxv&3HI3c3+t7JZcdnkdE^vfk{s>7Q`7itNS65vHES7gw!>76mUpMM$4yLE>Rsl2Gc zM+4Zx*#-uJR-%65=LrQ=AeAq0)9MaQ+^VI=41vFI@ZxZGj=Idjcc+2wPg{0|`oiwr zGmJ4NDukfy_6+)JgSuCeBO0XOG~M`p_@>7qaQXr~St*L0B0hZ>!pW8;?|r56MOJ#+ zP&SvF90)~8<_YeJxJz`DR-AG8Wr$IKEL%jtZ#tXfUzPRrA36c z_xqb+>tVAYkMqUT^mW8EO&U#o^X;aCM%yy}m|J?2<6$zcn%MEw(Y7wlSP{$p$3*gJ z8TGhKyo&vM5&o-=Hv1VBjQ+#rM17pq$mmCFIXr7uwcW;C<;L4QM%P7U?shtLiUy1M z0sdzK>m{ywv&-XI!Lk$7zAG;=Y+tQ1XzY zEuVxVl#A^7B>K3ALiOzIYB$$HiDwpOCKV%cNnv<`nCr?5u7 zdk*%a1!`jySW@L;Qan5wr?8Ci1G=^vbx#o7AHHKu=2RDVMsU_r0eW!RC$;$Z5i8!1 z9t1FKoRUUI)74#e{H{v>;)<2IUo zbJ<2oVUK8n;7+Ds&8qc;-&0cOVz-NBd`?bQqnx5H-%F>)IIB%upP7Fa);t(r2DdVr zxPy{f$C`FT#ySw&Lk-=Vj-+6*Y6mTG)rJ*GK)9Q96%3+CQeA?jLYfU&`AscE{qlyg zuD%l0nith6R+R4or%&*@z*_RaK zvJioyqQ&Kvs+AP+kl|Dt>1O4KO+aeCnSauM_%kP0XkXy6n5E?;@n+fkmox(S6e$_j zGd9sPqXbpd0;dCGBZvFDnfULVVZ-^XSG3A7R_b*4IeE=G^x^4YPOfBSakvYoitR>x zy)2K(+fsJ|KXL||^HZ^9A>oE|yiu=8*CnxrTS|lBy6CFQ@54;>zFz6E`Ar#d@ox=J zuRFXVRD6B~Dwoe{YRflH1=Hi+og(4a`&JJRYYDc4iu@0{J5aFJlM+YD$mQz+6!Lm`njJg_LvwzQG;~GX@MbD~IlIEs&F%ReX{ye1*pzBm zoG1E8*>ZLS91wtsxf~a34TN+4EIOUGs)qIo^V(^Ly;aQ)ab9`#w^ZJ{hp7S&N8t`{ zYdb0sdi#&ydbN!Dw(uP5U;;4n%vm_Fx45w0V;**2Sa0?T+(He+gc4d?PiRDltEVSTf*?RY_Cx4@xV+yFS;xe~%*e~n|IW3b_!&I-6!%B*yRp6I8!h(VO1%G2_OHc@wMOWl zmbo89pR)0^{z>5>^FQXpk+f5g`x&lBa{m8D^TYGsS@Xz5^fuHj*M$E+pq&f-do>g~S-cLlTJ z6XPQibjfqRXKGVAXwcM|032yd;ITquk-Ph!#*^}QCr!J=+nK6v${0bg=d&a4!3v=H zcl%wohKuKy_qBTK<%`KfBFeiT#Ef0ol`gtKlD{0u$q|CoRd0u99b1~)4-M{5RLr&= z5x$yF&-S4ezDTo_R^3*Z8}Y#+zwnRne|eRVM7}Q2Jea+6y+vyIZlhAd1`Y3ULdrD7 zIvNo-kr@$wzHnJ_KVS2roNfnTlCxJ2r%qsVGhQ+k8{eL%xCNBC$_6XM zdC_yH#3fMH4BWGg^!6{G8E^V^+ZUM@w`Y{wt+CxeA#3o$EYIcH2l!0h=T*L)rTdiH*~J65sdeLlGGX&Hg0Qh>jA@mp#%GO2k%U z*9U2928;+H&-V*}WlIT5{M6Lni9(1-sRg39V<+p}#;?5NMc{6xL7B-| z4_0Fy8Vkq8-3As^KfbcalP#_=buc$og+ee>ct71CZ$x*VT;+r+FuRhDeKqe@7o34Q z?k22J7uF2i$b}BJU^XpJy=N9%QlICiSd;1%NkrIEJIocHG#cQ}_R7OhNQ@nvlshXb z8@U!m_myjPJ4JIP+gTo{X6H>R-UL^eT_2^ZVXmTxj+uS2sPWju${Xc6JA^SOKYiN? z>%xjhtt`(XbAreD13k`WE1Xoj=Gw6pgu!Ac+{d;Kc7_dCZ)&DRO+@csjmBX2 zXM$iCoc=$MK_t*_Z{^7+ME+&ISz4K7@e?KwGhqu5M7)|)N1*kI??k2yN9WBeIT_%O zSh%>nz-q0du2+rHmNg){=ywNwy%!rLSW<2ra^*|2gUwjrmEFA+!1WwC+91LoTBXY+ zr7qA$o%Zi)zZ9nUrh@v{878Lxa%D1q_O02y^zC=h>JpRM?ti;C8)WL?x2>3$*xwkn zs?599f!wC~(|(@NqAedHp3M9Ky$?SPJ=N+wsal?!q7B{E=HG8BSQo=zN}`Di>Zm(( z?2_+>*}Q0QVgRX^KUm&Kc5W9>OKo8d)9#a+os<#gHU$n4T6Y|>M>bbnB{APywAQUN z?Z11Nu`GS2m=5OcK4>a$ZcN1^Vs|_zLGN0QNQ8M>SaGY-`bEhFMR2iZw+qX+%)?UGZ4W@iOA)7?MfAjvBQK83o&uO{7ykEd!DOtg!c+@E=uC49uCVp}bw)}8Y)HFwd z#~4e)(}Va!*E<@c)SIodzWjH&Oq+BQWLCkjk6I)uiXD&29pkS~`bfWK;$)m*IWF@= znEo5iZzTO4`~L;bCsBBqz6H!zAcq)IzZpbZAPojTCS9Z{p6}ev{EWuTa z z1t%9g{N23+RUwy4)E{iEFL>)-_Tco~wSKLoIk}7aH`+{!keLlm&#h7b!X0Z&*(fJG z4G4sb{VmLNq9CIw1$P5PNN&E;3U;YfJ(llk@uegHR?}ptD36Ii?)*aVc>TMI zq2`y!yrf;6V(b6G+&c!@8nkJ;W!vU1+ctOEwr%aQ?b>DAwr$(CZB2b&cSp~e?&&i# zXMUWYD`Q1Q=2|P>m2uryK2KdcpR&J*`r71)2$^&o(!CGk&zX>9oV}iu)hwuwM@UWM zo2Z*TzGorh9b5#E?U6wanK4Z);*`yV)=H?e3DHz5iCjBl45E@}oo=+v!|xPppCUWu_{-wm>Vk9Fe#rASQ;{jNgAQAi>@D>rN*X~}<$L>8Bx?m2|D zB~~NZUAwzYL8a>LRQF(ZKp`--8VpYH%-JDewUQ2?Zp&AHPu@`zd~Ig+3JaS}MF4Ww zdwm7Th?eA^Xi1YxN3MB!hi$n{{waJyet!j~)s_@qAcQ)4Vj@FEWa-8r*<$d z0~=t;c<%Od!pvMmOC15yW7|ZJ`6#@$>BImw;8Gh}9I6lEDPs=hI+Jc zp2v;og94O{>~%C7qPRv!n!Nf5pO>zth0x&)+2fs#QMgAwzQ}R=;y8q6jA~3!qwCBl z&{~s`QFMfCA%1OKA(g8^KjPxJlr}&w&%pN!-U2HMI7i-B#A9d?5T|!Y^tt&syk-P# zPZ5-?^Z-a`q>3lY*RkoBw`9v$5L+h=jF5!`r2-od_?Y>_EG`hlCKqG?y)FpT>-(2F z0d)-R4Y{L2SNi7{=?E^E2o7Hes8hZ7XhMrn@XJcW6@w#0#ju|RGEddg$?{A~@YE!O%MD}Mn7@q?|FT0E z!XHmR8z%@-SfEct%fmoj4=BT;BcK`koRA(~3?45;T^qMj7uFxFxyD#e1Z43KY(6@^ zSP}rgOy90hGJ(L*VGQ#NrOIq>q2XAom4zJ2!L}{VwxJl5eb1YH|h*)vSbiIOqDF&vjmm` za$F+7>C~7ggZ{MHdC<`Y06^5)b(YrwoyiDZIA7vw$Y~6y+F`?a*#u#Pf9I|IgQUSu63ub$`fCobkPsL?Re&9m~B3#F8ww&jK z#h0`O*`3)jXaoo+;uUsDsb`|tXBa;yP!HWt<`O6eET?H+;FG=n-UsJ$ToJ+JmXgt- z&W_|q@L{77>PEZPd%Pr509pcDHf(JH!A`PP=zYV-cb?o|%oDbf42>m`97Yo^v2vQ= zdGGP((&(Cn$|^_OziTT@k#9jvCPDq+?C={Rp-Bf2k2h8;{m>m+=Q-=CRP*~6+`lEn z`~G50>Jg}cgp|1El%9@!1nr9WqalJ@|%PKLL{qLYY$3e<}3#_yebi2zKdFb2%r}) zx4JV#bsX(JHAiGIoRVJ^+OYXr-4y~}6) z3=J)Bhc7AhbpR1}NA=zL!}dtss+sOgs*-I}a{9Io_YT7XUvW zB5@TQj1c)Z#cR(cOIt?pfl^;(JR)cAFKjHgSB#u^h1kE8z=hQ4Bc-F_2|r?{2fq^L zjv)vuSs}{#GSW{nM1bL`=O)Op!mlK$Wpx13zQ!D6@^F}`^z*1{PVBvs5H@zXuTiJb4f`#^-qZ~rEhP<2DxXc(DnjgHEuY!%(E#0 z^UsKmx>wmPgQVBiiGI~&B81imT!IXMBU-Qk1Tr!jAR|ckZIhr0TvI;^!|(cpWlIzu zBpbK{Cx^8V4is|r26q|Q)|oXmoPH9D9a|9N!AH#V;aQ+E?(4Pou&Y0TI^Bc22wf)T zCW67jwqg$w%fT|JrQxhJlR_$;UANbQ=!f2g^W_AgTW}T{y1Ha{@qpC6!EU;q3>3?J zE4e9f=&_avjjeVZ@6ode%9GYd2advnYbdAp2bbp9_?9Ps97G~|$m8cPaD4~u@HCa3 z96(04P_yFC@#;TKl6KY{;TnT<#-RZCF>gBC<8-Dt%an{cJiCoO7<`8L!AiodlJTc9 z|0J{5*KWm&=RE0^uR>D0nF$6O11~QwXX`vA4FJuqgh?WiF#0gPyIY=^QTNBr$R$0_ z{77l<$tLtVM3cuRwW%RywS;T4i)K0HI?}V9;LB^Vex$nJsoERV>64VnZ6DlA!l!ed zO>7!Sn8Kawn}HBsZizB1g$F)(Z(ijI1-W8%ie+_^+7)qrR2eq3LJ!z z`c-}>I`AjY>D!!>PCQ;kv6DR(aRULfGY7@g$4JvJ+VdfR#=>a`4O6VhE2kNV2Fq9C zc_T?LD)Yw&n6eQIeN?jr=)fHP#oA%BI)mrlI^&}1>v2Y`TynrO8^ORLDP0@!L&O%W!RAXD!z^p0nHeTcW>NwHdZ%ZpuEv_D%PdC9MuLTgyF??U-1jufzjvcOepG_3a8nLs`-mKn9jrU zU*mDv! z`=T#ysRVBaRzDv{(oRs~j`~tUU8}uphJz+^;jaNWg|NZ1n-3{`oyxbfNs78tioB!G z%x#cL-ou+i4Ra)AG+ISIUqjdR=?vHQ7P>>zQ*I&Bf~M8Kid-Bk&bM&&z~G%W4=+HJ zL&*m~^-MijGUY74rY$VvP6~DTDPQSAv2vLBaYEkW+*1IGzq?qrH6wXN|58Q(_SBPi zQi!q23e%OQweeR={%de6rR@(i=|cUdC$aRQzFM;f;n0IsR5BK`V8k+KPINH@1Q4@^ zG$h@-QVF*c;t{-M(2w8G-tB|kbR`MkMU{IsPM4G6Mf)TSJKRUbsFfZy7`T)*^Y=1f zw7*P2+`LPHI z-XyyuU8L*nO)K7*WCF1NP8IK-_UlA9{1jjo!X2n(NVjcQbcHOiYZ9We9PW z=q!i_IC>E7pd1;iIJJKe27=-ioVOU;`eH7e5_NqG>ei4JmT5VZZ<$H3f^L*QLkQul ziyyB?-(eFL5+({%iqznrV4}1Y?%AD2#oL(%N_nU+cLjAY+lzcyN2^Lm^ zMX1Af<8|vQfca|-$?h)coa}WETY#?6eX_1VaxUMF;0*!wCM$BKt?4#9OK-vB{fu@u zUjJI7a7;B_{?q`A+Dfz2>_?H{>X&4ViKq2|&mu0(x|grCOlOVWCc4}06Pmhx$eK?w_mXpBs_c|JxygK9%h279~0NH8~ybKSF>X z*B-CcVXe_}`Csk)V<6)#PeyJ=c9EahM`-b%F#xaJ(Ii+E5sR^yHc|Yu|7+>H4pVG;J%4otu*CL#_Rlvv{L70hxjhQ$Ad&kvLk1N@ zFZSkDBA@)v!dfOB>m1_a?#@9T@Hx>Ek_U+-!StdBF9d)Y9QIsSyjLBVJ6R<$! zJuhc#;O9R#eng}O2gwSgzO1^=-_q?aZ#a$ylY0@9moOBLAxEwFP=ATf-7{!@COiwr z|2ijXK1;*6`&8XIgHejPa1cY=DZQg!`n}5#hsgIX-GGN1BAPwX8%W7kW77xWWDAS| zFDn`VuCtr$)QQEyrxp{y1AU#vWX>7!eZC1!6`+wbg{yoEl##j)Z7uc@?6CP}u(PlW z@dPgj8dVrM&KJ|SQdxDvVM9BFZM(a&ahq}@BUUn=IW#eZ_Nsbhw4bC2KC#9Q_81;q zcya=dj|ec9d%cIzFwx1Ca!}r_Cl_N&*kq>UG>P0}G79qw7Bh)Gwxs{%G+X*(+UZT| zK>v{^q9|lCSpr~%WtbB%6`&0s4aDR<+ND|giAk9xvsB#K507P_{sU38LXHCK=DH2dds zEg1RMM{|ndtJv^{IvUnXAl&Z@oe9r5`7?1X>R|>TH(J0xUJ1Zlvj?9eg&Wa2cpHGX zdw_x{`vqWVi~r|*Tt!KfBCDLwJ3-p=Z*H%=SmRRIkc6$nE-F*?_Zu&^%3X1~j?mBG zR$^Msc5_H71>e9htDK!Zi&!IUVRe3zMzTVM)>0G^|0T4dn?T*tmtd9_`%g;^WkU*h zaEnFGwilKCoJVz%pP~@Ld@Q#o!-=I`vQD>$*YV_h!ki@69~RMq<${G%8vSpZ-Nv~7>Q=E?<>ljCCHwLXi)fb1g z6v`>WB#A`~<--O)ksbujPNXK3-Mqw5exiCbuS>6WJnycGwe{Iszhs)Te|XXN^7Lq8N5iAK^?ywm&#fUdKn;g<2%|G=J9iY z)As<*#Fjexu6Nzql~>lp_IXat%KFWiAP5AgKu$>LB+QQ>Tt+~Uhhn}5iYhPPD}z$4 zg?Jc#h>Q~Za|M43a!xQoJPu79NKl+O5JbUb>Q6TxsUJYXgm3NEE6?=H&cOt&Wp(E= zheu`oGM#H&SaIE7Oc|_VBD@R-_)d^9M#D6-JBFF>+Cu6bWztSG*=ML!yT^Efz;Hrf zwA+pfdebeT2b3HmcmMLL_yd+ITs(0|q)%`f(_U+u7SM6JkCFg75{R zR;hoe*kn-akjppCVCd(x@dqJ)7e>FU6lQJF%fyQzoQsMuP*MiVQjaxRW*j))ud=EA zbT%5@aU{mB^-%1peO{Y1Bx8PnfH28%Ro7*Cd{KzskIa#H@_f2Q4V0U2mf_Z94l{|Z zW#Vg>lZQJTI;HM*Zy?Oiv)aeS%V{9cN`kAovyho{W`(iG}~7D?Eis{ zuKsj)Xa*Y~YI{(Lx-Q(%h7kvDm=RH8iP?}f%%Y3{Y+s&A8;rS>}BE^Qv_6^AWrdJ_&D;Aq@$wlpW|@ZoZ7Gir0lhDk>9 zj{Wq67I<^OV|!&#B^5tNO-`Qca7*R#j1p*Ml>CinifC+7i90FI0^#f>!>N}W&k@4I z4Vqh~FFCB;k!aYj6nHT0CwXsM{;dL~3Rn0WNI6%igCFJKR_k$(ExHfmz65I$tJ@f4;3UPMq#~|06Zo zo6jT-D1E*2=jfW0g7`-o@Dvh3?QG;{&5QBEl98f~xN9z)irR4->|6ci$Ch{X5;W&2%g1l5K76ss8_KznFqydvXuEODx_*LvM zDqs<8nbu5ZDDQ8e<8X4+6mH=4PhP(SZG4g(SEQ7XU6M01pX> z8zIzA*v^28poKn*OJNu75jv<}8=W{vSVy zhiExkz3D7wwOql2excbdSs<84`h#268DH{^W^A#GK^qK%S0ld#Brmsmb?Jd8te9z6u9wV5=B}Tsf)ku1{m)&`H3!{1h~!@*9D93o0`f$>*%JsSnt-cT< zP0|>I0>;IFPisyI`|238kF_>8x~%x7sg@EA-_`~!_t75Ae;~MS@Q1Yp|x66Dt z))*jmXW0Tm)ES}*;fuVt-FQViP)MWpx-&PW=H-`C96YhUApQdFYN%`0CzKdqda=M? zXnsEhd36*_MnDL*0p2^9w&l`THWf2OWbHqA>LxS;E&BNSnz{IEkzVk!O;5eItNRSb zhFiGLJMM{xlhibRpPsd@zD!WRC%mw9*hMiB+_8up> z5N>ysK3tz1F4S)cvo!8;o9H%T{&-z>-l#+#JcH|hq~VC~2HYNgm?Gvke-5Vpd{HqX z#&G_>4+x3tG5A-TQ{Osi`yBj2?DxGdXle{Fe{ZAm$ z)dcH!oW+05fJnZ=4Nf&@#EqE7yzY^_=GH&Uqp9XJjP)k;^pe|15M9HP7q z&9W2}3V_3qBAf8iT%oP6)|A(M zgpTd4%Q5{A1&{{E1pQhFGaFc>zy7lwf22#irFu&X zbq&`Y?N3fzfE^tepq#-v9DGsVs4a`dr5cFd~0K@z`kMAy=iz9+U$cER)d~-C{W2~mUOvyQWMrpXM z;Pi6y(q|_C=x~+GI6b|St=YQV#xa7+#>S%EvNR*+UVnoJObS#!88*|+sVOAhYoDAT zYXt!Qbb90;-AIO}9WlCfkA_*8>DNksG&EEnmX!n)Xnr@LS0=3_Gp(!7Hlh-mn;*8y zixH3>Wly2XfJjiF^&0=`YVrk7bd{9(O?wpQnyx^`p9`z^$(s4X|Ni!nz<N}06}0dLp6R!r}PrBXLD&+1uP&89du-9c-eIH z^;Pj;*w6Fl`OsxMERiC6Ll$2f_bIb2GJSdSh>6ClGy-dQ04V;h`lHAP_HDr@IvqNr z<;%TRjJ>rs2~f782|0}7e!?{Kv>Z;qnS!eIdBjMA$^M+v`+YaT>a;pfH>R4h;<&(Jm&s+Wy>>Wb zV;jZpKHXCWq3eX%i)u**HyZ9yY6eZauMi1-0p`h9#6_%p6uZWIjlQA$9QHhs$jpC# z=Dbj3$RneKm8lAIC8Llwr5x^ZvIX+@(ojZ*@^+O9dspd0t@_W_)i^Q z+%VbYi9TNf$POrw0pb4Q18x}{-f(Cbf%>j$gWU@G5)#0&(Bc}WcOt2aWu#qE$RG~V zzfUdy<^ud7cP{r6owN|tJ#f1he|hR%XsyoikRpJoLoCvx!Qy^~J&L>zPVq%5xL6B} z;+pT-aX0((%~Ns?#{ZK4Jq&#-dd&`k86KAz2X9>i{&$>TuDkkXKO6+c0^1M3n!&-b zTEMU>M#Y(1-q^9j!(i?C+EDJ??xYT$$bMf1r|Tl` z<*2ip#oaqD5$Bhe9fee0N6?|u? zbCwW;>!F94>d_Q|^>=Q15#6`zT|A0`z%>AO(}sT=oz*a% zqUI{56*{d}i`~TcRGWII5@|EvcOb-(!ENaK&6~{o6oPd9Z#6zs1pn`7IBnJKuPm=} zG^32K3O;vfKB)E(##7GSfC9{uqm|RMzQONbK3(?ysL4J&VaQ|`*=&U@$E(>JKG0fT z@47|5*kH+IGirRy%DVuew01XXwUoxPZ8HKf^Ow*K7qS&v!NF*$`g*p@U-%kryP3WZ zx|E?Q7qXoI3KPCATw3gIpAURo<_CkIf0p~vKg-PuyfS8) zu&^*6Ii`P(W&hh)Rjl|ww+N?7{<c!^bP95CEGjf_WjK1!=W`a`Y&@I zzT9s+mXU_bSP$ea{$?+1=&7G}4o+2H={BNh!r_eQ@wPRz%kMT({qqW;;?;}?uCOul z`dB>E<{oYRuH7O%i8vo{IwKG|YtM!2^ANwvz)O5|~ml1n~vBdBI z-?3j=)P{Pz1eJXC)ytk-^jRI9kZKp>HKen<^U%3W9y+r z)K1Cx^!25rxS5!_;Dg+JepUJLxir}g)_tjT=K~S+zHDHq!Rr85IZ%|~v)}Jrwk?QS z=P1uI(K`i?Cv;Cwhp{1DvU~{YnQibrLYISb%YO3-F-APsc%rCZ)we)t58 z%}~1mn+H1tiYiHOu=;$fM<~jFNB@Rj&I-Syako#~_^T?i`$%W`I#4#7;)S!3^~vM8 zrv=UD_Cf>gSJZv}nx|0Iov)@rnxgrO}_K9=3+R^YkzG%U?^@nz& z4CN$&0P=Pu0GXdVBdq7~0%^W0dqGlA^DI88puf`yxqNLih~Mpz~)dSKvo=AN{idBoelTvb*qeO0H2Rva!M& zUK8kVce`e;*MgG`%FOY&IxCB{j(V%J4P#oc9+pZ=@8OISa_`+lNhyx)o+pY|O1o08 zIC;D*SWLf{lbH%b3*v`GK0V0^PByPKb!e$*zuN&H5Oa+1h89(E`Q9jAM?c)PR9Ru1 zidBv$XI{24S`L)d>uAWDA8pnJsZ8d;;p?rftkXJS7fK!uk82lMAR1 zC3y@m_X_iucKUnTCfP62m6CMrFAtDp&gKJibvVzs{a13v=RYVpwo5^@T$&%LLtcMx zyDhQ)Q7B)U(W@9dg6|$mG&jaZ4}xA5vz5G5{E`pinH*4vr#@_jReVu9Uwc@zJhcfo zdf&x&G<8w8JXE6fd9-&4ua-^-`9Y#&laF}E9gZqG8uvi!OXUK(0l`0mz~{~fC9=;9 zvZca9GI9L0AiKAeWe~h;3FP6IyE#F!de{5~87Q}3w#OoGY zBUE>bT|ZT9xu|f5`P;8)#uBAE9;Lr^8P(6WQyRSP7Mqh5+ou#$8<~z?8;1|q99R%wU!g;buR?bB9C_l>0fIj}G&H4=2_6s-`|@ zaBor>^({*JBTxpzs14lL^=15lpH=jYKZUGnlZan#ad9Ic5r4pM$N+6_+(D!6zR8E{ z`b$vq1ZU`^%>)aCa%RWddc~O7&ML$9S99*2k$fflic+&kNoWG(%PpwDGY!AdIo~&j zmLOa@oy{bWA~wd`*0RylhW8z`oIEfbuQx6d<|#+QH~v+BitpNOZ=FTv&fMFsnk|ZO zH!`wJgs-zw`s$LeNjltN|5pQZ@G4{8QahcW2Mv_tGVdTnB|)HHQQIucy^13LO!=o1 zUNt#7L>WvM%*}Jb)&qztdfcF82-C{?L`|D~B*as}Pr=$C`-dUHtwejum7(Lj#-@u( z4Z5_6t%cwIc9h7p;U>K)WF?)dp3jaI+*;3d1msA(I>VHCY^O<|0=sagr!2rv! z*CORln@rUdL7}w8raqL5;<=J}HbHZ%hQr=z4hmZ@4)^3^ zD0=)l>-admt}GUd;h0dSJZpsqu0C%SHk__LRg=z)!zEV&!veOQAk4O@cay1-IL0|I zkLStu@&qMZc&J34Mw8CNG`g6WrrpS+N?g3O^UCwuKM1_q8ml}n@-)F(H``qC5crCU zp!RrmXb2X_XIo>VWwQ?|&w|Kn+O=#-M~zpid+~wCP;-1S$l~@%C23Y;A9hZ%+k`-7 zc_h2_JscwWbMHQvV?B*&YJB81A7QiR!NGGWN_IFu?-sL^2~XE1sPS~0Xa+l(&#H7H z#IOtppYt;_w_2{2>5DWk*%5W``t@ZWs@q-`o6BBbp_I|lA>m`ajoq|XWZ)^dy)wqE z@sPgJ;lbX;85v?}y#3dEGXs-)aIK?l!!v@M1DnC)l|haXyo&8aFcIc#>$BI5o!s}L z^Ywn_0C(t2?+Y&p?66Fs{xi08PE(Qo*Kv7yuNXn@)5@YaUv^Mr=XFjcx}4Jia!36M z5Xvi#!kh&EZm^4OuHk<+7dTy@?cu-$o=7%{bZVi8L5oE zie?kS!91~Zig&xG1%`smv>34oAXOJnzt&OIr>Y=$Jk1QhiVG@o@$9fEcmCy9$BfB$ z)ZzFhz5Qr>hR$-jU#~z-)*LRXr*yQ{S$?Zz5`fjwDNl~1jC4mk#m(vLsWb9HocpZ1 z1}d~rcL}#Ho@M=A%7w+K&QgxHKrZ-P(fh;j`cK4PE_H4iHP`#iH6Q*!&YiVtKD6i@ zsiP?_-S7j8z$sr_b?HyXxtD>M!#*OSLoOXC`Cv4~H4cON;Ec%h+usCf79aRr806;IuS?uz&CkRGpX?2);r%yn2Tl{4Ki z(V7?$9#ieyp+bDsR3*5T7^L0KbHo~cE7RhRS`Y*}8^UOI0*`;f?+>b2#6VQCU!bf` zhg&9l&wp5VS`N-g^^c|MtgaB%NM7bUm3_v|&th#im-VbU^3Rtgg_eFSv!{DhxjYI-C z+1bqvI-Ie&XqSNS;)21$0p#4C_*yq}lHJfTo;p)#?YA~6ATBfW_tp*ZGg z@%;6ez{vRT7dJf|sVrUy7-+GBfO+fFghW9Ut@1u3^v}S8Y#M@yn)Acp8EC-K2M9D{ zO5coVrqd_Q{H62UV9?TW9sXgBts58!fy%Wg1{Y^M6oeqqpccn>TamG8G5bW{qTjX; zQ47GaIrElR?1lCJ%UrzDJXozE+A+k}zanB4&Q_|Rrs`x%4OYXuFS-E5YTX?sJO+_B z51_s2axnL@J+`m};X@Cec^+svdZM>vsrTlAk&w9qPNTC4ngm2m&9~eITpYA>Ys-O> zFh@jExCB6>J}y;L8d>~r;^v3ws(SK8a^=sCTm*HM5#jSph1ej*c{^4sNDVxqhL77X zN1Ec>GS#zf(6t{=uQv4utN9{Div7wFd8|oHoZwsJtIOcBG+cmA(eLF`ykMwwqlNFd zI%nU##Mp#T3G}Lo4|P{PBl^DZ%iD_53iq}61BurE3W&=o&z6R79B`wGI(b!Ke#&=R46%jZp?Z{DHy6;b8^*q3D!l78@~S;M*VGev~D!K)-TEN~75WeoA{!nkdQx z>;}o;U-BDlfWJ&nw5oO5GHl^~04H7O26)Z)5sm{8^j0q-M4lmN^{z?Y*kHP#Uu&!V z&nUVd;~*uvFHqqgwX)F&2*C!z`v}vCj+pbltzbmT3Dma_^n^yJeLrtslOSIM<~v@N z)n&kb{jlNKPzTpl?<3`Sy1E9?`>Vq>fa#mdlwVd7o49-iYZK~Y3LG&INCPJg2!u!+ z?s9Hf03Hwa?jq0VN|zIs1KbG2A1frV3r|RVn=dfkmn>iE)LXr3m6_4vo8AP&n?)UH z!8WOZ@&JRJ^>@UqH+D!yw_!GT!SVnVfE*YR`;T+3xXq}##IkW06Deou+qWa!&=)(( zFD?&`r-~N@*~(YAb@Z#@j;`iz6tilZkn@C2UZsGmvIznDFm59wG+^>}0fjnyhKQR= z)8MJA>I!jPZlFxjkI)y)bb3>9Ba&r6EJGAdo@XC7Q&$Wy8g0J>xye)h^z=B1(0TpJcI2{93b}gem~~;v>ffZZ5uT7oNWVN z%vgH<6~=c(1qOor>bT_&j`76$JXiImBg&TTO|=AJj$$y|Q{(2<188HuX)!n0rroFH z3JxW;zljGJ{|AXTyxO$3PudsWE}K)N`t{S$R|rT?nj=y-+{&k3?V>>fO7#ASCD%Vv zQL=leNeh&mGrINOwFe0B`322Pj>f)`XJOLPdXZES_+ARI_sug&36LRXL{1J&ZNz`y>9lGXy z!rpVXAHB~Vk1C0b!twFvsy|KXAs2;ejn}=-NTX)HaxBR7k@XD3=*nNU zFQwl`BsN2Yh#?+n?Z9VmEyaa&4n7-^0f!z5Bz*e!(cCYMW>F<);ef7`1cI#tu!cmX zsK!$&w#bv-R|{g&J9v_WuANAdLZBh}0(Bgkz#h|wtLMBd(D1U+&51u?y$OHSnR2c0 z1c0phCI;31*m>m0&}x_ovwgbhyf!gxOe)VKf@E|}K8W+dzScXkhD zjpe1eF4b6;fqjT^7pY2)*g@=6WR5H0wXpaIl~lov(Q%?$Vs3eJWWM zJ-kroLv(Hq`%sNKDQYjzj15A_9Qo%n$I*E{?D2P(S7mJG%%*1RX05I97}@i@m}-bu zG41}5bN#E7r0!YX5JRnR!3S0Mscr!8WA^?LP`m_K_t}+5^HQ~AzbyEl8@<R@=2Xe;>;5%P z;&xB(A+)eEt%GJ@{}M=VUt8+6ktk^0{ZBuvVD7F!bz=)<&m9_8`+L27eWq56dkJsG z2ExtFbSA=8`;-vF)>XTo!o4qf^ADA3T+ZyA3rqr&5plkw|^n0r3aaO>k1F57tKuqDIv=0o4bsl zuho?d#3n&GI>=q91jAw}hxCd=UMK1DqIbTcI>AGR2(SvIP>TSY_6p$GZzm+p0ikGT z)!(0DrfQInP(dl~r2Bxz0*?`h3S&;KXUW2oT?BK@FkvAmqN%ak8=sHj*8R#)Ec_x*bcS zdHFc4GDE2l(Ki93?FY|&)XbF(0lR6-r?D8d-!5BFM(Hac23XXUgtiSdKo3c@cHJzd$vNKEg+(!V?!TBHvF3Z2( z0q-7M3gG+;4|@*G9U#_XCyWkl_c0`$Kscj+iv~N+tq!9ow5X-pD`5MRJd-Of^PU|~ zadj`C4R%tFQPJ+e4ol+0aMpT5RgIl+>UVY^@_B-hX=Mr8rgHAo3Fyo2rHObZFN8L} zJK{Nah{4s^^Gwx5roh^l5I_x%z?`3;*7_qW=P;Y-{?K7I8b|Q25y{%D;brg!p z!@DP{3Ab7(hpda(^Xzs`%P^T5Oa1esD}eJYa;?*CeM#%H^n=e0Vux1_T-LWYq)v3F z(6YktkGP}H)E_;Pzl6BvzM82Mj+$9U-N z)DgkHbOygmz1}ll$(}1UQWfMA>|xp7qb;usU^;z6=;^%&iqM zrZ~JFmqpc;!@R>YS-|vnKhE|{7;sE^!dcmRYpa?s3!SS_HsQ41Suk`r4QyltHyZZ*U7VvC32WvDHhz`kBi9db6LZtj^>lc!-GKjyGc*{Swq^BW_ytv|s}_-N z)#iz%sX{``^-axMy&5s@6ZM|djW@>OIX4P_)HX?Pu-m%rM)dj(@gGwVh2Q=zM?|Of zrZe@wX9y09=7P`ZV-rLfT_Bqk8**YQ<6Ug-BWP3WU`C6Wrtc7v|4$J&gb`=>&*U$$k%ITf=s7^FB zkjc;I->E zC)E5sUWkjj=&?4#KVv_X>0Tgnm|uAwPTv8;-(-3}N^`~2`5=+;ub@AxtlcPz(vE5s zN5Ot8v`z%*@(ttJvz$~aDjwsGkqRB(MoKA6?OvtI)+%~2Z92QZEm%x_RnwVDApRza ziFkXJRgK`L1@vyq%QW_S++r z4-0+3)vP&KsE0-SpV`K8rN4Uh<;NVlwz@eiUr$Oujsyb%08r(A3kxVYv((gcHoF=p;J~5q{+d=Pb652X0 z(jAAYR2WijWXZ&DT-+*fonbyY=wHI#C;v0EeegSTDf8$p^Yvk)zoF1XDJ7dZL6&DB z_)*ByY^KEg2G+2g&86;d^by-=Uw;Nu6Y_cbX6bC(EC$k`$56FFv>@3Ttzr~a2>bPG zRwVRofz0t>Y)h7>_ZFcJMYfAe0*gj!?H?#`X88PO8Oi6^;7I0Dh!V`lI~@(mzdrwc zOmZ0Ci02VA>8x|{DIJY`E(FQOhbn0OFuq|~JNn;6B0K*ciR?*bMvvEIrH!d@(z$$7 z=2t}hK=;+etT&I>-T&6xSp~(xZCMz1CrIPaSaA2o-2w>&Y23Zhpur(n(4dVIG!PO9 z-jD=ug8sM%NN8L;SR=z%8pRPp8he*4lu42UZi1Z58L9+L6En zquH0N2@N+|IbX&i({V_7lS9mZ%HHu^G$5~Tcj&y!nCHJnhJQG&t#j`D~^%CoY%|0PRx+xS5@s40e^TY^Z-hyf+LEmT9 z98Q-uh%O{V>%E=;qd~RQEJn5r-JjC~9SaFsPdi+|IVRwo7mz)6C8=zDnkR0ObeJjr z^BO+N&uh1OjBBeUQ?h=M_jei^rX(e8P$oAW(sBJQ+9!T#sAwQ^wK5#LzxEDt(EaLs z0iPIz5e?X}oHNk_^}PF~t3FBZr?GQO%fzU!P{a~j{JHVSc1kZmb0+wDdL8^Mjma+$ zY_TZCGiqtV$%W_EeY<0Pe_SKH;$6!F0M~gP1#Gdq;Ndwxs{AYHklUkKk6JvknUH*! zi>YkKoVUNuWs@O)F|&#xPe@A}p-~N0#pMUy;o$a&=a_C-+$+(GpMw1)2rivhohi&h zvQl_HK;Qp1HDfF_=$f9Nf6IfeEOK`LP+VG>*2VaaPMs5SL8`y3WgQVOUO&fXZrFmW3m$WeHgQ-;_NwaLX~`LsZNh>< zu9K~Zl;$SN02u+`G1SU`dhN%qtK^*EHBxM?A^_2s{4qV}br^@a%6Sbj)s4YuOCM&9 z=M^2Pj6ntXS})%_zZnbiwT;l^XJY8uLLyg4c3I<>0gTOty8)n0*{*FsV9I`HC% zq3XG(T-P6pre|DRLvO*KLl>J}N;y9sE~czvQ#BF}Ovb(OxxMnaZE~b2NPLm^XO8c^ zDbfYx$o#k%Gycnq^V0;gpChS)LXsWP&or)nejWe}kMu-GBlCCaas7)Fh8AJgK76i( z5=U3$(_J^pnSwUUIrf3s_nT*XfV|h`Pfqp{Tos(z&h1FTL%I^P@EMJ3M7<~loPL+H zC;%Ws)iMWQ;}CPhA+bwNo6FBzQsK8`P70f8%YmnS3zw&0({QDz&Nb6|h1}%AQ3u_w zb==K8blnKZ;#lbIl761gt%-w!$B>S&+oUf6uL;8|Zqb|xM&s8h>xD_8L*qY33xsQU zskTmXtv82~yf8dz!g31Y4t~T9TitS}{C?XLz#v{Nv_H0}`81nM#+y|i@rwm9BZnl- zF$nRc=A6*`P@2^?+@9aub?eqI_mx^Bjd|!4sB>zh8mIeg3HNj{Ke;_WAJAd&OV_~xeY-Si zallvz8z<5XFkD~7OztI;8qGlD69gjTz}^1E4a?09E4GSy(1Qo-De%t!#!?zDSQ?+1G$jT;jT>dV zkr3uBlWLC66UW{DGLA#w24717tVnG;|AV-U1|xHqoNvSkO`6g9ufsO=hItiY!sof& zk*O`P4FFlRc?-S$!JZs93jra)aqoo9e?=+>v@vz$<{*ELM^{pI@f!IE_Q7Hn4zE!{ zSP=ITN-MIADPC?3hmI@`^=c~t^us-WieC3!0v(HPO)lW+I1FJBq6- zx%?67e5g9%W<`56GSul(OPfhwII{Xrx)@Jyp@kCbbRs^xFQ7Q+O?WSvxRpY!n=w9$!8ecIG@xIcOcv3?)+by^W!e;TG z+zq#`eqVLyz<#utDt^AEib`pls{Qdb!$RH#>p-tm)-5AO#3S6y_94yK(r-w#%dN1> z__+hr7ayO83)}C%@hW&D7qAp_JnI_jd=!cq!p^&d>q;f?($TskJ<$>q`}(Iu&nhcfo(N(P z>z1*YZ`~1noE0Rw?-X|0oS7**Z<8&J$C>daWz-JMQBqS|Lcoc<=g(-(!L?31ZkbgW z+1|i<3`xMpQroWGE94O3B$z8O53~E*d%|I($n>UxN^Vig>)u$FW~}U4rJ`O#038-q zercv?Mn)x?xF-#_8BIomiwy}f=Di5(Fj4))aJ~05D{$XVBK9d>m0Ks=r;autCa8Z0 z%d{}T%$j|R!1HZKNC#T^-%bWfn^TF=qEL(hgY@oTl9%t{kunT=P%K5XlOquf%wD7P z7tR|Ei>~RjQ?zn0eul4t)#|EYz#sxe_*h))m6`_@ErOa`FPTPqsi3Nrv3UenWI*3bIebbK@!rJ7@ig z!olb*CxNKocSMi#>ALt=7XH&$mkQBg~w9r*=o@BD7A-{cxfPM=@mwU`5_Sh>kQPE69ZpFIqx+-H1OB=Mm2X;V-uqG7wF_89A31PS`FNO zqaje23%_c4H;hYDf{y)cQ#u|jRa$l>MLV104!O|PYNw42_-U&M%g)>d5bC!MNFwp} zB_t(?B;)A>j1isuWms9)hX`D>`;gR@Vp~&Je~^IaXyGm@@fUh0t$w#~m@JlK^gE13 z<_2rtCK9TOZXj~`JDO0$3|0)p6h{ipJKZgq&Yey_SDZZT0qACH?ZfjA4!9Oa(ve7E z=#IU?3lWm-r1H8hN6N#tgJXBU_Bt5YSKC)O4YvGF2A#kYZNF`|A9rcKx&%vkKBJ`A zM3Vzbh`Py^4g>*csg*0%`z+E0&wDqch>Vvr702< z{xcNF(o6Qke;M~ONegL!sc_&}rAlmc;;XG2V$vCsC3m8!gmt{#P_=$TpSpIgU{X~3? zxyE*QODE5GE@cxF@;MQTOHT}i8$(UbvdV~ekJbwD;p12stD_Ktgvb-M>Rs^XS9{+6 zHO|qkI}`%hL8VTgaQT{K&GptI|4hf1Sz1C&8=ojqGQ6=8;IJY?h~30!rAaJb zvnz^}P58x-A02F!$TMZlac=(b0ZmHH5W+HJ!UpJD=pARBH{LID_VaOzVqf1gwR73${(N0SJ#U; zHF@K7%~I*K8Z7&!H)Xe#0U`x4E{FplyA7PyaAr{R2G1PqNTM~F5gRf46gspDC-gst~*;L=U+!2UzIs53{tX0 zXj$>&8Kyl|qlgP2(Vqi9?c~KAi^Xj-QPNJ$`2AE;=25c$d?}&Nst@R%``N`K{j}Y& ze)(n0>eORE=GY34kuT!orRyTZNo@%u9)$uVKGP5t4)Y5nj(PpHk)+%_MDnJcRiF^m z2Z^YiT6fi0Hbo&JzJA8oK;!%T7B59Fb%b(_HuPfG+;oHu;nn8s*jcfg4-Gv+Hcc@Lw zur(!tqR&1q&eCde10J%IqXV-Ow)=HMuppU53% zXSaL%7VE@{k8$`LqR_Ef?gPl5LuwJFgHmJal7;HN?Qm#JiGhQ60ttX1l?ij7<0Bi zvKsHOxTl8;j5>0UbjIJP-$)<(ZUhcRory${=xni(%V!Ac6Y$TC8`hIc2g@xheC@pe z2V5m=ze*nay`s#9qw>df@B@%>XtY*>p(^mq`+Xl7MxL`JJdR9xlmy*b9|QtzB&p!B zZesx?Re&}=zA{C&1b^2!k98l!MiPUF6CcXxMpcc&Y7T{txE?$WrsySuyFq2GJ=J!g!w_b<5l zvQinTC&{WLH8ba|FnL)qcvx&$5D*Y}32|XX5D?G_5D>7jZ%|()HUo;}Uq28|f)dK# zzLNJhm1`96og9U&PnYbN#e2V*nfTHY$a1=&G{sH*$l_v{<^K%3LDni>=$OS9Pr3}yA3xv%G|H{OF|7rj8D;CydkHk-1Lw{$Iqv<)$6>1-zEA z%!x#1TXZ{CO4;Hf$T}YX+RMr|uh@CiWu{X#^vgE)K_-HHEk5>2p}b-O6w%$wm>YXu ziSXPt4*NpfgQh?0=GrAl0$HkKs&+SMKJf=Uod)MEpY~Pzs>7W#oP_>h1?ji<7a=o; z?u6wZ$ijnY6L5b-aX$@l>qb`17BV1%_T6CUUZG*R7;R6 zq&2&b*TcH@W|a1GYr7D1`cBTonHJb>`!>&9LMb_6>?ITXee_tZiYnW-Ib~nVnzQJk z()8t6Kg;8h4ByXO=B>zS+mXjokCdg|3SsXt?Q@i~Jn@t#owef==)$dUfur8YmPsK7 zm*a!iDMpNhC76CQA71F^#*DVlxBq}&M0&ldh4<76kAZAOl=itv!-mh@`}}+NOoKHk zj$|`8&ToK>?0$-7!%ig(?5}G%J`q~kE$n7FUpAI?P@f3FRJ|n?ER>f4b9&7yMy}?d z0Gvk=SrV~#wFVCi?FWlbAs+Gj3)>~?tFQxcV0(2cZSylmj^HeB&mht0mpZKh1}2WHcQ;Sl)9bJd^%OsQGCITxuz}V zH;}J8wv0xPF?(T+*>yJyM5>EKf8Bcm3oTM^9`%F3XBIofG(P3ABRNBUB-qQ`kciRD~&U$s_7lXRrd)?*b7vX;&^exuv;EVk(oVx%tCfkA?;)uh-w{csO_ z(6iav!87!H>5+w4$ueF-#v{#|cCj%}bDfPT(WhSkfDCH#mSc<99}Ee(x1hsmqWH~D z>A#FD4%v+@O+4HoW}s5wY({sh-r=$7oQ95iEcmS_VF!WR*LfhRmNMFX)W(!8BJzXX zzAWSkI^%n<)238p{Z2L?(qcw1^X1h(!_!UQ@*ja)iTAnGrL#4>8+D;dnuKP5)DK@c z?WgzKFenH3@NKdq^w!P%Mtabjwab84N!ZOJ%}3Er#iZf*5W5Bb$gGl05b8BmZW1|Yr{((+zyT#cGKp3zbR|`CmV;p20zVTBD}LsLD9}fhS!IY+iF{#!XMB1LnTno|bZ`3qFz1)4pM8R8T#yb+gQ0?R}8EddKOlw{g6m-H1 zQJF46-zdNM1UlrFn>v;{B6IZXQ4HGY>1oCM0siGJBnh2Gy^*Kjq1ily@Y{u5+|SIp zhLwW-xiOM6PGTc$sBn%ms^iyPZjE;R_Vz`>PIYgqkt?6*u}9n_0&K`Ij{MekUdvz_ z&31lYJZ2R(9Ta4Tkv|0qCJXWNwZ7+uSetF(PpKpYi)oQY$((sn?wVG^>=Z7@$P!vp z3Si|b#zzLgPypHWk?nGeQ_eVCKX>ss~i7je(XDRwd(Wzvh zW#fY)4u{-2N+n}Clr^QfiRh4#7m2WLeyg-w{-t2{)%z)RAQvAX0~^|Z?(poLtt6aL zYo;y(b5lhL&dz1~DbM*hy4djES7!{3#b_2)uE(V6$$LP(kn3aOa}sp}&PAWdvPhM> z&MLY_l9ePO#yr{z(8XRYS9H3188fQU+M#Q`xWcfnnBZY5&QR&m(;+{O?|n~nUViqI z3k6pAvz&eAjcK&2AIRX7I6W9A+PgdoKAUVXX|(w^jD0pnB)T?9^vgYjGPg~R_g~K} znIXS}Q`V$}bO62Q) zyQNWJQ~^zrnW`dzL8EFTGc8WWy{TSjow;w&>*xSHzjmr1nQsH_Gd3D8cx+O@dmq>c z7@0AV*3n!iy12476LT8^#+)#>x-duCPUQBnB;F*x%+@|ROS%sz;kt!5JGs-`LAgY; zH~2u)q!%L;VW3vi*VEG??~u|E=_UM_lmX_ymTHmud?I7K zMa%4U>v{!7*Ih$LK`e(f=h$D$mj-qnU79N46tYRF2vQN@nZ^4!N~e@|28vOy3sRf7 zc1+8p!4O=){B&bRek$?h)^Y@tkwKBikp%` z6U4-5U4c)IhT%(jQsQY6+0Y(3s3F})rM1J*c)k)kS%Uo?GrANzBZ_I$#0XR^-5TJ(XNOz6bs(;qdR-2cJaM> z%E)DUT&X_yypMVcAHQq~O6Th%y}k;1t&fYIc4P7YjEYaG2+EJ_{vF@Wv#SEJwnqkB z&RT6qWk_-iO<>dc??~h6PR&1ll;D~`Ot;v72sUa2Kz)|BzX*zq9g*5>bY&kuDzT@K z{8S$%8y_DRF2KA<|R!VObt!82SeSGJ&hi>evJC5<#c_5fh`%7vz-?Y=QzCNx|1 zb1uEy+|SYT1};FOKH;Pat*me?J+DCjaR&{&gcK3&V94t5He;rTNih5RppVF`Jb3KL zM1_|)-!%{Xn|12oU=*=k1qshHDFr>Z<>T>>{Yxh;$5=)^BG?qx4{46xQj<3EDg~%f z9oV7P#_%<(KR+UVP=j#NVdQ93tF!P=-E--nvzDt$iRMh*rw8&^o-EL)SERbMI76t3 z;xKG|s(ZCAz>PZgF8aIedoLH_TA@>8$+-SG1zTxADs&pu84AOtQ6y8BAwaNxD<9@9q39m7H^`|8qJ)BY`uX?=(cqiGoypH~BCOO{C@5Dp(P^dbU}q z*Uf66)Y)4*JlEkFWi(5RBBy%8QLa3EOrN0 z1QzZQ)!dY0`VTzL8HaDNzM)vm?7RHE72+_&;|52Fxwb<1l0by+<`;kj3@6!2r_d=S zJIf5fixkvLST~Ym89aggFFq8&FL}LfXS!@p#EH{Bb&SI^l4$q)uYlNEli|~}o*5OO zdu4#y0DZz@owE<@N5g3Brg@iqEh$kY7L7)#a5|I*$*+X>^rkr63YAT2CGuiau( zL)&s(e#F{2qpH#$$Ha%$t~3)Zi83v!-S3H{wT{Q=cSDtlH_v4p$)v*BJrlbD zcNKftGF9Alw2)d%o{HbSvn*u?3;+D4c~7C}xa{yz0deA)W{O$mTJi=AZ2zE}kketq~Ap^Y=uk6o?dUyfrK3 z89sinAS~FQy@zvdgF-+6a~F3=`*h=N?O4gS=yjp-v^I+b?R9U0legwK>4^)$>dzOx z@r}RvpEO<>$NZhUS+vK;9d%;-cnX5pJv|UGfA*!38#NFb-!IWw$M{8B*X*Tmvo|Tl zKUDYTd3>SKkR}EK>I$817HQUd%rX|!A-TC?A?;rB<5EVuZ=}DIp<0IGuatKrFOQWz zIE{*K!DGU3y~v^as+3986`r!MPtymog#2gB%BXC7utXf$eKYkah3 zj9TM@ee`mrqw$m5y;Lg-B-uSxBz6~VWsa`rS$y?s0!5g8w<7c!H`&4hh??>B#-8HS zih`#e8Z`PgFU1SGfgpZSzg3h{TTZzvGtP?Cz;*o^01myHv+bWTV;Zf21-BH;7SAwn z(`2F3jZC&zF>fD(s*N_QJs)-K5BCd`oSE=`8T=!zNUCaojwhJhHuwFRdGQaP^KEE^+Dsuc3=dK`U)Zihb ztj(D7GVmW;%8qM|Yj+E8|Dh#%$oUb_w}u``XqiYit)QTqV??a$<(9g| z#rctLB!0`+TO!?bDI@6@^p<`-6an`-U2mZm~YTv=CZX%1bYI3;I|?gm4r zDd#D>M5b1GuFW+i$Gc}s5e8zeO2N0cGxW2K zZZyjE$SPOU{M#aeG=a^5g5gt}-jf3#NJWMft?Gnjg;`YQ{GgPRGC<*RjgM}aLM1E`VG&qRocuiGJN8e zrmq*R;Sel2r|Y`u99M>xvLP6zh)47_6S=Xl)tN|tm-noY#c=qb`czvcPBR4#LSM!!BTr9$5Lf(y4=gEeuNx+s$Rv=&avI=nAh7OC zQvlA!F@{*ZI38zBQkq{Vn(a+TLI${m04n9KPSA>!fOn&2`%iH~wHJAV{iQ_7KZW+I zrTScUv`TghVW9jhxXER4i5)Ej%a>{)Ca9e_#bZGkgPVYKOXUhw@=r2b1X42_Z zzQQ-)HVjKj=lQ@+;4!+Nfur&E9J-3xea(P02Zo0;abR;b_Ie|NGvC_a-4&Xcs{C9F zH@(o{n`Qr=JD#=!ZnTp@WU1Wlj;(og6kKs^wqi}kldF#4fTUa@6N@EgK&r9tCAyGs zf$Cj)^RN6d71_!QY^lY;-m5!U;&y)0n2xq&7SI;QRAbP?3mI#vA1qobnoUE%aYtR* zhtHZgVLa`@!5Pvi|H5S|9(`tn-}QMR9I1H?EDafzys6ztvPqs~H8!vDYA=)f&1uG> z{}!>ESL8H(d`kW=(}C5+?3sJLf`KXElh4UxMwjq5d+Y4f<^UT#JZVq_#w@?vHVVH} zo^lyJOXL_6bDQZO;Yj52eGj`~%iLh!0X4p{y_jVo508(k@)r-(-E`>NxvO~^dY<2t zV$nt$161B?w&=5L<7T_D_M)9jQ3ULOA*D;amnPZ(mtK3_4@Vw4QWC7g7qG=;+MA5- z{gKeff%XQE8XYq4y7oy-&7dwX?i8cwaH9BS>kX6`U5-l|_K?{|JL8a9$!TK9=D&{v z-_m?o<6=(#3hqvfmpB}Fp?^Ga2VFEWHUH-Ux@r-y*aSGkGJJ#b^Dxj&3nd+1!|3S) z$z3iRsTQI`KIFyHo~F@{J0Xb<8*LHdrL;k3kTl<|GG6T;Yx6%2a|F;iY0wOMaCjV8 z4s_1$?6rwvJ>u|Wr&E>06Pw9pjcT7zX*|GH605>9q90L}Rno*`2*C#)&`!#ZHXo+` zDI-ejh`h`2(^eD_ z76$GmgzPJiR6rBqOz@cuq1qbK&8Zw@c9>|hyTo`uCuXMzW+zx2I=B2S*Ep!v{^AMS ziklMetHlEvmG+*_ts<$u+V>P!p7HXYYeGi?Dl?EB! z8ni}uJvCk0bABst(!KoQ| zo|cnZ(C4MqANP{(48s^9W-macS(+&Pq9rVZ+JsHA7rNUVS$Rl>otEOAyH}&>Qs7#G z#AphWkxMa@Aq&}8?y#!)dokgcBHO4~61On80CRY{UNyuQQZtWC+_0Jz>7()ro{uN| zlyuWRWpsI5ym2npIDi0C z7Lmh9>cmjl^$Bk+PV7abh(FRE4-Xl2!Wj1VgQgQaE>j)%rlac&j){{m<;W;Yxmimw zMy=&Q;KeHSKQPuU6pFA_tXQ|7IfEEM)B3|jz^zS=oAfz0AFYK`?dkVsIO3h{h)2*U z=G@Pb|%`kKB6Nv>Z0w8gf5wWBm_)}lBC=i zV13}eaof>|w@b-oa< ze|i>~R}%#^Pxu~jU29T)ubpY2kT;_aNAGYSS|6rtS!L9fn!rB{D9Fr*0vG8N0-gIvccd;I3xc)p%0S;`Hr^3SnxQPHKQVor;f9koH!;SpE6fN(;L5&QDI`=XG2SFa%lylHTAGh`OU*%at z$r_!t?wkD~H=&XLrJuzzrYD?EQJ#eSo_>0boVWiP{>N}_YS{yWo^nmD1vzZ{U(Qj; z#o=VP2%`lS8WUdQLq9-c0sWb9j)G9#{O)TVSo6}o2p%!m8Y2!ICY7T;qP-z8Q1K8tz%vR6;VVBr@_=z`S)Yg&dXVSQ~_606Yno zQbjDEYXlBaf!FQpz7=>7r-Pnqujv%S58s4)1b)8mp)@wQg_B4?Avl6+r!G>0X;;OD zTZY4#p>c$x@R!WRs%3vm&tGtG$;eJuH(5?#ve0HzqC^+3FNb4VhCw8<%Cs#ks^O;h zo9j@z;I;0;niv>Q@8G?T=uCSKGwk*B$I6X7YvL$#!=>AL>ttiNeY%+&@_pUmJEib_ zDRL$Y{=R&W8?*x;Z`Y40f zZl49%&p;TSr=BOei!L_&u~?DeD-7&V-vtprJZcB6_ikqpz<2c~`FXo`SSH@=TB^v- zxYL52ICdD5-XEyNkL*b&IsE=X{iTq0>zz4ZGwN}OXxC=Of_>y?^NjqNM?G5QPHOim zy0X^CUM=z&tr}IDmbQI*X-!B@3FheGx09Lz-y*l_3KhleZ5jm|vYi9%Ngv7BBSLfMB!c4wGV5s?@a!j1;tdBce+K4-G||80@bx zSmD>qMl>Fbzd&=oFxzlTP)DQ3Jm-D#;E>?78=&VWaoDz+i^S^(4T;Jb3UnvOHSSMY zEcO#O>)lxl{hMu@+a1N7q^G|(d2LO7kAOq>Ht?In%zTwKv`yx`C_78AsED(+X2#2# zw+;%_?11^I!M5+jW9j*Q)_wEM^uRf$;Bex^yUCk@^_@i=tolpefDLB1hlXV+} zHk6YtFOZjqn({L=q_oE}>*%w?o2SgaSZUd#Wx+R|pM}maE8Tal2ADGsHSRCi(s+LQ zqN8u>m89DWGijsmYXA1JRnsaLym%rxb=!0rp3}+u;dv2ZH%<|^Lh+pv07tD?eEoNJ z*kJzSb_N+?y3syzHqbmYPos+bh|sKL;t~t3{SFjz&;;R*dVUmbVn>hG`s;ZJA%AD> zSn{1E(n~Z}n6#MsELROhplgCX37F{cqEAb{P6o}SyJcWY2HZ-*G-lB(S>+rlhtpZ; znckFAcq-C3J~b*`xcG|Q4A~N)`QI5^*?|?x=tMM!J$>X0tpeI3N>C^WE1hRLT@2(9 z>mwXmawMk~fm({w6N+Q2e7AgsF3Px$#UZ@p?uos`b@0(J*3Hpc!1D`C-2YY!R-S7l z^#@^?KBH=Peys3|UQE=skN%gg)RH2)(IN(wR44gVHp;Z(=FwP>$RZ`3-Qd_^%p*YF z^bN`B+RY?Fh}1`iAdD$w_`4I`2+slthJ+R=eX!>KH4$__8L{V^o8qD+Fj-0Ia>ew$ z2jy6yk32xmj~W`6I5#`d?zA~VFJjp7L$lz;1B>94?%~B~uiQ8Lwd$YM!tL0>(i{pr zxSV}hSZuNGF7b|;=A)Q-J(zB9i)=B9H!*o^pL+SRm?Z-KbbsBNZwpKavq5W)vtZZ6tkGEKudB z1eQN5b+}@|)v?#y=SKL;;Dj-&ZoQXoG;NB_qpH8}uX972c21bXbXRLdL`0dxMXuBlTYlNnDkV=EFN08#(*%ffPTK!@DT|A9Zc z%vTRhyOV@NlH; zLLFti6hN;RE**2a0c0vKn+Od_+t&dH4^%F*HFMShXiwQ@x?c$St-vdTGb{uluJ{*K zDOQJ9V-cno2lt&$Azs^Hp=|+`-dx`q?P1F6H~>NUQc#)ALNZx$5-HseqSr0bECUL; zioMXNVHWPgf!K(m**-0Nrrd){I^SuJAD0epuVOKiH?!1`H+^hLH#Okac1=o=ZJ@8* z#v-F54T$ceTbS4cii-ho#3sI3-z18!zdh4=0KfeE0@;?S|IJ49a3J9E$Yz;JmlGS^ zW;fK#;3U}0NuJ58sxXiii>CyVSB|yFXjWb@*>@~q<1t&P%p&KM6D;L>QeOQcHou=IKTuF(J$ zIFd+Kg^a7kS18+ZGUw6C%!fbJ4W2cS>wK}8W}kq>_bL(&84_F7b^Dg#F2y&f7>6$N zU4JSQFf8;QY;s+)@VnKQQ?^L|=RzPlnT&tZ&VGc^w02*VrEDt2seI?WJlnppA+`}? zYx``VeZs`?@;Vjjo|JtkAO6J$nvVzhp%Vv*ztbtmh)0`&%lCZsclK0YO8D_2V!O!K zK=Kc4x5-jg#qwPomIrmo%_UiHUJs&CH5!A7r|n1`x8p|Lod1j3K7)kP1AAjdQI#<{ zsJw0FKD`yKa?E(DrM2P99Etz&9c#6TjHeLCV(F1=B&ADnU&WIzb*y=!=F8v!6`@9B zwq35A^*<5(5~D(8 z8|On!Rt)3W@39WI$u)+{-sCa3<2y>0ClFs`O#JGFr>-0VlceyZiblK!Zf%QSO(aB$ z)v$nPyfyxN?i>vY4Zi6wbI}NQh&|wrO4>*)-wavXUJj1p{?%Gc{y3@v%8@O4-Z0$; zQBc10_yd#YVbpKj!v|2sk>8lNe`z9=goWdcJ#qXP%~d^!nU{ryg%ux$_cq}3jq&Mg zFj*K=*8Aw~kW1FKcr=^^CfPWSf>}72{9@}XG}QySX$)DG`zY)fOQzq|I1b`h*RpWG z-ZQ-?Qr#fzj25A$3}Nfk>cpcwWRo2nlF0<4n%PPZaAi~iA3sdC4pvcPgoP{5o;6G>oFVS3}XxC@mWmHR58n2)9kUv*y;ZdRQ5TPf?c;BkUZhWT? z%O{9S^}&%GmPcg7i8OS0OsE#z&wp>uOFg94XpA%`A`A51JHFhtv%PJ&b+%GWt4MwG zCcD)hf92Oyqoe$AtxK`~8($?Yh1I~tOd>@FO%;B_SEwrXkMvE{emWdeDf{OLi~;!p z_`Jt0KSd2XPEK{Ne8&XmWa4|jw9WfkPUKJPkl_v)NjoK4Io(rD}CRR!mQR z!d6V@=~|7%IaP~Q5-(EV?g`5$}z|APwM&Zpn0Rm-2aR=&f9ImyH??&gFfI{m+WoNL$kMK}IrgV%%ME@uGq!kdUm`JNXycJY7bTY|?6heGSE1ZV~OHJO~`-cYt3%2yEWKnDAy5B2>*=3Xse82A^? zfx|(xwRqG5Dg!B@ygiCd(pdS#r&77;J=~mM>@pBXVtli5ekUGuBuaN z+#!(fjkS2SeAz}DIbp0icM3TBVeabgA1{)++8^>jxsXt zVctXFw$}rcIO^|xB-2<~oT#9z-At|bR)#}NXw0Fg<={AeGuxu*;8RN%H{sPSN87nu zU_YewZIrP$Jw3F7;AW_^q#S04_D$OMlK|4NGsUVOw3A8fUD#1|%cQ3D-Dn;_!rQOD z7kx}zwB}MGh6PnCs+8zpq*4mi%Ik8fvP7<(3KsNak z-)TE1eK*1}sN?J1J9X>VetWhcv-Z$6IKG#)Q#ZgE-ZJ{2cT;7#}zgpIiM~Im1;y)?8?5Y6XU4&1oKHR zig&w2%Yy%%zi+2_drZHS1O}Ck7?33^o9(!Lo~J`E!t9BRjujqKXn=St+}4ns8)c4p z&&c(7vHvI{=24|g6sk3L>6M}XQ!Wn*Ii-CezM39%oIYcyOA42-_LoPmF5v`8a0Nkf zhm?-sPS0X77FRep0**i&i&pf8-i2SI}omq05I@l zK0pNxlF)`AqNu8S?i$*(ek^8R5(S^E!u~`T2UbGLQ>m%n`R4H@QZM-6@TT-n`$i{~ zPM|A8hfa#8@($6^JEJS!;np;Zuv6X&bZ5;&4DA`8Q-*^Kw_|lp*bR7-)AkrB8=YR? z2rJ(-+ghlv0i;wDfS<1?=F(^U0xc>BO0Sk@o7-$ccWdoIfIG{@xR)f^&h^Y_^KXYW z-VIvvYAKaS!a!V#eXYrdUB;Y?>^ob`I6}8JQ}Td<=#@=xy;Im1xzfASZ>4e5ZKN(? z_F0>RyLhPxJOwf0#q1Rbi5lcv9;yNN;_%cbt_WnMtH2*`wvoNt?Y!ErqHwSvVj=MJ zuQQu)bag<4{ixVo`zH=z4nMlnxRmioL_xuqAZ7d-t;sk5e!j*z!FTH4h}je?9K&Eg z;*Ue$D?a<&2u_sIA0moefESg(MPZBTlc#RB^&=^i{CFq7AUs~M)pPxKnxRZK=RCLq{sjU%N&6W}VlYN=vJ@#k z&;m11laWIgS`9a3R~ztXYbMv4vs^mWE&(9#GOjZm8D>=c+VxNe@f**iw|634e=plC zS+iL`FmO(Egx}u5OcZaggb+UpHKbw4!Rw9R%m_Vs?*R^MGI|EjO&M)wz1a}T!VY!V zWOSEeTnWZWHag(&c>SU{iOf<~st9JIVZd_QrXcxy&Bw?`%ryt9%a0!0vn_uvr zTWPehV3x|Kf0|>quar?H(x^7~nG5ZBdK%?Q^~s`s)g7+wG9qR635AHyb)(HkZt$$m zwz?RNKp9&EdP$7*rSS6&wofMjQsS>`ZxvC7!JzE)+TjVQUY8sQ^O%ZyQ+-?pFjrhF zgK^X}Ual!y*T)A4OTPlIUGelM1Gu!M zWke{I*7{0pUWu53Q&MpFrjb9IHX9W`C>#j@v#K*y+LRweacI=__KmurF$Wg?<34umY zcOxCFJ@>^PT0J2%1ZT;xumHe&Sllb3g0Xg?3AHF*m^+5)aV70vuiTph^rNn!rbpaLX=g% z2xd2flC|H#-O$gCUUH)2nq##l5gIjI*8{~sBE$;MCX&s{ZWg}>p@DI&fpC-CRBlI> z2)Po6l@PMU0Ht`MYGF8D3yNo_Lt9x1{GTkBlaofRC=M|*<#KRai*~yCrjZ!HhjPQ~ zc$tiv-%4*`p31Q7AW%J;F(&(wp!Fq+-}rieOi;93QKLoPmm=6UVBU=)WmTa?7oCC? zp|A$>z-hM%#mL^;2v1Hhrm(((##VVvow8t!F zl_^&-l*0+|rHbM*O+}QFuI(qc&-H)$($KGD%RISNsyNYTXC4 z5;-`Mb!kr&UylzxPG8AzTZ@Am@zG`W4vOC9TOGf!MN3{~`s1Mjk1e8oyc*rg7wVl;!xPu0K_ZJgN8Pg1n35Y^F zZ%V!MMXVJ5he)4`6zZ7&p&m)PwVJ^e!~XD)WrQyPB9_%^`@z1(Xium2_l=0{%Y(5$ zT~wL=3GW7%5u@o@{e_M<_Z57~cP?Gen|Unfc>V``>z;#3Ncl+re#e$vwLJ1spUb^i zs5s!=^3~!9U-XH?2gwm`_8n}LrgQ!yyH#`SY)hJsngpOeGjp(VF&w+1Wgr_r~7Ykpb8Rl4k z^N0?AZm({GR>*KxaBGtE08!rZHin@CPIR`C960X8``zRJ{M6(W6Xlc3^F1AY z9l(o^b^Y#osSlfIJD|o@RZ+0B)tB4N z{V7c9>q`CQ$C*3g4s1AdWMXcQZT{~+Gckdola-}=jzpM;Mcp#(r_EuQ=ky+kPgt%z zbZpxj<~CZKk`6lRSO7ixj~AuTpybW(%q_sz`X|q+TCTV|2)-&a`XhgCX}v+$7G!#(3eU_uKhc$cZiRgD?`G-}X}%34c~Os?iyWM?vq$Sk4w7hN zLjFMby7`1^6!;?^k4>3D82ytkS3^W3_8*|}=4SOl0QvlbHe^QDC4QYgp0Sp}HG?2I)^l<7nT-%o~$mA(vL zj=tTaC??Yo<8%T+hNRg|5-cp&rM%J)9CkMQ721x*O9_UcUmC#<0rpQ{wqNFi1MQi{ zy79(yH4JqKEH&=W5sM9eVr~hWsP|qf#E$!B7N$ZBqLNjt$L#Y~m3Jj?$7l}Lo?_u? zOff?GDZii8g~7X#Xrg=p#B|7Uqttu%DUpk;5kLq?uZZ~^7{X+V;2|PlXs8g|9WD6= zs0UfCy#dT5&EVFiHhJc}h_(Y$d#MONs+T@!Safvqk)e#qo^Mfev|J*(f#@_#p8M|~ z$vp=6hI2?lSIkV;`KKGN$J~^gtlvL8WdHveiMZl=Q*SnqWikjt&yq|2@o(i!^`o!x?y>Uba> z7bo8?QbSNrGoQCN)i#1XCR7`4{$aZ^#Z|Dd?l}Ju4vnKKlUb@b-Xo3!8y1>6gu(S4t;fX-l#6=0TJrq zvpxd2VK8O@ziSqy)gXR^2^ZU#+0cJo^88_!#6T1cK9<^Xd$7?YMzeXgH2BxYYngM8 zil(JZKmngq?agARA2_AI8ifHb@EgiP8-Q0ODueIv=nyk2y8W?jXW_gZl~kQO*wo{M3D6(X4u#cj;n{ z>GWDg`D1+3qHuQX|0DpKn4ibL{C9P8m?&RU@FlTC*Hd$1ZAm({DKc zXWPtavpq~si$bXiBDN^DopsSaz}nt$aFD{Jre7ntQ&l#m&H z4o;g776G4{hOD=Orsj#_B8woWKND+QA3BIAd51ccGMmR8Z5O3RVM31BAE-`-%J1wGR>fGTiD zgqs0aF3rIXFQL=j?BEw4F544dGsb%YrUW_y{4VT7?nurP%tjzYFR_e?DvoVdbx#eIFxmb*=CduSUEF@*;pMJ$jC z(H9n%oT8fkz~jaKg5Thq1!6^&?Q=OGZlBnOx$>#;?v`I+0~D+#%LySP(H#VnPm7u?Y{ znIwu$RIu}0rr<*4rt*-Hatm|IU`ep*vi?ncmqm`Fu6;OHG@OiQFl09egMzW(l#Om- ziWdl1KSM!6CLxm&ld9H1WvNSaw<`nmkrfrz$6bv%GNYKj^{tnOogWjBZfALLHBn@T z8s)X?^pWZ(1@n=G)3s)#uZqB?HH--t;S38L1nL-XwBV9L536WwkSI!H=UT8`!;ex6 z;FC|+N_h&PCV1uS!QUfC1m`U0-?ib8sR)z0M8K7vrN+S-RY&+`;p}TmAxP@nx1zoI zJxp+Mm+n|~U9O!U`y|BvmKPM1L1OTSL=&J2EDrtg9hTVD&ljCWyoLx}z8V8f1Og2r z?x%Q_qA&~+3~LmUq~OTK#_fAtjsV!QFk0@TPnHk<(poBq)4^E=7qgkE^tTbB>0!Kl zC*Z>j13zk_P-VIz4Kcn1!E6n;_~1{0X$O=aXcXXw$)R4WfMXQR4bwE8zzk%RJ%$MM z897rl52f+>8*8i0+8lBd{N9R!-e}+@U*hX3 zR0Qp{fQIv3d6>t<^(7*LB6==%-ygh@qB)9SHum+Y>xzbIj0J zapdG9G*2G3FaqATAa!`|NVasLT~oWxU+uq3sd$VyBL&$jO+zFjtImIO8m8CphD)WRBtE z`S6?n%L@Q?$W4yD_RcXI(qM3S+B9`uD)zkb$wuz|N{sh5(P$0j_k+xF2jMp1B8fV4 z)W5m19f&I4iIwU-)0>jXhj-y3ByRi4-I>yQJ^hiohHi+H5f6S6%xjIQ6$c!t+4@xk~xEN>IpMQ&ryLsaCP`kMV`nY3jk5^=lw3$E!Gg z!e#q!vt$+gWMd2F-yGR%g7prCi2(rt0tIsjgH&S6owQN)l1&CFj0BXtBoq#bdnVPi zwD4*se)a78N1ep=v*Uj{R>h^txQz!-#;V@>ySu&;$kl*@X4{o*a){d}sDW%)1$#EZ z{PUX~ZJ%%g(J3X~&7vb?6ZSCr;t(^*%w~WilfEn?_(UTv)K_XiGZLKm7o;l(JHxTB z#r=B6PZ0qe9Q>DN%(UGHJD^}!yWV`7k23gQFV6>$*gtVHW*in8uk`;_suQtUS9NIQ zsBBsZt{Je<|Ci+9;RgTjoaG?sw@*+>X7+0HWjSkViyj&FLKN$axyL5?w1zklsl~bu zK-j6BWr&&>TDFl1hE4Whz6SMjWzkKSkdD1-vfMVEDy?pJAs+e)LVNJRfJ7=3lo^~+ zpNDKt-QlfQbL}J^Hc_`>am$wB0tBYYrI)IzQ&wUCLP-G6Y32z%~m>iv3Xb*1^`#}As2hpTWx3AXmZ zxrM00gJra2cFM$vP(in=RLdGV-BH7_$g;`_<5FCE51%dn6}mj-JkEdzjhe-mP>bo% zx4gZ&B9Qrk2QJmXhG0@*?86ey*9FOf^ST-Fckk3e!M&H*8|g^0Uj06VgM6Lgx%QUF zhgcBp-MD9L8lBnCke;9{rdY`{?2#+5^-*(3h<&*2qi{vBkWl==fv`C?zkC_j>$BD# zl=Sd4ZzvIa&0vv$$HfasOQGNuks7_Mq=8g484zZ$Knm8?paWWV{AGXNBA|Qu&bZP2 z=LSz8L|frBJ1aC_8hDv4J0#lr;G}J%q7p&bbjA#ZgYpmO7wbj8qZ!MWc|M#^fr}cA%V-<^b2hewAvI-9&w`>7ZUna^q3oOB+nf zlhcS-rhF2?q%|MgyPCx>Ar=l8U>iCk1fSuf$}uN+I_PrErRaH8pm+(K&H0=-NaoD$ zm!o{hqrK9J8_dNeBqWxKguNcU>jOuzu1T>B&bB&S&+-;_RWJXvoimVo&=~g9?rgL^ zTvcsNuxVyF5xEXww~UlPN`%tTE94Tr^yR_>gpbyEwQHRbVg=M6Z`?teZymm7F^eoD zRK?TBBEpxO9PH<@m!VL^<;Ehz1KnzmpA~M#_5>RQm6Cjk9|(;VC~4yl1eh z?cvUrb>6jYLbWVM=~>{z(NXc_Xh|MP#_gfnS9mL=CXd|0oZJe%M*ptOo&2tsPIlq33IpY zM21&rjDts@V3XqT@Pn1nZe(NisHGL2h&6BO4G|}hSR!7;pnXeZ z^z2?&v8d8x%?Bx>u`tzK>k*rDYF`s;~K(oP#i399TjpzJN%oOgEG{y*ftknbc!xL(F z`lg*Ob4m40s#VW*!-IDoxH>Qj2dqTu1(&4cZw)zt-604imE_1U`V#?{1dD;~i{A|x1bg+XVrBR|`P$v*9x@|45kukf z42(Kwx)hE>sg?>}Ql7N)AmDhd0z0!MHC|zYz)v!rwPb0R`OoukmVKe|OePx9i()?W zF7wo>1JNVAdx>gD9&ZMym97~VTGyf`b;idkjc?qM{E(bw1JK6HJ_OCrTvkg7BFmLo z7&Aj~Lyw6~z-4V?p6D^(P*c-={CSgAHcvDN$GSl-z!1PEc2ihWRaMro>5PQQ5i**E zji~a}i_7pjG|v{O6R}aBP@(C&TQrqXsiHz-v^|V&U#CGItIO(x^5w}n{ZP{CX{*3OKwYW3`S$P?Hn*GK zYeXSieib^~+zrRvgoZABBe}nrD$FykgUYmGbo_&XBCo$ZAtYqP9v3fu2L%1-@ug$= z4qH2OFyD0py#RlYs!~Pwr@_|l1>`yT$V|= z_zcCb!pCYgHON!lhaQ@?2Gn|?ZHYeGev|nDSt{lC9zUu*{CX#^*N?@S{Y^JC=Fe!1 z2l{D{f2e8P?k$VB{OmDlvg@w=!V~`3;~9$%5;b$uK4R&Jh4kdc55%sx(V5;pWK^&7 zK*M!{9dD0sMGdU&WpeZROo;1Ja9&FI@JD)idUxkQ&)g9L&N+(G@c@3DVL$IKO5{^N?Vx8QvewO?F%!>r=k$e?MYxro(SF2Y61>D3R zEjWI#f3&66Y{iNGu{oWIb2-pAUiB_o^zieg=NMiWNOC_uEGkOQt|90COoeqfyzVxx z(Dsg`Mu#;Pv~d@wYM0DIwVu;t$q3fE-oxf@j7g~(^zKN6@jPh(#sUR9UA|cU=R$m&PPfsaE6~jSWmD*LG2Jb1Ti|E%CLCg8;jhWn*vbwr? z=FV-|HRPccEBN;(NPcFNK*Z9y5lhTe1^hnbB)2TNr4u!jUs1H#QB-C10gB+>s6KoF zK0T(frz>$QI_|${W7VZF7z0Y$0=F^wGNUV1Pi(l|xIba$oZ1-wDVRc8k7E9^(y#p3 zc;o3KQFgOLxzZ})p`d21g6vo62~Qp_@D;wp1INO7TDcxcTa-Pa!aYY7<>x+IVlBBb zb5%szpK_LX_s$Kc!pheumMYhq+>`CuNs$6ToP@1@6())gcn7W1#oDT}*S*B25h$?c zmp;)!cMJ0HqR!!1=Yq|R$TC_eAqU>wF0OSwdwz1X^=Cq`6^NSKp5PCQlaI6nZ^_!b zvsve?xg!`|M?(J#YZZgBI75f05d3ZcN6He4)vTb53N0mJ*mpx<4~H~Ms_207(YJsX z62+9H!_SV~W0=qS=2M9(ygw*(Mgt(tk@Mo)Hjzw>-2P}q&sTKIpZOalklJroPpDt6 zsH}G|qVKK`MA)h*9Ig0W$Bp8X>|lrIOyu>TGZ=g&%JdF+b6p#7mLC6kQI&^c9Zy64 z`l*iZEwzKz?liNT?D}ddAz^10xpbAtCW@F-0Bp#3thSy~q?lGh^7D@+c4tz}wQO*< z9CI8h$O?t;lYbgqzHIx9mgPGxa3p7r^W%&{s$LKJH#xlC@^H|V3pkp;eT7Jsb6ae1 zhYALt)Nam)rJ#sShKJohVg}xd2SuZIg&@c}`o4c&UF%q<&=zzjF<-0&fBT_j$+vxK z>~2h?M6q|Vg?PT)s^P)OqM=9s8&hmbh1Htw`n!$lT)aDrwCj^p$CM>Q3kRyk21}SZ z->Fd#6q-R^01i{9aUL7&m?RcU+K7hS#OhF`0X>_gbgec)U$~p^i$~Oa?{l%3hn6D( zEG)zIw)xKT%FJmYuaoT5;`^MamoM@c&ikz}y+G3qSRXD@J@X+N+4YgJ1>Ff0>DP#w zI8}h<&}snG^0)Z=?Jqu@Z(tPa6MhAc0fmiZQ;X+zHrM;Y6N=v;ev^j-h5ALpfGyN| z1e07ux#?n$#v#aBOz z_`c9l-=4>*L$rT~jWeu0c>89c5~AP&`N8tP^VJm(`{~FT+`%Lz7vtAZel<=46fKC= zgos^wU@Xl8smTQ##mqAJt~hY9mD+a*N^Aw>U_)ij@H5)#lY%~{W@9_70Spi>#h#L* zcKJC~Aj6{Ki?i7=*qek_SNg@1Ca+j$P7rq@V(^32>tlQv^9Iu_DicjLpA*4ZD8e6t zxkyGgNVl4N$@)G5u!b9>V^X}`LqY|@jSUQ1ObG#-H1)!wZg)oVapG*DYsjSLWO~Tf z9hfEM4-+=UjJqM(fWN=g^o8$8LA^ku@==3Z)GCTO0-ve3d0aX*j6g-M9LRAX zX?@A;vbzh>lCq8=D?oH77}Rss;~&{q2oDhV>M_ihDUMDCGotZu#+N_ zI&hm_uDmR3eq{%WZ45yOqakqIG^}FhKuRD1TdjAdX)_9>)3uRFLp$yvC@N0w)uL2S zb)Yy(Hf~N559#2Nnt-V^P-rxOlq_C4wuTSc{mCgKQB^z60uNb**GqmM&bHB>pY3kN zN@{4;`6Hu&;3-U8Dee2)CcZI2DL?jdj+cbDfth2Eq}TJV9-`Ez&U!4~B#x?e{@|eB z&C$sEp)xrOhVc(ub($e_f3|<*>NAmf+`;;+r}K-eF|OgggwVawfiq#sd-H+@Fo>FW zL0`Ko`gV44dO%hJZQtIIGh!dHgw5`o9`v8+D`g`GigvyibY9kX=%dg0x`o^5Wcco7 z_Y79K``V#la*ZLInA)p>BQ2Eks5)7w;F{<}{r`?bDJci07M6WcMDa)v_->miBsau6 z&1$O;oXr_)kiG1K01LS<9baE6uGp^g;=JP282FYg&>g}EF?pE~IO^WOmw_3a2WuLq ztVhb+36ACrOb*h)Z7NFdXsPi0o?08bT!tl|8YVBRgD+L4yCoO^LwC%pJQU|#~k z?y1{)yj0rDCudc--PTHPjK<2((NZbH+e13RK#X(}z+fm>pKPmYr4ua}sP#%m2LIK> z=t>-JdvauEONT9Br*YysyaWsHG;7G3p#N*)?=B$DHt%?4t!sYF0f}TyWi7M`cQFAH2QRf9^ zr&n1p&>0x$>Efj>>GQPhX}eo%zCV){4UWh5MV)!yHK7=$j0QO+f?pc6Mk_>W`&h0< zLvCEN)1i|UM}O7Ph)S3I+Ys^*Ll__Y>w^nf*bO7>gZjeX#W|oqngx#t@Nr`k5al?P zEBu~~{tsir%iDOdwUZc7U+MOT`RFu`+Tg6&x3t&1Zh_U0S@c2HKG+%yJ@&u9D#n4p ze}Nz$Tm6%3%vV#956QNcEKzN*u(2UvJ}-hwqdjp4BCZ6M;OOAUdqpK^*L&l}n^YLJ zWe$VEId`e6^9Hu5HMoJNxu}_OU0|kSkr`v6r!zz#>~s7c&zv~l8s>Gq7<@3^@$`g_ z+edFX#9C^9UfiHuTM{JaX;0Ffx8DBQ5EaC!%$f13yFQTMeYTmymK1Xzo%z+daCm;` z&4%5T?*_U`YW>fa;?DLuW0X&ajou$aq(wEYnc9y}O4(zV-hko_XX^~xw^R+|>Qhs6 zGCt~yQ2=#b;A4h8#u=&CS7P(R>wgFh{RS)HBC}Ol_(sg#3X&agZ(VwRt<#z#{J|HD zCqJZmy}J5J_L8`J@NFH-jsZr%>jKhT>_Z)7q}atr1?(C^kE)v|bmM-);K;jA@7wMt zVky>_p80s({5M3Jk9P$=^_GrActsi9CvwzcyNnN0n4(}Yr-qJ1>p=2u1ymQulxDwGdER7{$2 zz-PhhN!iJJ=w6QP3#Xkb0y1@l*P=`4oRA@Vb@jZA;qWE)R@{cPSF``hS(4d3CGw?(L`tfdO|VnYo0ENMUDrs^t;9mp zrWGgU9feEllab3XI?HnIrRqp5^c~ygQ4E2sH@t=+(A5@a9!_E6(PSG#cBMuJIUo z6N`wmMTZ)gO|5W#cNkX@9)dhPTr`2SadrMG=-*pO!Jm>pej9I;B`P$LL9Plx#h*gb z+~pp(Oevl#&^oyC?2Ir)Vta=#UQNl*L7b}}*9)F1oI{+weOx>DYD)1wipMriRTmu~ zA}k)FjRbBGc1TwTE2)mLk(Bfs(eh{USL}CJkY0+XT-DR^xgXt*1(#pRZ|?T;n*fLC z4X5{KnEPSEx2-8e3?me9zdZmI-zo(zLTq06Zccvfc>>E%!t~z;Gk{M99eVZooD|b1Z=sxC)J*b<30n$Cgjk0jKpxq_=B%-& zVIJAB!VPwci)2JHaxOsHFJlX2OHoI5!m~RgsLa;V>=VmI3xf(nviFBw+~NQ^SOCzH z3H|aGbAaM&I85lHzlC_R9pjSMC^>X)T@p%zFV+3!6y&-~7FXn{Pc`t221v&4Ld4yp zy9-${R*6Mp%o0qWwG9h7z@7T=z3>gqj9L8?u--gu{KwRd@Gt?WO5xr~WArz}*xZymxuAYe*1X!RFDTb~rh0ZXu{Q;LnQ-BCI|FlR zKB@QBhiPkim*~F>y>X_DqV`UL#A*lL46YujC!R0K&KJsSnW?(nP3)v46c`&rzsBGc zVAint@IUMaDM z*6T&$rXT+b$90TD%W5Vc@wDrxn!0=yDJD02R(U#i6z=+;YF32?9UjlcSb0BkJWg)5 z;N7zMIvPtjq{=HU{&K}cZ9%NLd#|D7s=F=uSgq$n7l&Gh!4zM<+cJB8>#ch_CI@+5 z%0@gz7o&^gJ7^W|l^=Cn{-NiiAi2PjAJ>mWYDoz_U4CAu>l+u*=2XWh?HwOQ0tCT# zWIBIurduqA{QwmXrR?nBZXHzGJJop7J#&Olvm0;LlckMf+ldG(CEqb2AYer87{yh1 zvd5%EuIvzVf}+XlL&OM~(Uwx&O&fuX#i%T5+x7qNaGd2!?rr(%fk1x9ZZum@C9;N) zE~)YH=2D++TPfa?L8z@{nip5oC(BSvFK+HR8^?AJ0!8py+l6Pu6VA#RHaon>J^3E9jglj8II z3&wD`XE{nob2MKrnzofS7t9{}Vx^vub&)wa)LY)wiY|gPuu0hNC*pgJ$;)uCbPncL zQYolJ?9 zO9ebT52|>3n385oi9aiF11@LdLfz=Oai>ugH(SyORWS%!|Hx*mxj%Y8?-<3C1r8VP zmxx)LCWf84Q(zNO|@-oauNv%Bp$ce=KnXq z+7Z7#&%bH`{v*g1T_mMgm^^~Mc;4&XTUZPQxxj9Ix`b6)pYTpu_&qnBo{Xu@FVbUh zXjoWS2q7osdvw^Jogq>t(co6&`NDmp*fLE!ks!?24AWSP1D*dJ`0USf(SL`}sMANoJq!t#Whr@GG4cjYh+ujbyizXBSWNK|yG>~9M73DPSg+Ga0;!wl^A5Hc^4|u8{`IvRq3S-GPhb`X2;w=^#?@dRk;`uNHNd$+fl&eRNDC z-bz*Ml5ialn3*@{B4vU&PG#xHLkD*p=z4td{Ok}V0G`l}YBNMnnz&<~?Wq#;l-{MXG%z%SC+4zfWTS#^3b!!p5c zP<+0yL@qmygdD2*+o&uEG5RFSE3Y?kUlWxciLeuus6Ya2gDf5;Ehc1o5?ts1yjmYb-H#npR1VmJLaR!HD07sXv}MOguvCfg&qELtq$_{+1uKOnI9o4r}^ z6~%36&KL~c%rae8;SmqlkWvl=K~OV)J*91F$B*vVly!u>3b7ue3C14cpc;^=7XY>NTCaI&e{ej z_^_v$mjyQ~jtKD7KXVtPFZI4f)9UX0mf{af1l)V=(l$F`R!q+5(?^lgs=L#gD#74K zX)ym$)fD4;b}SNZMg_i$~2sIyP99gggt{Q_`_8IdQ!skzbC~c8_b4vG#Rn(*KrX5Ny z3ADj-oH+MV5zVat^AA`+gL*itbB$-7lqgo_s}I5U`KgEl4xZMdJ9b@P41BpFzH-wC zZ*}7I;$*lQnOaT93MwP?=R6;!O`KXcA!WUkQMSomZUy;iMTLwoh+wS)|!CObL53yKSkNEv? z!npBIE!FiA$o!26Dc(bUY!7$N)#IR;)jM||2IuX(SEd%4-fBBAe_yVC?{t>D#-j?C zuT=@-`@W}Vh9>$23~t2GsiSOg%q0e=m6Y81KyRE5oehD4GX2K!2px{ZvnZg_;F9Q7 zve!NdcU}2aHL+wG!RX@-YVO3`)KBZAcPqVv(Lb9J3S8Jl*v9XT%+r)>LHhk(*$)xe zJgRX?U5)k3l#$!je+6R(+kpQo2Ge~E@8J;qw;`ky8n)|Qw$`ZO&fmbg(-AdodHG7O zA2&2YLP-VZ3k#VB?( zI-M8UVg0`%uw#a&vG;F9PU+#OG*1wH^dGJG75#P3*I$uyd>Iwj>Z6lgV{yd_Md+Wcsvc%!^1h)DY5_A!-U2(L%p%h_Ye9U1M7kWVb*zWo|J;iE_ zueUK}!_##l@AvO6^}LVh@XX>!wmXlCl5cz_dtRPvyp*N*SAvYJcXv2H)Fjt>4@$o= z%eQa)#80smqnPj96H%$Xdx8R9O?5P}T`9Ro2>(yXPshW3#9%sKTxZE;0DC$6myB7- zmQ*%2JU*S}zIAsxhih<{@AiuqA5!R{3`2FFKhG_Lpc`8ZmlR^-Mcw*}1H0Bw@_*<881GcT{|muC0ZPa_-HKa29eds*=Yu$oOoPD~hA zB^8+`{RbfBr>N=14Vs(9jaeoxLm+f?8jCEkzWk5dqUhHv*P%opZTJVdbe{TOh{y7p z^tXLfj~uGlhx`%Q2S&M?}ht;Gy0K zI+4$HdUm!Js;yRpRAf&iXS?WH?pupeTn2^gUQvl5J}T{w3sCEm{$zi61=CtSwr*@Y z5n?Wm_C13|^}YC{J>#r0;rpwX@B0I}kb~`JN{*=}u0NAF%RIaP!L$0e=UT^y z$J0#qgz$)opv(0tTw+f~BAxe80hDeOL$-KF{OGm1Z_|T2`WpcKh?Px|2?nw;09X`_YK@vw@Oj{kDHTTgguREx&wGI7|O6F;o*U@AdAF=VK_SZR^c8vG8N;pL% zT%GFp4WHM@jZQ2n!HVRubKpdD!p7MRL?+M4AUQt(4kt6M=Sul_DX}3KQuR^!syby4 z&UfJNrn+_4GtE;4)*3aDFi3-LZ)fDEE96|uv{Y}JaIB1bIePwk_FmYF=wNJw$7iyB}Ka*DE z{t*9UXRR@`Eb#fBE~1nEQvvetjCG@6h?|#;lhG>Bot&d&%}#iO*K(h~=%-ylhmGgMsqF6d^4e+clG5AHaG4I# zCUxqvkANYi#ApDrqCyGqqcq6B?VQ-4{_dR2%mBtj0OTYkpWlsV6pqzglE%cJ2BG?8z9D!I*2zKmXJdZOqG|5iGa8A2SbBd9HReZ;%*$P zB_lc2mDIwm0==_}DNT4W*MlI2gqb z1P;D9|I5Ak`PQ2&9By{P@_;@bPKOQ#5bm5TpmCSKPVCg!_o$b$x8vCW&jDWFTQZT z_9pQUd1&YbN}a$T>d@#a)T9tM#(v5DU$v&AJ_v|NIMgoEkd%LeVroLdGwbbqNi%nE zmbR}PM8t#os5JRmqvVJPQ@eivSiL0#=vqNLZ9)cX<`m_%m5AHoZkJon%bG|*o-P&Viov*M3}UR zBycR`Wo3J0@-Rpw3^gY+H>pj=!OwKV1|LBJ3E6ELiv26GEL)v;&pS=hw699JW9H1! zDR%!ei_HI?{Iq)w^u8He#Gx*?MD(6axiAEWMU|}o1yYe)L#ylMC^%J}H-Z$CTPpa%9hhqQkYHWR=?6fDyr`Ok92d?kHh57q;` zt6vVljn2e~Xev(XXo#W}-ctW%amf{uQ48gM)M=S+fetwy_T>0|lNLKf#$z!O^(-iL zfYDd6(JJoc#lZ)h>}NmyFbr(kcKxe~0ajOmGwZ#fEjlGI0fuMnUjoydMk@xFD(}n@ z+l)V*!{59#(7iX_fA23Z&@(RnbqZ|S7Pzp>a16g$o9Ck_ow1KWgi%G?@QB75F69>$ z$RE;Tozkl(bmIRL7p6lgow8SGyRnV*w7vhLFY297n}|O0(y0*Qze$r90s_VJfquIn zwn00fed9l2B7O1a{!{XQR?cs8n*gHmzHqenc!$5E&3Z)mTm!o(aP~ibZEySxf7c}o zzJJNM)-?t~V10B-r?EJ0467Tn@0eqaV3GNkG_4-{zrT{>g8fNfjL9*2SHM^Ml*IGZ z8B^>&XpT>`gxL`+Jn^-+F^+WA$&j*3XO&&$e@?BMuPT`6>0Qw6>oS67m!9Wvm|-AE z4-+Sr3bL%-$G=n>w@*G(+H~%#7oHXmZJly@@&bdYo>#kwwv)EjUgtTNHemM8f-|=g zhOpSR%5KgI@g1>_eTRCT8-*D@B4y0V%Qu3?v6sw95m_@kja4h`<**pe z(E9fcD_ATtY|M1=8}Eo?BS8w~#{uM?O+1N-M!+}U-@Zdi75`Ik+pzlu&A%>aV4EL0 z*!;pZJJ4~i?Ak1@3XIdU6MYmXYkn)`P}V*cT#ogK7%Pc=*5to*5e&*q816oJ9!xVM z^0oPIdX(IFyKo-%0ZoPey@_B+`^^>2bH8J742>DTSHEW>Wst-*%J}iK?cTf7owV=o zB9Z95Lj=Xai%$+Q35MsJpa6QWa+jB$Z8Mi8yF?V#6pW`c3hqUgg#H@+hb3w??;i2y z3y~n7x{TayxE5a%tt=I0yKj1RYL}1|qDKuA^o|Sm1?iog1&6xVZWG7`Urraj{6Zzz zv)+)H*mYL^qs-+XUhtA6Z`r__cyLe-dpWOAA$Rv`-rV)q0gUgHk7Z2!F ztjwGi;T&oT?xvD<=6g(B+1hMouB7&sv{KD>fc6IPPX*3Laj2#0#TUq`6~LV;tiDU- zCxN#U(ZfmNM0h+ZRLxKF(zJ$~E0!ELWGD%vXR^(Oa>FwFuC+0k^Gkm=;mU=`bfpHO za1Ix1!1h1RO=J?DJOM=+Z#bKY`g!9R+O`Iu+UGo|RXhZ+>~*;h48HG3oJ3hBE4gvg z!I{$)7*sjJLHXk4C&n>Yk}ova3I$~OZ$31x-V7r;Sk5Vk)9n9$r2S8vO(GtnQD8Uy zH9G}g@F=MzQCe?~N|j?BG_pr|jrcF>{$s@dRo#D?KXMTMg(P2w#~gr8m*YZ`-P^Q{ zE?!mInd#)dZGb0FUK2QAt02$O22ESZha>O;GH6sA13yhip-a<&-%#Oaa6^I zK64x2dN-8?n0=Q(%9<2|yk$rlK__(`r7dZV6Ga;l6gR$AnEw=N_bp6X@k_|9-&EY$ z$AN)#SN#QDxjDVq=w#wB86de3#)bIb8+eEvh#lb}gI$o*RODj0ASB|N--=$$B;;kx zo*o&^T2#AJU*ogO9lFX6KO748jY%YPe&m*(7#%%|ft#&H(#K@ZW5(`5EEY8TY*mUT zo{5c&7M>~aeRa>0PT7heOi~GfdSEh&ABD_WrYb%mt@^!^B7?t~kNh8y+udH zic*jLH%XvvxNyGM^!g!%42*oD3Zp@|=1qBYelQTR9=-)MR-r(QJCx$x#LP&PoHiKa zOlxn8OG-oYJ(2ZlwcpU#y-gd0ieO(fG-6$m3lLleKGY$9)YH8#&^n*}>WJj-P*E0U z@zMgeVEn$46bjRuZdXOko}YxKs?2Nra>j1`k99NODO#$dqBHrY)Gcld-pFg*$|-{E zrT)-zhhpQHk6dah!tMb{7EavELX%VMDw-)Gpvnm&iJ*XxuvJpa*cX5jO&S6zE#T2k z*QP}X8jfkAy+$f1cbGcn>f3%2&os9|rI+kpt0P$;A`~qeitHM;97y3i)PPh9`Efv8 z^{PQkNV~+C+n`3tKJI608~+`Kp>LgXW9Nuaffy^O5fMt_#X}b0|3raZ)ZBMEUvOg< zb+VK~OBK|FmL^b+)=3Ikv(fMhi5T%>~8PEat`b1(NO4L46aZ_sg#g(&6Wj~k1slb#8 z4`2x+xH3!9ve46BN7!VgQEA3Q#nuW>Veq&;R!tR%I+Y~8#Oda8sab%gnh7i1ThP%= zFV@rXN<$lME0x4wRik&FMMl`sizqnZSw%Jlo#}0<-li%FTVXUBNg?zAfq}S>lbsXpa<(l5JXRa!bp7fSvA6=1lX$IMnpw z<%>7FcV+Fj?EcmA6u? z%jWImASNyrP` zH~hG8PtZjTC{OM>IpIbf*Wrgm)zFzqqja6ZUKwoFT@67U-(}O{cPmhKskuIWpQ7M} zf4|`DU9cVMsJog%_z{JgzIQgCy6~~s<8|Fa9kZd?9~}D#ah)ulx|~Froh7?NyYA`K zLO;KV7OUIEnDm(_<7XcmugMv|EDmKjbyh7{onBG<#skfRz* zDf`dTrbo-?YS$J#+_;}2CtaEtH>VGNKuux(w%(px)O_*31C(JU6M zCKz?SQo*9TU5dN05WAK>3bsd?m|2d9gJ)c<+V*0$6Y0$2 zLp}*bT~I&J-%qAWuq*7gkPGZ90uMolN$ts_#R9oqA4u@ww2U>9Ym;^MR~XC0x9=EO zXw{rcutD^cs!siC4XkrGC_B^NOva^l5Gly zOdSajVW-5BsMZ7%C6xB^-(s{B>iiFHUff8flK%uGWfg4v+H#8O^nRvIDdNCeO_k58 z+TYv)zk!|0D$GBcgk)MFau%IFG!RHu<7mJ$y5nFrq~NmT9};>g8Xc2Ts8Dckrjthh8xlze zG~@$@F-wTuVF=BV3qyW{k?74G@85LVV1S?N0bR*ieU>~V%2$B46=R4`SV>9yqxkSJ z`3MG70Ev`Lzr`W~cQ}yk19PePM{kg>J~h3!M)KDyIz;>xd|%2Fn8>BjeWjD7TB4=b z_uB+Fub;UJ`x&c|8y5L!#D|pdb z@Q9ilY@P^Q?Y8OiILDl5E*u+Nn}bg)IXo}1V0r@GeJg5y;lcl3+4PMW4D&pF5(_VoynuPhzdDjy-Gh2O*04&1dT3I(+&2ks>CmbrA@Tw1qqCQ zHps_)j7eHGSSa?55y6GA@jP_nP6!*p@Wd6X{5TiXd)FpZS8b4vxqOe&ZU*OLX>AB>BWIShnqOK^$WDQjx29WnAM#RI)?tzED=pIgGn7WG`=GZ)s-4;5 zsk%2zgB%AZVPyH~L-K@N?UcGSnq-j2i6iN7MMFpN z1!F7+K5E-a5nlmN!mhOb+yi=`wvN7O+hD{T2)o>I%Q7{N0y>!CHhr!ve+K#>R=RQH zS2iBVso!fyGLOSP#T3DdZTsXc>vLud2HzZYLb`rM{gPpNFdzD0c<8^Q4-pR!m(X8x zOBBZg(^oPhk!v#qLZKM!Sk3X;;4%ja3=A_L^gdUy)dIM&I^c{)IAaoGBZq*P)&sP9 zm(7^TQ;LU&whu=ss7jYZyOV`1h-$IR(zw~}+Z7{a!0)`uxposg;-F0L^f-JF zSCsMiJ{aW8Z(aj>D7JB9r=sNm&!a)kc-?*F?Ge2pgr8l&*CJwSOxb&g;cXkfk&;=- zRtvD;V)T{X2Mn> zR1R;@DWzEYH3^WD`#@yJo}|a8+ws}&_QIDpq<_$*vJM?F7IHZGhanp)G^M+LOFoFq zZsr6fR7!bMW>&bB%7R1x;w@&9t-q#lXnhxP3b@N0>kq7A`ly70>s3vG)5?r51V>IP zMGC&E!DRfla#M~K#c}G2s8VG*!J*o1cr?R44~L@{;vKs*12LW5PL?i9N;3hbY6yFW z2v8T?*6nHFc-OpuwT7lbUn{_XV>=#9&cZHUBS-b+`Q;~CS5p4?CTt+<)5ny7n=_Qh z<91osfZgd+mCz3yxJ|5gt6F~NWpkUec=2ECH9?A96=bXkOgcc0@9k_=l-N5@k52Vb zf-TU+ecc_JqFWAWf}MKgrX8t$2ajc84$G;JN zn72*;ea*~lthZdtr;N#9gxw7gTc?0Jm2Xo2Y#sFjqQw{6_j(xJ;8mMooud(VR~VFWUvTo z1#Rv~_L)pDyvt1u=n3GG%}W*P=8=14jIp}`azujdsrht-fJxAeopZ>11E^l^jR$Z? ztND-6#XADM*J&-RwM7y~vhjznKh?RhN7)}mri>2IPFn3)@Q!Qn+!_>N1Xf3rwZD`s z9|`ZFA>1tZ?5Ub`Ob46Hjew5jWA}Yp%x_NLu~WC&_ZMqc!fjs8PRXuz%Ve*+0TXVO zi8X`Fm>>V4c=F<@>;5W+jquz%j;P+xoRLhtMW)9LR_$s+S0^;>*Ea*$Rg59`dp;B8 z3z~-O7vHp>rM3?eN~*Zy*u1+O$3SM>EG3v!pM!kF1Sv7ksJtz7o-_Rn@x^qu;l> z?V#K&EF|{r!*?bVnx|Mf8a(`*D%C}}sYPm zzxWA7ZQ2kUDWCs*!I9c<3%`#L(D zN_0xI@XcsG7&a-Oov=G?>27HMHC4jPD8`K)Q`pui@s~x#?&Et0fh=<*s0mX;Ew`Sde;Sl#{1-L3(qBn$E`qXWRYOk+L0#Vbit zw;6Xj!B$YXq`d6ySU8w{>fYiWSgC3JTujcu?%aX-iObYKqF6x(6XsE9zH>u`nk|5* zek9bKhM2lBzMauGlUs91ODsAxq&V*CkjTL01mCf)lGpSY3Z)nOG1satZ1K%a&>|Vk z_#$p5!pb@%L%pVb2smm$_Dh$>IH83CtB}urvzTeF;ex8#QJI`#FvmAT^p2ZQI)V_)en|4s7AQhXS zO-)OMMMfruqsYhFEs`Qy=DHgLZqWTvBB*74^{9`7&QD0biK9--PXELN2v_-Xg|Eo= z=JyVaRltUemql&R@(hF)gU3()r4#v=I2x6yUFtInO9Oec4N9{7NHpdju5gHxp4y94 z-{q1F{y{y7(wKQR6xSuQx>* z(Waf_%t}bDpPOe_=E!FhD--I=LZzBcJ!mxO_p+0tct}azc;1ywom}YYIIP8Ga*EqM zLYCsI&^1O3-7iTDbdV10H;a?<7(RFzk%75Hx!{MqkTk}CTw+Z$W0B_6Y< zZvq8b(g%;%R3$mh&VY9h!Bs0!0pHXDdI(W^vTyI>$proYT34PoD2y7S-5X7zgV+U; zjj%Nyze~kvLZCR7P|%H#qUHW1Xz>B?bhLxA;)goEFmyRqiV;5-bA8pfFJe_;oAC^f zpa@6EP*i)NMH)7A&)Qha7T=br*v03kYDw%oXN5oY7$MYLVR|srvg&CfY< zS2Y7$cLDYg&L4nTtg29JWvd>!%+qPl(O#+N%k}&tOFN@KPAJB4HzYT4N#A73GW%+7 zB3TdoL_5d!mSLp<3IELAI~LDmg6M+B8KGHkTPrPiy@Z^aa4uNt-(EWWDqg()+VuK) zoi=DMS2OiW1V`gOw9xG!_ts%EHEgjs?MA_79aD*ajqPa8FSW~7x)H{kwL#NV#}DU^ z?#-a;6r^LE{7F+hmP!I4lbq>QTcZzO8@#`qt!#U<`v;5&YjPwX*aTO~eNfVQIsCdN z*z}eh)2ie!%yuP?8w(nP&7vM4 zpMJI$Z7jKWTKAsFLQ6f!99%Ub(mz?-H$?qRqY>ZbA3bPn-}3kwRf99xc#KU6wT{t@ zHEO6pYo*zY2Byx>c;H!1%rLo$!1}VfN=}Cs><(cP&>l=?!qAH?g%5~B+}ZNt z&}z9m7!Qd|a#v)s=dPh(dZuF}?*yvjw)&3<%hVR$ zozYHnzPp^w7I2Hs##o1L!JaZTOwp%%mojC@`cl0r9?U!B`|i~7qSx7#8wU+JN;!D& zK@w4v348?p`6_&Znpvvi@ztJM5;!Nc$^d>F`_yUpsY~X|2u;e*SHbY~!6UVv{5+z4 z(u+DLTLXW>^AB{Vi|Xz2O-%dva|LCd+pF(co3pVF5AyrUnnstZw_e*2xJU#O`4juo z`_mcxg6HHLXny8C3mrx|8N-Ce>e=bPR4O`b{)Sr6q_aGto@JASc`rG@%c|y)@l#JwV4z= zOyTD{mN(1GJ3fmn!BojvW+QnFyN%^CdWpXV_?)FDkc;MS0!#OcplrTKk9_^66Peof z9b^JEq%7Wvh1=7t)ER9nu_gzE0j&lCSNFXG2;^DC`OVpU%-yv8!(d))svHx z6QAXhc%uT-oDjkt&tMc;cyMTZd)a(-v*^^}Ip7$;SbzYf+tvT<+0A0EMv8SNQRe0! zc}#(0kGp>9xG3yJ`*L5D18#g&t6huto>p6ytAejS0dM0xH|%0JshUstXXw-YjU-PT zo=EDL&^0y^Qk2MHz==|EWYUTaCm>fBLGRN`oNI+FAZI@{mDDm1;3k)2+PxswU7wZK zNvFrz{6VaN=6+7W)Pjks0G)!N2|8Bqb2>JTpS9InZjb0gQ&8(SPpJH>{{oTc;>-=W zC7qp4kx6=0>fRW?uORu{3JLfn^gs|;GX&eNs1Ql>q*iIh+HCU)BKKFK=dLK49emYY z$TZjFPtZ~tOJ6yN*z(EA1JkFolX}hs7si(NRX55)$`z_fKfwy^ILN_6yllaGbeZEo z2@}eAZ!~8~)8x;g!f$M~_!LB{#U&^qeozzMR1S{8AxhIe=3?JxaC}H zi_en9JFGia-)J-Q82638{_?i*BsZL@GMHZ?)nHF#AG%*9!XQSScBNEw5SQKR@RtH( zorMRxuzA4M{l%dBf$Xb=eHdd2Uj7#`lVoRGY|U(96r$HoeW%-!?L7)&-saroz6CT;Y21XW1&oAa~ifx}K5!)y}4- zk5^u6ovHZT&&>`dH}C}>6p?o1)chYw^!zhNL&FeO8oZghtI`J(s_99n>foY!5nBYN zf9ni3%Hcl~3fZppWmS%OSU#@VGA|Zc32?l*5~z07TZ7$HXHPDD6s zf55kdB^iB9O`9vdj z{XzC=vD5a6BjQwk#|jrq*7kJIkU@f;$$IA?Gw1Kt8(VVc8W8C4Wv)Sj9>v8oaH}tS zcTJp0c?0%2bbDO?75-{ytJt#dSk3>?Nsu3PYAoV_xxuGPom~TOln^_7KTb`zD(xSXi!dMT#kUU)|I^zo-6AiRd-)3A~6uPzLX-+ZUs@z=?jbNm zA552h!`~V%-pCU)GV{z}Zg*>(M!yHgct%t=6(nC~b;o!e(Mvv@?@H}A9uTeEE9Tp2 zyZeA7YCz)xh}?EllxLTW_xMv?_ie5S4(_&Nrh!8&AAj_tjb2a$*8Dt>$wK&t$}bpp zzMoGVKPs%ogD(XCq>q|ydJ4VQ=&H@l-$O3DeZEV$KPw36w0zkU*xFO-_U-YC#!9Fj zFO8XAM~CM8C*c?n(}E?W9Act` zk$B>dZ1+fc6THry!5p4$|HTKUg8X9C6_0O7y~jhIP=dDmcFpy`%F@|vK`rZ8Df^IE zkzdzWI9+dIr<%yH$Zp{ikN^l*vY4f*nb}tD(&z6{*3@7$WY6SPu(Rn$YDk%69->Pp zG5>4{skFvqf_AJxAkx2*rslmbjTQEm)@=fE4 z{yIxslYaMhLm*%y@HfH%labFM5~rM%f*&_LG|UIQX4J2@&s!)C6FEfdR(v%m4v3Xf zY16Rl2z8@tPn2hT3Y5h=Vixl7+lLh_k>}H>V?{_ET6+dZrR|Y=k|H7j9oM6DnoP_0 zBbG5-$t|JZuN>LKdv@vHG4Z6gnl;C+Iz+fur-t`qzKr$F_<_utLcO)*kNEB5@JCzY z!n{lLWQ5=Hwe)-`Ds=cf{hJ&AUU&jb7Q{R&?6t3IQu{}m1K3Zw)cvIE4Vggl zHPY9sVY7`EB`s<4APhBa+Q(yPL^K{ihMu3y{V!(FOaYsdHQ&IKnji1@`8 z^=V7O`w|MP;ChtA2hSS5k=T&b?MrzhKx+CwPjAP^QkiV9=;R^6?n|0Ww9%TZQDr~a z&|bv}gSLNQbk1l)mNh0Hd^`m6&yef*5XhTc!6xtuY{khRS?9VZP~xE_P=29ybzw_xHgVVTDo5r189zZ5&~x(1{T&NfpQj3Na1mu`CTb} ztih$^sL)#vsNXKQy$P@^0k4Tq6I2DSJ%$b ziH9qsq{mMi*LU@m6qLi_;})85;Z_V&ex16LSZY&9u0D8n(xN-8T!#}K4A&yi`JZpK zY15!nZ2h}vI3}uQ&okKa4BWfl{*srP0#YWoEc99}*4PY8oitDt8%E!0P{PX{ z@ScwQ8+la(I<=l9e#^YzWb6ipFo5PhBwJT07)L-^=b*dCYmp^*(}cGR_ODwlSY~}I zCxsZOBk|_Gul?9h9kz_q?(sJf{JK5Vzo7gQBgFSTo$X7SRlq~;O?ZGx|LH!{uP#Ot zwXYcYzaO;eY05%}#aFIXfZUjR?pFY;2nAG(&d;uybW~@g)~obZ7rC(E!u+EH(v`=` zXn$AK1bGU>LurrbzR3QTM#X@+Vj!Z(wf~~5)Ze(>Ubm&;BYf@FNzw?!fDlKV6a-xu zqjvUkj86KQB)h=RN8}w2@YDpT!Mc3w3^;t{9j-`lqgizf*VCC%gxq)~Z}2G)``i{P zEIb`DnzNv7!UrzG$3sO_W(8!ZgOgfsc|qK?p(NMhD;mJ$PFrrw1dm_|88fp2HJ={= zw)Nj~=Mg#)5;C$My|u)Y;Um*bzPTHs)KZ84Gzj=o5sLsgYgZmOKqJItG_Vj?q6g=e zG)8D7I?97pQDL$j|DiL(GH1K_P(g@P;fLerAooH$ZmCMHhcJ2gB;lE`l?C074SLY} zF`cw#M9K+h^cPxzC`M+)@4@-_$59dx=12Nzt#nux!=lh$rvzahlro|MXnQ+#i_#@) zb~jM0R%Rx+o*aCQPM&dnvmQYp3{y3yz!M);E0m zT|A>BiE8(Lv$#S7qcaYJH%HlS0q|aYR0rUa;Y`l? zxe)15H0FL*lkF+nl`E=Zm6SQ5?+`D-g8p~T$l46^nJt++Fdw^zJ=!i;s@xwpE*9tX z_P$Ih*@Ew}XIMRYzKGpG84{j1Y9b*$?!%@eJ5@vDZJM6^0rq(FXP2T$7Vt(hN=DpUl}d;>2*J4IEEM|L(D zoLY$zph&~GBK|W4xXYVN`D7RrxrI;%jM$5(lvii?iT$nf4! z35(Jv>K;&$bvMA#E1mYr$Sjx&zMeG{NPCwcA#MJ1i^_Q{9acpDs7* z3*vEynH2s;drK%|LquS*Hq^9LS988OejgEFV)eb+AWmy~d#;7cuJ_J}Z+y|t>X0;j zuC0zN+?B7oKP6Wu!!osrhha-=?{zR)UqcMA%wEU(OLCYDy}&$j*04 z#bXo{{+$$MzI=_S_PYW>J5(@UL4SvZM&q7Ad-PX@p;oOR`bJmWWCxNS zd7lWh1rmDGNcrLjBL=h}TmLjG;~lr2)DXPJO7_yux?J*Ayn!3uDRc3!Nr3h%3(Acu zW}*b07^%bQIK13R20vM^ayAh%f}&Ci7_5H0&T$L%_G?$G_-;{yeCe^}Cw~jvvYxPF z_`Ld7tIzYm$-RISX>V~x`|LHDWRHC$y1afiB@C{c>0;(E4gI}9=W*jrsAv$<=Bn%_+zk082h7m}#wtyL@ENM<1tl#ODGc(k~CdOd*sbN9CWeoW-Pk6-X# z*p+ocFYk(8pp%V1B`8zc=W>9wiP1BpN62SrkcaQFt(2hsQ#jc|8^81zAV`vpvP>+= zD8}H|r`A?aE@|*f9L)5#dNCIi9`rZsNZxhpkZ(_zB&E*SGY`zp2(tfi0R%YBJe3`m zGx(L4zzweW`}!AfG|w5?JPE`f_z@?;4})$ut%GVRin^S<@W#+r6ul{jTWpK z*RqyV-P$Z^%rrmC_Wct}?q5+LML$nmDO9_kV5#-8AtSh62Ncd75Yja)Q8z#pm$}SUveliP zZsG9i{^XN{7_Z6*@#Kra5j9sDASo_n5A0UDv}aB)gvqAZ*`;B1J?FM1lt{AgRxM8c z;Ao@qezWaTEbUFuFUL^%D>8^;SigJAa$4#8RQI>cdaO@^j^7_|j7fJ$xiWT;%S+<= zaxVM&d0)wkQLpzGR=LuqSuc5-8L)kI$yL}KcJUxC-VIs(LiQ(7g`5HnyQn4AoeOTb z!>6u?5y7Z6W+3e%Kb{e(=?@8D(sYy!P@Lr{mCdRPQ&jfW*;=}M1_mml-Ox{J{S zF%3?yNCc`XmXw|?@pF_U5BmjKGIEAOd(O$+XH@i^nD+y8v!YV9BZ2fP)M)D6`1HhJ zQ7vY#Iu>jU47J&WXn?`-U$_6N_`GxaZSqEQRbwkm2e}v39)H#sDCi}ceZflQ^XiY_$QwJE=E3gFfhsKiG4gq`*`yI z5{EnDpDACa`l`)GsJJ0G>o&eUwgv8z*jBSs?s?kqYqI*w)!wb+pIHClpf{#y{Q3Y( zIB=LH!SX+IxQqvafk;V5ZhK3On_D`BkxH(xuG*kGLqgr;O4580!*of#dW&DW?TX9l zd?#hX2`*iD)2Z%If<}F7c3~~0xayyvp_#=ra`U#?n(PDuI+ZcJ{v@}gh_%x>nfuP* zba>`8!6f+Wj9s*%sE#4-z7MWuvt5>HrVPKBXAj;*WO2D7Ln=oyf`~v&jKMxba7!UY zqYxRPQpvWs>hk#TJTN?m7!a9{%^5U9XHN>)nDWRVf^QV+rlbQ?!l)SeYB&$=S_6#5 z`>rXm0X~=s<&V#v;R(v9WBG$X{kW@8c=Jxg?F1C%^(E6?*;WTLbEYNna)bAJ>39^aIwb*^;9^G$LzM z=+57Wy8Z*dsQI>~Y09VE=auR2%tmw3yr9ahSc~&De?din`-grBE7pWb%k-o>AVxWA zyp0dt9X%F?$P-0fB8=Drf2zWze&-c)I-!we3=2&svB`==s}J}|Dq-nxbpt?6n55>| zlIiF<`YUk?@UZTK9AZjnfeDuR>0QEKOWO?VDK_apCqYmA9C~2xten-OIjRrT`yb^l zGy&b~`;o*i_L{&zet}GQVBmT|Poi%;73uMcLt(w7U;AA9LUI^iC55#tE^bik%*X&` zh@dU$`95IQY4)M+eu!yzWR=h)8lMCXORWDr#b=I3Ydt?Zxe*KObMg55G~3E+QL=3j z(npPQhnmjsT-(jMR!%f&8bok}DQ52t@ZFbk@xtuczsMP+^Q$ITD8&9&N7&u5#FLGk z(LV0d|EpA7H2Qk}%jD8a$pF|a?YH%s*`c+iN}ptqD8KULsrP)RUV45=um0jkKDNl1 zghEtJub{wT-aDE1=HgJjDFZ`6q(vtIIU1D5mm-{H%6zr4X>!yoI`VPo^#53*LL09- z8bjJ`N2>Rz_2oAQ0;FXXhK}*Y%zV%Z!^RBBrX>+Awg+1y&($&<6$!L0ug6q4x)77` zArRGk_YsN&4PxrfzW_UYoLp=s5}zFA>g~Dr8@8miIV~PIkqnM3nV}a()&w2^bSiyA1Rn(p;bBDQh-1~%I zaUeCNav2^sEYW;0ntOOE41j?@t zod+H`l`f}J32Oa0iJ~8{K>5j%jvBGV;=4>H=1pg2PV*=s)LV0OqQy^&OP1r&U^zRk z+)x3E3EV*6`tBPB9$p43XH@N{ZNJ6DP#tY#@vnmW8n}1@(`2^_gnP1N<6b{@AIRu* z4IL!<7NI=eUR1@9GRGX&x^kSCT}4|u0$)9`MaDw5dLm;S-`C;Xd|zmNb-8k-%LO}{ zR3sxtgs$J}XP(%Y!coqIv?r-DZ|U)8Q+1+(Yv#>AIVVRKM~nI~?zha)10OKVn>s}f z_VuwpYlfkyW#O^kjx3(C9ZFU6a6S(kcZ83b-b>sN=& zAsnfevl(V1nlB%Bc6`L{bSBGzSs-KXz|OzYP*OC`Q3nUVrX}|n<2Q~$mai-xgb`Wc zT`(|p2sCUu=mN)FiKpz776@H-hI<2FQlg47`?6h%CfZ-9XR+pAnG>2j6cIw&sjpp^ z-8y>6IxPWpB|XKH8I0ML2McWvBUZ%!utSO*7i@*rKc!G%4jI`(5uX5YCFx`nX%8M zK=kTn`;y5&dne+u3&H$DRgt3B{uYrJjn=vWh$YT(rq~@sg{WUy|EvA zC%7T6YM=J%df-1|)H!A$xMOMOKA9Q-QNip8_PV zq$pwh-^1+ufqy!$&>N`zAEuN=CY=vckZjl75mQ3G%$4;L4CfTpnf8b*o(V?#7hm0$ zd2wZw$UGCl*{;V8r{yHy+TrKv>;sY)riOM-2qnf)?!Wnx&g&0JXk0`1y^uY9gzI-K39S0QhfaNNAFJ#^ZG9wE|vg3tVN zWFEEn$88Uyel@f-)5NP}k+nQK;R~iiTeY|y_iqcGPgqg@2%@eMa^6_li3+yelaJI7 zI>a?=XQ39%A0Ih4xo3$BVwNR8N5gnTk6v8A?tF_W>_Sx<|ExX-NtI|3PUN)u@%Khh zzQlk8R*?!_q8uK+bm~KMpJt<0(AbBehGvqiQw-e>kY|K?)Xm#?rub8ry8@}qZ#4yZ#gRC~I-C)4Wcd4udfHN433xm}x?&+F?~cg{vMzK0gITm4FIU4d9=?@qbO! zXn9-aZgoS8;VSvroR1@U0nhH?gJz>>AXHfacy5PKyK?UV9g07FfqFEPv=H8~tiR7Z z@Hol5ihKhozRw?S{jmA8@^Zk|@s(KT@9Pr3C(ATK4S%6YFq zUrauO;!FTTm?c!QmAh13Kn9XOW0a~`4ON-Kcfcr&89Zwwao1ix`TPVie0U3|kLLYa zc*KxhN`d=H1YEz?34?S)n4TVwQh-Zno^#xEst%eVk5~uDFhO8opW=TbUTE_xI1GRH z1Zv{L*MbEsX%j5eTB78UyN!M}YCT%9;II^NRHgpXZ8aV}Jv&q&J5=uqC*A+ zFY)hGS(V+132q|}rb6EuSTI;xy~0lR{3Vf-)GZMX5)7Fp1^qzA8rRu-Yrl9fwkw_4?WJ5KrUx>!W9vWWe3hGxykJC{xP zNNWathuYzFYjt7c0ClZsS27X0U%cyB@@Uf>C(nzyEQ{{y7_h?Tf(r&Yxv`a$v<#@r zCP&Ntd9=^eVx)~hXbdDxP*AZVyXxc0YRHvDYH#51(cOyF=cygHCk1L)1Qm;7n7XFb zOi>;m;mpp8qk^I5Y2aHtnVYmeixe&PSHv0Cea-Ua`^Xe6vH%uvZhQpMU*~E)G*pf|-J|1Tbny0GA&sdfdJf#`Ja^LQEP>dh!6U2A);&AnV^rC6 z5(BR#<5HU9*9yY{Lu28DVnpwWuWCjqww&$DOy;9UJJJB70oYQr?-(JR*AqPd3~Oj+ z9wi1!z=9)5NeV?VvBq$YkC?irC)k;M^r__R5ti~$t(eged7!wmoXW=rd9AL0ijz7b zoqfgDX?;QERhO8T27t z15Dy!itxKA<=^Yy6?DgvkQ9T%2*o3lvQrEvBSKx-^CB1B>`Gu4kboOT8(gM|*5R^T<=+d(y{X#mBWj;G(*7qgt>)m=bV-=+@b$-RDe5NgAG5)V_8Vv} zQ7L&);2kj!aB5r>v8R6tRwN`gGiy>jcbRqT5mzs)S``i|Crf%e(hJ7(Q%SM*jSe2E zK;{)Zy{Jdw`8dBZOzUtEscFOUWvO}wtUxG3RH6$ez|Z`qieKzN^k!%c)%LUL4@oZ$ zS4pbY*RCfn0K!tpKqMja=jL2f0ZEM7TK}ncQpos^sHUPU(8HFZv7};9lYZ%uiW(`M z^p=q14l|dYPXH_(-dN2ZP6@0wR-HXg$YPr}0v{v%^1Bg0Kodi}r}7F0zb-I6PD~}Z ziCO2k=Do+k8^HbG%LGY4uSNfj9{Ja;esUDU6O)3?T%&sspEL4o8gs@q)x8==_YG!Y zUA2=I^ODi}YIc~1B^r@WZD^(Jrtu}pR;|+{a+kgJ5aUmfR3oF}Y@J^2n}2v1t#WfW zkrEM0qIgE2r~j~=(hx8xV#uFdZLMz1u5|VizWAi zL=XISUi!l`YGvIEXu5 z4YAyM+40aQ)bhV~7ymf+Pk%BUtohrrKJQ3P>HPkJ_L9VzKIQts*z|0>ySHaUmRWUp zG0x{;(`!DHNb3Dn>KI?*);9)8+h88=o>Fn%)$#*tirL9^aG{PC`l0N&aX|U(N!Joa*UA zI3y^OlvgxT7V)}jyM_m!$xn=RHa)EOYu^u0@{XLcPIyDc;7<>o|e`Ug=oJ5J~;HApD&L~ zzrBfa@`aO17QaljJslOiXE;;$0TZ28%+x{o0!~SIn02EG^(YRX&B2gQn;?ubeg9&G z%5KU&K{JOwu{y1HexCy9!La7J+O|$r(eW9y(0)pV;zX|d2U~V(rtfg3k#2&P2aC0y zcb$KI8lp+m`;iRhPxL~WNePCedOf7|zvS^a3GSa$sA+hp_>tQg$^`gb?!@GDdDl1L z7b}Fk*$=>_MblmRQ<@2rqlclatmz+TY>;|h)$+hVu_PEjvR(!8`S%x<{Wuiq2AY;d zB9Sd_sBlBeLB#6tK;Yicw)*AWeV$x|6@Japr2|Jg$pX@{n|zUU30BuOW~oLwjp-6l zR2}hm5?!EyVHiX{AF7Fxv;iwn%Mj)DwbRkHEkoTTIW_4O<#CMAsBks9MjTUuliYM;)k1DN6U8nY$?^2 zea}RxFEcKSz}bDPi*FQM!0@=AfZW4L^VkDyIiKcct>-f((%Vl8^PYB?bj7!Y!k%2V z2hPl22B-H2S}Q(^s7QEgbT?=WS5P?izIs34Q!KUPGDwUYO(L*hPoqv-eGy_v`#uFj zcFCeoDjSPv zG}GM!a{=v-zqhy7l)MNM$!|2`pSK3$-_wcr!l+gHgOLUTsPpn*O~h-7sY9vZA!&c3 z=^V~_I~)aJNVPtkcL<>o8xg}1e_h^sxDwp5aGzXhb)E#WdJl$;&pW3=TF<@RwT}Dm z5RM;~UaiN-`FF8#yaBi|IunN*hhlTA*{zonM<+G?4d1jc%e675fODr?tr^%fqHhM( zH*?LRr6o|lhmL-_0vVd#T%^mok`^jilQs+0KPhBC1^vu2MCWjzf6LGqoQo5Y6XCS5 zfpyh!EmB_j0O>{KZ9N4KPbXPUA8cK!tugydzzb^l1Jk=km{fyjipuzoyfx}wRxChr zd$10xalw1xFIDh@nL?HWueG;!TBrv=7l1zPY{{0^e)S_zK@i@Eyg=JBeW@%oh9se7 zncd}m0X(lOt88+oqU=NOu0eJsjt&1*fVvQ~*Wb4W`_1r|=i7qZYGe%P{E<-5Op;k<(u1PQFRYO$&-0eQ+9yq^ytFgkes zdS|x`izbhb__tmRR$aY9Q@XVWw{`t-hc^25W<>B=;~h8`)GK{UQ>!i`}S zaP96_gQN0i-;zkZ6mQw`B4r;L8<)e%qK4y27a)FOdme|D!lhRYW?<|{6SGN$KH%MR@H=8i}?EURn17FhRb`v#; z-x=NF6^G*Np_V8`u{~nk+9HDo&!mnVf$_rQzur!nZs(w&f(uU3KEE#WI3Is{c{sp$ z)=c1Nj8#OI!E^89ONI~spD<5NPzm-stMU(v ztd9T8bHk`Qh?rM}w!+l2s#$EnYKJG~M%3fz^xL)DU95-B+9M74NJf36nylFA?^pUC zQL0OUQKduVX~8z;L$`NabP2LRP>MD$d^08T^-_Gwpu)ii5Z@TlDF6aBed;ii+*wU*ltc~Yc?TW(>smsjyeCn?h(%7+-&xazw61;`X z^IatrUz^B(D%|DC7E7E;6BRJ0P-{5`5#PX80mhW}Q0zmVDfxqy=E^RJM@}c&4bSub z+18^@se1^`aj9+r=Kl5tCBKnd$FlR{6+L^`qD}R;B`>UhtY=A~*-*X5eA(M@0$dgA z3}HRpDQyoKeIq|OtVS%|mN*=C4ML%(NJ6@B|8u;ac2ONGb2-YYhNdBJW;7?`!IS26 zRV~v_)oto6k%d39JTji*J=aplG-*NAK|6o&YX*VaKq=HKeo7}p8DkG5| zjA3DO?~qn-bhlb1)w6{3Em)0aQ}}5qFqd%3j{_$yMJ{uQu)n%ww*;9Fd8Q~Gw-6~Q zUzG^-zJxq`JmKKU_lk(H=@$l}-_v#^nvPXxo?mofI+N&*15Ts!O~GRF>HA4C*3JmA zUR5?f0iI|kkN6U`f_J7ZZ(BbYW}p4X9kLhtj+QSXJwZ-wyPs=bG89@eg2sXHx1Qc# zE*=qz^91)6uHLXQY9fZI==LWX$O9V?1zaT=K_~CRo=X^HT_o;`Dgk+xz-R%Vmmhq` z>NJ7P5pek{QGbr@JZ*bb6bxA5r7hkPsScIMK50gv8}uL@u~`U))HA`7x*hS|T4|v= zP@yXjY6OsZU9U{u$cAz%h({mf7Lj=Hr8zYQ&v;!Hi&0@x@s!7+J$)0s}_{%3(wLTxBndMZnC@H2PDEO9kOI8aKeNX05aodUeaO-ryN+;KCGLA z?8>pVC!iu$l78YoT#uu)bL*<$i0An-{Ko}=uDt+7Y~I1vwwZFKJKn<;8CP>@dH87^ zEcZ3^Jj~X3i9`CXGvhIMBzK1xn10jeyrz>mI2bWIl6?RRs&G_O@a(4_5OnC*ch+me z^U4>jPaAPQgqhW8_3f!wE-==gTTEEn|Ef08P8evZb^Hd!j}9_@nX9WqQJ}PhBsDOS zmgs?*sy)re^8CoJ?RbBi?3i(68BJ&)IHxvP$As4zfwumsayzF!M# zET>}=PxV?P=0{pyO`*gM6+&5)lyZ#kYIZN&s?7zMQt&zP*l##7?yywT6Omvp8*9N- z!>rsQm@(f{UTX&$lOC(R{Ybr#yxZSo6n*q~rnV+{N{`v6z=}^4p|H4*;JDjrcZM>#_e=I@RM;rSZpZ{)_*$Ktu|43bVGM}d$v*huzZG;9M4!Q}L z7fhCSQ9PRrsa6}##`{b+EgivbZqs>tDz=C`1%)~ZbqAa)8EI%}C~=+}(rDW^4*0IX z7XGQ_q#Gw%lY7`$Vt+0cT5uSG$b6B}B>i6%co#oY!ZMb3(%D{|~0;nfQRSPLDMMby@Y+7Ry4W~sK zFb27t(e0;%z6Q{Sr>?&Z99vB$7?+STRZK9ZMFzAA5QLbUIHO!KqSLGQ(uT+U0~`w% zr~u1OrIYgKi;%qT$&^IrV^bSi4Ub4kzc6A#^_Lr6&#+%f{*r2@M@8Sn%C(b?kmv&e`o$plCb|?s8sOysXe%$a}O(@ zqB-;$kc^o0i6HNAqfjo184jVG7Ilg#4j*;o{e%VXx<=+{)EEtSm+}j=u%Q8A%s72N zCFH0f=V(cp1e_1Q3IHBGk5>#6wbm|aW_$nj-sy?K(|34f1tqkw#-~Socv$ED zcOqMILWp13$cZEXWpt==gWT~CzjUXbnnK=bs(_w#t7d$UKlx~okmXduho^_sL-qae z$S%DThNQ4&PQ(5VeGg$Vm9HQF;L+n}H{TV^a+96q7}F=OYN)X6zLpVP9&l3iYA%zU zoWj{;hPmvJ0Ql}oZ)jVeKS;3uqv&TY^3;1%@Lk3OT0%`|&o6jx+$gqnpdogWWzUcI3fjB+TW^FBAJIEEsetZvM z8wnqSx3rAJ-m6R-?Kbvwa|ez=IYtu~!>v@r&gwVv-=KsB1= zNv0f+%fE_Oj@1nn++sI{2>dUq4&aJ|sl z3!~NIxPp=|B4DA{Lxf)X%K7lXwK47o%b4@oU?RcGa+Xe=TttQfv|biQ~hq> zFh_an`UpZMS#Z+WgF!qe!xW`S=o-r}i*L}9<0AW&Q_y$RbDNLA4Q=@MOd(@$6ILg< zQgS>05Xv@i?gZDJDEd(gJPqRa@;#7vrqHi<_|n=djJCPbLu%{EqQmf(d{~_1MF-Zi zLU?+=667dCpZK1K#yx>G=hs_hO@1c+AC`S|3r>LRVvTMh7UY-oD}5iP;iPf=$X{8u z9<7X+J{wn+i)ktQgtV`j=So#1+`}SCzZ7Q|6hoHX>#wrqZ21;zk1BGsbgnP^jKSL9 zk{j^`IGPh04~_~u6uHQ^X<-YuLw8uL{(fdZb+gbWruMVKU#oi~SnAtMe(yikq#uIg z(V9I@K(ZlO+`7EKYdNUuh{V>oRyefbicq*g$HHnV398D!oZE}o8l=D4pT8#}Am?5k z{s4R?XA2y2MxlfMtClf{%1B*8PC=feHR!oJ?j?9c^s76A3Suv?6tHFHwkA9;$2y65 z9Saf;q~u`!WG#XpMmsS3BreT6{*5fBuh`Xr_wzBhqE+9U4WTqMZE%ARJqHV9`B11L zg+S;tCcQ_qdEkDek-yC+o*epVDmPf;>gp0QTm zF{laY!pGa@9b32Ts?YBGNeh~5*=_YLO@E0q;p(L!Ak-)3EQ~kSDOd>hy**4kS=1hbyRU<)%!c9 z%WOZML(bn9$@w70d@p^e)P`0>@DnG%IRbCm`R=+Glj1@^yzuX512jL-iZD?<>s*M1 zVw9y8y5ha>1OJ71kwGCfx`lHW61SR|Ypi7#yw{s~o6-jq_^E3%tqXOf2{vS>zFgx) zrAz4r{8gXO8GhfL_`Wxtn9JYXg(-BAB}L_K^axAnS4v}-<&&g9;GdUbxtaOxLy7cN zH0;NGI`1)I?~q?_-jeR@RDKRW)OpVY&h-;{6T|tigLCtx-;GIy+y+FV(O{{`SR<6k z@o}rzAnzZgxJ&CQY#WZetoi`6YO}$}H?#cs7F%9_eCMAesouv=xy!1dW0f)=we?!= zW<;tN4){-dR7~OpkKQ1`|10|hN z2a@(h4UDxux^8XMOU;D5oXoHhJ>$xvRq=W3p#VHym)yuMpWS@Y<6`de9jYl48H|PW z=rcb~N^OzfxPzd>Xv1@P?@n62k!lY|D)!Xh!3w@OqYUL45U&%P*=J0F`ozcUfMhMj zZ4lz}CfkWP_^+C=D#m)9{FxzGguJm8zNg%&SUBd*D^5Vc{y1mIL0&|UI~u~{wUlTM zy$HSIw$CX#^QG3GIKsqLEuE<8F6smXqUtA`|4?$AW4oB+&9wwXA<1ew`#lUenemLp z@Xq9D_cH((Iqx0T#O+s@MW!V0XwT>EH1H0_P4VUFD|`>2ceiVdo%O8Xkft#OW4l#p z{7zEXEgq1}#D5}Kn(mC$(6u!C1qiz$Q9r|Yo*l(#3;OZ-3+bbOurMyG&JC-$e!lfc zqB_Q@ambWvrzDEQR{KS)NaFW_qc-q;AQt}cjsyGJ+M(;{1D)!_Bc1oX8gyX(*H(r$ z`hE)l*v^04JTx3oO7d#dS2CG<+N*8Df5}5*lwRYJgp)uYh|Og(479d~=TmjRwm(Vc zu8st~d-t-ub*1eX-sepUp(e5cE$WeEX45fjW_Y{*7h`W7)n*i~`J$zGaVYK%#T`m< zcemm$!JQV@P@oVTin|5Z;_mM5?hZFSGqdK*y64V4`RiK=i@g)V{`ULq-}AhUcg~@F z=vpQrdtwRI#CnR?fgMT z$HVh}{yROS-9^U43O{UFBIunHHQjEnP;i!}_^n``6<@m3P=_yK8h(-Rr=GeTV{Vq4 zYHhI_TFv~*>J}Vaz>|vDGJw3(7iXmcVa~AW4z!%3oc&9;pqpr$);5>j(FU0q0R&S& zl7M>ZbwQCp(OEl`%9NoOo_=uo78$O`y@ZMq2iS-*b7?lMiI8Tppo;ATwO_pLt{jkc zi#Xnn*JW!^`W>h@403!E)m-?vq&~B~@XoOvFrKHI@yT~|vB%*#L+0GR3p{;ad#?_K z;1l2t5~;COrCmVwdotGazbBdA67wp#7xL(}!;)qH7xESQTgrnbQGdT?=KNk5+2222 zS?C9H%(`LMmQ0Y9&LXUjH*R0{$L2P{G#nAM>~u6DZY72=J(X?rYTJD_XJ6at$3^Rx z8&$Xuu5fpmeI+7&Y}pcY{sngN3FF4KnuUOH!g)_CB@9QyB&ou&y%>Ze6-mCqzTG%+ z;(|pa35un0d|!dlR6GUNRe7jSvn3Tvh0A@Q zy*v4C6d&=+0@m|gp5j6^O3a2`B0vb$oH=a-$x?!A|mxW$2iX5q&N%hiEY_X^`w*` zOYYOF!V^zvbOTGm4kyA|VtMj7;M!hw8||CYz&1Epq+VEx7Cl?-KJMl8?jr|obfI~B z0`}l@R+^w&;OlPA?%vdWUC>UcataIpo}p8sa+ zaJ*j4eZVzI;aDjfjSO_tXu#wdfG;Fan>#I5$xd6xmb?ExDxgrgDLn{CZeC+TA*M=V@k!wH%+F!~TESZIkUoYBx`lbsNOPjw z#NUN2Q{bXV`|RvLZKpBcYEk7M(FI|;{^D4sGip_|rriE}h3?ksb#Jv3huBhGeMX@S z@=HS{52cQI&e19kc;)mwtNsmB4pECV0i}GE4O5}@{r|(Hssh9LUtIl*!)l0hp*1k43k^(x3ZoSITkQwf3Z*}nt^|S; zsbYd`HLC&RGJ?x!xi03XzO>3RYgoNUqt7x7(;=ev!>c_QCov4morAOBYd^)z`Aic}kuYBqaw~N<+-6Cv9 z5qMt5+aW{)O@p+@3v<_^m1i%G4}>q?e+?gm{<|vEd`ANK1(}@Z6x*ksh^%;qSl*15 z;|}WmB%!Ve$yS-jbc|GNw{^WzQY74)0v?Kfd zG_$piYA@a!k2%*h_y4VDJ)WZv@yu*5R?<*j#}{8O1U=v+7LOSBljlz#^Ae%STF-Jh zkXiC{NZq%KVD76=ES#RCPb8deyLcWX!222ucP7z~U)nVwOq3h-%5fnbr-nq|phTCi zoA2g54-9`T+Q3$(?x56y6vyOe0jFf;YcEL+-Q!gory%?ZoU%0Gye8yd1ieMi2<+WH zbfhmU1oy;77gt3UD6Z>Wq_G-bX%S!UJS{Wz4KqN(8FFemy}@{-g)a5z%LQ)TQa@Mh zw+|O>DRkP_O!tDFiP9cFR|-!c%iYKtR#*p{4A9*Un2SqD45m4@dtClG7z zrZOD+7rRBBtxHI7Bbt{!zdk%>b%5ac+LAf|%{CnLYZ>s2KR ze_4ayyLHBjV|)Kg$z$Q_J(H0Z)Jpqqt18pVL<3P5DauKn#ph`FSt?D)i{e{%6b{!~ zli;3PA{M`Yit-4r^>j`5eUNpuR_+uVq0vGt0%Wy59Fq_78SR>Km^R3DOZ9noU4h)28c}&ACqRW$!7$ z!p?=$KnAg&;Z}S827P#40~LApo8Nm5b|cqZSD2suK2)j$9ckjRrKb0`$R~69`TwCR z?tP?JHS6Awm5KV~SdE|2qg!6`n~gqjOzXpIA|(X@JGe#SD{G=rbkYjN(6A1>I*jYb z^!LJWx%H-a=90XZ$DG8KV6qBOz-&d9X9Hd+h?I=jh ztU`qn!NK_4tU4lhmPi*jUN?hSfoSEPj(0^S;gDX&SOr-I7DiC)c-zjI8QxvJ<~L zcFVA(BqzOFR_T`jh6ma>Y(8+(zdF+Jao~FCI%CH`o*LpiWo4uWTKZR&Esvg&5``Zl z4To+rmuE-{3VuIP?vzQ>E}e^{n=rGb6%Xb zmfyvgG{(jn)LW0#@st#c0YWsQB%1U_eDdFdR(i6@4u*%f<5EHv#Irs1TyuUL3=7jl zP5rZDfVbcgJ%*+u#2~}08R|h9OFhDGo8^tZ+ z4MvuyR!E-uo2H`S)iQmK#}%fC5l{7l2t!m&WX1gKvM$>Y3(y{Ck9QXC(f+H>Bx86^ za_{gFtwZ07yC)C7;3sF7-y$$FP?`vY$v}e-jPHbs#)P zRX^F*?3(_B(9Z*{~$U8n`+EyA$$?2PH_7GgwTPsVddEBHa$+ zVa5dX-n>VY@VUhD_m{hIyjv!kY$XNI)uhIgfy7?VNWF`i47O%7jRA`#Hvu$5HpU})byE8+zG zs=kvecvVX_JS7rwMQJ&>IOk5;J?5@@l@|1&WuWo6`nDf#ynEG#?F$x6 z3nzEzhfCGLCSpTQX%NPn$(8rnc5%0)fADw09zKvLgMFbr~2m7~ooe+VkNfk9`?d~sA{LX0>xBYF=+>&rQIo2}Q3 zu8}j-gI8918=>t;@B0HcaU2#z)kFQO>V|+}HSy;R8qoimDpv{h`%#PigBBJff5?l{ z^6s58h3|O^5S}qIjldU2Bdy5Y6vBL2dbp52M2?JrWjKa>|Kp=~M_cgpqevK~aFXXG z#e_}ML-bhI*MN|j1y5&)&MTdrL!VlD$H#v;lek6Oq}Up}({cs8sBvW${6Yrk?nC z!Btb4iP`I6^9$3#g{{CZhfeP*Qj6|Bao0chlUyqVz4Z}1$6DF$4f~Ha1}2%9;5Xw} zcR~ss@mQpO2#xsi{6)YJz8?U@PKCO%fzXIn;yAG3sgk;(kz3P0B>#ASN}SnISKnBv zI7G3>3{Gni*$ou4668-Mf*u^%xSENNgyEen00bi=F~}gwP$vOM_`sP;Pf{D_doo`i zzn84=q^!Ds;!22i2@%ZmoXD0zMfs8XNh(LS+SOkr9u3_4@!-OGH4W`2WY(thYhCWg zqnu}JDNt>nL|RxAzv*5K2wd|fYedIHl&#u{96)6xy02&@Aazi&9m=VCP^*9v!zSTL^y=Go;t76MW?7TRR9G z6oESL^0x5%*d4Gwe%Z*$j&)!2T^*;sZ90gA^dxVgn-?1CQE1F)XgNU(>3w_ljQ-+1 ziZZv>#lBONAIl;E2Bsu+J{W0wnEEHie^=Z5q`{^FKH@^rQLwlYCQt+LUhO2yBas!A zAOVBq3GS$-xtHjsFEtEFm9eh-VhP>`y@>{JFijbgM`(Wg5;!ZcpV&c?iC9PZezXt4 zM*9@gCOT7iS@v+c!-k0DedkV7hEln9lVEc-#J6GLYl1;KKytG}`sr5)9xFt|5`KzL zk(>MSddoGEA(E76YgkxO{P7WLVSw3nKp$U=l~rHmYE5lMj^AJAA;J>DZ)8ZGG*yTz z(^x@p@qYEX6)(j*F-Jam^pSf*8DsWm>iWR#)3ECw!wv-o*>-siX7&+o>DOz7w{QVqZmG!nzLCah(?S#Jd4Qqi zX(&&T!JWUb_GC&!yds9jTyVq4e&=UO-ua!(m%mwzrZ;H0uP#Q{W6f7652xs8t@;Ti zb6;N))lIg+KVL#iXfQ!!o*G~iP5r75jg-fx;aY#>+g-*YV`&vGX(w!YSSLu^C^Hw} za2R%0fAMJZn4n&W7T(iNAVryOBiBkHAOa%j-hMtD-$)EM8hf|9{oWu}w`+3$Ds3PH z8qh|-XTUTZ#W5|HoN>|ob&@!eT%Bd7jtNrOk2OX60_UP6B!caO1ZdJY6miEr2A-H6 zy&BTpO~0@?3N^9R73s}F#fx<{)!dNqB>}ek*c?WX1-W;uNy^?x;okXyE*DlTIgL}k z1B@ug>mdJf2Ann`9aVx4VupDo+t^aTr9 zTXpU*|MadvahzV|j3+mJA^#1P`^Vu=y1y-iz2f!jCaz;wv^YW%lU=~8*U-L-5}x8`TL z9P^Pn*N@bmoMJHV??jKq%*;NW{o^_rViH4=1E}Xe87G4``zI+^iF+S89>tpQoKy1x% zh?OKpRd4gOMc$phuDp*)w2jd*y_K<+A+}nXC#3p;Kpp6-51{6n7;F8`zR>(qzK-o} z-kS7XsSK+LCKyV|BW~Ltj}y*Gq(bXS(>iT-lt5VpE25(Aa#0SO>X>|$eJz~e;3D%A zWeTgMzwjut)`sns!T*Z|SQ?1h(HLIveq#is2WZ#R;ADj=eB@BBjTZPh%p+8ZPXw2P zze<&@|D%!MDiCo+FMz@ez@38mDr*m5_$0rPrv~SWIbj-_?z96xsLsI`;n-WE%19w! zrWl!xpT9?_*Gq#Ikuq&Gv^%UJf41d#i;I^_k)*gTV-h!+jt46mjk*qcb;szBP|8n4 z?9?_|Teigm5P@R#lHsDKzP|`p3n&iF039It!krxIpBvh=mS)jDwFTl+uEi7Ib;0;J z@0@q~L+r`tQn<42Y-UCDBEvz3UgwZD98~A_!$}6`Ym^r7D)RoSZ8|-!uf`;YF0P(hH;64o z=#A#I1oZ-#`qlhFVK4s-+4D%Z`3hlosvWHo*f*+Y#W?YL;R@mQ39YK5WZ#om)(gWK z(D+`}Na+(uiE<%Z+R_dXFCSMNI{IS8G>wGadvsE2U+X^ent6WuteQXxmHI!dp+xy~tgl*)Ehq?X=mR4um8) z^;VYsz69XD8W3stK;PDEkYNDwh~mm2T^`QYqZ6e*>-6rZC>d%K1(0tESgu+XZxfSc z6cAyzgzwnBNJ=WDlLv@P%FM(!vG0b|tM(l*Cgw&y{~5iUP>|E-InwI3wUzgYe0O`e)q_}oEN*h z{)_VQ7{EO<+?6deTioHDH~-wTV!mD4YP&WfzFT5Ez&tlPe0A-BDh zPHph|Q7EE2wAfb(wtD_7e6kS$=)f3UPZ(J?looM`KNacd&8|4L@_5f}ZPS+8j|jMa zMxMRy=$OO0dSKd%Egzp8gguz?qY7vd>WYTnw)XM|43;xv;rfq56^00|w_%|NI0uHF zkL-t$KZ5mpWzQ>duMYiEYCeu?XQ(b-*nJYE;=!qKak zUC13{=IW;}yPFUZ?39iIbf*8pSgC)88Jg4)TZtv;`4FpJ;4xWd^p8a((-j`G&HX#vaaB zmGznu;-LzGGbwU$xhhEc@WwY^pXqPpPz*i;w4UDf6ve?Zs4*=`ImCsePGen+o6UFqw^~kBZx0CLkVV z%Tx?~Lv37VRxbav%La$MHm2H$-QhQfCHgx%zOPdmB@F$15T?X(KfV__h6G@;BymH$%WG>0 zw!2R#Z-+Z>7I>2rjWYiQ9;rAvZoP7cZCX6{vl~EJz(%vwE}U_hIjpQFB-&4rP_|O! zd=5k8PUK8`EbGLpetM!6qZ&LkS&R;Nw?(kN-D_wi_78=3?CALcDSdCqN6&xxj(ITl zt_!;mS*WUEMdo}jxN_!D2H$ra+II_AIwUT@>1m2_3`itna z`Tal*f~hURf1tgwGk9R}_MpDW?%+3bp%&(J2;k--T4sGey2l?xU!F$hg+>4j_fJxhw80*;g#+bS<+c-a1O_z4tI69A-Zo;xlxhIdUJsAlT zxSCV97ZA?&nb%5byQH#=A!*DI=-KR%BMux}72RqV$3AxsR=*(Qu|SBT5g7{Fe*d#U zQJ>>cD4!^35b~kv4_V%eP$0K~q4{33cCvpls{d`d$^cd~Iy= z0gNx`2T)7kt9oI~Ja~f%$j58d>KtTEhJ37DCF(s=a;T+%w4BUHny2JA$y2?J%m4+W zCRF=Cuy%(9qXh7s%fUACoY!sFbEhx^BCx~?4IAaQcP1uf=x&u1QQ7N{lj{+cYMykW zE!>yW@RNi5CJCig+&t?J#j%EWVM1zirKD9wN!fo{s~j&hUc+5Z=6;+vDqq>^(9JJ= z+vz#h<~ePZg^E_-N5O=X{l1p~DL_Gt2*eD`&eSYj*|^bg^If&$ydw(wY>k1~ZH;iG$)q}5O#Qvy+7$peRw3u0CCd<`81xxtesrWEbwC+H(j(XTb+%kk` zV{SY77o{XsS7Bz)qsb5WDl*G8aj#9)Co75Jk(pI2ZAHfgKTP`U+Ff=19Ohj^xe>cY zSF@Zu$vH`g6@Wj?ESzSslXc#!{991tm~x~-OjWR5-~5t^D+9NArH~ZLbfCCG59-3I zOv!53!8d4Q$8a|l4!klbgxo~kz)S{o!EcPD#Jn1&8)BnhD9jvm`zvf;=womQRYar_ zY(8Hi_GUEwi#A>%ip=bw-xmPj+}2Rb_f}G_rt)3x$D_VH{t=k8Ty64rL8fYbqhZ`v zr0|ntX>`l+5kRNw!8sJmGFxk|@#Wt%ZO;Tp<{Jr**BIemKR!talDO_V%SD>7o2o(q zvP-;f>FjzNs4ls@{nqTHoNET@?_7{9!1R@NTp|n99Ta?gS|7vQh|GqnXjAGZB~yTv zS5H4ulX?6{E|liBzh0Tsd6|f}-CX0${uG1iOD#{l&G$-kP| zuMK}o1ERl7y~#_yLO>5ZQpWg=Y*A!b$p(3ca5w*3U$AH3ife@HEKS9+2mNtXyNRCO z_z5aSn}KsbkCcM%aTs3KQ-e9U`s$L-+ud~@Ksh-TtoVMcu>X;mtwVnM9L33TJq>6W z;wUexOLS=7ay8Vz$m{9J0y})Hl8~E>ml&;IzH;27U5as=jzi@`&0%puOPJVWKK;0z z42#maqBdTs-0zJcG)Knh4S&)yUn5-iG4}g#FU{98WB;rMctWHk8!kk)4=N3cpS(|q zQm^5W74d4WH+jc8bDP4qWUt99zmx?SJ=K+nciMmdPs|i;w*xo`Qrc^B{n+{CRKe6H z1vFo21Vj^F!^EO`r6QE#u{lh^W2gu~38A6AT;T*{j(MClS?BG-O=gS2-|+H2EZ>*O zYm(cNa#&P!h6c*5hR%HXt*oR~7Di}D;*Cv21Tj=`LaP&UGjmICb4zH|v&AqjEL6F2 z5j@TlNhN__im*^qCG@BKxn#+m2~<>6uJ+O#4O_EmhLqZT#SwDwSE#am2`=xvzFOq( zzVqGb9qyETq}!Tth)j20FduKZ566JZXt+s9peP9s=iNEj*)xG*Z_K^iyj>=WVDJZ>rn_a!mXZ%B?$_@Qa{U z)=0(l)?sM^$0$0a9DIhs3u;RU7ybOFa!1%G11cvOa!_UW!!sEhIw&}1iH^HpCS_?- z0UMjmGgNlF{$93a2|7pXlWC7$O*adLC8fEiSKAK?{{K`n6aNq4fZ*z=IC-Ua{fL=G zQi3U?Jwd$g;J)_sU4F@He|~&}LReGevZ`_5by}^*-?#au>ga07?Fy5WCzp;aQqA6r znjhTbJp@LE0Ne3eB?NFIQ@hXDD!t@kBG%L_kT4I-@$4(i10BOq=0W-=c!;qE=KYAL zmnBTtkv4Ph(;?TUhtA!-$CW%KD_fWlqY6PoS!x6E`<&ik_5nfR)^JePOHVQh1{}Y6 z0)AR3{Q%Cb{91g9$!RBLM%yI4#b@_p@oYt!mVOC7-v{O@bt6H@qi}_hPI|K6-uBHC zc`x~L+5F?*zX^h62GuW6kN<{~zL}}|pksd}W&drYIkvN#d0V^@FJ@xL&OOf_JnPus z=Jq;yICOO|v)Fq1RcMHgcCk?~z6f^lA#6?TSd;5Q6>7h}_4-9=!2V3-bM7+dxT}ZEmMN`NW9rWah)r9IGk1)5*dkA0edy+!HM>{nDGiC z+Vh3WM1d~cO+lE)hYU*vQ&o;BmtAe@*BnmV!kI1=Z@i0RHpZ`?8|b~ikz_$!g13D9 zUQij$u{(|pQpwfd1(^*T-Bvj(@F3A13iQmCD>2`87y8RT`nYpVUHG||`$aJ5eky_> zMG{6TdWI|8(zv zaTIrxZ6j^fTT}a2_v#eSj=VToxh_0LjS`S!o6Gy!Ze)w_&panFX< z!hgjTYTgW=qG?ntpT+w_2XoI#kPZO@xI!)*7)hxa$FcQc_ON3k^nd*5YDm(N%D-|; zEEi56(AVU=JCvq_{x7*o^?zC2BqbzPd;TL^|F{4DtkS@>AqckrZb=NX$Cfw^sn_0& z_EtXsGoXqZvLycts)EFF{qL*n8NYt7{cSSqaam2rH6_`m3c;o3Xzu1m*8dY)a@_L@ z=P9w{`Z?+nVEPEeg1aFZ?yf^}9BYC!57TW)gyoxkGfKf3z#-ZF1~*8KOsyTlC(;!frJ|MeuQdS{_&vo$9Qwv zir=ORyE|pHjUD%7hRHQawxcHyCmtX!hj@R{=-~KR&PkkAc->EkI6A{13a^Is76k$K zZS?M&s?rS9dA8^cf&(W6dN*C#TnKcbNBJA__L7?!BMC@MoXCr?TrgjUhBXb1(Pe0og zhRAC5St;JlaWB>lOFOG8YdWo3lft#@n+))2sM&$jjHD=X6^qgPWw z($SN9!Nam8uv^~7=9>yO3Qnf6xg=Hh(1D68%Zi8)0kktnL`J7AGrJ!Y4=eq`Yh;@tx*})(3soZnhf+1Cl3WtXC!i5Uy z(hPEL>nt)}0tRi?r&P3uUm6kcxWcn$b4p%;Xk0K(b-bH6E5C$I_gr|NhvHdH_Om88 z@p0hgAbl{Cq+CE>IKRf`Tw82(5q*`8AQEdR*i^sq=r`fqJi(HGC82yY_VI?M-5_&# z^nBtf`%%~%ZM(SF3f`OosmG3+NN+R1dGi``@^5Oe1hR~eo1B}MCdJ>u8ZaY8=5*Js zJMKVz|1WFE>(nJh7UP${Nx_dT-`SfsjOv&jE5~5So-cg+ZmgxNKv)*@P&Tfff%L}- z*P!I6;=+8>!$pd~iJCW0{b^z^KgcmqF8aR2P`1Kb0#ZwlZ zE8<{0bgI5YWX!_S_gm`3`zm!he+REB(Q>=)qSGjMs*kI1rCV%vbQ%HJlDb)8QR;He zP8}F9g&C#Tsk}=)w+wZQZB$#G&C#$mjm7ZYbkrsB3mt#DfWJce&Zk#J1RJfJ-dVGr zog*cyYiRj!*?|UV!j=HvBm}+rf}Bc5i(L3|_Bt;M66wuaWSd zE^|IIkbTU7kyZNi?VS1dgv>{Ba>Tr`5uNqeA20>-P@NlISz)AR7I(Tn8E$t==BI76 zRm>-dZ#-Ub)fqnQS5fbmo(D9-#2tZGEYb%n+Ue-&kp%1ERyn!3#TW#^rNhKMwYQRq zzTB0ovgBxJRAsxh1po^_m}g?;DmP+1WO%MBShj|gcU7~qDbV5bjk+c&168)7m<@b) zu^l|C8ol$dyj@jX?UALX1K;04y9<o?05jY+NDgA<&e*ue-&uo}U3WVKB6X#gf%k`t z+c*~Xyo$g<U=9PLJ2))ZSnkSvf>s{Fn0H+TJFa+dFr zgMyRSI{u^WH!Vmk&>9gyAWgF*&dX4GU1Lw^8M%5akcPkc!{!=DzBat?(S>`ZZFGm) zal6pte7imN4qE}Vcn`h^;LvW~{c?z=GxdQghwVei&ykKKu1p7kmI%jnE zznKpLh=s^9^4dzF1zXb<$joJ0xO8U3!aW42C~-l_pBs`;A;$%&7np9S`GaolHnW-f zJ=F=tL@x}uq#D~NHIHS22=V-B)ufKpQkT>gFN%s;GW!c%#xq}dc^|t8ed|x3yvwTx ztuTSq3GOC*Qcygs^sjIb5Un&r1oAo0R*9PBz9 znKZf2EuK6pIMcm8nzYwL%C1P`Y`^98FfU7%^7=;hO-;^pyRec&M5kN(SO%9=X*QWJB8U?~5A{l|A($9Z0d!A6O1BTV( zj^Jcl0HxZ`Zp;rOD#{c$M#% z2~CCGfaxFVVP@9I#XueCo}UFchbrklNwg6?-|0T{ON$D_E50jsny}#w&aqIn=qu-C zX9ttiZ$l-7qFNq(AWSqdG@c2%t+IOv?iVo#gKbg{RaK9+0x1nzqY=T-RdC{8V6RfJ z5}eUl9L+M>_7rS3Drc7@t@Jm~I5>x3R1PUdK7QL5ogn%gcojxyv+Md{cbqh7lHloj zF}y&<#xQk{E>)3Ba&lW<5NYIpu>gl}GcB%xfAM8ZdEmvI=-Y`m(@#n-Cq)eByI?9D zMl5mwu@&OJH*B<;il#N@BC>6kp11Xmo^Q*&^64pxB^G&-4QDfK-^OyZ2p;ynsjhxz zd+4A%#P*FOHS|kt>d$-081E3g*Q1Iqa!|MNW<6I}>8~juz3e`Jyg9;^mj1+~DATr+ z;pX(D{#o|>$DdDorvaSiA|$zZ!eaMR^VjQRs+o%7ey;QH+0 z*Vlt3WooZ6b$TUdi)@1udd-CSfX*9EzK zA%y@McA{~^VrRKRQ3`1_cn@bZ2+x80iN8jv%4@+htTvoY65;1<1Gs}|wyxv_2jwX<_F zqa~_7(Tdy7ykfG%^U?DU%w3ScVgka(UyGLd%;aiC(<43XiK&6B*0ea&K6dvLZNq>~ z3=#t2Uj-!Q5A|5=>DMZ-lCWP{`q2t=jHyaw!~`{}>{G`qOQ$mQms^Qkz`{M6+~3zP zQKxyV)z21!$b|$pEcJ$K9q&zeB&S;(VWX|gsHo%q{A4TP85b4`_c=QhH4SK+Wlce5 zj;iMNSlbkPD%rJTZoE+G#7(Lcjxqw{iXTf&zvR*{cPlhZ{wYT<*{QMlS%1 z+%tovgY!67(&MRGCe}{#C{$B+YwqX%Yt>+?9>HkxC zh(#c6<;k3>tADW{8^dKi^%qs5Cgs}6RM$29&vNoG_$=CO$wjLO+D;FDx(qY18}{SD z3qk*He!(YMk*=7}^dhm@D%t3wo&Jq2k1EfXkBViYsn&d3@Ml*VRp;-4nHs*A=x9C5 zrI_erd`i<8xd#||-AG5lmgdB9ANx~u zGhiKQWneH4eO4HW$qwx6)0<-r91XB(A*P3iZL~*Prt95o)iS(khW|~eBb+)6As5XERDHSQbg_&c zznfQZF~UtoxK;n-V^FpH+a`}@U!~b*!}%A_4XG2vvr(b)@*jl6QYeLQ)e$IuCYZ=t zOdH_-Q1(M{%FtM=u;bA=qeAr|wDd@kSCNdt14C3qz#uI%HL^>QfUbp#ktX6%??y#? zQDlfkwZTUf)S2*82|ui&&NhOT-ooI}Zv$>o*?`8l=^=jIi}tdwLTivBCw0Z8HrL zAc>waEaqTNIWm;`lMDxy=YIUMpOBduMPI}{N+~qpLg5cd!s={&aP<5G8Oz7UtT0Oq zwM6CzIU%(P*?QmTR_{V+aH=!k{sm3R=%+qr!>+W^)gk*r)iQ-TVE* zV>>JA|`VDsmg2KKTKw(V3Tm11^h z9C*sg1jPV^`TR_txX6lk1&D?y!q4Bp`@S{nL;UZNaiz8u&LJ%PCVBKHH#b*pb8>k! z0D=0$SYu-&VNoa$2vqEqa^!g@{*BMesOwz?d17odC&!_*aZ$fkPeROSduHR> zE>$2FM*xVia*q08j`hurMSf9oCAIv7wP^J9o{MpF3LY|LOF)Uj+!Y6JjV)nVO~tM{ zMhuoqsB-^&SStG1MiG`}oe^mhPK*nacQIodf>1AlTzp0=t}NXMe`n_CGoMRo>QJ1V z?v%Sb$uRSC3&?oicgVl~M5gH_^wTo`+WB(AGwHjC|3W|;s}gI_G4*nL467u{es6=Y zcUY{9c2w{KJBl$J91!sv&<#7G?5YsEaBOWOX(j5fFm+aO;b1Y z=u>gWUxzwM-t(_x|7={K3+na<746R(0@~6tusUK2EMGsZ7_1;#Po&L*sEqd3ogE0@ z371l(jkNUrN~iZA-xa`H-%;hYRysa_--)+ED$2CrwaB{l`2^g0J&JF_#3Osw@gJJv zP(4IbJS`c^_@^mO6-++%=a|!84g0`auiXf@uYuk{4KtXvEI z&wfu=>lt`mrRf%HrUnFWQ6WD45-MLb z{UaY-S z4gtsuHTZq?fxH%Y)jw<3OGy^kOuS~M;13aLTH_ji+>)YZ8zfj!1o}F0q&Zzi?TjJ3 z50})+(FoVM&M9~YD!K&%8k#rM>Ih&Z-^HhlUR&@_?7YfZXe2t9c1v>(C%dRoP`R^< z+EH|sp*|AGNn!a|8kE3!n{cH7ZcAmS4acuX?@r_be*~#9uI;W_Bwp6*I-%S4>K(II z=DE9mh~=&n4F@69Bp?fKq8{Fjpxha<+0yH*Pjr`L(=nXgkL4cl+Y!wvZ4gbrI~JbJY59QXS>X%~Fu74W`VmFr0+mgpDQi zt5ZbOS`I3CeA(yJpN+cumr-_|evYi0J-ve%mcA{Vi%zq`x}FcThNp|aWJz$D)@H*q zeru0KwG3aRV4iM~`MgqP9?VAa8w)q4Dl|T67W5{=+5O&S%+6=oe+_E^NV(Fju~>|p zL5sUR|6awpGPH}?(HXy-xqs|Yd;`YSRp|GR86Os6E#Th4y{8EVE{q8gJodhO3c6xt z0a=@i*5Wub(DyS7uLagG_Fg?)?qMCTSM`VoA$PrYK4+mb>bWMH9i*+TEwhniwt%Imu1 z_&=Cs6p@{3471@$2M4KB7b(=LWT|=YPmSU!IHG{KzeiluUBh%pk)HLh+L)*2vufgU zDYQW)$>L4Q=<8$w01s;l$J6?#?FWx3o*78vfg+Z*t$o0zk3+-Y0cw+$W#KQ8Q?rjg z+PD@G;oC*NQ%g+Q;c{R9$%aV>{B1knS96UGrNcEK%h6AYl3u+) z7@?j#GCG#2lhsudkesUoMo?5N+=EXz*9Eyw@_peu9j-{RF1X}D2V1mSHb~`|T50IQ z-qK89J2#*@KRi7O^ghFhCxih0f2e!Q;5foAJ6N_X$&v*YGcz+YGcz+wBWAM2%xJ;b zVrE8*S+ba!nWYhD{BGXOw_lRYR+368{i~~Irk?JZr|#T)?z!g_eS-Fi5Lf^7^(LE8 zMkS;3EO;{6Dor-0zy!tP9Xx92matEV_ff1QXtJZd*G? z;e5ZM<;32U7#H{Q_ySZwob@^9SQ`n~qIh1shM1}TE|o@IN{39P zsH&ylPD#Ecl{@zA=WCqw8{8XHsO$PK>&O zr{POZ-xv+w6N5Nt>DQNrY9M{Moi}-*KT(@iy#*eb&(BPxUgd$#!x0)rUVWPcvu{c6j5nFx773Oz7*w_htVL6hx6lcOU?5p%p3M z9~6a=GD#8W2567f$exvYy1bvFDHNHzoF7`cjiT_ zD}&3rJyS6=tcbH25-)SsL^gf2yUAGFH;j~K~w5+U_Es|Y#+Bq{_@;zq5 zKA5cCE*Y_7h^tCB1-)EaPqR!WaXcIN(%N8D=c?o$$Ev9)h$=~KlF#`66Z*3O>!{_M zaDGC@0UdD8{xd3^3k?=6)9M@!ZHwa^=ov4U z;p+P_2jfVUQ-3mr5oh90xX6&2x=wQsiF%}0fRRn`9==Ce=|K`NBUHy5&hl+>MoNC6 z`b=wd08hB4(RlN>ReABJdO|Tfb;!jetuEp-hn7L*JKOOjq_tSE`t&&NqGo!}gub;E zR`~gR+5iC@6$V}EsA1Ep{tVVK2^!L*P*0h}^h?7hxKFz;+%g>k%nn~p8dts!lWrBX zYN121J3`c{ush=IDb=L07zC!u_5280WJYnjn7J*}nPtIQ<#$Xlo39cK#D`>mf-kH9 z!Z9>p-3rOdeLmoVBZ>P#(vp0Yt{h2LYke^|fHy3%T zHCTxE;h8|d?kFOc%CiCZTNuX563GQbw|8@9VrG~fx6Iw_$I1P%kfP&Iw->HhtGqA~ z_mZ|n2FR>a(SKpDN~aa0c|r?aKCkm}= zi9#1s8R&FsbHv~8$cP7%CN2aJN z+|#7`y}^$j&{=G^hVWtN)UICVrqidR3*lMmBoOpJ8hRy4!H4Gl3PRge-Mqa(qr-4X zusai}9fcBygOia0v$Tw+tGDFVjPg{*XrsNW2~rV&m!P5(M~{r|7Q}w_;7eei5}2b~v$Lph|6q{mNy}C^WIIN>B6C8C?lJjsGI13TXbNp z_F{~k{YwxSV}(<&Utk@ZJ0@w0#-DSp2yO;_j!s~6S7rK?w!MNpG2Dqm_`2gpPq$f$ zQeL%WDO1E|_^4_8nT+f+Mp3@O^#l)>9}NOIQ?SsSkdPD+v?L965qq)T?VELxVnSiY zXSn;B2agT0k;aaxFN7ZG^)sjI>bLRz09qLirNE>4B6J3`+RYQMd~Mx7dvE@(_Qi>O zmNZ@2kdv{m;nc>14QAOSyyY;A0WFD;(k|F&#zL}jrv7?7DnC|j`_$eMe>Brb`SZ;b zU62yS)bGuJ9>XD{)mtj0g|P8?|DVR+C}N8Lx8iS=COj89euZ3Wmc9iq{Tf!$4J|i_ zgwvYzmbTp?UFdekkH>W&c3a_kWIlj{v|nQl-TC%upd$}}n>RUh)vIk~d`5pTVwZ?s z-s1ia6?!t-y6!dvLbW_bKq07Sh5JjXr)f zWmp)I`$d_SS=X1WU4q|Qt0=L#EYj$6+F;sV3U!ki0|gBBn^v?3-jkHh4`exyAs862 zsfz)M0f2}hjm)6ASR7WYyQ6Tlh6R;M3{R3mT5>W*8-l$vP0p^>m1`BS z7w_fc5j!z&Sz99vmA-juujN12!`BdIQ*8M`t6r`no0IL$Cf@-8HK)YxExMY>Lf&f|!x zNbmb`>R{RrXA&h9q?dlwlDxjLEAL)~dDd^Or z`6h)}>zYu4PE2};A|vli&A=@scFozvAg{Mp@D@L5)_rYgw!PW9z3<)@z2@Fs)u4L) z%1>+pcSe_INuN@{YM6xB4Hnw?t;eljGp0(?1aOYuR(w~qWNFnD716tvmL2ZFJC04& zo=sB^^k!}@V=OiKrEP~Pq6W!s;-kFr$;QzE+T)7a_?BX~Rs0Xvs?@;N+9_9jjAX?< zd?r&`)3WwxvX6c9)XO+I9RX)B$6QCX<$*Drz0_$`ofdfhE0~uVCimY#*N*7^_o8b% zGkPmG^ACjALUoROc$U%=N}ruIhcQy({Z+N^ZnOyO@WsDz18;BjO~0o!*;KEUAG( zec&)@tCHaC*yAi3%N0etyNk*4TzB0Vd#&w5hKZ~jc#GI;;dJ%1FOinC8^ga|5d6l? zB=x!vYiXbTt#=DW9`2un$@F%9BZoU51BmmJ^?!(<1C?Wbc+xqL7e{XK{U!V#Ld%+J zjs9C`*;pT~-hj)vWcAO*WOY*IxJiq*p=C7?m)6vY_S9V$>}vdjq~rlcQ4f^Duspf- zhCt+aq-GtL`tcjQWk?pO*So-dHe;tSN6zrOZe!W1BaJsdZR;E7ciii@!^72WwdUW} zHm|Ufr@)sE5xS0aCk4|1cJy2o^IaM^2?NUPH^fR=eg~5OFT!?PA;j~KWAA4lxEl6- ziJ29Zl$0#$$!p4dN0foja&mIwR6_WRr$xrC1KxP4m>o6hZ;k$5$OLooG@R;MtePT2 zI|oX@AR8EFL%jX{@114AW$E}sOS;?dD9txy(d0UrOnf~CJt^IEW8=APm*1a|K43_& zXg&F=e(WgwW8cFN*^(>=z9Gp29#!|vv5a2+H?YR_Vp>F7)gFf&ZyN?9(Ul#=Gv>>{ zeXY*p3B?1(Ktf+6)QZ$7&%m`f?$GQrCJ)a$7aZdI?>slYaBu&L^_C8C$L%407{tLl ztAz1-LiQMsjy4PJyQ?#a$~xU$Zf6HsLachWr&rp9c{? zOWoI8Of_KhV(BJ88Q>kJOz1ykYZ|Z_4jB(WA44c|LKUDluEF<_q$1`6PUxbe?eXJ6s1~zR*L}E5?!ml^=T^6`6D zY-DRGmClrW&kpI-(7cVg6!^Su&=c_ma-Qs>j;|hc8V)2sZujTBV20nzw)?SjlFc2Me`=%q>W?*hLMb;GJ5Zd~L$=VpA9Alh zA0kNjb)aUthda^JgE@fxf~@lE%RxIo+`xc@6XL&xKW=%w0pwYNyZX#KxCSW@m`b8S zj&3bM>5c1h841Jt4Z75Lu3(Mjd1sv&qrYS({P5-0*ou%qoZ0W`D`VA<*CZ}!opKvR zMTyZcTX(o9W{Uetje%*fZ_U;qnHKZdQZRccXS;H7a>?*BMI1!n3mjLHb3A>~MD2a^ z0BJ*3i6g^5NIHx3yFXrwRp`@Cdr#82X;S&UuF8FG+z-6*?>nL|+%Vs@6C#e$749xS z#sXdwJfQ7J{kkVF*54^Wu!c`G0FUPXU zC@5+z`^(taTW1PoaaYk}SaIi_{@r%oJMpdIkv|41>c!j(FmNi{@G^4-1wp$ zhU6pxDY{Ek2(~)toiNR(XcyBLwRRr;GxLvyOVAJ+QZZOhrSdiqR&*o=aNzQI7 z!5)t!%+wZ!l5CmnORo>6q&YcFKio3!6Memj&PAAEum*ra02*kGb8HVogN6VVH!2QNeCW<4$&c*+9{6V>}5!s zN_^G}dGrMRF9Oy2bo3qSr!W-LEi&{x{NS+}MLkmzrFDyW0Hc_iU@Jd!x>Z^k?Lea` zex~!MQy)-u0cB+F=A8dWuqtvk|JYXDgcvsO^MV7JDCFRk^)0-TG8!gM>{2wsugxK1 zh13LnyYSXb0ylkSJ6!CUdKLr|pz-xGo6Ktle*`rWe_t959%%KB!G-dcNUOyiBpc@Z zPcDGI>)#_Qqg0*YKI3mGrSTsurD#L`2aE{xFvzgaMgum~C5Asum>@nwJwo*xx|efD zT13St-+HzR-Z98qut-cV%6|y@0Qrp`7>MHP$`vWV6zpcS_}c=cHx0!eTu=71yfSJJ zyvH7Gas{5g!5WTRzD^QefpWQD1YA%zZry12VIJ*#-i33tU7;I}g$jK+6TRn8@}t0A zkJwn$?=y!Xacyu%U{(PwdWAVw;MxIw1M0`8Tz8(GcQE_o(6hZivVJjnqu1uOD6Pqtg*Qu2$sg^sQS|p@uoB;DxOD zQoFj@>f8rIAL5W@@@sWNksy7gAqwj=okaXQ*pAv|HyqWC-0F6s7`n+e?8ocuPgvav ze!)9XTWVJjl1k)(@Q$^PxLpohiW?jO><(%>Po7jh!*Ys)tJlm1ZM^VbL;%;wwZ3I2 zYV&%r{6VTA>@62D*sFraSgKDPW{ZrJk6v#)aLQ8x@s=`Q&Ea*wVikb#a_sCpz9}WHO{05( zF+QN}Ywg&3w6Nmd(40SQc$_j1D*nSlBPOvP_`2lb!0I1l|1Ula;*i)i!p=R zo^ok0e)PK&i?J(_He%k%|Ac<9aKr?nU<`%mHGRej!yV>o2M>4DUKP*f7^CiAYw+4; zILi(QRlJYw4{|!CXA=wiUW#C~R)L_b z0rlU@3NJ*0+2;=)msq`+8;MiOAM*>$j2=Gc?VsUCSfi+!>DihZyt2nqD|!?uH&Z71 zh0={j(Zn31m{9p-&l=wpdan`H&jy_?dj)@dhj#VzreQU-0xcvo?NGrWVv>B@EpYPO z5V+R|rMdDTwMhI+G!Q~g7}Ximy*6m+5r>C8o0zTZgxB^=WHcS))FlO{N4nvzK9j>qT!rgq?erQ2+u{{u-M&8mO`KIIVi$P=jH zl*?*6*4U4s3uT-dFKY>P@*?v-0eL+r?7VbW3~99cYQyq!<|4vO0%>bcAR&N1*x+x< zhw-}vxhNIbouI$MmFz}ebZwQD&Kk_qDKP8n2rZe-g?v!DD+3J{4FDT|pts5|s=q|= zHF4dq0hyWZ4MID)vd1M897A3i3^CgD`m2pZ2|V))Ow`eo}@R| zB8QATmUyr}`mFT7l$kb-wJhkR2HfIZ_}xP1b*^i(sb|(j9g*@VQ!e7y^&@mu|ETwM zCq6Q7Oob{ZTu9?UAcehm@3(XA=4~g|4^=d=U+y24+B&V8oGEnY>a4Vo!%!5l@i1CE z$b7>)OQqL=mgVN%$gFmkaIHdO+k%E3Uj;56*9Syvbf*T$QN zcxDhdSqL5?Qlz6`t@Ky7o_~1Pa=aaRT3WnRk&gxsLjcAW^<&G*>tbUw{yc-PY-x2ft)^~Az2#X^=G==9 z2CU^NIY#}2L9-&Uba2J-UlV@K@hAHl)jxPDs&l50Zk=SUb1WVI4oPvwhY~ z;|;ey8BfN}urp{&=@IsDVlt*LVn>*#tlpUVUR-gqs6$mOj33@I4xh=$$l`G{dJ7J3 z!=7Iax`>7E%@ECLJu^1rxa(;5Qhh#ITi)TpM5QH_uw0)8V%q<*PtX-;&%rc*DAL4Z-dtpc`Yz&Q zBCss~{aAeODdvhla|k?7#a&=eJm-X@j!?N9T_lqq?|(~$yC1uka|_{4M=0DN z>*@wuED~IW9j$&@$E9k?PpyAILoH>t)Xt6-=G!0lN^2?r4Ds>%LY8(XN^JXB=X%Z= zH!|t_`+-n>fAxa!3aH%S$>s=i108y-OAh$t3YV03vU}qut@eY*hA8M zRg-QVd?Jbh({Vbe+u#(3EYIk2zfsL2UVAxzxAUv0#8U3Rcq1g&#d8MjXqZ4Vv->FC zFR@F%wFhYlMJp?gT+7LeiJfxjVmps+XZbG7&=|!ZPm)Xh+8ti~0G5xE`O{IiS1&5o zN6eAALo+f>=5w<$y{w9i#(DJ8FHKD|!Qehl!ELE9k|3&UE}}`O%q&xp#u-_m_GrHN zyHjz!tCucvn}mLSjr@@$H~Y=t999L&)ZWWHNghvfP*%5$q_}(fV#U-1HK84ppfdi= z=!|&7ELDBbSNmRzTv@qq+AO(za$c-n^;(=V>YH+|;W7~ib#lX`WW)z`-!}P#JPza< z{DcVnRdMC9>EoT=ZBSGNgVGQCQ=i?l6}eoa#88J*w6?4yuZPF*K;cqBA8BmzO)h3_ zo)((GV@m!`J{k_N@ezkcIZ~>pI)Y7X#Mq0oykhLp;K%=bBKhtlRkbM}` znyFRZVZQ%38sGryr-nxEj!E&QOO9)2e%dK7Sm~?e{k# z$u67Co!KF*dFg8)BSH@g+2?OVvHPlGxhC2$G!O4@D*$s{X>q06op*%_Z+j3(;$>&h z1RygetEaUYv^o>hhsmz$_ZJ+0{frQ-(y6*h z`{V{VvyF}j`$zC9Gfcqdy{RYLbP^)2zU zyO3818c)YaZw9{yw`$esRA}joVAm31`i#IT9L+Rv2G)_b-fQltjAj6FTu3TilP^{9Dr-!GKRswOZ}Jf^Y*QGcGrkI3 zC-T3e!pR)f&B2Z;DDEQu5t@}VJR>SV{MJrL5A;nlrf-*iImTYs_MYc{ELbA?^H+T! z1sXCZS9h13mlU1#&IV@_6!I=h45iujupe0R1x{w<)@9QXq8P+Z*!RahO#!CFa}8Pf z^j2x9QMqLP;)BlHhK`0EaPVOR&vMur!SrtZwrxCO7xv6VCGNT?(;zfE<^pj)Wa*iC z_ON#m(_6K|h}t10YXs1|nHcb<(Ka?!SvkSOqZt=yqoiDIOnaq|AqF3elr<4}fU7tE zjdmWQ^fs`?*=Iv-6M93ppIcL?%yfS=Bm2*sDq9LXOIQ2OT=k_(|U z!!Cu_E8Lf-i*Z=M?J&Vax5(3Kg7%j^8AP&ibEV)9^!$ITKPfOQ5#lJo4tdcg!D2i` z5ZlKutDooSzoJN|;OswU#9069Lu|5PdK(Xta`bGuChFdX1>{)exs zgV}<%daW-i_L~~O0rnUosH_Quyp0Ges#Cr$@TtMD*7;rP@OUA-_YXX=L8Gi zGwSq>A4VoTYe0vs)RxR^#Xr1Udi+PgTU6z?WXs?O9?iiNm!^OWm-%Tk3qwJ$CtP0G zCrLFF!!7|9(vzM+YC{=J4{;&nG{mIc;9drw^-g&?qT)^#Bll1=`8LCrfus0d!UmMO ze#su&*`!c6nuxa2&!LW9zS;)hnrnZgp|S2-U(tivCsgLH6`{buoROI-{CGvtq9Cn- z$gze_Oqp&QABW;YWhoVrjgnzT!s*xH7iEIaYEa_n4GbnbDYffI=RP4O18>0Gm0EjO zKW4BdgSZIvc+wb)y{Z^sRaM-!mMF5JAe#4Z_xKk$b>!1)kfKO?r`6f1AFXV1K#dBd z+J`7vtIcx-=<2?xeUkOba8*G?4kPnh)>uXR)K9nm3mIh_pSU*_jY}CEXhHVKdkQO3JmaGUfcMThkdTogyi)yAYG4>dM3(yz~p{wTPW)F-FEBlFW~W zBb$!>Pb!^g-frJrDe`>S!P3RR`eeGXU*}dC;;-KSPLWI~gXNHTf+4))NQxciQMV(+ zLC~TDX+@cxE{~ z-Ci>Pq)b8ol;|U(c6r?0Ot^l~jiFwXZ%+<5l5w~2R%tM%UFL)np;s3uSb_NOH+Uc- zDed3e0{eFe3jHb|lFQK)vN^Xk3j``!8w*=e2hXrc!dNKZcnwb1l1Bhg`33o=E>&W# zGl;GLyl^+%hYXW({kPMu+OsuH;y8UhNKuBdUD~E9b@_ARw$YV~((*GAgD~tQ?}2{P4pJUomkuDo@a- z^o1`!5=*JCVP!#_)EPe;_uD$a03Ur0fc{r$p|I(Q&x(gKN;l`9wZG%JT5Rp;Q^=w1 z^gW@_7E6w;Kp+%%|DD7?S=1ZBcRa*yHb^#I?NH>LG0}R&)dwf-ivxE(2Z)IR7mBBY zZBd2`jjp@B4G!_l?E*oVnP8eix91xgCF}K9Q7*_(_d!KLb{TLX?eOzoJ4&$_xTm#T z>UQ6npK57*Hfm@T9qgbGkE2$6Hx8GdR5(+8khIKmElP79>KxWEIW> z9sAyfV`$4v2_Ez@tvO+387;DA(w*w=Kwl(qy3i(v(`24j{{^0~!S0vSn)r57o&iQA z6|4_+r~U#UuC61(5u_6!5D2#r=#Y4$t6OZ5f_MiPL z6hcBpJ-w`TKx$HydzsatZPwqn^6=~lA^q29KYPx0+G)t5Aj>psj3=GiSCH~--6$u9 zrxfa}&-pY?xg;FBII*_Z@`Wl#Hw5n&ShKE#_hV42<0t>zW0TEsZ<)nN0LY*Vc|X0 zFS&~eB;jhpkwoQ4vnA0$?`1@n=1ak_$#JU{-)kaK+>EB2sAF{|eTvEtm+4E?2b~L`F8WK)Ck>UTt zG@4RmBKBtBW2>i^aIXre*&?cJg|c@2QwbLzl?x*jT@Vh^;%GOG!0vK+7FVGZagcToZ9g#nm=h4JY#~* z+|;AV2uvn2@kKfI)PZDpH_nKdq;Oy&^6OcC|rf-g_@G#s*fon#X}zwhjt z^jQxA0?_D9a=TYpGq{||2;W&^3%Lv1yw#jB81#i4(BH28BAekS=mdR@^ z%vin0R6Vbr-A>_j$9T=-Sg%49e7&~k%e7Ug@^tUaBuq>Na+-pH&`&kyuD0H9c;Dfu zEQqPu1+xbyO&&)=e3yEY@KM{Q55ioUQ2M?kbalwxXfEtLkJxdXEsvcNciMT^vHMx- z?|(=V*w;T-NOM;Fk(S>wz3!};xViZY<;P{(Ez%#@$e{WxWZKg3{7PK9M^~OJ!fFW> zF|n8zndg8MOrZyPS|2^Eg6A}$;R6W7~R5_ZExMGmnD{g|W{d_8etiTX*Nl_$aMLvL#6)p#R-Zl*?5W(=WVm}Y$t2-u#C4VS zsjnHJVMl9i0U%fA*fxrm@^V*~4#U!0g44||3HHhxH>@*r)Y7n_3_Cu}%)?+*>UUYu zovoHZK%qUjM#b*$dZ^WH6zX%g%T;4;lWJS4dFXU#X%Y0wgJ)C~J4Sj_nD?90pxfVP zd9^7_oaPw4478xWITw)b@;h6Wvo^1hI?-Mb7fUI|rtcrtlNaE2TA|WQE8u_WQ!|M^ zRY>!Z-e9~)VUN(<5G3z)E*we(Fql+6r(|_ybFTQy2vh(tssw1s*`r7Zir20`x5SmI z%XxkZ`-*}hQq$APrByanUx~{jZ9LiNmv2nTXNNmgDsvjkq_9lvw&8wX0LudmWvei2 zEgH(SUC-^kJhJ0X3iT+tPQDBp`UIu9Z6$}VAn?!N-@742S zAVlc0M?cXIGFh&mM}H5wl;iImuKP_H6J?aYO86$_B6m{EDBp85kl8U~1$Eyk6ob~5`xdla6_o)kZw*-4~M2dxsZun_cfLTt#TZgejoVMc5_ z&)R=6G9A<ehG*hAcVNYDL}Y&qzoQnW{j$h= znX$A1VmvO{TrWm%z(cbeY(#OlKs|9U+Sxr1CALS(?3t)`ljU;n4|%FM+0N>}@H=k$ zRhWsW3sU=mMkTx>Y*>B1-39+iMxf`rBk!TF^a66X%-$3idsLTI{U=KP;I;~;=Zw- zK@>lSHlLjapNw0Re5U(uvYwAP6jxF5Fd%^fK>sxWJODxck_$6nqc!{J$3v;9a1fU_ z9M|OiPV7veP1}^ICJq*1$o1qbke8>WeP8Y#QPlt3wk47jK^o!jgNKXhr_htb<_J(& z39XD5Z#DCqBBooy7}8AY+#P(()OudCE!VC7QL>`R#6;xN(n6YDUvC!}rN-<20S#uv z%|9hIgcZRF9o0P2vSadQptR-|t1FOd5bpNL6)WbIM4QW(Hx8l!XS6x{^A{3r=3$ zt{UWlvU#y=Zj;{L!rP?MpY6tVXE?t4gQXsW?us=LhqQrIB8;l00QW(nekM9Qol&GeNR zQ7z{r^!iK}$lB6)n*&6k9#UVAQ1S=;0niS9Zt<>uO!%z}z08B5(Aj0Yl|1pgJ9zMu zT2M6wg1Hr3t-i^O7hwd%0ulvslg!aTrth$*>sxc3uh@&ZL@itxBBG+Cn1>(BRQUsV zc;^8WSM%)88F%G^wgocd*I4g+uiD04zVV{Wh-k2|FVFfMpEQcRg^9PvyzH<_wCdpY zRZp}7?(bhzK4vbQ^%)dH&%M?;!+_f-xJF8)M+vEIb^!YGUrt7ihB29 zGw_qnXmIl?n3ChnyGc@VYVp>SJzm3X?=7T2!9u|KSMVq=Alq^9rXE*Vo$3Z_*pC=j zFI7{Yvz@Ibm%kRu^xaON-CmP$<vX;d>Lg=~I}?c)4u3L* zDDZv#%m6}hVZHE}PRrd$kNt$LR{6r53@307dh*)dJ(M06mSwqT!;VyGZN?T?Spw;Z z>%4n@sDeX>ofUoRjI(;t)5qZq$T-^axI?qGhwjuvOd>-87MpzbO2~O&xU`7)r3QHn zbx7>TxKp}M8W7L9F5kKJLaOb{rhGCt#dfJyo&7_)X*F+VZtz(lG5w zJW1>2oXKCWEvy%&vG%ez!Pn(?3bZ_pu}Jqp2=Xo5a_?Y=1hdPXw?A+;)7V9|L@<{S z9l=PDISz?Z!Sw1_i6f-aWPMlI_1O~>FwQ$OB>%yb8fP9{S`XkxXVgW!hCLuZjq0}F zjf%hEvgNu0_+H*2z&}%el!EI1wL#l8NLMwy1tpI9H|(8vD7&Wb%2Zhh3{St@9LM<8 z!V&d+A-;Lc&l_9Cs{QjNKG&^6QzrBv_`YoZp6<&}S+Z3v5t;eBsfqx$j0lXD=czr_uB^wrdl^K||yC5%V6Ox}e?& z<11V)q@DB>p1~uwC=W-W{|+Mj>l4KHSBqM}Q0?ohXL|zPJ3d+hug9vb*B${u+ghwa)y~sE!mJ%zhUt0s+Q4x-Jd@o zLy5XfBuNANCcdV#fyQUjAGcX4cY3W8Q4@mpo3A*Erk$GEEgwW6-4$A zhuonXEBBivBHcl39Gs_5bAL^9ho1lQrHqB1Aa?LNIT&7Uy8Sa?*7zQKJ-fo+IvODc z=3n(Bk-qJ7{e2r78^JfernsA5@t!?Rb|wc?Uz*vmgrZ1Vh9Gk#>U^e5<_Y=Y3N)F@gZ+n*!V_@;;!)XzE(hvYFq^3c44Q{KpjQN|SvuYAbbEK_a7n_xdRp3qjM!vW{dU@@$)ch~E29ct8*O4> zYi^X=|BQz+t!i6HVg5Z~c$_!CA|OO0r#l(I^xZxquD$_i@p}E**9Wv*8_CuSkST(* zC+ba7CR&AQIgmFIj?$_0;ionDFSe! zbb?rD9ZNklV3A3_{U) zvvoA1U$*(q#!nyH%e6b|hVu!#Dv8S*S$Zt=^>pc*BBdGc#HBpLU#g>O^JhQ$ZV;h! za2vAl`DZ}S2y%kmaef9CPTwumI%t6lF>8b~jiYuX6&|=>h?$9zO*`TiL0+tQ5K7fg z=+A@Li;S1gf9B!!jwDvzIv%a0V6R~?`_D9#t)D5UJ9{n3JV5E(Nr7CPmUN~Dhu_HU zNF7qWBv713^n*h~N1B~>-df_>G?#V1f)0QJp2fiwFXXR~u7TS`^p<^*G$bV_(6R#A z3NJ9_324a5qyfnk7~+30leaRH_D*+oz>suY%mhsK|GvcA3+(g*PKhz{)O}^q(tIhz|9D9Q1yvo^k|{9Xp0rx$sL0MS zlDMAGl3M+WhUT$+27s!XvVw-$KbvLa%&D9FlSp(a*%E6X`TM7=zUJjMp_M!)`qmQ5 zoZ4iKoo&*u#c>`0a6kha*ZTi zGBSOsY>APg6B~Bl;U;}QEC5I!w+?0RCXs~vh-7^qpN&+IOdq~UpJVPyx9l1&j7fK7palA7|8 z6Klef-p~HC9A2LQNZX;JNT157o08kEEe#H7*zUV@(D@LZzQ28q6Jvm7)(#=lUSH9SCeAZkv9C*S zSC_zGl@aPvcP1TWsXNGVA0^!tDAi!zLdFx3lKR{a_q=UXAu-bmI3x?rZEfGFygT&w*ui&9WEM??ZUZWa!9f|`!$Y4@i5ID4Yj7- z+LAklUD{OTgWL}j+7ur(P48>H?>J)hq=XLkW4>r|*I3ZxZKRJyl16A{-I3`pk8VG- zU5>0$eNtk4lbLMI1|2@U>bZ<4`lpG-R=q`b%0DqZIG z8gaX8Pd?^58%;gXedZr}^z0s9L)O)!(O?YAx>K3F)CZ=4%$9^r#-XTm3yCIRF{K(k z*J|eDJM4%7cV)A-ZxpPY*(6hk(acw9nHHVWH_*M>F7$26u7IaRa1<@Z?ETDZnXi+# z+`QUa$fyE*f=XKIG~eKNg0tyy+pgK(I=Xyd_p6Cac9Gc~>UMdM3zC`rxc=tJ*)Qok z6!kr0{o^yX|1oPk{&?YyYMcnx1-2u_SMcOV(i=2#+D=OmZ4qGqWY{>qF+&r0)4xVa zSda;CKG5A4oltp(rQ6iMrCfxJ?l6g?lapsm&uP+RzqeagQ+f}vqU-H29Z?}t=5*pLOD4{2CNysmV&!T)SiH!I&kn|^6!GC{yu>MP=Ii!k4zUK5 z-&T$s(%Qg;=s^S=uDtc@>+b%~qBEkIE*;SoYE7a}>!< zyIKpp*}P z!Xd2~|J3i_MT1?7ga3=@&WoA!{~z5cADxX#e5idD;vpFhj$w!qSN`+py5B^Feu|tm z%(6uJQ=-=07gvNY>k@uQv0H_aZaC;%N5+O_ZG)?PqkO4rTGyWsW-PM5p zP-~L!w5BR`19WVkEz-q6*~P2u$L8 zMUAXuL{m`0&HBXMEugi(`ku4R4*&9fzhRENBM>272w;%C6_Mx)NNh|RP}wNVSwY`mw_TvJghX1k1A_C?QIXvmxm7T@rLl>0$ z{O>L<%N(vy=h^GinD{-3_TPQkk7WI`x*{4jItE&y$}(F0pdcY@(=vIdrAGlW@(re# zLQX&R4m7Q`xn{mu)*nHL)OUnHUB}q!-D2NhokhtsqfK>T^aq6`Ztfo zgDMzj2E&!ZZZ5>n93Bb_mQ&<)TLOO4qNHDLJKsQ53(Y7gADa%$X0b@}pdL^W(&&39F7qK2U`=N%9{H5?!?$k-c-@Ytpy!8+!ATR1Z zGwsZ)&!vXOXn3CbWmfR67wAz$Ioy8!H5E?y=%~xIhWXw-%sHymXVyb@rPua0iCa0E z1{CtcQD$CHz7qgF_Z-tIXV`tqJ{-bABMk7(Ng#D|t}N_E_n^ctp2FkKn$r3UP3iv+ zp{a2T8`=NM$P{TC((kcDy)~z~?_`7rMxFz=h4in;^f-st%AM_m-oYTO6c+wLCjXvK z6mp(nn$P!^Mp1!Ft&M}FhLyK6#1zX?1^!7|?H6>wwymPjwolvvi7$HARuw55EIHWxC%?HTUPwA7b4kYNqE)}}-r#JEeCS(<{14u#Ny z>OfEjs7RBTy9>I%Opa5mr&pzsmHL+d6K(Gp9a-1#c}E>}Y}>YNyJK{cj@7Y~jykq& z+qP}nwpDrSdfxkX6nYcnxtB6QoOjOKp3d=!^6Sj(XOi1T1)74tg^M}Xg!PfI4r|Qw8lR1)- zYS+#B%7fT!lVolUiLMUj!a=WP`_eK}1J0;N7T(mAfkP;6c-hU^*x(a^Jw-+ITzYB`TbPUHuZ>$=8y(csOvR)+&*o&#MpD> zbwru`=4*V%{VLt7v3yA&pi!y|e^uxXJF{SjQgVQTBu`?z4-=D*zA*n@r^078w-s;IwzuhN4{F4R} zJA~IWFklykE7`o~Pc`PPp|@<5!*xEB&!sP!( zu%(_WQ{GoD$MZ7JEnTI>vQJ5G)9Laq*^?dO}^XQ&2F;Esv<&BJ5w+Xokh7w@09 zK|Lw1w$#RR7gP(_yoY=FQXHptYWRe+XBv4uG^Gl)>*ZkB*3Gs52FyhTO0vn8F!F>9 zru!Pp4O^l;%*3L3MzRG9jj2yXlXGK=5XAd%pOSO-w9DCVs4&{^%^)+#3edQ^rMJ$% zsS;_Zio#~m7u6Y_LOGRcg9I-?%;9}ksyv6H+hj^SYMwe_UG|PP5V#O4B*H{PWc+w+ zJ(b15(aJ5p=`qlH6E>MrxKg-p%GW&H2-g~?P&ro;DPj>6{y5c|55p4V^dxWZ=XeD$^_7-xslAt~cGuGtu#d>v(%2%zQ@~SCjBE@7d za%z45%M4J=4cTbT*BhsOzFE0+dGNqr=8P&Z%^$^cyq>U=^eN0=u5bKZm3v!}yE++6 zTHU0Rl^)x42?(L!H?yZMM^ehf%=BIBd1ucWIz{KyT55;(US5$3k0c+xlB?w0F5I*! zrq)RPa^TB&tiz|mZPhe-SpX(pSudc%wKQ)SHzNF6{h(4Ad=YcryB|1mG!va_kWB&i1 z{i%{-ZiM_C9Vji2;Z-vg4zxl`W)6$53qrzr8)=4@u#>G)D7Q>90FUt9DA zbU&@`g;yde%MDys>`)xvK!T^Xz1Gsq%|E8&>i0Wp|1jw5qtR{ql}Z98bFDZKs`aOC zt(Li4y?#u&qYbyn(K}3#p`&!-Hp;X z`IScU-O{V1CLikqkvv~{N%BjPA23|84p5BvYvgambl}nm?g7v-6}1VV>WB`7w445w zx6ZTB7GROK|FA1M#?CiOhK!y_yqvy9J2M~zjE{2_HG^oWx^%L}g zXx9^eSN9q_>9BZz#gWlh1x@_!7(8>F?A6i!U}K35Iw;rWph$`wG?Y*B+$;Pv?*-_9 z0cJcN_$Ee%T4BW~0t-nC7AMM~W14)R{XFlrTbOumw{^rwBy&~ltZe32Vc;9mdT!$mNc@E`9l&;|h zr?NLN7k*?grQ+Yh(J4&E(zc}K2T7X@ALEy(b}{+bBBVs9Z}^YRdyO<(hqv-C)9JoD zqB+Yz?(Y8VGk59j{v`_O)%>143tXBWuRxya>axZrXE8O7S5p7|J#Jz9tdD&4bP8poAKmcND1hAh@YJHL62m>KgZH~zuJsAmNf zrn6ekTFg1<9Ct5n50s!(X-+7-BwF^BcjFyB6%^sz(teg<_HQqS&6*GLm+;XWyeAqz zr<*k3AEif4E3YhntDg-FFcU6waZ5f#Zsz&>Ni}a^E3z2(?dI8O$hu(%LpK!Rx{`=Q zbvt%Ibi$_-9vw`tMpGB2V&?C%86nASmO$*l8Yd0%iRIu~Qg#`mPv6jqj!kw~JfVc| zz(KSGGdo?r$(+L1)XCBzj8o%Jm*2zP=IC!z+>#9H6g>okN7axsElwZ|rlb^-h+yFw zxu-MRT;*T|C{KpQW9T=1qrjG!1JPOS%~UiTZ;jW}3p_o%fV`X(8kXJP5@u2e1{3Ry zY9uR*{)60q<|g$`hWsEVX9XPOfKwa!%{J#!oo0j|u&p`s?@Dbhn-NydWvUBx!PRYx z`PMa*2o$IY`s7$$M-mX46VCW1Grx;AdV+O-u z@Pj590cCn@|1jp#$H8%IGLy=2O&j7t<=~M?I^JoOzbOF=UCbUjX6{Fs)T+O}%E6+N z^f>eM^JN{{tk%Y$V2>=`BQUmciN$WDC@`X31#m$NTGfT3OC}Zu$rUavJOee}!!Aw2 zJ<@H58$zS;g-0eXAC{vzSS)1^nfY_?s-5B5ia5h&)Y-${5>oQ(wu z$!VRV6Se&#Y(n2)m~>3KG#XN-8PqGgHac*=o!lso{W;&B6BrmMB~|KjHz}RI!Z;Bd z1?r)U*2(n^E#3%J0az=TdHtQ`IF8@!lkIAp-{s!U+QG{RotfJ?agaKd!Qe|=TPH%r z@P@ID#wotSpi9n0VvWpJLY7%)ZG5AE*VhgKi|kxQTuiOC>j|CO6h216_;}>)Qs>uc z0cJtWs_NQA2bue&wCT3(myN?U*JIFOV`ZvdB#nkyGb-jcW_2va;CY5I&PO|JNm1pe!k!y0HOkY-XK~p#hRn*);unzvBEoyoi1E&$=Xe;ac!8yiNUOy4} z)*AWgb7HnA?ux<)<)U03m3k@jZB0Q)2oAqu+she*s5Evxuigc+fwW#Y$;n5-{>6P9 z1IZ|Eur%>!0PocLH2K*w!d|H;nxpY6AdJRx0nfcxm6FQTf`eXS0q@+ac>bMR9lfesop=u(udgWVwFn)xpwu!NHH{dGoOKibm=b%O~6~f}LjVcJ`MYmOe3wbS7 z2t!VHMOSJop2lehk+RqYO1no58Y>Z2*!W52>GPP(_r%;@4`KU+q3w+E7bewt^35bi z#d?DeeH>mY|0z{%#qby>Wa*Sm2|8EcU=~eL7nA7hlSvOQVhsuNtKSNNRK+eRdJPkHDWSS)(2B~HtZBS6l+cwx_I1b{XP-L9QXVCIUChZSiP$NELvF) zEp1gU2^l#B@qb+oWdA|(g=#qLaseVa4>70`k6;tP1Oge3-7i{wN#pjTRDu~i1V_;; zFKkaM$Wr`~h-SG;Uv#o|$~CbBWXVB|@jAp`#@dOF=J)-L+}kpA&+1@sj(<=oKmn^1 zEG|LH#p1^yu$-<#lYr2S^TYqUO2z80s1>0FG88-0ThhTq8g>~&kcAT|BQ;#grpq0m zJ6T5WRBskr+&))jdMbhQ4^DkNelhE`<2jQ~#2j|y2jHvX0G!caQ3*y{dJ_h-9R%TAJ`<1SS`#8 zB2Fo}w&}ofkN-q2q3L6&s$+Z-I)md{0iK9p6&Q3F-!ZH}ypy&ETnYB6Ilg-*i8}H{$ORvO3PEA^@&!E?7O3(fu zQQ-Sesu$PPQI2UGxwk#uqmRa*R69Gv**V0OmO~qwG=c^`R8z;H=f5R1DE!I0-d(?p z<0#n;Qb+#a*y4S?vJ2=t(!1VWFGC8rT!2v_B%FWH(0@0B6;zf>psou3-V899lk!&c zw39PsF=|%3c!JZShzEnVtUAj;2kOpX4qdRHes|$B1)lQGk1jjG*|{lSbJ-}DmJc4k+b zUXvH*P|(j`p<0wU^TB%CL|9BwDO!t;@W&Mbc$H=c^~(e4LKcjJj{#g&3-75=X5)jEj1=#D$m>VnBYdjFk}MzrX0%aoyLU@C%(?=;HKE-WFaqJ zEmvJLh3%@r59>mLfKRLs!k+0gD*1ZnbAr5QtTy(5*Z%@MTc%xn{mb=iKYCaI6p24O zNQFRG%UVA-PL_Nyb3%@Y1$gpvc3|Hiwf;x8FYz}%_Z|B`L!zO0dkXVY@}p~ZWnyyIa>9&^ z{-R?lr2tyGjuQW^V3BWnN>{Yc!Xd}ee#9?sNBQQdVGACol2NO|$GR~hU9F!}O{lj& zj6d$a?HUx&cI(yU-&;(^FnnV|FOzZyex;ShzZ%Qf%;)@0Q<6i`@0R^x$e}fU!yJ|aOCM72-R2Ado9sRU?Sdh|5{+`6 zk|!Q?ugV||Bdf`=6SriumKnD{AH`EDM`RF(G5cm6{8FNiPeCxF+NNS57nT`v&7+j7 zZk*-}@ZUt_nsc#c37c)yRkpP~{yYP?xqrL)kZhM$)-8P;g2vgfADeYPdZrSq>1HH< zrzci{gXn%i_>?i-wN&9`e&Y7!`-D|3^McowUz1LwO8Jg9HQRdb-+t%O42E?`}Ls|X9oL%G-CgoQey?5T$q zT^6m>keu&P?RBxF*bMXB+dU3%4?qPZ4xyyI{k$x`ePQ}xB`dqF>LuvJwN0-#{ukbA z)s1W(DuID8Q}T)EGO{kEk})gzl_iO5!90Y(vd|r8n-9Hf2*+@5>@#>YV0D! z0@I^<)!Ds2VBG6}OTs%m!eIwB?mDKrNEvy}hvFu%%KJTe<<;>U z7N6gNS3>1CG)+YGCC+UbZjj~k{(9p4$zC=1@{(8A4QksmRS35!A&?0CK56^@ZG1UT z_?v;#b0Y9s0Ko^~7uTOg!#Gvzq1mazYE*9xoan@z1!YQO2-)Xe6`yN;x*Omm+(?9W zi~NcModz>0UCiSUCm6Q(eB-q z?BtI(?Gztp@zJ~_c!5ZvVmIPoHLw!KzE)ERsx+Bga+DH+DP#A_B0*?f*w&2Om0iB? zr5sLkbX(au>bp17D21}9CL68&TOu-DKG`r0VlTQs{Anhi<#SiZ&=d5z4?FO!BVMz<3WexEe8fGY~nAO^)C<$EBDSfr6e9 z=amjyT}5Z#qhf{oQ;%{@P9W|Pr?V9~+vfdG3$e**<61~{Ht*j8DTYCE(l;WWXS<-{ zBQ|z60P6wwFg0gBm-ik7vKCLf8%h28@J)=#q5Cj(sIGv!KdP3k2Gh?F`B{Z1cd!C| z{^3zn$~py}pVZ=c`~<2>KgJ;Nb~h5k$<8;aG#kHV8sJW^s1qP!xE1E+-f9AMi1V87 zvQw*FUJfSgGDm}wB$w2Nw!_t-G~EI={8<(c33%)Lu^#|_?TP+h zT0o+8Cp?ht&ntur9JV~`}x!c-? zZF9%prlC@x8WK1T5c8;iI<|q_4|E3apZ09v3&CTKpX>$b=SUEd+DsUCsRvKzD&Isi z5908;{%)&VQ|eZZ)gVXK^IbpG+TZiFbJ*_pP~#^WTuE_TO$oX$6zHugo-BE1$k9tT zW0spbzI6X{P~UXjNUp^Mt||Pj>C_ir$DxR^!EfGFhr0^5SKTPF;q_~TksFeJV~6gd z7K^orq+MGRi@`D?IVg<|eBhEDns_n;}N`yl-#~$ z^F7*8!$)SsXg2vQMbh*#XySWQ%|19|WVISsqIQJgV5stlZZi3_ujAP@sN%o=a!wsi8NCsdWGJ${ zT)B+ylSwtdd6J`Os}!x|P5GDCkli6%=RG>rz6q&aPlvhVY=0X3KIiVjg*VR`CX&x@ zY?V+yZ*k@KSycMa%iOXS2LQgU8FE`MTV?!~_(~Dh#Hl=M;_hUc@?Lw#;RbgLZ!;~6 z!Sf~G&c`V_4=&X7wg&@F%OH-kOMBqHE>yZ2u|PbG^1ZmZSJkiK6V+mSmo<*pYa&Bh zI3t}kiJ8%<2c;>N6Xhn!DTI|1crP63G>7hUF6$$y&L{I^708k!b`xpuNJ*@q3@UI@{C(_Za@4`afu7k9s~nJ>Q4<>C@uNxpl)f>{ zEo{*ogR+{4dT|GQHO`B$)xy=vi<7$U^FB(Y+DQ4#NY*cK0ezZfwHjHFcCk02Y2{q10sZK|H*&5hgP7`*s_xI^wN;W60!3z+!oA6QFxv9=hw*J4;zyf3!QpFZk;&7dk@HfKy%#6F?w`Tu2DgtQEcl{ zO&qpGYvAC7mK+y8&aP>2YtDeo`x%ScLTKh)s+Q$-aMKc__>aS!%LNN}vdc36e<36q z5{}jmQ-7zjID-v(iQ1MIDL;qHgYtk=Zhi=4WNH3-v7rWPN>Bv~=GLQRm7q;=C?gL5 zE4R(GOqSGH>ENAdxkKFaARR?{`U`p~owsQ=dV9fmRRN6d z+MPW{d@q%HG?>a^kn6A2%dpj`k!f#S*+i7r!RdZ!Imr4JNPbGjj`ol1HZ#7y^E$gB zXp0{LO1hJf%W%GI;oSec{|HT+RmqrGJcwT(60I7aBk(bzVU}$@BJ88X^ArNS?NIGdS!kh-dNaOo~2{tFDGBPu{&BjvsyI#1DW8HRu&7kX<_Um^bO4B})*-#!# zKD!7C$2_}LvOs_LH1<{-{`JL3SCfz=?65JTu>)&dd`)uU^<%+(3qD%oAF|Tz`wLRL zn%~3R&{e{fVFCwPqkShD{zSi0ak1tB$Xj0)j4emjkO<&H-mpDUw^I& zs7&TAR48LSkDHCS+_jr33Lj%g1=TS1_0b^jnSf+|=awFBHGd#&@M2=nP^k63h46U( z@<*lF;gWs0W49J`NMmcK^%0<`%Gey-yN$kip_Kfdl;RNbY5d+N(l?85h63#KmR^jq#FZ>M!X|x%w$c1E zr;vyw+W)b4X+h1k-og`O1+ku+2R{4Aro_>Cai)}^GTl8Er!&>}XYjFs@F;vSLm=S( z+3{cm8KDwi);j=qa-DNnUK!|}PUO-d!Ck`dLs*Hn{w3=`AlYP1D!;h9S;rrlSv9TH zg68Y5hW>E)&df@acKNOt-(Ui__}EO-d=RaEwThmhz&Cl9n|lIkoawp}n=o){A_}wk z{b@ytM3D;w{E^~#vQ|3qfWlyz>I}M7UGNpqvv|vEZrCarR$*%|BEQB{ToQi>susMu=-Nlja7S6G)P468c zO`hoO2003Eb(k~yE%#*Xwy%v%C2F|F@R29a*UQfma(AprDvuR*`&9hey_hT6Ev*n- zCO?_>;=_X6Of6<%u!Gh9d$N7;s7^X}c9NKi2hXj6QxCfZF>28Tv8+L&a_nsemzO_k zH(aqlL-418_uyY0YaNS?Y_2#IDHJ=f${@7tDUlp;Vt(S&zkZgrr{v)S1F}?0L=`~| zP2ubyh|+sraUM{2HL+NHSVf(2!jDhnL0cA1D{Dr zdgf<^{RF&`!6M}|Pp8qLe4fR@bOZ}_YWSo%`fXBwsalrpq@2NXiel5M{L@C#ZuOw= z?fW@P;7rQ-r!rU(V(3`HB8*{%X8tZ9&iSPY%gAv2Bn1p7o1>qFw)-;W9$MaCggL1N zPDD(k4|Q}n{NxbDf!e(?BrdRa$LXvM(ghuz$QhZ*n`$)2y9)D{aWH}{^T@JUS1^)P zEF}1}*ewS?rg`wJqm~Gqz@NoyDUy-rN=5I9T;u0eSp@<9uOVc$*AAZ33dkLwF;F$( zcj$G)HMT_I z4^6EP7aCnO=x&TrMh6?)Ka=D*Y0>cW1qHYuvByWyimT=ANsH}UVu7%8$D*NIdvc&; z5>qMAV{_yo3-Exj+S*cBzcQfUx;SRzE@%%3A5l1_e9fSF!fs0BckH+S<@k)s#?BF zK||9%ea-jBOihv7m9#6!zpFSnwaLt6%rSe4jrd6zOGXI9?vFs~H)SJB-L% zJ>sEkifl=D?wJs^jgE z$0(Yy$iRaCEQl&eLQlx{#}&Rf`Q?lKoEnqb#Z)-y+ibh5+1bmc{`cP*;rj#VpvE?D z9Zz(MPgnBombS|g{cwLj*v;9AvsAhN03)z&-9z>y>U*nM9&Wezmhv_#lUb`JF~!8h zuphWW^jN|lG1W!{`ZCPlA;Jb69F|)U%GK-K?_Lu570B60?fgAnu#n5j^?jG_kwTD|7aH-3S(n5^r`fDWaByo*7 z!CZbP<53Cn?svh%Rb3qsYZ;W>xRNs5{Qi4%!i{ON=}nP^{=o`L8Hcgr+~Ok)Bu)5t zmgoV0w0NduP#^cA?+U~zsd(&ZRSmS8Y)Btvc|-m6MY8lLfd{X5X$d?53N{P(!s{5` zBfM6p^nfitZa)$vDhcA28o|V>iH2_M>!$S+jxDMx@D3Px&~8t=bsfY8>@Ay|>dzq1 zX1P7XtJ{^)kOo_mI@-v$9bcZk(Bj=76XJ*%>#2xNJ_`H4@G;vrG?Q$OtyBgN=zMY$ z3r*cpG&9kKnPnn5pQUQ23b@-3H~(I}I-Fz-a<(LBZouVjjFoekCBh*vg{3!Us8#-u z9ee*rFPRJ_nCKQ9V)Klv(x0-ul;I^K9kM8E+1Vxf)TN;~0h^cdQD`~Oai%T_dmFuqL(O~m5pwxQiIuTQ_PA&r38~5Te@M#wl7N{D57E^pomOWGRostdu`(&3d(CN2V11Wr z$EJksIKCDlu*~z?=2eOiMjX}ou0DSDYA1CLk`E!+su}lP*U!-iT$NsFMB zQ*9>U-&z2mU|RHjjpaPkwx*@N2ITx6QW%r~cEU(f6G3ZL&9y0WU2147y#!cT+xL!C0Te|?%# zTy=QRb*?f8HV$ljUc9L2X+3|x&PXhYU6IYAi9AwVnj@+7r52l+Q82#!5r`Z&QdkvT zICH+}J`rBMF>NTb5r<0A=@@Lj77*z~LPR4J1dA+mI~)Jjv6|1&JBHm+~93dSOy-Y_revhl>i>%eB@r22U;$AWs(-&k#H(kS5Ydqq3 zO_g;-%v1#o^OmH*l60-H;;$s~G=oHf`7tt|!${W9d8DLoydeJZUiPr1+ z5GhUL#%~;^;J|&~aDv?WK-V<-ObGrXJA_B3Cyuem%0Zt;Eet7EeJfdw;L=ttUuZB$WYc>pMQ>@XLEa|?84V= z`gwYTaFDBc%5Kb^Es!yKk&A!IWzE2ySyb!g!TIt1t)g|xicaeV%%rJ%k`z&JjJJL4 zelKq@qSSeRH1vxmY@{*WP+OP#A9L}#=zO|wlxO4CRV%v1`_!{r$u`~;I{VD>>+Olg zIo9%vz^ljLrU*mL@0oy(*4C3u1>?`!inPt(oc94`As{~l>}9cLIFN_#OGZ1CN%gyj z4v?(B<=t$d?Qk=rOM=`hr)`{Rd8dAzX3yTX_AXp}bVRZo+!z3`-;~cffLJ zJ_ItlnU?S7;THeA3f5QSK{@Yyw{>9a;&%+Kh7g|mKS(|0n%n&o_`fOA#e7M)1{LIRkJcD0*x;O68>Bw z$UZmkw}_10c3ud2AFiD@ReH_T(<<_PQiMl+_HXO7iJI_6O`A=yGBVqqIYb?9UGM`x zB?ZCh`k61@hu5Hw$r1SD0=2>!T7om{)6sRi{HZQC)lSyDQ0nx@Qg7l>^ai6_du-eI zv>N#Zzpg$K7bzKI+w8Ed0HEYPo`0zA~B`Vy}klnApd_wMo zn7UpxGhQp47vppp0h#r*+j&-#LK_*I&=rfB_T8D?MS7Uo!=3WLyDdH!o~wv#s`=xa zHHC+GUX$Bjm*rrgul2D-Dr)`BZ8LQ*ZWj+1Be8()FVWX#ovi*{p{r2}VlF(I=YkU) z-_(lC>-olKU6jn~?Q|p-7q5wY{;%QI5CEK)1&%Ropivh!VS;DAWvG)5;+m7ioav9-`D7o3Jccg{QGcL)n*`si5NO|S zjh&XNQK0k_)sT$}DGB|eMC7LS@2ZGcN%1yjGJo_J^Sw)8E8n4=lBjQa4U%e9zaA;N z&`C(w&<&ZzTjgm@=F=?BgYe)92a??BIpn(&T*A?ouE_%b==c=fkAl5`{)6<+)b8zi zcM;rDtsxEFNJ!?kH`$;f#WAS!I9RX&0P={o{S$I{aePX~XF(r0_(6T82Do#1+|#)1 zolv66-la8Kz^EQrSwkFg_H~TY;~JljuMe_)Mh0a7j(P$JA|*3c#z46m{V9_}7Ty@m z%X_IfOPMigsXm94*XE=VQ>*mP$0rs`fMKi3NooCl(pY-5m{dtJaCns}jg#{%{5*|9 z01@H*M~qUn{0KMR^g;TPBq#iKIfWy=c=h`j^D*ct83gL&X` zzZL^Ib^}%Ru#usWY-Ey*Vwy#zvZMLL7JWop%wa!igf`Psx+YjlZb~0TDYpwQ1(y+_ zcC>1_Jk+nxrjM?&TMB8h$)M=5g7lE?%7_JPaFtSx#G`|FyhkMh?3|%*4!q!z{A`1H2v!@P{a_P2pyyfFDhJ3$} z(k1=Eoe{=SgGE!M%*WFEED#AUb?DItYqeb*mQiX73Gr~x_IfumgF$q`tX~!N@8=y4 z-@b|;Kbu^O3<=RgA=yseLM|Mfw?z} zZWR}>+9v}9sE;j{{h0SnBtDdtZnoPi5F^g1Umg#^K`%IDHIO8ULD~V$(&u>Y^9fQ(9kI_xYeuE4t9kZ&Nr!b1nB#WFN zn6o$SyY{&J(6}7jEKT+D>LVset^1%Z$LiNlI>{(^ut~zC#U?kukr|DPJ5cuO{9LDQ z-Bpv@Q{|EC>+35R3qbwVepyH7z9H$P%bwuE&D7O|B^# zF3}&`Xc+==jwF7}P}q+v^?63Tr{mjk(Y#%@u}45HdgSbkq9k^FKo6>}$V~lhOlCH; zKDyg24GmNr?uS7lzB>sC$ErY4n%i4q++8A#bV%|)@=ZI%R|c)bJ_*D@qADNIz5R-D zMbs5VpRwkMxchSY4<`F$OSpTjDIGs#tPS%V#4oP5Ur}R&AaTr=Vbj(Z= zDvDCT5JRmrabW@0;H01w7~j2#YQ&F&#LCyrNqf_*2e`R#e-a)u##?93qhk9?N;{ir z&g$;t01^*MyV|$*!|~#j!lRYTI8sg;A$F52r)PDDL87h0bE$x~MDm&9N-ul;M59qn zMp9bL5z9NI@I}=L9`TmMT_?QcQp6UZe%JJisT5OLhejLhpjI54BnYl}ie8B(Fi2Wc z6W-lYIIJXU|4x}z@fL=GHnC4+aGCn+v#sexEQ+@v#!qSBz%Y?0yd4W2Vo)5%*G=rs zK)mU~y*t#63ObC$R54t$0~qBNN*+h7RreUlvsXqGwyH6v&OqB`Dz21Kuynpo*51u(My0UeE8v~G&Vq3z1rQg5jL$*5SNl6^nm+lPm&C7 zF!@t&Dl)G$bU1VxjcLJjAWq(T(>>*Wlokt3^Fw=l5Iy^Dau%2pGkDM`O-5c+_nz6d z;UK(Z|FS8}#@>9v_(9KW}hkDp} zrN#q)Ra*A4%sYr(Dg&!bBAh%EZ^iDXyNQQx(lEoj+I90%xEjMC1|{5{-AO) zLs6E5nm_Y}k!L5ZAqGbEDWdh;g%P+syD@~4UMUve< zX#qRc3&4agG}NsD{BdNx_Y2UDCGHkuR%>#}aVJ9J=I{XLqm4WIT)nt6^R9RL)Z3MQ z_FdfdzUx>@hDf>trU65xiRZf?M$khbnZy62&xtH^VsP0b+yoGK`-OA1p ze>_e}u=93+Z8(eYf6^o@+h8q8tcF~PCx+p-b6658pST>SxPKkqzWYKdZ8Zz<4dt zjgrlJ(nR>2BEr3$R^0UZuDyPzsw;n^_1^Wzai#KcDpRrIgq+>={|%P67LKwn6sZm? zMf|#5zZnbUa7(DYf3h#d;-$xIz8l@&)!gWWrqWB2K~&t$!55n(+m}I3EOA4bZNY&Q zl2;%T|F)hLDP{&8rYie4(mXI4p(i3e9cI0&@?rv|hHx*xh=3~0->iH(#PLrWfuh$N z{zj)7U&(ffP}2gLN_d4ezkeo5TKjPed8QW&{wre zpf=1yk5c_(eYUWawfmzf(m>$hr)&}wqP$n^!Sc50=iaF9RwC5Z<>}fQY8Vs#TUfsq zj$W7|rTpO+hV9Qu%>0lKL-hHks-K!OljEIwa!3BLRsDY8N^+)huzzyOVj3Ojyoala zMjJUk1-<;5Ktt(pc#{T;504P=!%Bsaq07qhD5lK@{*+csKNOzUqhhbEgp+PZ?0qht za$-*Wi`chE?zmLndSv6IG%FsSDmLxyn3~Y0Oi-Xf{BYKdFRIy(NazWdxN{gq6`IYZ zwS+dQ8cvT*FL}8o6L>eJUiSf{6#KsVMc%vK*(jdVzxQ2FQZPP4GWMl!9|)(HriZ|O zg&-DB8xH=%?Vl+zu0MkCRSASz(@M2hW{0MtLO-!PZ?N8E_*0nLcn=*k?ytVHazl%= z^9uD^bL4{L@%)C);kko?2t)XML@YXL#)U&a<=aEHM9bpB9#b7|E$sq+I3%@)7q@9My69uRQ?=5LMya|JoGzQIUeKQ2|OY zvUE>YiljSpTpu6TQlJUlfOCu3kmCxxb5w|=en7ki$dWFB7&v)+_fywQ{9ktj#ETgP zq6Jk8zgWV2ApjrGc4P=zSPFC`I7KvYmlBlsjBg(O3*e#~1nM8oBD@ z%|>r^@~RGBKLNw_bnz&wps}pxj~mV0>s;;DemJ%dwP3-$o*P?=@D)3N3{xBJ-y;o1 zng$q{oa}eHSBSUjFq-K{GlYbMu$at{Uwhp!$w@L|CY7vjW&_MH7WNXfq+C?SkmTSsIoE zj_y9SkrzC6{34?>1`8P4d#>EwK>!E&01{Q7!DRxjhvaMtl{>7k(RWJ6>pGp;_8Cs) z=JM{}8H`ZFNbxr%1`p=yb!)Sz-g%Z)jYF$MeKM>~Fb!WL+CmC47%|I#1~w27 zYNvc6!=9&%jk~^NeQ<^lv_GJ|G{lQK%Z5iPamE(egVzk)u+YHWHsO%49agSljs$aV zkz^~RV6F(2p*AP{?{_9_l@I>M&V*^s3m*U+%?>}}(R{NHH}hBRQEM4QGIfwakMYNE zY$4*DdD4d8-`Y3*RPz-Y8r5@DleMufTrPnAZUP(Ec*cQ}jI>uOlg|$-eD!z_`fQU~ zE%XPBvBXEAR+Cm&6#S$$XR1P|^{i)eo#t;g>xf3C7nc_7qOClE?fhFPr_FKqg0mYj@|Sc)GOSoW5^Z15tG+teBTCfMettPPy&rKQWC~smbqS%)M{=(} z`-q*-b!m>N=c)%Zb#xt(lo=5Qy~d<6Qy-D$Kwsgo90W-jbT3&mMahHfxi1}dDd{FKA)N2eUvvm&hK(D zhZ7vqLjmYQ?>=V3H{d>D9X`f1KgyrIK+f%kx5QMjCf_YusHMZQLo` z-QC@3+@W!IDBNLr=AGI7VrSls`L+KlG9$AxGBUHWu5+FHoY3f}=E#i42bGzC_zrbP zonHR=v;4wx`N~nL1;p~ze?@%MFYu_ZL~2?(ds%f$Yr5`TV6x2VteC0(pKTTtQv3Ho~YVEEmz;$BVpF*H0|i}DKaI)eDB>o^;Q#xQVuS5q49J%PRd@h4Gut;8oM1H zNR?R`ZJQuk{Dt??HfXCga5_m9pG(Vbvk?r(NKqm7HRo3LE6^?xYOX#4QpF9=vdu0D z)@h*FYu4(9*Dr%Lm~#=_u~~0lv^day=q30$MZD;Qn(NN`u=5E;?G4eCg2HiVGO&T+ ztS0M1H6pPhRZtx*N)7jVUc1}pfwcUE#?+ivvs`en!Jzt}1XYa%EW+KLa?&$7h6BvO z?a2^!u7F7>n0RS=FZuGGW-ndlM?a*EU0?8`{iK*H{b%?=drw5&5{{ql8!bm{x8$Ql z^vOq`bh@neRAqP3jL3(1wBpaL0`9vc;s-*ZE|r`o8sDE=_h#ztj}}w);p{!921lC_ z(&HAhBc413?ad`uR3(reGKT2c&W8 zS?6#<2$Q1u>l*(Ju%NS{f}e>|dm>dEvfv7B-?64()b)R=p~QBsxp$9U&PO|*ABoBvoTyL8vn7k;)CxkSmqe#!Lj>L^cu+$L$n=-0?s zQx>^@8QxP4^SDPZ_h+BdJpV+;UV5^3dHOr34B%p7-Xll+qk}tM6`V}I24&z)c~1&x z{$qvPIo1$Ww65t^wRj`i#{Q=&&N+kdpQ^ZRF7AKTSGKeHMzkngNrjp^#F!1Zxf zu`0+LoUhTAF8SnyYut}@ZnvIOovuN~{WXWGxv~FtKfBC2zW#G4>>p}x(B_%6(mhcY zE2CV`4n_z7)n1|jz%=4fvU?B7jmyW8?zyD-Oj9QIhpPn8`m|?HhB0RE7WZ~SCMu&n z)1>VaDeaPg*1{i0n1k4!_VGDJ#ouJ*(SNVwD`5BO>V}v(lY-3osHRfTGkn2!T+{j7 z?}cQ2qkrm`+Bi>wU%bh zHXukivlR~-2+?9P628QT5Ae>`DwAw3eZ1+Gu4I~Qv6b7Jz3F9LQ*Q(a65H>H=3N(? z4g&qfh;ln);TZ)ne+r)CKj=l{o~}&uHOWy&I1Nm%yL=718V0@T3HLw3JKPG;czjI5 zG>FN{&cc}$e%*f9iVat(@P%vq4CcQ%R1P1JEztaNa z|7ilm{QkY-)=jB~2y^(rdK*alji>&;fapfJ%E|cN1*@P>nqOuyAuK12Q73`02atu| zBCda;DibI$VeIsf5CC^?*1PV}mcIL^ZiuznTMzb{IdYXjvAoG!-h5aWt?6lmRg5HvhT;xVVP;OBBIuh z2`$(RN!S$`Mg1~5>yO?kck@W7BT@wzjIW)rdZ<#QLQTh(xK4jp1m@xl zZ7j?DlTCI^(fSC+bEHUdzgE3lO~N~_QqM0Vz8~?%%Pj12JXkZWJVq@qy7r~b&*`{O z8w2SXlIb*uv_vq(v~b&R@=N{yI+%}kR?C0ojAeFGFpnze{*z%b#UU{sI{qb!Fscwc zphAA1_%Kpt%5PR}gcvo6yeV?aOrz=wyvwKda)d7@PYzuFl5lvfk<;9(Jt%aOc3i$z z3k2Qe=l-5rmGRk#{+iTVeZOCIp-~7F7GY`qZEu+S#C;+TIt##~ri1e;dw=?rhW6L{ zxP?$nenC|mv{Hwdm~&Luz7E}R)D^Tkq!H{hD^k`hed0Y>j<$|e*l!gRx2a?8s){|H zkvS>xOL!_(QeG_lN(AWe@%~Ht9r0(qI%2#bVgB)Yk&6d-nSp?~&si+s{b4t_ZfvS< zaj=vmLMBc%_IKP@D_3Fvtx|%9xWJ(;4jp1e@E5C34OWB0iVb|Us$|#Bzg2Amp3N)I z7Q#d{Xws^t%w*gHxluoD#bS z>1z?tJ!>;-s3XSQ#vHO(mr!V$e;@R*=8i{mCw^GpyL9S$Ccp!56OQqS-EM zWP8W?kS(dAN#J4mOH?*&FcP06oZx}})AeO9|38iD@Ne$Cn1o!FomiA60``Ie zIZjuThEhY{w7YGwt+~t%s}`e=N*KSfo>F}*(`$}-l^+c7;F>2zcI+u9I#w# zh(AUUakLuyYNZ3tf~Gn;vl*>nPsY%HEtB8sHsa13=us1DPg)Fm=U!_r3GkOBvOWvn zl`=f2Cp5J)_!HrbimZV|X(n>R0L}nrha4{;G)U2h&p~D-L zu>!oLbwj<9)--MRukz{ThjPPw;HMQ*L1tc z`=ep)<|fZyyLZMJ1o>Zd6~OE*uRpypt_TWwZzOTQI3)vZC$q8#R_W8sM>x{cJ9q5- zI2O=yvnJE+820i5f*~@)4h%`M+iU?~H96Cb2xL^v6|ANC;2mWY%s?~lx0$j}$S=?J zm@z-4%ZIbjZoq8b)KQVQ8Z-9j|53Bkul`CxI^Z*oj`Z`gl)nKZ69*}*iiPqObW2+}rD zw^65y1(Ljw%0MuA9W1`Ry{Ukl3?(OBLy`ZGOM&D(!Dyf&qFtco;|o zd=rsxcFX~Maz$#X(le*|!Phfg#2#@lzc4W_2|V?Nsr*0r>UwicrY+ajQpG^^E4Z3pP@E*pOpd)0sSyVX7gOUNTFZ*pK>nuOO;AQw4sOE3P3~Mn@qg zlNz#AAQ0pZ2B55Jb>|&@jZVpI)Q`_E6PO}c$~-eQ{%wG(-h5lPy}4QRgC0JeOkp_j?fOR=0Bz zRX$H~=&2Qo1!x3Y%~?qmgtU(^(0e3>qFS;zW%KM{&g);~)!>2)u!_@0DA_j54!5vF zG@Fz@SW8J;l&b(YrWAj!(r<~TG>W<67l+xCJy>H4eVXy%Be2n++jHVMU|y9@*@IX^ zP6;V88@@XKP2E7l5e}bW%_gn>U{QNlca&#Ts|4nlJa>(N=l%7gUhW~bCNT#(qLrVS zW2>)_-1I8p8gX45RAe?XGFI(PU?P7R0Kjh9x6eANBxEFg3GAfnrar4z7;8+KdY0#a zGB^PqNRM@uIiuuLn_t7xQhYEz%X9KtD?DFzijn_&Q~G_y#qz9Td|K>?TwguXg9l&y zXkNpQ@mEP{`a>80Zq`+6{-X``)yT@fgziJW`{T^6PX{EYYKj`Wv7~QZzis*U7imMS z3H)!6e5fb+lkoXvWDW^T0pM4Q=na9?l>TLZ8>9Ib^2?u2Q6{SPZ=K6a%Y?>NitYmi z#a|fY;yYU{=l-goem77LeM?slo=^FA?z>}hGUIJ~d-xcc6s;;U0MCs7HmlahYb+4{ z)OAV(N&2-e7UKA3EmG2<$cHMgmKnB!MG;HCcKkUcR^~Tp&n9MKm}aHmu9fOw**jN) zGMf8O#OSlu*|8x;$9e5YKwIZs=3Rk!^_xVt{V~H~Eh<_v>9?2$*2j3|T{YK2as4Y& zPnDxw>$W}~DFUb6$mU&Jm=vTdro;yvw47lwv!y>$ zS~nu@G!_r>Zfx@V-WL_&H@;WGEKmaE_6(W~6-Pa!Rp3!*Z~Mm5j0;=~w+2%9RM;o% zucG1dC`p~0mxvJta;lw{Y-WT#@_16?v{CU9+>Pe>iLP=N z^<%H0;OBw07xZZyJ+MXzw`Sd$V8cJ|7ESg&yN2!aj5PRE*x=eS#YVT+x(iQvNmA0> zir15|s8p*5L4I{;`V!;nQa1gTme3kv`yAMZRw${_&g5Kx1^fYU2#nGgb?{KCdj_Gb zd5OM`{_!CvXwsxazXoKmzdIWYn$!jY3pOmbOTfSP{ba>z2&gFe`)|3W#tQe&_illL z-NgsFVfUK>O+OUknDS_8fiW(!EwkI?vs;Yn)|6uT-@h84g3cqmqP&yU5#DWBu9{tF zfBY6wWHLdhmhoIhkSG2-60eGWknynd{yk@A{OE-WbuMNkz8GwG`=-r=K){Rr)BT(t zaLxS;Bs-E64<}vk4!81Xk(v8Qye^mX3*1n zN66XE1xC$D3cc%854TS$xdh0*z#ZVUrXk^Xw@KSE2rS)#kB$vXuj(&rX;AR&2-mlBjKN;1?g^bbvAU zjWIa+x?kBiIyIp8~foD**8zBAAoCNWK-fy@eZh(TGA3QCVSpdXv9wyg^D~! zjW++g!>j}Ss+Nw9JTh!e?qiY7^-PWZoap?mf@fijYn2^=#R)#0eeIy4zHMl@0WderB8RFDcy=qPXZ?Y7~1{;9p|t{ zN4u+EXIA^}5npb}<#I#C)%|hR#*LVEnL)w)Tx%+;Y0sugJ_9Mp!}-mj99Mbu&A!}y zZ2PUQfMAmTuLGX4^X0Zv%0Jl>278xy@c7|Qu@rr`9~ao6inaj6lyKo>0Dm_j~QK;SdXW55gn6@ULDIb5C5fT*>N#;7J=O9JIX(B>2=pmF)F;Fqy@?20sMFL%-8WkzNNx{a=%!$EBORaW2 zzJAofLTIiBK*qiED|m>yJxolbr8ysN@;qh_9BnV33Eb7V`nx3pVo45Pqq#l)z}iy9 z2+!a2L@f0MjV?N`e992`@loXMICm+&ZcV(uaxMCfYd556Fhj%gwU}%XAMMP(c)C2z zCpjV!@O!*LS94<)zO%y)jK~g-jDHO}1F0P}xh#$Mu)P7Scul1XjyU-swW~h_*muop z=6L#dhDiv%GwA#8V0QA6K~Y6s#&W*ZW#6KR7_x6H*xOy2cDUI+u zgJ4ip_s*PFmp)h#*!3VpIQosVBUoZ06+|LIO-KBkG5NNX)!~SQnf-=jvKBh9yb98? zEphxfW@nnb0nLKmQGEYhwH6;?NEv^OE}9@soIwf>l}v12dk5t)|N^ts3FvA1s{SC=QIiTFV>F?8DSBfMXT$;TC05cd{xYLOrY zcigWxVJs`}^}zQImmONVNaMk?&ZBD{WMbMjl4h%F_%rm?=;$nqW&5n57E39P`%%j! zy%zcx)5O5(6=i|z1OMF_KPcbLI8D%rCPyc@MD^m%Ng7?J3bxl1Uk1+RAKrNcy~iNnjgGp*)G`Kn#(EUSjVj2P`8==XBY{q3P}dIrb){U7qj z`Z*$Yyr;TvT3s8|e&{(W)%(>g&0|0M$`&(M_##M&ysWb1qHNd#8}?XbPE}Kzp7q?U z^)uAyp5``k^M=O&MpfBIncx8XQm)UH$IO`uR60+s*CTTTqe=6P=bb4h>Ww!(i~Guv zKN1>Ni+FgQqBx6t4ApY9lv)!eIf4@Iu&R)V+ouR2`y}ipP0!m971`|3JRE(Za}!T3)=CVzEAKjIm480-m;@lH zJ?{^Y+d|ecH$t0ez2>zFiicMLY8e9Kv3~M&vqmhj^j>LQ+DpdY?JcYpdE22Ec1u$U z{#a-@*o;tCD$xXNR`ZrOG2Ye8W}EZ~3|AcBs2uu^U%ucchE)VgvNX3&c*ro`v9v3f zxE9mYGe|PLWqDGVWHeZCO1jt%kHQhL|6E}AfNB_9>(G?-J_|72ywU|4<4dGxmd*0c zR*2#diw+(hRgw}q?WIE-5O8?fcB|--?78bOOoh)gM%-`}? zF74>Z#ZYQP-^XIeDTYqL_%wf)^=x_K-9A|5Vm+(^PFmUM?^}$^EXSebGKPPe^~SmlcZi0Xw;+qcRdR1;b-0$di+QIhVm?~s9c4L?v& z*cS+cnDX56TY7}L?$98G)wWTp z>9kl<<_+qCYD_O;4ao5&zRwh*L%5uCeFC@YRoE1zIQ%~m(M?8JmFcLTU61meS#5Dw-=YUJMZiM#|7`O{uhdGQbm7bIUSaXs z()^tQh^dN~6{sdWo8b#Usv_V$vqk|P!y4b8P)9-2^|Sby|#ZGVNR|M8PLh`$j7 zgyJe&H(lTDyo&ek=03kcFlz5HwI()vW@d;h!0&SD3Qi8fU`+*yO7WCCBt^f^2mv16 zWYA=G-GY&AQ&jQHyl_m+BDo6bpj9t~E2ipevk^eHJQQbIy71~HFY|40@aY1KA~$5M zger!$+Ktg7z*uYIX2I9O`Ko;{DUJJ4+Dn=wSe(jU$cwLxCLnCxF+tVzjF*jgSLeVX zlglZDCFrfJ>Ke=OUQzY94E*A$9B5jar>&xk9N=yee8vyenfH z-QrOiwSEK4<(2Pfu}W)0yYj8Y*VvAb+}sS=m$8%@pOV-G5N>BS6U^oiiFN};JJNc{ z1lZt@mIPuIlcOT(HV4ihk*oSS5>DyMO8)5{`?&*!L68JZFZMoX+kIZ;%9PSz|N%GIYOv z5M1&T4M)A_bVV+X!7ZRN8Sg#Qn-&k5yzCL7JVRffKntxqVC7YRa=PUxNSyAJ{Vc|g zOEjtVQeX)%K2K-C#b>hgs8;nWtJ3kiQ!eYxMnb@+*(UuiDH4s zo&Bi1T<^!qZBI&9R-tI}tWI2dcF20BN9S^_>|IkcqB&7dKvEK+ zM^!N%JC)%@Y!5(aBON37E3Q4}Yi!+KtS!2tI(wtS?l@bSl-y>_vUR!0IBunm_Y>6W zY3WY_S-Fv-P8%EVGFrIvYH;|e>>q(4Xi<8CL(Q#SPZzRyF>=;^CV>xYOaENLcqs!< z=7;9G$_u3rZ@igu3L@rzo%k;->DC`e#iUyK7^tOQE&27eE`vW*B=TbLKyKsH)4`44 z0?HYNy z^;}MIzz}chEKRvZ@dQF(Wst}uK@^p1{kOD66&+{}c~lo*VB}yV?8d-p_Rdcl-2F7G z$xW^{$u)CDf5!g$dBOl13WMO$jLS}P^DwgW8*#h})-oy-6fJk3grN&WF~RXzk=3*m z0%FO@%pMuvP+JiBZrOoA zmtQ_Md~VsRAs0RAq%P4jWm&MI(nBfIdMJknU{?VVrK<0^^!!ZV6Oz@FnK=P7r+E!c zcnD3ijWKKx7=qF+FoT8d$Tr}}nY1x)K%`+l34AZBKowNW@t0huWGozR zPYyIVJ3_<^*R6wfu|B1@6cX?R*w{xaNMhIkb?B^phfMb{NbIG~-<;ExVo!HXsjIV( zFJ9jatsOVwi!Vk=Y^6F+dN{||53qJbhCaY!B5d?#L<7q+gNEyyr9JX|w~&C(MzB*= zdZB4UXO8mfd7_WqKh3V3&;#goCc|lgj{I9asDJEC)jlS-=|CYLhLX@3Tqu?jBfwy| z8q?~~>s*U>_EzER>^_}3xBL;+iJo9zOEd!M!2=+!D8ZR4)r8aZenUJC_xXQigI6L+ z^8pq`;j`pHNHDZOMW`tZP~AE57*;;}grf}n(8_0seiLmGCP#9b+>CE+QwLaU6?sBZ z`OEcUf&~OlJZ*5tFXIZfI4+c1k=5}sqOg@r^=gc_n2ZBV>h52oQ=2RZL)Sc)rrW+- zG+yieE|TOlS!%(Kul9tv>fHGitG^YRQ~bOwOqA(|jF0RK2>_tI4jQyw`=jnR< z=NTXZ6P?oVfM@f&s10b9JUpnVlIiOfcF~J^?#g|=nHilw?b-(~3iG{I0QXgK>dP2F zv)=MHlE?MGTmUMpw=MZf-CH8R)o1@wS7_I*+4YW4+@YbE+|r&uthK| zZwD(o@SaPQqJ)$}50q9Lp8BM0RFR$e*;7mzcgG%#v$!yM^>|E5VKRXEW#B0;IOFd{ zal4nm!UAk9GuG|lFaSpy_Zi?!YQ-VpO9hJc5f}omkc$Gx+RWG+E6ti)9Sn6})j{#- zHl3$X3+J@U-}&l_X6nlQf@}%;I&D~?RHe_*3MV}FC$M?dp%arZ`uJGd<2Hm8N3f7W z7EH0CN<~M6vIsq=FleYW_&ur)(1nCM>TNnal?-oG0Ni=}@ZlJIVzS4~!He2u@r+G%y`}bFlFEr?_NK2p|+;0Em=q_&8juZOcGr z(3{p^E(|BEs`|Z@gv@Bs=PjWK1ahR#VN5=ibX&IuwH$DyholT*wFLYHflKB0gqOjj zJG6LrD>cbfV?-xm+^;nnx4mVDp-*_j{&Hs%(TmS3ZaNq>6gM5)hqi*$mF}5)40J*8 zu|==9S3z`EcBH6f+f03ALzAx=3^2q@s?@BEnXJExTh}g-URj$oG3^pSz~td+Y*yHr3zOo3#j zb1atgv;Fm7EbSOq;r$}Lz^}Nrz1y4*VdZn~1x0kvYwOx;7oJPsBW9j5`CM~T<(6yDTS}Tn;yK(M;5YZHGTbq85H`wfE%Qr!{3|Mh)u+G!JW&rf;`(K%owf zAp-q%=UNZ7DpmaTa!Q{Vg3q&{Acn1CCb3jKNxP{H2g57CnHbJ$$-|Rj+eJ}*l$?BnxJj1+jh~IJhlTCP_ zGsj-M)E?%IM(~Cuqaz$YpRY>j02ZkpIQoiMik>_3!dkC|FF4DZu!#nvXYJ>xl$Y>K z1`SyOqlN-qIc0cVrscD)r&vnsxbh;y8Q5$}={J|DokbNSYF#gX59bmOkq5W?o8dqa zYm83lp+pG`6!EP4k&odAYmZldnYd6DPlahe<|WR+A6k_D-hjWc^>S;)m8X=b{k@xX za9@$;rzru^I^fL_tvfshKTz)Qa0Wzm5Ic+%*`90_?0^f8E&u*Yhs$RYfak4@&6!?Tq?Gz# zN?rC=9V^5EssOw&%V>LEYk6wT3B}CT8@od*Q1SrkEx9`xRrNLCHn=iaAXbW498pV* zY&~lVmG+4ocoPiy#OnwV^{dDGxh$4{#W1s+*FCm2#1&FRuL5mm z9)dP)Q1<1IBvd@sMX?BU`jx5%j?c~qKCG)hELE$V9mvI^rxs4TG>e85r+9w)W{Qvf z9pzeEFwG5%p_P^k(x8yI_ldNg(#;|9^rt_{=)!BLk40akMa=3H(>w}qX+2SHBa+%# zZL+U7k@IhFZ9M8sNK5m#;W0Q<2K=()waW6reK|Jt`-&4(=hGG(k*4h==t;FlJRTfL zZ`n^&aJ3*K4oC5F)3M@8Nu%eUGTY;=RfpuVq7aeZ6R+uo;k;yy&uF-P8b7vDg%^pX z>Y-gF!DRW^@LGTsaed;x9PMe#!XnCA14LK6%xRG+&9%?oqs8;)=7vhXS+k!FTY5)6$MVW2 zINeZouT>#|>vE`C>Kfqduk9-IXKEiGIBW6XPY2@L)KbTkS8`jUWND4&--!U^gQzkQdF8zbVSnr+dn&5S(T zVPHoh*Oaz5Yf@&=o6{&6g5$E%k$(7O=+r?Tof#d=t<404Te(qT;!=2Vx*oUVyXRl{ zm>!d^jS-leVX>$%sv2FQ6t~;&TZ`tZzGl!Lsed6p%a*^T&c~-#N;-V!Dh0tH$ck`_ z)*pE2FZXagQu0}N5?a)OM!KXyj@@|k_Efxgv%ivJgCEcFMnHGRTzk`;cL3vLJPr$h zzV3Y{dYnWPB2?ha3Wd-9VCL0{b7jHQmxEX|9yCPWBkh~PTA;X73^xU?SRzdqMuZ&z0+?QV5Ak7=l;D5``Av1M8Mq&;Q!4f zLdfl~{dL6N!?rct{?7*h|Av3oIRJb2YBP|bTU;~i>gqm?*}fKSZ}*KDaSBPiPCA!?hWX#{sx~ zP29Sx8uu_yjzG+YO3V~183Wdf7TZ=JvD-9eV0^}7{X#H)&2y~13f_G$Vd8uEeoch; zeAnXf9-;xfjKayJZx{M|_n5cc{}`mSF^<0Tp0ax`_9#J^&wT{;!R>*@CErr9!kq`+ zj))V2%%$6UYkK4#-cxZ2cAhGTP(bPP8`68uKBIZs$%XU9-4ky>U+4E0!O1YeTi6pa z_tZ_iyP*Il)d9&ZP}_#*dAob`s;@7Q4By0o|CLpK*rwN>b4%%QtH@(5vhlmlH^9*yU-=LoJ^sfk7o1lKN)(ViFEQBeLydWn%7+FAz44I-BAhFVgDrWX+93UdU&6* zzWJrWSbiQIqL9!ni7W#3@myZKTX)(8N(2YO%{PjIBg7QdluD$ zWa}|yH3*VnqV|LS^N^ch)G?nR8{YC_BGWXQtQ;`XGL+ksL;`tR@9_E+&v7eD8MjfZ z6S2j*hk4H?D`oN|8?IKT>p`0SU+IAH(tpyTG6Vu`zsb2Qot&Ve4Q--p+^T{~Q)G;L zc`6FebS=Ag>V9K9dKJ7w$u-5BaOFCT09%((x9A4|5%Yg z8pw|(uu6d08T+uVYCE=@l++v*nD?sve({IB4Zl?B?1V@&L!p~Fqhr3tVwI7%DWb-; zd=l>KhYFGCuHtW85H?kPQIm4GFMKP`bn6nm~r|t6{Dp?_th#-TlK1K8TX)2JSv0GCa-K~B;D@3%{P-RXVDso z%6+qooD7y=jttkMFuw6Tm)on%#Reiee7!O?>z_b4Bh}cJR?f0iQ<(c@t$J-zesd4IJSE*RSHp0L=F?Sc_bc*H2 z-3pE$@-nYilQ+B?rp7n1jmkF$9m$DUkfX|3lb`FHlT1&a2(5m!Bf}Jw;x&yTaqpR< zqx>Uevzm#AJkrx|9^=7QJ1yIbAuBr1hG(Dc@(*=ZJQk&WjQLe*#NC2*sXH5Mp&Qo} zzYXQM8kcmsr_s11!Aw407os*^XGPsmKy2%QsHNTH+CJN)Q2%$PCbQt#aJ(+A3JLYX zNRYg%iSK~toI~{vn%lQbB!sfH$g4`tDMzlOy~bY$PZ*poR{dqL_0bBL#gkW3*m@>@ zE(}U%X4NNGlD2~x{DzcTond=o9CyTxDLh52vA(_3S@<0cQ-#V%xGBM9Hu=sbBlgPG z2lsLx2Hx$b8MldStYiGc2&_uz543wf=6$fvcUu(^!9(o~dFUevy998O1{|?~Gf9%i zl)^9~Ssn2b$l$&&)y9msZAVpJE?6YNq1Bs23o?NywCcwVQP^f7X_4)7Y5@t3C?+@3 zH?+&h*98#ag750{c0bJiKS2MHE7`XbffM5GFEY&$*_C(bVb>$7EI5hFkV#EadW&ch zRN7u?ez!MIS6P+v1@UV@Fhdk)DM*s?Kp9nG6k`@^;hNY41hPueNXVH4Kb~RJ z$7AB-N}X@>Vmq=Kv#^-QW58qxOBCDcM1SH8^!IC7oKs>Y+m`$hX6iY+ohlgM&jvhG z`O=s#y`WYWV+J$NXmVusU0T7xv^TvajW84@)CF${K$0%pRVv3upN7qERWS6(s@o-b zueD>E+ftW-2WoTKwgftSg|=XpXV`7fBde3C z_g_f67nVFC!!DWapMy@hFR~q3ohde74Y{g&F=IQs;Ku-SaVc5RgG_wAJhnXfLIS}) zJ0Wgt{*qqiSW{KrM0vEh*czQ-tJkcZf8E4nut6!T*_MVd!M=Of**QaDjKSM>2tZ-; z-5=RM!6ceAkOsb(43&`+$ViE-O;nZV-?{5tQe8tybNBKVGW5N4Atiz`Dl)}2)&q?l}hU+5OG+YL*M zhA-Ma&+lz(P;%=;Y!ChqjOtW$ijtkbqmLTm(y(GrV!eg;i3dxF-y8lfpm70?fxgPna~skftYeX8owD@YrQig z*04@);aBi35vZ$PlztPsHxhxE(+e;E5tChlcSidG#Dw>ebtY{J%H2c@cUD^XMDX`x zLXy0veSTT(R8wbtx}%-}i!Fm-`nMTpC9pY_`S|VwiuJLi*J)DnB=>X>5O|8#=d6Co z$%@|n%kV$3T>0=3|IL^44V zkubHryTz#pR(-GEds~HoLf_lh6=tNg{JkWGbcTdDLn4%{a8@O?cAwb^0)ztc^)V|1 z0TN)GE%rS+U?$jbKZQSzbt+rX`zY+-2kTd+av&sa@B_~8lub0l-@O4A86x1{?EML1 zR(@O01+z;GfWRs`#ii(jP>*rDP~)R4#{hvIV#gdllUK~n8Le3JoXN6a41^{PhcM?{3;58<^E9$N|PDzTMT6RG*k zm~w;wkpf0rMOo1Xq#zU;)C=T=s3Ysm)~wuS;F^NE_2`3pk&@u%^w+aP#!c@|XyKMy zDdamNQo-E%b_?7<95V9F*~LHva$jJ}TkzUC!tjEXe$ z4yU7~%P`_MKs*#Y!zDIdEusjnc;>3O6@KI~Ix8rMcx}N*LdOJsw0D@B;mH#!@C>F7 zxJ-dv(bjb@H0EQi9J%R#2=iAzoYKDTH4);Lim!^4mr_i2p0WEKj-vl_s|DQ0PczWj z8en98?Uk9RRyxzgq`lg)4nZ=$L{S6hLooel4>8S(S^k#MrvV0Mce}wS39lDkRFn_W zk7#RtE^d=$`Q-5+TDjIkaP?>qlnm|H|8bH`G7du0*D(fyV2!D0j{U8#%MoY*r^Q$y z_7|QNRr}Zl3?y$94n~ybIij@!*0%2h8Or$WCQ&iuLHO?JvQ)PbH__k&N)l9eoOY6q z``H?-8U+NybogpGymJcJ7G0pOh9AD@_$!si6{=gin^32lAhK%9ey~#DjJfkPM=yQ! zGp@Sk>@Yp_vpRrCDc_UU?TR0!3Q1c@S^YWj1rwJQ6ozWS@07x&Qdz=6G%|gMR(Kf% zz2#E8NuhZ6wnoF`DmKS(P+`ktqy@UR^r#9MZmat;!;h&(TQe#@j6U$iUeZY-BvfH< zK#QujXopI(t^j4Dop)gW2VEX*r52@svScO8>zeVN+O$30CyTnS@YS zjGm23c^oW&i}&<`pO-KnmrxCYSp}JNr)o-#m-^O8TJ}{11jSdG`!ivUds2prBH@oi zoN&Z`?xv#GMPzjfp@|de>AWWsRT%X)-iFeISJ0gZ0`K3~A3~)F1Bt7Hy*)yXtEeFw z^wNz2vc4t|CfG+i8h#7BT$YYo+&LC1ghX&Vg<2?8uffuV%r!L=S+n8ZjEIqNxg+m< z?ngl&oyl+Zf%Cjve`ycz)@vt0aMz~TOdl1{< za#8r-5q~NqZ8+gcJi$1Uy+LFHliQGr)^tCjvgd;)zac4EZg^a@x3%d03bAa7oNPH% z53#V{QK@w}H_t4ZU~73AY{(dd*)hLrY(*bz%Z=n|t0CNm@o)Vk`cIN^P$~lxFe4Z1 zwEXP=MLi1Vc5|^tE4?{ir zL$)6i>=X}CQOCXs{quEKW1kl+;*;_Rm;3)w3GkxYhjX*pfNpbAVZR2DKyr#CMwp*( z)PuKCQ0%j<)iTTml$yKd(u5%_mztM`c-VwbQfnoOd!4HjV7UUNlGt)%2FaoY;+uTw zkdimOs9!L3+|36%lvIKR=%+2$eU3fHjjEi&zn<&LbU(nzzZ4x$J{4llZ0||W^>+zd zygv}oxa1EY|2-V7I z30l^?7W7+f$%9lW>MNr6rXekODw6x~R0|&ds}2yyk=jQ_64x!iM1nuIkSLRAR_{O- z_F{Q#Ts|@Sw@GVAM+Q`+WW=0RiIRcJqM@qAG@10|#E5lnAh&pbF-ItcOj)(HfaCru&<3x=-d{-lICG>#bE+tyQ(^ci+$R&;?949AaXkQ>O_yD%3%TVWN?vanBYrL>O(-jPVd!X^U`ds z%Ds$JS{H-oS&WxLCktFi-`<*G6*f*{=eZQj?2DPjMbsD=`-$L3 zfG?C)Q>ARSC8%~y58ddg5ft>Z79EdFe|vW`;65C$ud;>gz)9zBhRpZn!_ziuV=z_h zs@{%{qbvyGW@y&K%>-7zqO3@Ljw!RtfJWVCJ}b#Z&zj>%MykIf1YpW1hp3DGnE49Y zWc!@X!1h>3Z_R2XUBGxTqM-DmOXz0_lD)k)#_n|2WOw5r08hi)mUu!wLB;z_f5y{N zv$(wyz0#Uo53!hA?}S#cR~-iXLFd^PSL7}3g5^gM=|t^p;GF#2iztjJGWqGHc8r8bKk+ierq7qnbqR8_v;Kz8A z2|}6w#o^(wS1sW;Wx|NQ@K5g)<81JPWxx6f0!lLdd?TT-a6;`W8rPS92gAE6*Iy{d zKds_{Kp0mETbUSKJtQ+&Rn9R+R>cijpcjvmf)clD-;3TX{&(;$r@DV%%~Lz3xmL_i zaj7v0En>#1(cm&lo@iErh(W#^>jg|Nvkl%#Tw<3Y%b2@iaQ~`!XGL7dKB+#zTdI4X zBx$|2MVsvT=@nwlFIv0{V`00TPE>7C)m$dQTxsCjtTV2MEL&t0lHv&=?V`*Z2C#ES ze?n}278UDX3O=t>DY-XxjW1hZaYx zw7;`d!Q@$~!lRlPMd0F}*7E}pO92|qkD$C-p0uX&p^SM;nONiA%!shrSK+aJm<2U+ zwEg3k5L=0+$EO_;s$=@%13p)|Prv1NI)YRgZ3lj~>uE=YfeUTAm)u?NCyn*Z+IAkd ztwxtspLY`~XS}nCcyt-aNf_1<^q_C#f+*NKQ?Igl!mq)W?=ECX+$ipSso3FjHUoHh zUGJ_AgcjC1I!GsIY5gvlWY$gI?#2H5T%tY7Y1}XT>$ctHJBio5yiu$IPhrUHk3ek% zSqHBCfs1&lZE|I^HIBg$yi^_79t_*;-UAX;3h~{Z3;~rgM%2ZH!gWL5(~Le*{@Y~Z zU5i!D($u)jo%9R)KlkLjtlUGRfLqfDH+U*^#N@pb0nUB_^l!~|0*6xU)p|}nuX%gg zt+~Rs+G?bi|2pZgmGEd8GCKQ*FhuQ{CVta_Z<1gSx&9b=m!j|c!m+mV4>CaHMQD4m zx7QxR#>IVOVE^t02#G-8jrTnhb-OY4P6!s!^6#wY`mC z&-ouce3ozjE+#i%Nv>QA0xLq+BbdXWDBkNHRb6&{#(Ved2>WcfoU*;WMeg_RLWY#* z%Dd=CHrM)8pZPpn&);hsR#VGFiCfkUv#+B<@yTgn63hJO*m?ApfmJspb8|QxP5)W zp2Vm}-fCt4K`(=T-44ld?VxA1Tt6Vw!j=rjMj)R<%wM(&;^Gjg7fvEHB?z~Dx3}<6 zVST*-{O#1ic_%by4t>5N{4q96H^akRVU+?GeFtv_@-706BRyGtGUw@wn2J5NjGVa0 zGneQUoF;ENS8&2@uSo{? z(cLs{1le?(A0CKzgxv*iXyft~(V&-EjIA?4nb@MCv|k*wCtX|=b+ke3`XSnc1xMa6 zRg=OS#2wW|a;<>%I2%X%Fe=H+H(G94Nx?CqM9pv-`rptDb3i0Q@Q`{UJ34TKknR3hEY*QK~nG=CR5riC5G834EgoR>;Voe&K_0;1f- zDSntS+de`yMzGFk0ss<{XkPL<6UyLZN!Ijd2SUI?PkLLmtSL5=Pd>-Qf+MV{XSWxW z@T!ixTz<}8i`P!YYDbf~o*U1HqhMNdglm#2I)4*HWKoEDSNv>_VU|Y{&ec;y z9QZEupB0<7Xk8IgwZEy-n=DYJ{$mnA25!6AbY9iP#MMCUj%E}kRhN3iA#1QzoODvU zG`oT|nk>SbBPL432ai+MP<0<= zK4GALZr13Ci=Psd&r<$knz9OKKcjS}m#|Rot^`uzV~is|*7&MFP8y|WTAkdWF|(GY z)ew6L+`vgqG32{7ERY5;C3x>TRoVJ8{dU8kT(VAK;rt(!JygfEdAouSxmx$)B!L`X zloe>534h^Sa6PUWwQJE|Xr#7trFRri&nj2vZB7iwuWoSO)uc6%)g{Y2t&(ctN{z5p zVEX*QSk)#)__VOC%^2|Fgv@shto+)T}yr( zM*}?IIyJ=j1|hbpkVZ;NPsHoo!v)u~W7rd+9A(&6KW8@!P}P|L$(G>4g=YqgDC-ey3qp>)^Iw!iA>zA`-T*Hr?Y5r zDhetb;SNG5x@b-u4*}FQwCQs6d-Z^+Ve;~&1*t?pbxKfjjZb^XF%Q;UPqdb-x#V70 z0c>sn^^l)%@UVGJ-{9&fIp^l@_*ty7`3XASU`-99I4{P8k7!p%llI|82~Fr+Hvm9B<}|_AYzZ&yZ5L+7gflRU#uK!vM=M# z>~q|;YqObWA>(h)p3R}3eH>f6_TS%Oz2v&^^NpD!PD?UT z8xuP@MU@o#_Qh5O#ft zRXusEzi9?WluQd_Sm_aphvSF2ntD+0bVwA#UF)8T>dvz6z*2~&+;zdA7L38KImFnS z_`<|b{I^sxVf+ye#8jKJqTKFQo+P@zRHlb!dd9P9BJb@%l;kDYH-o-e9J$3hv3ISg%8@V{8rEHQe(ds^gAS+JnM2XIydx7X7*12=c`70b zs7#R=EGD<7ioUnakmOLUZu-441LAAdmj`ET3t;on-giJ3D@bEkSG83T!NtT|AC>z! z3x>r5rWsWQg+un{Z15q|b*u4gGK!=1;^%QGL0y9ruJxRks%mp8+pyYW&5~RZEL!HJ zS9PiCwy&KZw;0)(o6!&=y0NiRhMoB}&yFNZaaCe5owzB7nq5lQL%ffrhoVbmHk{_7 z42zGIgJu}nR1WjZQ+}I?+yGBJb~8FRyy&CPhnyO9bbP+G34>i(h_S}K_FfY{-4cYe zK6&*)N56L=+mBPOMMqb|zLBn#tf$BO$J}2WoWtJckh_l1d3GT?1sUDaAVBy!I)51n zWBWe6g^vlvzCI16MFQ>4fRB}=z%}HIt~%kmn@F>5&k%?Kzxq}`hS!>K&ge(uy?J)h z*lk@co3{5iKbi}HZePHUPicgpKu_D9kW7)pq|EGZsz`Wki2u75JO_Sb3gUkjVlN1A zt;VVjs{xxr%eUJl)0r_@O&ZG`?P2E-Vn! zm^?m+{Gui>EE2};71L?|;WffA(#GA?|H)|Y-u$bg>V=YJ#VM5fw3ii%uGluFF&rQp4!^pi?{YjQ0OPz0~^ z$I%XKbkb-wnzl6$!rVm*3MrCgty<)Nk!J%cAw6M{zZ3^-^__Gv}f0gNT%tD8c$sD=8?*i?lEwS?* zwv`kB$9F;7A-uwsF@@E->!_A6f-uCq<7}LD`6sY&?{xP=nK$4NT`@b-7A-(ELxA^y zn#E_A0`)vnL}6&rP6q&>l|241Jdf z{MW0`WR#-X5t1+?`RStz}_`+oAqB!1E=bkWlKwcaLF#@z^X62nCEBrem`Eg4~X^% zTg9TdghE&;PM;wEVg%1vtZ4OCH;FTx!8Hgmx3gxM9)%)XI9{>tRx|cun^9*iNR!<_ zYm=2yc#?MSb|R1(a{T@T-ZwY3@*t%MA@k{65S4g)#~B&x@jn6WMyY&muoXz#1Zzla z1ftiu7clhbY9etn0)}rVMyZ>dkbxHRt zMh80C1;!h<7daS?&*+rEM+s}TCpoy+T+S7DucT_mJnwt-8PJw7F2v`Gl#vxvL2x;( z1)7P*VWW*lv>m!!&!0*SDhN~dk4#dW2_gH$Sm$!{TFR{V4k_O%`B@1hj8uBZJhyu$ z=d*8r`S;Lq4RVI{4-^7(I0$e8aF%I`Mt|e&cB+OK4$|@BJ&jjx6wUs{L29VD7h`a4 ze#VHfpZs?$4u!C0s{b*f{{TwlviQQe6kM2|t4yDxXsbQxf2DDIN zt*{Tf-9q9+-#uEc;qf*+t#hG~*$u?U$Y%j6umj#th+Q(SoAA!YHjPHB3GVJbzPQ5ZuiBuM8iilgm0xdlNsuNF?*q z`skNfXC;ucY~+e6w&`j^w#C?p!T}z@mUb#hDrV+LI{r10 za#$PDovDM%25#Kn#rz_6x?pY5`r|G%U3=Ow-X&3Zy}@{ivmT$*bitd7A}#boAxBbm zL|K%Nwg>4s0Ehml8CHvN@7MZ^aeBQz`sD#+HkbPnmiW1)^PQf4y7!m4Bdq)zvlK#x zm7sn{_UlS&92_GEhr>ai^#*IaH83meMY|PUtDV>-&XL}Jri!CtqokB#$BE4jowuCJ zii$gf{+CHxWi=c#70)z*E)urvR#QF zafzvT?(^qO5P}Y)j0kTF_M9hV=$T%146_a8!W?fCL^C*xDQ*LP))LiuoQdd?rG&3d z&Hau8XA1Alm0A#oQ8yf**z4xD6lSR&|m28}wC&fGk6- z{k*)NM#h4boA1P8B%0m(TSIkbb7ic!4JxG%_XwpufUWlx{_r1_dxib?vR|fClqAPz z#l=)5rBAH&N-`{uX%77aHM@dClV>&@fUcF$8DhQp((kwafrWuSgixTy+^6U-i!H65z>XAz9>QE`nq%e$q*93CUBPu(yGWo>yM!XWF=_t?hj!Rv$*S zcQ>}Svd2*k)z^LXdySKBsOL$^V)L9p9rm02Mh{AD*f>wE|5anDP*hv8r?LtEN1P^a zs#lXtkxOP_(vg~XFQ#G8N#4Q?O9ir*{HBAxkoiHw9@_94rX4|!*MZ}W5v5;ydw=BU zmF-}{jf0zP4%PQ+?%mttAf!D|wTpW!fiyJ~`U2d{s15CqroWQbTB^x|B@Jyuh&A5+ zSOXR6EwyXS+bisIBXofPYE4e9F@jm;*56lGP&lUAJj&$1Wz^E%k#us0Z{i2x1`9^x3c%Rz|v z^MVr<1%<$n3GI803Pf&s^HMeJ(c10wz{O4&fFi+j2{6ITB@otXrhYVW*za2%|`TnBQ$_!0LU!J*dMRAY-RVpwxIw zIp`qRhIP^j6YfgB10T?}9nzq!sW!*(AXd})gXJ)~0NahA_xxY|c>ZcxZbjAGu*l z5+t6i%<}i0DDSbJ@7Y!ce$ab{8kwELwjn!3JiS{4m}@Co}y-pVyQ@1t=y-WP&c@ZwY)u(Xk+nxS&OP*&Qhr(DVS!mh9<&r}x@DU@SLo6}aT z+Y2&#DT>qkGYCHum`nPf3t7V0!;;ybFspIA5vDog-Kpv7wa(-A_vb(wu3xzFn=Q0X zMv$F&uWXa-gwfTTr7#;S=&9E;X2+jp;E+r00g)s?NL)*fxgM`9)|tp(ytaKNfj zcrCCVhZNf%j~z^FtVsIr^BCEX#XB8OJh@Frdhd(XAfm-aGr5iKMm}hv@|xk4&Cq*W z_`!`Yos#dHz6P!ayGO_CCLp)1(S7STd7Lb6Wa919;IB5gH+q$~=h{~RJo-ZTo!n#J zt)~tZ8snhzQsiW_)Tz!M)B5I6q)wQdm?SQk#REb8V1mHAe>k-d*UlPtW(|s)J$Oz4nbT09y_hf%t*H#!m=BicZ^{6Dk>tPKPnMG0* zI3Cs3e?EF)s~Xrb=f#u3de}a2cCI(0_}pu?W|Y@yc8#p3E2?PyNR#IG4xuxmnX#T% ztbE8VdZo#9njEC*>KZFv5IHuQR;YaVCHR1W_xxbHS`F0ERk|8DC(W5}#2OT>W!XVp zBOZ)6z|2rbzn{ltS8r9nTCsg5SE(gYoxAGoq}k}SGR%BFKiWaKnETq;M(g5zn4vOS zB=d4WPPZemfm4&;l&$3kTcFiBX|cwzRpIvI0JC$BK9;R7==dzJK+DSb*95z_*{0&A zeKFPx%#rJDE>ME6Px4Am*CT41CydI{bNpKcnT-TzV^yessLK$6!A@Gs`Bn)GDDQ(x z`S;IZgy~wIlSg66hh)m)t-jA%oxy^j=Ly?<^INUsTDALd@)SO5+s(;x862C}oNPhy zrbf3Vn?^5D3{JP%BWdna{0!sIr- zLS5D-HvZnj2HY99T`5P6lHBe3txb!3k#+IXC5#``sngAie*RvI8cS+Q>MOIvV*N#~ zYky>NR++giJd`%{v6r0-_`VG<`MwQ5x5?cai%RV~TW6w9z66015<%3rI~5B+Qh~eV z&%q>1OF80|M+8QjsCHa&No?;y{{W#9F4UhQAOH56+zk18tJB{Uf(0AYvC35&r-}Rn zxihOP+kd`v@=R*O%g?_tG7iUGQU~}8id62bJI53{N9wb=6aWQqe@QM{DTJwx1ACuf zdX*MAIjyisiU}#qV5uaj21Tnt=bC#2I5BX#{Y2oAJ3*u5JrPs3=ln<3uin(7cryKc z0pHlXhxV5>M2wl4IdLH;_K3cGd>p;HK2nVc40Ix?oM~M8``YBokge0_)vdsUQo;5< zzB&@>cu}RFBE3_;H;PXQF-RiSBp(j41P-Q-4qDv5?c|=!jKR~Al?KkueJEVn70aSR zGI7luZ~=j_mKS*S zII*x6h)Q#m`X062Ez5&xamx66a$+8RBOq{8P`SI3eSgChVk)Vf_k%ve$+p^YP{;KV z8A?btCtQQQLOUj)Snd6A7r#+SYg6t672hqBe);=cJ>e*tLtPO)8f8N%PP(b=n02Q97S8s(2Ck>bkwda4LOf2G@wfK}JDm$*9PodT&kw=1TwBzw}b z<4a@%fWohu{@IxD_%|0I(^1WE5fT5S*eCJ#?nA{L;3;{8Gwz zH*@O2$u1~O?yy1@@Z9ZC0KaaC4bRg~f00w_M?11VdK-mbpcp1}PUANZ7->_M{YQm4!_*_Z1Y{)&f!|s_n4VD6_P*I9>nQ-jul%$ z81dRGHU8`jkyzo$O3;LBCQmSs_S@+uU#z=Kb!m7ldBYPs$gE=-|AVw61$k0I z?iSEC<_gYM15$cPAlN6vyGdNC^)7g?TPW4zdiSbioa>W-kd$hogKf16-8T3${+J+y z5YAcy)X5}PRvRnt-q~4+=UVV0w&1y>fJl1l1>v{*$fcQ*lL0w1@44a-cP5yTQsMd4jjz0 zE~{XGr467+fpybrB_yXqYyK3mOoB7LkCNJz_5wyfD39@*+70H3VW@?J3rBf*8g7&BB5!AxcjhCr$*g76xC z1uUiG5-}ov>;L|;x{w!Zs=*LTwXZRjCCk@h(V1RAD$ zLw67fvUY{PSTGY6Z97Nq&6a#emj+_DyKXc2`Ix-#L5j6X_;7mJ;Mk(Ja`cxz)`lXp zgkcU@{>8SG&ZT9LM8be2&f}6%@qe;;d;0Iw)l+^Lv)1VddpTEl0rNFCtr`letl)`8 zHWxUd&5H+t>$WM>oHf=V3<&!qg*g-9ySXyExw-_r)|0_lESdGXJz8B8^rkREqs@8< zXhsvUt{U-i%VSA3xGi3KNI*=`_Kt34bmG8CucCw%Y26Iv${M@hs1^M^9BXTO=&q7OA4`M(P75{PG#0bK_F5sc8Qq z(yvw91#!~#3K4hmiyMp1i&ml`LYk>4>#NdR71`bA#-UJWRNQt@k=Cb+8fC~;7{HJT zKmSv(JAr6`aAm0ud`Lg}VO(T(QXl-NN2t!2Lq#aK3@#*PzflmRCzzd~DP(K6g|p3> zzZk8mTT?1zdy=7`rG7~kDACxCqAcF*TB~~PNxl>$qgx;BHj{FXoVNQ-m3L7qfu_{(#>qBX-SgsZE z)dgBS)feBXZ(vl#kPi2WSIzb~SbMeE)6Ay%MB6h#8x^Ig;otivR|KVQ%yZN)7X?c; zMvn;gbCoGedX&aNRyHqAJ*`TS*L}8-?LrenfM-y72WF>5R{r*R$gWV(O(6 zUZQ!A6aHTAZ#lzWE4%GCT{W;Kf&fIX#X#F7b$6dz1j%haL~F2b;T;%=e7NpTe=F+J zwumkztC7Z~tUN4k(MBkVVQ5cai>_yKtF!;C>nnQV?(vsgN*f0s&*)mIpCefxXpL?X zIg%+cY^lWyP$Yz%BLsaBD*poAG}n6){14dI0+TP<@T@wFdQWw_11>?MNYSypudv#+ zzG?y~v6D4!O8L9#u1|E$T$h_cdY1V2?4m%B{&RVh4PCSIXPMJpZwqA``0nV zBn5yA|6>iNG9Fm6RJzt)nt-ZAu~Ln#Bq8p*)6FS7R-D+|?*z^&XlE+HSv%>QFtq5@ zRG<<;W3%Cs1uw9{{vlF+U$o6r4y2&f(1QD_G7a8_59||OIe-Q*8p(?>3|Tmo^#{f0 z$`!fu)hre=3!Phe zx7SEWqc%fHyj^OITBi$Bw5d>Oz><+q9WZ=)KW9Z-JKxRmtQ~wlMOFVfGxx+AG7uD! z&Zu(GG6j6Dv{u~1$N-DNQjNz3y<`#&DZN6lL=Z(ujA(@{l`k&kEb8KpKT;4SPBZ8! z74OV2WLn0ef!~d{QxAW|S|a#XXbkd8^Z8t})68AffYu9)dY597Rvy)L_n1snHP1sr zU&~zjm<^%moPy!a2GJ@gA_(`y;Y!BYm>K~ds+Bw_$VR&FfqlRRv4?J?a zJ#U)Y>jvt|A1Ph$Y9U!m+Ho9o4EYg#)Z(~-(f)o$Th1mg&f#aRuGA zJ}ZGymGEI9VaWWxf_-Pw-W>qv!zyb8m?2s+&ghaVY#s8L4%7P7KP?P+5743nN*aZ{ zT5pz-pM{y?t`9CPnnZ4~KY#2%EN6QMUiVc|0k5K|ickoRzrf zQ=lPj^wqJ6)~gW3Q6R7`U0$%A`$zL!1#OxH=ey(q5pxcc=;HMFEp5 z7@CW$=*uq)KM2fiYsZW>8?)*>0B z!Go|bUj=v&n>hRDE~8D=zEtAUqNBYDooq~))PC7xWfyI-zkBl z>~#IR`zKCS8b{<5m^s$_sBkf6n(F7Kyynux@Tw2MVWqVwa}_|bv4P9TKjSg{+o>Z7 zF|$hpmfxJq0#q&fD(rA!V){_GCkcd<8VuVYX&srvlub?cJPJVV)#FNsn4XK}>}|)- z{yvSG$DyoVFz8QgzDEa=kDV3vDBN6LegCQvt&`yYg}uWb%C-biaawplXo4AlAQ z1JMc@x$V;3rN-utsndNE6y}i7Q?hj+GM>SNgf1qidf-C4%1ii-5rmtiL@y6$(ON@t zT|Z!^k6IYP59hG=%>4Xa8$f==%P>M(C>k9jras7N&)+f#&Nen^)IY#sso=+Cdw?Wu z073a4vwINmU!nyGitsZl)#t%83wOeRxqc92_(n-g6{~JnPe_;^!kg17hDTisrVZSj z`bTtm2V}?e<&7=J1+(GWUzbt#8fA+{abtwXI-j4rXsJUIzIPIAb*?}(1pOVBl^#%a zCuOBZsLWXMKjNc{2%j|hhFHKZSx?t{_W|x@bN>`KyNMP>lSK)^Q{*6Lr&QQw0r)BOfZm7q= zf^a?Mb4Pmxn5QHfoMF0WOGG=O{Nt*pJ13@WpZ9xPmtt;Jd+l=M}&IYn=#XTfC2WVM<=|eEPqs}k>5Zj@Qy3t4~^-EHIvj^9eUG_MT858 z5C8W^A z*)x~Wm6lm%0=LOxxcz!Ts~Tu?uro~So7Q0~YwrY--i&qZ)T_?D#<~elakOdn`~-_7 zbZak{vKncv<_q?NM_VB-dmV!f{$KLr4i-xO4=Upal+x zpKlaXM1=D2ZkFYWfBb`FsMZmSIvs4+n{Nz~MTCTe^41HUCx(KQcqW5`gTpl7{=pu% z|AsxrwB2j|ffxn(?Y8Ca^Jr(4?Rgar)c?8<=+B}5JMFk`;vD0@+Q%8!?*yeh2zB=| zr_o4_<%7jE_z0+nI_Addu>4{h*k2IQc)X)Nmvs~=kJFRbgLH>G4LYR9-x_);2`P<` zTy7<#gvq~lj0^R9#V&_IjKDRk_1bfNPawirxEZ z4%zC&1uwor9xXfg0>@Q)|_3p zM!fnwNa8`r0W(=QC#O+yNwV*3M~q)!?L2XP&7 zpq<_EtaRaCTv3|IP+eYK+B{qmd$W)d$!LxiTce}*PpWf2hJTwsj=)8idZ0@W!AVRZ z_?tX+-Rx&6_XHv=o-tcK;Kp&3y$}glU=PMU1L?xhY(Kx-ew!Ej|4YW9e(RhJF;{D@ zpxs`}57P&r?MGb?zPUmWUh~!`^W8%31W`zR=Hhls^KYl$V|4STnjFWEKxS|b@Ldyc zS1;#%TlCtVR96VAp1f7P%;EZTVI+K9q^x{|KHBPYc>PyYsdfBfPS0$N>f2rR^i;R* zwk4!9P$H1xin?p_RQ4CFt`fMhRCHVsa04^}MJ9LZ_-4r^)T44aN#ZWT*#p#$2Bw$g z@M^CMKN9uT0hU+kkt0X3&iX8wabqhhy|)r4*Z-KiX_J$DP~9iuPKKt zU*VsgN@a)|k3s=>El}B{-RIe5RbF*gNGe^eF~0|!u~1delcFVsz009do37ow-q}+{ z5{rYE2K^ehIqG{hbBpWB-r|m~?bW*h)F_ja=8C%F>+<5&wR41YZak-H*Kz_e(vNY1 zd5aB;2+IKuGs41;RlBqmp5mK_qYOss|;^O$oI`_^PKuF)VO$gI``S8oKykkmNtqoz$ zw6Q>ODO`Wu+F$*}sv5|$JZ_tyoW_G&zR%T>ch5Q7`pL&q5!*!X-456DIA=cvK%Us5 z^H>&=KJ4e{bi2q(ZwPy^LYYBf;Y(-Y2 z{9aay3l25^B2lLo#9d*Cur1xSEKQe{F;*o+4HFnSm|IqXV_f(XNNfx?)2pY4?Zz$g zu%z}UfOAKRAw959ldX=#=i!mLg(Jhd%cJvu6MyX1CVB=4CiR4U`V!1%=pWqjGW&@M zKzL|ksc|K$2J)6*2$uA6n#ER6!$BV0E8c5;02Y>mU%= zblH1sMP+4ug=Fe3msFE=>1-zS6+PwsI6f)@ z)PB&K9;T?pF|(*+oIry#WOv@ve!A`B>i87V;%kr5Urxj-=k^F>CL|<8SIk7r$^ALz z1P{=XXQA8VZlj-9<+8T=w0R_j!om4?KiM<=wf_A3PiCVX%~N!RPz(@^tx&~EF0fVD zTkWkjG;O=AMA3oltmj`UL|BgqJvEnt{Zayq=AP}_rnCV12dtiA0cSTT$i@tu+GeEiiG;bND5jZ0q0xOos3^N78?_YWftbx&8@+3FFG?* zIjiXf&07)J8&dy21o@932u}Y0mmo;>4-0L>(b_vn*^THY%Tl2;DcX;VT;;6V=8Ka2 z>&aH*Ony%sfS(^|(ttPK`;}j9>3-dxMz&5VIf=mq9b;N|=;B@BEzP*}->EkvZj#h! z$g4&Ka=M+-W0Xg^+VSDNs4R@8k7+3E9$~R(ZLO9C;*|z8_Rq^qcszfhs?~ouC<7k3 zyq#WA=ylhFpR7uS)+}iCJ-69)OK|&*u8N#>Mkab+o2W^rEZ!Q4$xq;{(F`s*q0gGG z`UwGZ_RhV(o7R-DJ@oY-!1<+zK{jx9_AW|?$7Kc<6CQGp&?~;IK^Bj=66=97ISt4CKxXk2z<2z3YCS~^=Hmp1g97;*3Wysk zMoR6;@c-JWBje}{opO}r8TMf^IbH%A!vgKAoNp$9Kr>g-p2fl700$P6Y-(h{DhZF4 z{&~})JzBs|T_=SwMYID84kRn$WiVysBVRZMvFh*+OYI&-yGWqYKl2df}8FRcn z1YqV_1`|$vR!zk7?(8!#YYE1@2ud&^fAy#@A}Nsl3S%-euGv-?&yKykFs3LpO<{@Zw_@S7^Dz4I8Q0yT+a8$E@K$z_J}F*OnEKU_Gc0o?YQs zY(!?~y_3oatD%{ZL1D~9A|b=Bx=3flK!FI{8#!%IRTkV3&^#P_Ph2^SNfhAF)S6j6a!U}}8(X)l^8gO!O zJ@J5z!P;5?SEJdIb&%$Ya{#*cA{B_STV0UZ6L`pPyIqt5Gz>m z_O%=C{?FzzsjGmCyC)XPfu7MCsgQMcZn04kPbIU3WmXU`Eo}*SVdYji*aRHb>5@ z;~G)6PY#9n3@|MzWGyTn69j()b$lf^9>&oC@g^=Chx!Xtt~B=(E}X1Tf7JgMLhuXI z3SdV?#wA8$eir(dFNmp(d_NnAfAE>Q6VbeFaH}myY8P_78KDHfv2jI4$kwE!KR4d@ zgk2G!?S)o>*053{@~LjXR#re^RjA%adhdqnJVrUr9a9Y|lJ9e<*%}sb5VG#1y zahT9YdJhSZWy40oXz7UL2qX~tIpIe4%04Tgz)w35of+td%MXn2p&sCDWea5xY_|ys zRYD!Wd$s0%%qT-_0SMrduQoqE;{%aVQGw9I&*88eJmb1kzl#ip;(E0|APg1O?Eh@4 zqDKK51xT}oPS9xlg~@Vc0>F?1D!2Lt-a3LWb#X`wjfE{4p0UIP<%V`dTvyENdk^(+ zYZvB$VB>vB!F0d4aBjYj_{$yZ72X|nBmXBMsT5|S{VN2pEdD(n6IQkJRqP^LbQAn{ zFLZ~rz`UOuSniSjE>qSrYkxfa<`xHRKbI=A7@04ieKv5Ctt=^Xi8jhJdcOTMt}4H*2m*SMUpzYZetK{X2HZ0xNJFmoVdZ zbX~{$&U=KpSa~#=LY8utco94+G7ojuPx-7xC(AVj6>Vf=xqQ?00GHysQH@nR?3j%^JU3s0yz^|+Ll*y-)v}_Z4U>aAEm4kb*QUww@9`$PD^$m6#&`liDl7kdX8MC` zNsN3!;TkA3x#jo`h{NnB_YHDn-F3m$bj zJR2x>2Z+$@il4lltn8r97?Hb~oIz0NPo10(CUpps^gp%{Y8IjAB%f$IqXOD5wrgwM zxAZOTnxuyU+k}o9+i{? zQVM+WD*)mDslZ4L9aE3n?5Ve&9~^KUs(R2iO=PKqv-*?jS$WN(h5mkRjd@HsRM*dVmlFNK0Ry<)q7Q89@^a?^>PM!d6`yJ!vaT30^jmc#QJS-2M2h zg8np@x#K=j5$$5l^l+=GJrhKv8o?vqbKgrzlR1g`53Pcld2wjepOUyyd4B^ls(p@( zZ;M)Fh;PWi=Q{qV9~3^gN&%+B*_+cLO-G?J_bj5p7)eF4)u^`Slb*Y1)1=z%^Nj4~ z-{pb4rJRjY**PGpHZy!!LVmvAkx7oj2$U5&P%9;OmokF09p+tAFK8bf4MO5u#6hTUv+X1$pKPU-Z zxFGE9gJM%VxaL%>PSR7yNmQpjpQ|u~3_xrVw@kC~r3I^NAFzlKY0!KeB;8A8b!(W- z7rrZ<3QQ=$Vau=0E18v^Q-*Sd4QmCC?RMiYvm#D`;W2Dm=69WRW@mqpWl~jjmwTwi2t6n+75a=R2Ns}vVr~cbF{R zSY9dFcDaAj+CNpH>E2js(pC&3M3QWICP(LPajRFXdE@{lGmQZIt}Q-_HjfnY^-#W_ z4;Bi3gF;-R1HKuq%ghq$8Igj-Kir#B0-cHpP3d@}sucUpbo5#;dbcLn@GEcMw69*< z@S~MbWhyjdZXe_C;nE~ktaN04{Q##wba^MA0hjd$$^5WBOPB;a2>7B+b@s^I2|Hc$ z#Q$d}*<`JHI(x0LOS{zr7C{F@^;as|#6siLj*CdCG7<9LGD0g{R&@*`D%Kls3PWP_ zU$cT7zr-KuV&zYH0bt4Z;dbMLJ?ptPWhcJc9C*B5n@JpYv15B7T)a#6?1UReuvK^t z==0`MQoeCflgm=)r>&VWURZ~&&AgB9ODBoUKr30ORNHbV7uStly=B%(W@?zh zwJCg|_5TpeiJiYA7!xG%kQPQd|CV6JYyXL0a0dTPMr_-gx(kE;;3^d@LRCPjb>y9~ z=7WfamsxqgJ|_z_GWs1+-Hxw5Ty{f<0zd;&>=9 z<$Dnya#Ot6*-c|~J#&=;x>4$)!AUtT8sgqaCcRoUU(OAUaLjJLJF{|K@8DlyI}f)Y zZ5uF8EzBfJ?g6oX3a{qY$H22T28vBm$oAEo@5NU4x40h+s4Buu5u-UWnharmG57L3? XiOI=(SH+P(0Q}{QaCfBJ2a*2+{3E69 diff --git a/localization/autoware_ekf_localizer/src/diagnostics.cpp b/localization/autoware_ekf_localizer/src/diagnostics.cpp index a1af492487fe4..45468abf72d6c 100644 --- a/localization/autoware_ekf_localizer/src/diagnostics.cpp +++ b/localization/autoware_ekf_localizer/src/diagnostics.cpp @@ -41,6 +41,25 @@ diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_act return stat; } +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "is_set_initialpose"; + key_value.value = is_set_initialpose ? "True" : "False"; + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_set_initialpose) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]initial pose is not set"; + } + + return stat; +} + diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index d34be2a537ef1..11d7788adfade 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -56,6 +56,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) twist_queue_(params_.twist_smoothing_steps) { is_activated_ = false; + is_set_initialpose_ = false; /* initialize ros system */ timer_control_ = rclcpp::create_timer( @@ -143,6 +144,13 @@ void EKFLocalizer::timer_callback() return; } + if (!is_set_initialpose_) { + warning_->warn_throttle( + "Initial pose is not set. Provide initial pose to pose_initializer", 2000); + publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time); + return; + } + DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ @@ -264,6 +272,8 @@ void EKFLocalizer::callback_initial_pose( params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } ekf_module_->initialize(*msg, transform); + + is_set_initialpose_ = true; } /* @@ -272,7 +282,7 @@ void EKFLocalizer::callback_initial_pose( void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - if (!is_activated_) { + if (!is_activated_ && !is_set_initialpose_) { return; } @@ -359,8 +369,9 @@ void EKFLocalizer::publish_diagnostics( std::vector diag_status_array; diag_status_array.push_back(check_process_activated(is_activated_)); + diag_status_array.push_back(check_set_initialpose(is_set_initialpose_)); - if (is_activated_) { + if (is_activated_ && is_set_initialpose_) { diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); @@ -439,6 +450,7 @@ void EKFLocalizer::service_trigger_node( is_activated_ = true; } else { is_activated_ = false; + is_set_initialpose_ = false; } res->success = true; } diff --git a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp index 165102adec1d7..5ce39df484e98 100644 --- a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp +++ b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp @@ -35,6 +35,19 @@ TEST(TestEkfDiagnostics, check_process_activated) EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } +TEST(TestEkfDiagnostics, check_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_set_initialpose = true; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_set_initialpose = false; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; From 1e686028828e17913a1885480c8d27101585adae Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 8 Jan 2025 14:18:01 +0900 Subject: [PATCH 39/45] test(blind_spot): add unit tests for util functions (#9597) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 16 +- .../manager.hpp | 3 +- .../parameter.hpp | 40 + .../scene.hpp | 15 +- .../util.hpp | 32 +- .../package.xml | 1 + .../src/manager.cpp | 18 +- .../src/parameter.cpp | 45 + .../src/scene.cpp | 24 +- .../src/util.cpp | 49 +- .../test/test_util.cpp | 448 +++ .../test_data/object_on_adj_lane.yaml | 2698 +++++++++++++++++ .../test_data/object_on_shoulder.yaml | 2512 +++++++++++++++ 13 files changed, 5824 insertions(+), 77 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index b28e486c9be05..220c380eb0fd7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_blind_spot_module) +option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) @@ -11,16 +13,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/decisions.cpp src/util.cpp + src/parameter.cpp ) if(BUILD_TESTING) + if(EXPORT_TEST_PLOT_FIGURE) + add_definitions(-DEXPORT_TEST_PLOT_FIGURE "-Wno-attributes") # // cspell: ignore DEXPORT + endif() find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - file(GLOB_RECURSE TEST_SOURCES test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - ${TEST_SOURCES} + # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globally, not in the FixtureClass instantiated for each test suite + ament_add_gtest(test_${PROJECT_NAME}_util + test/test_util.cpp ) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + target_link_libraries(test_${PROJECT_NAME}_util ${PROJECT_NAME}) endif() -ament_auto_package(INSTALL_TO_SHARE config) +ament_auto_package(INSTALL_TO_SHARE config test_data) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index bf01986d5c28f..41330e459a524 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" #include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include @@ -38,7 +39,7 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "blind_spot"; } private: - BlindSpotModule::PlannerParam planner_param_; + PlannerParam planner_param_; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp new file mode 100644 index 0000000000000..59322caecb62f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerParam +{ + static PlannerParam init(rclcpp::Node & node, const std::string & ns); + bool use_pass_judge_line{}; + double stop_line_margin{}; + double backward_detection_length{}; + double ignore_width_from_center_line{}; + double adjacent_extend_width{}; + double opposite_adjacent_extend_width{}; + double max_future_movement_time{}; + double ttc_min{}; + double ttc_max{}; + double ttc_ego_minimal_velocity{}; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 50ce8634ec600..5e8ef9fc8a063 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#include #include #include #include @@ -70,20 +71,6 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC }; public: - struct PlannerParam - { - bool use_pass_judge_line; - double stop_line_margin; - double backward_detection_length; - double ignore_width_from_center_line; - double adjacent_extend_width; - double opposite_adjacent_extend_width; - double max_future_movement_time; - double ttc_min; - double ttc_max; - double ttc_ego_minimal_velocity; - }; - BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 9d908414b6d95..e18d96709ef92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ #include +#include #include @@ -25,6 +26,7 @@ #include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -50,31 +52,49 @@ std::optional generateInterpolatedPathInfo( const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr); /** - * @brief Create half lanelet - * @param lanelet input lanelet - * @return Half lanelet + * @brief generate a new lanelet object on the `turn_direction` side of `lanelet` which is offset + * from `ignore_width_from_centerline` from the centerline of `lanelet` + * @return new lanelet object */ lanelet::ConstLanelet generateHalfLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, const double ignore_width_from_centerline); +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring lanelet of the + * input `lanelet` by the width of `adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double adjacent_extend_width); + +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring opposite lanelet + * of the input `lanelet` by the width of `opposite_adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double opposite_adjacent_extend_width); +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); + lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width); + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width); /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index 8c34678f439ed..dabd3045b31d2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -25,6 +25,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils geometry_msgs pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index a1410d2fecfd5..d6cae9004600a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(BlindSpotModuleManager::getModuleName()); - planner_param_.use_pass_judge_line = - getOrDeclareParameter(node, ns + ".use_pass_judge_line"); - planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - planner_param_.backward_detection_length = - getOrDeclareParameter(node, ns + ".backward_detection_length"); - planner_param_.ignore_width_from_center_line = - getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - planner_param_.adjacent_extend_width = - getOrDeclareParameter(node, ns + ".adjacent_extend_width"); - planner_param_.opposite_adjacent_extend_width = - getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); - planner_param_.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); - planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); - planner_param_.ttc_ego_minimal_velocity = - getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + planner_param_ = PlannerParam::init(node, ns); } void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp new file mode 100644 index 0000000000000..0984de6f73262 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp @@ -0,0 +1,45 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns) +{ + using autoware::universe_utils::getOrDeclareParameter; + PlannerParam param; + param.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + param.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + param.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); + param.ignore_width_from_center_line = + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); + param.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + param.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + param.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + param.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + param.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + param.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + return param; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7d1a68c5006a0..8a9401646aaea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -101,8 +101,9 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * pat } if (!blind_spot_lanelets_) { + const auto lane_ids_upto_intersection = find_lane_ids_upto(input_path, lane_id_); const auto blind_spot_lanelets = generateBlindSpotLanelets( - planner_data_->route_handler_, turn_direction_, lane_id_, input_path, + planner_data_->route_handler_, turn_direction_, lane_ids_upto_intersection, planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width, planner_param_.opposite_adjacent_extend_width); if (!blind_spot_lanelets.empty()) { @@ -179,27 +180,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) return true; } -static std::optional getFirstPointIntersectsLineByFootprint( - const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - const auto line2d = line.basicLineString(); - for (auto i = start; i <= lane_end; ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, line2d)) { - return std::make_optional(i); - } - } - return std::nullopt; -} - static std::optional getDuplicatedPointIdx( const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 5c5aa0a26b3b4..8451661b2b71f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -88,6 +88,27 @@ std::optional generateInterpolatedPathInfo( return interpolated_path_info; } +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + const auto line2d = line.basicLineString(); + for (auto i = start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); + if (boost::geometry::intersects(path_footprint, line2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr) @@ -200,15 +221,9 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) return lanelet::LineString3d(lanelet::InvalId, pts); } -lanelet::ConstLanelets generateBlindSpotLanelets( - const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width) +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - std::vector lane_ids; /* get lane ids until intersection */ for (const auto & point : path.points) { @@ -216,19 +231,29 @@ lanelet::ConstLanelets generateBlindSpotLanelets( for (const auto id : point.lane_ids) { if (id == lane_id) { found_intersection_lane = true; - lane_ids.push_back(lane_id); break; } // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); + if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); } } if (found_intersection_lane) break; } + return lane_ids; +} + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelets blind_spot_lanelets; - for (const auto i : lane_ids) { + for (const auto i : lane_ids_upto_intersection) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane, turn_direction, ignore_width_from_centerline); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp new file mode 100644 index 0000000000000..5c2a239e4142f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -0,0 +1,448 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#ifdef EXPORT_TEST_PLOT_FIGURE +#include +#include +#include +#include + +#include +#include // needed for passing std::string to Args + +#endif + +using autoware::test_utils::parse; + +class TestWithAdjLaneData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_adj_lane.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{2200}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithAdjLaneData, getSiblingStraightLanelet) +{ + const auto sibling_straight_lanelet_opt = + autoware::behavior_velocity_planner::getSiblingStraightLanelet( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), + route_handler->getRoutingGraphPtr()); + ASSERT_NO_FATAL_FAILURE({ ASSERT_TRUE(sibling_straight_lanelet_opt.has_value()); }); + EXPECT_EQ(sibling_straight_lanelet_opt.value().id(), 2100); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), ax, + autoware::test_utils::LaneConfig{"intersection turning lanes", LineConfig{"blue"}}); + + // for illustration + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(3010933), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2201), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2202), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2010), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + autoware::test_utils::plot_lanelet2_object( + sibling_straight_lanelet_opt.value(), ax, + LaneConfig{"sibling_straight_lanelet", LineConfig{"red"}}); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateHalfLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + + const auto half_lanelet = autoware::behavior_velocity_planner::generateHalfLanelet( + lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.ignore_width_from_center_line); + + /* + TODO(soblin): how to check if they overlap only on the left line string + EXPECT_EQ( + boost::geometry::within( + half_lanelet.polygon2d().basicPolygon(), lanelet.polygon2d().basicPolygon()), + true); + */ + EXPECT_EQ( + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / 2.0 > + boost::geometry::area(half_lanelet.polygon2d().basicPolygon()), + true); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"original", LineConfig{"blue", 1.5}}); + autoware::test_utils::plot_lanelet2_object( + half_lanelet, ax, LaneConfig{"half lanelet", LineConfig{"red", 0.75}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + // const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, 650)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateExtendedAdjacentLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + const auto adj_lanelet = *(route_handler->getRoutingGraphPtr()->adjacentLeft(lanelet)); + + const auto extended_adj_lanelet = + autoware::behavior_velocity_planner::generateExtendedAdjacentLanelet( + adj_lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.adjacent_extend_width); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"given", LineConfig{"blue", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + adj_lanelet, ax, autoware::test_utils::LaneConfig{"adjacent", LineConfig{"green", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + extended_adj_lanelet, ax, LaneConfig{"generated", LineConfig{"red", 2.0}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {2000, 2010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + const auto [y0, y1] = ax.get_ylim(); + const double height = y1 - y0; + ax.set_xlim(Args(300, 365)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_right) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::RIGHT, {3008067}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 1); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {3008057, 3008054, 3008056, 3008061, 3008062, 3008059, 3008067, 3008066}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(905, 920)); + ax.set_ylim(Args(650, 950)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateInterpolatedPathInfo) +{ + const auto interpolated_path_info_opt = + autoware::behavior_velocity_planner::generateInterpolatedPathInfo( + lane_id_, path_with_lane_id, rclcpp::get_logger("test")); + EXPECT_EQ(interpolated_path_info_opt.has_value(), true); + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + EXPECT_EQ(interpolated_path_info.lane_id_interval.has_value(), true); + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + for (auto i = start; i <= end; ++i) { + interpolated_path.points.push_back(interpolated_path_info.path.points.at(i)); + } +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + using autoware::test_utils::PathWithLaneIdConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000, 3500}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + autoware::test_utils::plot_autoware_object( + path_with_lane_id, ax, PathWithLaneIdConfig{"original path", "red", 0.75}); + autoware::test_utils::plot_autoware_object( + interpolated_path, ax, PathWithLaneIdConfig{"interpolated path on lane 2010", "blue", 1.0}); + const auto [x0, x1] = ax.get_xlim(); + ax.set_xlim(Args(335, x1)); + ax.set_ylim(Args(640, 670)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +class TestWithShoulderData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_shoulder.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{1010}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithShoulderData, generateBlindSpotLaneletsShoulder_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {1000, 1010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {1000, 1010, 3010907, 3010879, 1200, 1100}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(330, 365)); + ax.set_ylim(Args(580, 625)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper left"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +int main(int argc, char ** argv) +{ +#ifdef EXPORT_TEST_PLOT_FIGURE + py::scoped_interpreter guard{}; +#endif + rclcpp::init(0, nullptr); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml new file mode 100644 index 0000000000000..97068d69cd296 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml @@ -0,0 +1,2698 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 563 + nanosec: 125840339 + frame_id: map + points: + - point: + pose: + position: + x: 330.010 + y: 643.206 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00655991 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 332.010 + y: 643.233 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656125 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 334.009 + y: 643.259 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658745 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 336.009 + y: 643.285 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00660718 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 338.009 + y: 643.312 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658719 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 340.009 + y: 643.338 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656661 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.009 + y: 643.364 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00657433 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.809 + y: 643.375 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0162350 + w: 0.999868 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - 2200 + - point: + pose: + position: + x: 344.008 + y: 643.414 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0355542 + w: 0.999368 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 346.003 + y: 643.556 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0600763 + w: 0.998194 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 347.986 + y: 643.796 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.134724 + w: 0.990883 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 349.912 + y: 644.329 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.199426 + w: 0.979913 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 351.776 + y: 645.121 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.281519 + w: 0.959556 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 353.448 + y: 646.194 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.345347 + w: 0.938475 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 354.997 + y: 647.513 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.407454 + w: 0.913226 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 356.322 + y: 648.989 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.480104 + w: 0.877212 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 357.418 + y: 650.701 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.544895 + w: 0.838504 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.228 + y: 652.525 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.610262 + w: 0.792199 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.741 + y: 654.467 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.646506 + w: 0.762909 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.070 + y: 656.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697457 + w: 0.716627 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.124 + y: 658.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.713869 + w: 0.700280 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.085 + y: 660.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714886 + w: 0.699241 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.052 + y: 661.935 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - 3500 + - point: + pose: + position: + x: 359.040 + y: 662.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.993 + y: 664.437 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.945 + y: 666.436 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715334 + w: 0.698782 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.900 + y: 668.374 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714518 + w: 0.699617 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.858 + y: 670.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712894 + w: 0.701272 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.825 + y: 672.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710606 + w: 0.703590 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.807 + y: 674.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + left_bound: + - x: 329.488 + y: 644.822 + z: 100.000 + - x: 332.672 + y: 644.864 + z: 100.000 + - x: 334.352 + y: 644.886 + z: 100.000 + - x: 336.351 + y: 644.913 + z: 100.000 + - x: 337.719 + y: 644.931 + z: 100.000 + - x: 340.352 + y: 644.965 + z: 100.000 + - x: 342.813 + y: 645.020 + z: 100.000 + - x: 344.277 + y: 645.102 + z: 100.000 + - x: 346.200 + y: 645.256 + z: 100.000 + - x: 347.374 + y: 645.428 + z: 100.000 + - x: 348.470 + y: 645.697 + z: 100.000 + - x: 349.568 + y: 646.035 + z: 100.000 + - x: 350.592 + y: 646.513 + z: 100.000 + - x: 351.572 + y: 647.043 + z: 100.000 + - x: 352.507 + y: 647.713 + z: 100.000 + - x: 353.327 + y: 648.476 + z: 100.000 + - x: 354.109 + y: 649.196 + z: 100.000 + - x: 354.661 + y: 649.830 + z: 100.000 + - x: 355.275 + y: 650.523 + z: 100.000 + - x: 356.177 + y: 652.020 + z: 100.000 + - x: 356.615 + y: 653.084 + z: 100.000 + - x: 356.929 + y: 654.139 + z: 100.000 + - x: 357.146 + y: 655.241 + z: 100.000 + - x: 357.284 + y: 656.160 + z: 100.000 + - x: 357.312 + y: 657.420 + z: 100.000 + - x: 357.425 + y: 658.703 + z: 100.000 + - x: 357.383 + y: 660.465 + z: 100.000 + - x: 357.349 + y: 661.894 + z: 100.000 + - x: 357.386 + y: 664.700 + z: 100.000 + - x: 357.338 + y: 666.699 + z: 100.000 + - x: 357.291 + y: 668.699 + z: 100.000 + - x: 357.243 + y: 670.698 + z: 100.000 + - x: 357.195 + y: 672.698 + z: 100.000 + - x: 357.148 + y: 674.697 + z: 100.000 + - x: 357.021 + y: 680.023 + z: 100.000 + right_bound: + - x: 329.531 + y: 641.578 + z: 100.000 + - x: 332.717 + y: 641.619 + z: 100.000 + - x: 334.395 + y: 641.641 + z: 100.000 + - x: 336.394 + y: 641.668 + z: 100.000 + - x: 337.761 + y: 641.686 + z: 100.000 + - x: 340.394 + y: 641.721 + z: 100.000 + - x: 342.805 + y: 641.730 + z: 100.000 + - x: 344.462 + y: 641.823 + z: 100.000 + - x: 346.283 + y: 641.878 + z: 100.000 + - x: 347.708 + y: 641.985 + z: 100.000 + - x: 349.158 + y: 642.284 + z: 100.000 + - x: 350.570 + y: 642.681 + z: 100.000 + - x: 351.954 + y: 643.219 + z: 100.000 + - x: 353.269 + y: 643.933 + z: 100.000 + - x: 354.499 + y: 644.727 + z: 100.000 + - x: 355.646 + y: 645.678 + z: 100.000 + - x: 356.516 + y: 646.483 + z: 100.000 + - x: 357.282 + y: 647.228 + z: 100.000 + - x: 357.690 + y: 647.819 + z: 100.000 + - x: 358.527 + y: 649.081 + z: 100.000 + - x: 359.237 + y: 650.418 + z: 100.000 + - x: 359.789 + y: 651.774 + z: 100.000 + - x: 360.225 + y: 653.220 + z: 100.000 + - x: 360.523 + y: 654.391 + z: 100.000 + - x: 360.701 + y: 655.780 + z: 100.000 + - x: 360.976 + y: 657.487 + z: 100.000 + - x: 360.804 + y: 658.842 + z: 100.000 + - x: 360.774 + y: 660.766 + z: 100.000 + - x: 360.755 + y: 661.975 + z: 100.000 + - x: 360.585 + y: 664.776 + z: 100.000 + - x: 360.537 + y: 666.776 + z: 100.000 + - x: 360.490 + y: 668.775 + z: 100.000 + - x: 360.442 + y: 670.774 + z: 100.000 + - x: 360.394 + y: 672.774 + z: 100.000 + - x: 360.347 + y: 674.773 + z: 100.000 + - x: 360.220 + y: 680.099 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 563 + nanosec: 160706650 + frame_id: map + objects: + - object_id: + uuid: + - 6 + - 144 + - 167 + - 221 + - 26 + - 196 + - 145 + - 186 + - 207 + - 95 + - 21 + - 211 + - 96 + - 13 + - 92 + - 5 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + covariance: + - 0.415785 + - -0.000621846 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000621846 + - 0.388031 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0108039 + initial_twist_with_covariance: + twist: + linear: + x: 9.96635 + y: 3.24754e-05 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 5.69744e-05 + covariance: + - 1.01780 + - -2.02150e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -3.54650e-05 + - -2.02150e-05 + - 0.00403854 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -3.54650e-05 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0124301 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 409.670 + y: 647.155 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 414.653 + y: 647.245 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 419.635 + y: 647.335 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 424.617 + y: 647.425 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 429.600 + y: 647.515 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 434.582 + y: 647.605 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 439.565 + y: 647.694 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 444.547 + y: 647.784 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 449.529 + y: 647.874 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 454.512 + y: 647.964 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 459.494 + y: 648.054 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 464.476 + y: 648.144 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 469.459 + y: 648.233 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 474.441 + y: 648.323 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 479.423 + y: 648.413 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 484.406 + y: 648.503 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 489.388 + y: 648.593 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 494.371 + y: 648.683 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 499.353 + y: 648.773 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 504.335 + y: 648.862 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.00000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.501209 + z: 0.690476 + - object_id: + uuid: + - 32 + - 119 + - 141 + - 16 + - 72 + - 85 + - 32 + - 153 + - 66 + - 136 + - 88 + - 177 + - 247 + - 166 + - 151 + - 80 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + covariance: + - 0.0657687 + - -0.000366900 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000366900 + - 0.0451891 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00300423 + initial_twist_with_covariance: + twist: + linear: + x: 10.0048 + y: -0.00124906 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00219133 + covariance: + - 0.522271 + - -6.09993e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000107016 + - -6.09993e-05 + - 0.00263830 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000107016 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00812034 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00453640 + w: 0.999990 + - position: + x: 333.933 + y: 644.723 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00622365 + w: 0.999981 + - position: + x: 338.935 + y: 644.640 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00828910 + w: 0.999966 + - position: + x: 343.877 + y: 644.600 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00409926 + w: 0.999992 + - position: + x: 348.624 + y: 645.122 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0547342 + w: 0.998501 + - position: + x: 352.898 + y: 646.993 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.204867 + w: 0.978790 + - position: + x: 356.221 + y: 650.176 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.372718 + w: 0.927945 + - position: + x: 358.029 + y: 654.521 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.554937 + w: 0.831892 + - position: + x: 358.484 + y: 659.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.673390 + w: 0.739287 + - position: + x: 358.380 + y: 664.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714418 + w: 0.699720 + - position: + x: 358.261 + y: 669.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 674.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 679.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.904 + y: 684.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 689.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.665 + y: 694.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.546 + y: 699.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.427 + y: 704.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.308 + y: 709.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.188 + y: 714.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.069 + y: 719.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.950 + y: 724.396 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.831 + y: 729.397 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.712 + y: 734.398 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.593 + y: 739.399 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.473 + y: 744.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.354 + y: 749.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.235 + y: 754.401 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.116 + y: 759.402 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.997 + y: 764.403 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.880 + y: 644.606 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00392303 + w: 0.999992 + - position: + x: 348.786 + y: 644.906 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0304731 + w: 0.999536 + - position: + x: 353.463 + y: 645.966 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.111241 + w: 0.993793 + - position: + x: 357.696 + y: 648.148 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.235723 + w: 0.971820 + - position: + x: 361.186 + y: 651.334 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.361617 + w: 0.932327 + - position: + x: 363.716 + y: 655.456 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.488304 + w: 0.872674 + - position: + x: 364.853 + y: 660.052 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.616332 + w: 0.787487 + - position: + x: 364.887 + y: 665.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704715 + w: 0.709490 + - position: + x: 364.768 + y: 670.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.648 + y: 675.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.529 + y: 680.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.410 + y: 685.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.291 + y: 690.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.172 + y: 695.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.053 + y: 700.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.933 + y: 705.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.814 + y: 710.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.695 + y: 715.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.576 + y: 720.008 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.457 + y: 725.009 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.338 + y: 730.010 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.218 + y: 735.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.099 + y: 740.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.980 + y: 745.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.861 + y: 750.013 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.742 + y: 755.014 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.622 + y: 760.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.873 + y: 644.613 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00329511 + w: 0.999995 + - position: + x: 348.698 + y: 645.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0415760 + w: 0.999135 + - position: + x: 353.223 + y: 646.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.147464 + w: 0.989067 + - position: + x: 357.115 + y: 649.075 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.296991 + w: 0.954880 + - position: + x: 359.997 + y: 652.871 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.444541 + w: 0.895759 + - position: + x: 361.525 + y: 657.337 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.581548 + w: 0.813512 + - position: + x: 361.651 + y: 662.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697878 + w: 0.716217 + - position: + x: 361.533 + y: 667.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715386 + w: 0.698729 + - position: + x: 361.414 + y: 672.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.295 + y: 677.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.176 + y: 682.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.057 + y: 687.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.938 + y: 692.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.818 + y: 697.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.699 + y: 702.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.580 + y: 707.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.461 + y: 712.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.342 + y: 717.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.222 + y: 722.210 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.103 + y: 727.211 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.984 + y: 732.212 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.865 + y: 737.213 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.746 + y: 742.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.626 + y: 747.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.507 + y: 752.215 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.388 + y: 757.216 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.269 + y: 762.217 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00457230 + w: 0.999990 + - position: + x: 333.933 + y: 644.721 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00644958 + w: 0.999979 + - position: + x: 338.935 + y: 644.632 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00883007 + w: 0.999961 + - position: + x: 343.938 + y: 644.526 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0105871 + w: 0.999944 + - position: + x: 348.941 + y: 644.416 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0110042 + w: 0.999939 + - position: + x: 353.943 + y: 644.319 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00972075 + w: 0.999953 + - position: + x: 358.946 + y: 644.250 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00691289 + w: 0.999976 + - position: + x: 363.948 + y: 644.219 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00309863 + w: 0.999995 + - position: + x: 368.950 + y: 644.228 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.000886733 + w: 1.00000 + - position: + x: 373.952 + y: 644.264 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00363761 + w: 0.999993 + - position: + x: 378.954 + y: 644.305 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00413557 + w: 0.999991 + - position: + x: 383.955 + y: 644.347 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00420318 + w: 0.999991 + - position: + x: 388.957 + y: 644.388 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00411113 + w: 0.999992 + - position: + x: 393.958 + y: 644.432 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00439488 + w: 0.999990 + - position: + x: 398.956 + y: 644.454 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00215849 + w: 0.999998 + - position: + x: 403.941 + y: 644.630 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0176845 + w: 0.999844 + - position: + x: 408.939 + y: 644.821 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 413.938 + y: 645.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 418.936 + y: 645.202 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 423.935 + y: 645.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 428.933 + y: 645.584 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 433.932 + y: 645.774 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 438.931 + y: 645.965 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 443.929 + y: 646.156 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 448.928 + y: 646.346 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 453.927 + y: 646.537 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 458.925 + y: 646.728 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 463.924 + y: 646.918 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 468.923 + y: 647.109 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 473.922 + y: 647.300 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.604998 + z: 0.767451 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 21 + nanosec: 17351334 + frame_id: map + start_pose: + position: + x: 301.475 + y: 642.905 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00500123 + w: 0.999987 + goal_pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + segments: + - preferred_primitive: + id: 2000 + primitive_type: "" + primitives: + - id: 2000 + primitive_type: lane + - id: 2001 + primitive_type: lane + - id: 2002 + primitive_type: lane + - preferred_primitive: + id: 2010 + primitive_type: "" + primitives: + - id: 2010 + primitive_type: lane + - preferred_primitive: + id: 2200 + primitive_type: "" + primitives: + - id: 2200 + primitive_type: lane + - preferred_primitive: + id: 3500 + primitive_type: "" + primitives: + - id: 3500 + primitive_type: lane + - id: 3501 + primitive_type: lane + - id: 3502 + primitive_type: lane + uuid: + uuid: + - 195 + - 84 + - 241 + - 105 + - 133 + - 218 + - 136 + - 83 + - 104 + - 191 + - 48 + - 109 + - 85 + - 60 + - 177 + - 115 + allow_modification: false +operation_mode: + stamp: + sec: 550 + nanosec: 913829317 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: /base_link + accel: + accel: + linear: + x: -0.770147 + y: 0.0409905 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 337.371 + y: 643.219 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00401753 + w: 0.999992 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 4.13155 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00992133 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml new file mode 100644 index 0000000000000..9a20cbee9c1ff --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml @@ -0,0 +1,2512 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 74 + nanosec: 11949715 + frame_id: map + points: + - point: + pose: + position: + x: 360.306 + y: 597.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710800 + w: 0.703394 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.285 + y: 599.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710829 + w: 0.703365 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.264 + y: 601.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710857 + w: 0.703336 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.243 + y: 603.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710881 + w: 0.703312 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.222 + y: 605.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710951 + w: 0.703242 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.200 + y: 607.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711036 + w: 0.703156 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.177 + y: 609.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711601 + w: 0.702584 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.152 + y: 611.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711932 + w: 0.702248 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.133 + y: 612.628 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716985 + w: 0.697089 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - 1200 + - point: + pose: + position: + x: 360.116 + y: 613.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.732064 + w: 0.681236 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.972 + y: 615.239 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.740537 + w: 0.672016 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.778 + y: 617.231 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.759018 + w: 0.651069 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.474 + y: 619.204 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.784627 + w: 0.619968 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.010 + y: 621.159 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.835622 + w: 0.549305 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 358.221 + y: 622.986 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.876871 + w: 0.480726 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 357.127 + y: 624.700 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.906015 + w: 0.423245 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 355.847 + y: 626.230 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.934815 + w: 0.355135 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 354.341 + y: 627.567 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.956562 + w: 0.291529 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 352.674 + y: 628.688 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.976933 + w: 0.213544 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 350.854 + y: 629.523 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.989029 + w: 0.147720 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 348.920 + y: 630.114 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.997418 + w: 0.0718077 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 346.951 + y: 630.399 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121684 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 344.951 + y: 630.448 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 1.00000 + w: 0.000253960 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.951 + y: 630.449 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999959 + w: 0.00906744 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.741 + y: 630.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - 2500 + - point: + pose: + position: + x: 340.951 + y: 630.409 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999893 + w: 0.0146423 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 339.014 + y: 630.352 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999803 + w: 0.0198557 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 337.016 + y: 630.273 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999732 + w: 0.0231414 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 335.018 + y: 630.180 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999702 + w: 0.0243973 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 333.020 + y: 630.083 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999705 + w: 0.0242858 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 332.731 + y: 630.068 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + left_bound: + - x: 358.794 + y: 596.731 + z: 100.000 + - x: 358.788 + y: 597.342 + z: 100.000 + - x: 358.758 + y: 600.198 + z: 100.000 + - x: 358.736 + y: 602.392 + z: 100.000 + - x: 358.718 + y: 604.199 + z: 100.000 + - x: 358.698 + y: 606.199 + z: 100.000 + - x: 358.686 + y: 607.440 + z: 100.000 + - x: 358.610 + y: 610.191 + z: 100.000 + - x: 358.512 + y: 612.597 + z: 100.000 + - x: 358.405 + y: 614.096 + z: 100.000 + - x: 358.218 + y: 616.079 + z: 100.000 + - x: 358.086 + y: 617.465 + z: 100.000 + - x: 357.913 + y: 618.622 + z: 100.000 + - x: 357.666 + y: 619.746 + z: 100.000 + - x: 357.285 + y: 620.810 + z: 100.000 + - x: 356.832 + y: 621.812 + z: 100.000 + - x: 356.284 + y: 622.823 + z: 100.000 + - x: 355.663 + y: 623.752 + z: 100.000 + - x: 354.964 + y: 624.613 + z: 100.000 + - x: 354.158 + y: 625.437 + z: 100.000 + - x: 353.306 + y: 626.152 + z: 100.000 + - x: 352.395 + y: 626.788 + z: 100.000 + - x: 351.400 + y: 627.366 + z: 100.000 + - x: 350.389 + y: 627.820 + z: 100.000 + - x: 349.337 + y: 628.212 + z: 100.000 + - x: 348.219 + y: 628.479 + z: 100.000 + - x: 347.083 + y: 628.681 + z: 100.000 + - x: 346.042 + y: 628.729 + z: 100.000 + - x: 344.050 + y: 628.720 + z: 100.000 + - x: 342.764 + y: 628.777 + z: 100.000 + - x: 340.080 + y: 628.723 + z: 100.000 + - x: 338.078 + y: 628.751 + z: 100.000 + - x: 336.079 + y: 628.710 + z: 100.000 + - x: 334.080 + y: 628.670 + z: 100.000 + - x: 332.082 + y: 628.629 + z: 100.000 + - x: 326.872 + y: 628.524 + z: 100.000 + right_bound: + - x: 361.830 + y: 596.762 + z: 100.000 + - x: 361.823 + y: 597.400 + z: 100.000 + - x: 361.792 + y: 600.231 + z: 100.000 + - x: 361.767 + y: 602.446 + z: 100.000 + - x: 361.748 + y: 604.230 + z: 100.000 + - x: 361.726 + y: 606.230 + z: 100.000 + - x: 361.712 + y: 607.493 + z: 100.000 + - x: 361.718 + y: 610.230 + z: 100.000 + - x: 361.754 + y: 612.657 + z: 100.000 + - x: 361.627 + y: 614.331 + z: 100.000 + - x: 361.519 + y: 616.330 + z: 100.000 + - x: 361.452 + y: 617.634 + z: 100.000 + - x: 361.277 + y: 619.056 + z: 100.000 + - x: 360.998 + y: 620.486 + z: 100.000 + - x: 360.581 + y: 621.905 + z: 100.000 + - x: 359.991 + y: 623.293 + z: 100.000 + - x: 359.319 + y: 624.583 + z: 100.000 + - x: 358.490 + y: 625.825 + z: 100.000 + - x: 357.546 + y: 626.984 + z: 100.000 + - x: 356.525 + y: 628.025 + z: 100.000 + - x: 355.377 + y: 628.981 + z: 100.000 + - x: 354.158 + y: 629.845 + z: 100.000 + - x: 352.881 + y: 630.552 + z: 100.000 + - x: 351.515 + y: 631.167 + z: 100.000 + - x: 350.098 + y: 631.615 + z: 100.000 + - x: 348.672 + y: 631.919 + z: 100.000 + - x: 347.236 + y: 632.124 + z: 100.000 + - x: 346.083 + y: 632.098 + z: 100.000 + - x: 344.082 + y: 632.081 + z: 100.000 + - x: 342.719 + y: 632.113 + z: 100.000 + - x: 340.012 + y: 632.058 + z: 100.000 + - x: 338.014 + y: 631.950 + z: 100.000 + - x: 336.015 + y: 631.909 + z: 100.000 + - x: 334.016 + y: 631.869 + z: 100.000 + - x: 332.017 + y: 631.829 + z: 100.000 + - x: 326.807 + y: 631.724 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 73 + nanosec: 980215670 + frame_id: map + objects: + - object_id: + uuid: + - 251 + - 180 + - 23 + - 85 + - 112 + - 118 + - 142 + - 88 + - 241 + - 29 + - 194 + - 27 + - 222 + - 164 + - 130 + - 251 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + covariance: + - 0.299126 + - -8.23923e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -8.23923e-05 + - 0.349251 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00879510 + initial_twist_with_covariance: + twist: + linear: + x: 9.98083 + y: 0.00185652 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00325706 + covariance: + - 0.970951 + - 0.000139981 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000245582 + - 0.000139981 + - 0.00384142 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.000245582 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0118235 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.373 + y: 672.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709614 + w: 0.704590 + - position: + x: 357.520 + y: 677.298 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.696604 + w: 0.717456 + - position: + x: 357.917 + y: 682.299 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.678600 + w: 0.734508 + - position: + x: 358.531 + y: 687.305 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.662668 + w: 0.748913 + - position: + x: 359.266 + y: 692.315 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.653723 + w: 0.756734 + - position: + x: 359.995 + y: 697.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.654217 + w: 0.756307 + - position: + x: 360.590 + y: 702.329 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.664107 + w: 0.747638 + - position: + x: 360.955 + y: 707.330 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.680844 + w: 0.732428 + - position: + x: 361.063 + y: 712.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.699424 + w: 0.714707 + - position: + x: 360.983 + y: 717.314 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712725 + w: 0.701443 + - position: + x: 360.864 + y: 722.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.746 + y: 727.292 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.627 + y: 732.280 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.508 + y: 737.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.389 + y: 742.258 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.270 + y: 747.247 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.151 + y: 752.236 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.032 + y: 757.225 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.913 + y: 762.214 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.794 + y: 767.203 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.675 + y: 772.192 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.557 + y: 777.181 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.438 + y: 782.170 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.319 + y: 787.159 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.200 + y: 792.148 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.081 + y: 797.137 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.962 + y: 802.126 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.843 + y: 807.115 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.724 + y: 812.104 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.605 + y: 817.093 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.230769 + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.340 + y: 672.302 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711946 + w: 0.702234 + - position: + x: 357.296 + y: 677.293 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710176 + w: 0.704024 + - position: + x: 357.286 + y: 682.284 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707820 + w: 0.706393 + - position: + x: 357.304 + y: 687.276 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705868 + w: 0.708343 + - position: + x: 357.334 + y: 692.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704976 + w: 0.709232 + - position: + x: 357.357 + y: 697.261 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705452 + w: 0.708758 + - position: + x: 357.355 + y: 702.252 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707256 + w: 0.706957 + - position: + x: 357.314 + y: 707.243 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709999 + w: 0.704203 + - position: + x: 357.231 + y: 712.233 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712949 + w: 0.701216 + - position: + x: 357.119 + y: 717.222 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715047 + w: 0.699076 + - position: + x: 357.000 + y: 722.211 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.881 + y: 727.199 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.762 + y: 732.188 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.643 + y: 737.177 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.524 + y: 742.166 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.405 + y: 747.155 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.286 + y: 752.144 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.167 + y: 757.133 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.049 + y: 762.122 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.930 + y: 767.111 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.811 + y: 772.100 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.692 + y: 777.089 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.573 + y: 782.078 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.454 + y: 787.067 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.335 + y: 792.056 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.216 + y: 797.045 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.097 + y: 802.033 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.978 + y: 807.022 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.859 + y: 812.011 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.741 + y: 817.000 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.769231 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.89999 + y: 0.500442 + z: 0.612431 + - object_id: + uuid: + - 218 + - 141 + - 152 + - 171 + - 185 + - 127 + - 84 + - 175 + - 69 + - 40 + - 199 + - 89 + - 96 + - 107 + - 61 + - 243 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + covariance: + - 0.0636258 + - -0.000170219 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000170219 + - 0.0826995 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00394708 + initial_twist_with_covariance: + twist: + linear: + x: 10.0303 + y: -0.00247386 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00434011 + covariance: + - 0.579437 + - -9.81341e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000172165 + - -9.81341e-05 + - 0.00311875 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000172165 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00959910 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.224 + y: 608.637 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.706633 + w: 0.707581 + - position: + x: 358.279 + y: 613.653 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.703235 + w: 0.710958 + - position: + x: 358.397 + y: 618.671 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698737 + w: 0.715379 + - position: + x: 358.567 + y: 623.690 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695068 + w: 0.718944 + - position: + x: 358.758 + y: 628.710 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.693532 + w: 0.720426 + - position: + x: 358.932 + y: 633.729 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.694735 + w: 0.719266 + - position: + x: 359.054 + y: 638.747 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698453 + w: 0.715656 + - position: + x: 359.097 + y: 643.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704045 + w: 0.710155 + - position: + x: 359.057 + y: 648.778 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709938 + w: 0.704265 + - position: + x: 358.958 + y: 653.791 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714096 + w: 0.700048 + - position: + x: 358.849 + y: 658.805 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714730 + w: 0.699401 + - position: + x: 358.740 + y: 663.817 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714768 + w: 0.699361 + - position: + x: 358.620 + y: 668.830 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.501 + y: 673.843 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.381 + y: 678.856 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.262 + y: 683.870 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 688.883 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 693.896 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.903 + y: 698.909 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 703.923 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.664 + y: 708.936 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.545 + y: 713.950 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.425 + y: 718.963 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.306 + y: 723.977 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.186 + y: 728.991 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.067 + y: 734.004 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.947 + y: 739.018 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.828 + y: 744.032 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.708 + y: 749.045 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.589 + y: 754.059 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.206 + y: 608.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707930 + w: 0.706283 + - position: + x: 358.149 + y: 613.642 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711130 + w: 0.703060 + - position: + x: 358.010 + y: 618.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716853 + w: 0.697224 + - position: + x: 357.024 + y: 623.117 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.779378 + w: 0.626554 + - position: + x: 354.441 + y: 627.006 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.881248 + w: 0.472653 + - position: + x: 350.340 + y: 629.665 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.958935 + w: 0.283625 + - position: + x: 345.469 + y: 630.694 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.994592 + w: 0.103864 + - position: + x: 340.508 + y: 630.814 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121264 + - position: + x: 335.554 + y: 630.768 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999989 + w: 0.00460609 + - position: + x: 330.597 + y: 630.677 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999957 + w: 0.00925679 + - position: + x: 325.638 + y: 630.576 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 320.674 + y: 630.476 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 315.708 + y: 630.376 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 310.737 + y: 630.276 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 305.763 + y: 630.175 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 300.785 + y: 630.075 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 295.803 + y: 629.974 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 290.817 + y: 629.874 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 285.828 + y: 629.773 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 280.835 + y: 629.672 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 275.839 + y: 629.571 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 270.840 + y: 629.470 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 265.838 + y: 629.370 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 260.833 + y: 629.268 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 255.825 + y: 629.167 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 250.816 + y: 629.066 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 245.805 + y: 628.965 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 240.792 + y: 628.864 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 235.778 + y: 628.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 230.764 + y: 628.662 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.644288 + z: 0.991445 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 19 + nanosec: 549630704 + frame_id: map + start_pose: + position: + x: 360.481 + y: 591.844 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.721675 + w: 0.692232 + goal_pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999615 + w: -0.0277448 + segments: + - preferred_primitive: + id: 1010 + primitive_type: "" + primitives: + - id: 1010 + primitive_type: lane + - preferred_primitive: + id: 1200 + primitive_type: "" + primitives: + - id: 1200 + primitive_type: lane + - preferred_primitive: + id: 2500 + primitive_type: "" + primitives: + - id: 2500 + primitive_type: lane + - id: 2501 + primitive_type: lane + uuid: + uuid: + - 83 + - 230 + - 40 + - 201 + - 182 + - 232 + - 31 + - 237 + - 204 + - 91 + - 39 + - 90 + - 153 + - 179 + - 6 + - 123 + allow_modification: false +operation_mode: + stamp: + sec: 65 + nanosec: 819714871 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: /base_link + accel: + accel: + linear: + x: -1.01094 + y: 0.00400509 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 360.302 + y: 604.493 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707950 + w: 0.706263 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 2.31221 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00173214 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 From 9ce874c9131f37a57c8661a5e65affa95831b381 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 8 Jan 2025 16:02:09 +0900 Subject: [PATCH 40/45] feat(lidar_centerpoint, pointpainting): add diag publisher for max voxel size (#9720) --- .../pointpainting_fusion/node.hpp | 2 ++ .../src/pointpainting_fusion/node.cpp | 17 ++++++++++++++++- .../lidar_centerpoint/centerpoint_trt.hpp | 2 +- .../include/autoware/lidar_centerpoint/node.hpp | 2 ++ .../lib/centerpoint_trt.cpp | 6 +++++- .../autoware_lidar_centerpoint/src/node.cpp | 17 ++++++++++++++++- 6 files changed, 42 insertions(+), 4 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index cd6cd87976522..5bc428a8e7ab8 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -58,6 +59,7 @@ class PointPaintingFusionNode void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; rclcpp::Publisher::SharedPtr obj_pub_ptr_; + std::unique_ptr diagnostics_interface_ptr_; int omp_num_threads_{1}; float score_threshold_{0.0}; diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 25d9064ddbfc7..2f6f853d4281e 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -186,6 +186,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt // create detector detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); + diagnostics_interface_ptr_ = + std::make_unique(this, "pointpainting_trt"); obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); @@ -389,6 +391,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + diagnostics_interface_ptr_->clear(); const auto objects_sub_count = obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); @@ -403,10 +406,21 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte } std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "PointPaintingTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -425,6 +439,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte if (objects_sub_count > 0) { obj_pub_ptr_->publish(output_msg); } + diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp); } bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index ce5ec3ea0abfe..671ff54f8ce0d 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -62,7 +62,7 @@ class CenterPointTRT bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d, bool & is_num_pillars_within_range); protected: void initPtr(); diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index a49451fde9d0d..1748db0e48392 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -20,6 +20,7 @@ #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include +#include #include #include #include @@ -59,6 +60,7 @@ class LidarCenterPointNode : public rclcpp::Node DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; + std::unique_ptr diagnostics_interface_ptr_; // debugger std::unique_ptr> stop_watch_ptr_{ diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 6ee2b3733bdb0..530f59179d25b 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -19,6 +19,7 @@ #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include +#include #include #include @@ -127,8 +128,10 @@ void CenterPointTRT::initPtr() bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d, bool & is_num_pillars_within_range) { + is_num_pillars_within_range = true; + CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( @@ -155,6 +158,7 @@ bool CenterPointTRT::detect( "The actual number of pillars (%u) exceeds its maximum value (%zu). " "Please considering increasing it since it may limit the detection performance.", num_pillars, config_.max_voxel_size_); + is_num_pillars_within_range = false; } return true; diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 59acceeac54d6..825fc40234120 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -107,6 +107,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); + diagnostics_interface_ptr_ = + std::make_unique(this, "centerpoint_trt"); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -144,12 +146,24 @@ void LidarCenterPointNode::pointCloudCallback( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } + diagnostics_interface_ptr_->clear(); std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + *input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "CenterPointTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -169,6 +183,7 @@ void LidarCenterPointNode::pointCloudCallback( objects_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } + diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp); // add processing time for debug if (debug_publisher_ptr_ && stop_watch_ptr_) { From 14ee08b9d19c5f33ed37c72b9504fdcad464ef1b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 8 Jan 2025 16:38:33 +0900 Subject: [PATCH 41/45] refactor(autoware_image_projection_based_fusion): organize 2d-detection related members (#9789) * chore: input_camera_topics_ is only for debug Signed-off-by: Taekjin LEE * feat: fuse main message with cached roi messages in fusion_node.cpp Signed-off-by: Taekjin LEE * chore: add comments on each process step, organize methods Signed-off-by: Taekjin LEE * feat: Export process method in fusion_node.cpp Export the `exportProcess()` method in `fusion_node.cpp` to handle the post-processing and publishing of the fused messages. This method cancels the timer, performs the necessary post-processing steps, publishes the output message, and resets the flags. It also adds processing time for debugging purposes. This change improves the organization and readability of the code. Signed-off-by: Taekjin LEE * feat: Refactor fusion_node.hpp and fusion_node.cpp Refactor the `fusion_node.hpp` and `fusion_node.cpp` files to improve code organization and readability. This includes exporting the `exportProcess()` method in `fusion_node.cpp` to handle post-processing and publishing of fused messages, adding comments on each process step, organizing methods, and fusing the main message with cached ROI messages. These changes enhance the overall quality of the codebase. Signed-off-by: Taekjin LEE * Refactor fusion_node.cpp and fusion_node.hpp for improved code organization and readability Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * feat: Refactor fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * Refactor fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * feat: implement mutex per 2d detection process Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * revise template, inputs first and output at the last Signed-off-by: Taekjin LEE * explicit in and out types 1 Signed-off-by: Taekjin LEE * clarify pointcloud message type Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * Refactor publisher types in fusion_node.hpp and node.hpp Signed-off-by: Taekjin LEE * fix: resolve cppcheck issue shadowVariable Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * chore: rename Det2dManager to Det2dStatus Signed-off-by: Taekjin LEE * revert mutex related changes Signed-off-by: Taekjin LEE * refactor: review member and method's access Signed-off-by: Taekjin LEE * fix: resolve shadowVariable of 'det2d' Signed-off-by: Taekjin LEE * fix missing line Signed-off-by: Taekjin LEE * refactor message postprocess and publish methods Signed-off-by: Taekjin LEE * publish the main message is common Signed-off-by: Taekjin LEE * fix: replace pointcloud message type by the typename Signed-off-by: Taekjin LEE * review member access Signed-off-by: Taekjin LEE * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE * refactor: fix condition for publishing painted pointcloud message Signed-off-by: Taekjin LEE * fix: remove unused variable Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 124 +++-- .../pointpainting_fusion/node.hpp | 20 +- .../roi_cluster_fusion/node.hpp | 21 +- .../roi_detected_object_fusion/node.hpp | 16 +- .../roi_pointcloud_fusion/node.hpp | 35 +- .../segmentation_pointcloud_fusion/node.hpp | 48 +- .../utils/utils.hpp | 12 +- .../src/fusion_node.cpp | 518 +++++++++--------- .../src/pointpainting_fusion/node.cpp | 86 ++- .../src/roi_cluster_fusion/node.cpp | 63 +-- .../src/roi_detected_object_fusion/node.cpp | 63 ++- .../src/roi_pointcloud_fusion/node.cpp | 91 ++- .../segmentation_pointcloud_fusion/node.cpp | 73 ++- .../src/utils/utils.cpp | 12 +- 14 files changed, 603 insertions(+), 579 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index 239d406650ce8..b8881bc6abff7 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -52,14 +52,33 @@ using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::CameraInfo; using sensor_msgs::msg::Image; -using sensor_msgs::msg::PointCloud2; -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using PointCloudMsgType = sensor_msgs::msg::PointCloud2; +using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; -template +template +struct Det2dStatus +{ + // camera index + std::size_t id = 0; + // camera projection + std::unique_ptr camera_projector_ptr; + bool project_to_unrectified_image = false; + bool approximate_camera_projection = false; + // process flags + bool is_fused = false; + // timing + double input_offset_ms = 0.0; + // cache + std::map cached_det2d_msgs; +}; + +template class FusionNode : public rclcpp::Node { public: @@ -71,75 +90,72 @@ class FusionNode : public rclcpp::Node explicit FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); -protected: +private: + // Common process methods void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - virtual void preprocess(TargetMsg3D & output_msg); - - // callback for Msg subscription - virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); - - // callback for roi subscription - - virtual void roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); - - virtual void fuseOnSingleImage( - const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, - TargetMsg3D & output_msg) = 0; - - // set args if you need - virtual void postprocess(TargetMsg3D & output_msg); - - virtual void publish(const TargetMsg3D & output_msg); - + // callback for timer void timer_callback(); void setPeriod(const int64_t new_period); + void exportProcess(); + + // 2d detection management methods + bool checkAllDet2dFused() + { + for (const auto & det2d : det2d_list_) { + if (!det2d.is_fused) { + return false; + } + } + return true; + } - std::size_t rois_number_{1}; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - std::vector point_project_to_unrectified_image_; - - // camera_info - std::map camera_info_map_; - std::vector::SharedPtr> camera_info_subs_; // camera projection - std::vector camera_projectors_; - std::vector approx_camera_projection_; float approx_grid_cell_w_size_; float approx_grid_cell_h_size_; + // timer rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; double match_threshold_ms_{}; - std::vector input_rois_topics_; - std::vector input_camera_info_topics_; - std::vector input_camera_topics_; - /** \brief A vector of subscriber. */ - typename rclcpp::Subscription::SharedPtr sub_; std::vector::SharedPtr> rois_subs_; - - // offsets between cameras and the lidars - std::vector input_offset_ms_; + std::vector::SharedPtr> camera_info_subs_; // cache for fusion - std::vector is_fused_; - std::pair - cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + int64_t cached_det3d_msg_timestamp_; + typename Msg3D::SharedPtr cached_det3d_msg_ptr_; std::mutex mutex_cached_msgs_; - // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; +protected: + void setDet2DStatus(std::size_t rois_number); - // debugger - std::shared_ptr debugger_; - virtual bool out_of_scope(const ObjType & obj) = 0; + // callback for main subscription + void subCallback(const typename Msg3D::ConstSharedPtr input_msg); + // callback for roi subscription + void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + + // Custom process methods + virtual void preprocess(Msg3D & output_msg); + virtual void fuseOnSingleImage( + const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, + Msg3D & output_msg) = 0; + virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg); + virtual void publish(const ExportObj & output_msg); + + // Members + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // 2d detection management + std::vector> det2d_list_; + + /** \brief A vector of subscriber. */ + typename rclcpp::Subscription::SharedPtr sub_; + + // parameters for out_of_scope filter float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; @@ -147,6 +163,12 @@ class FusionNode : public rclcpp::Node float filter_scope_min_z_; float filter_scope_max_z_; + // output publisher + typename rclcpp::Publisher::SharedPtr pub_ptr_; + + // debugger + std::shared_ptr debugger_; + /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 5bc428a8e7ab8..9505a926df2f8 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -42,27 +42,25 @@ inline bool isInsideBbox( proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; } -class PointPaintingFusionNode -: public FusionNode +class PointPaintingFusionNode : public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; +private: + void preprocess(PointCloudMsgType & pointcloud_msg) override; void fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + void postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override; - rclcpp::Publisher::SharedPtr obj_pub_ptr_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; std::unique_ptr diagnostics_interface_ptr_; int omp_num_threads_{1}; - float score_threshold_{0.0}; std::vector class_names_; std::map class_index_; std::map> isClassTable_; @@ -74,8 +72,6 @@ class PointPaintingFusionNode autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; - - bool out_of_scope(const DetectedObjects & obj) override; }; } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 903b153af0681..b7bf8738b68a4 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -24,21 +24,19 @@ namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; -class RoiClusterFusionNode -: public FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> +class RoiClusterFusionNode : public FusionNode { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override; - void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; +private: + void preprocess(ClusterMsgType & output_cluster_msg) override; void fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - DetectedObjectsWithFeature & output_cluster_msg) override; + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; + + void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override; std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; @@ -53,12 +51,11 @@ class RoiClusterFusionNode double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; - bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + bool is_far_enough(const ClusterObjType & obj, const double distance_threshold); + bool out_of_scope(const ClusterObjType & obj); double cal_iou_by_mode( const sensor_msgs::msg::RegionOfInterest & roi_1, const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); - // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 43ec134168ef9..ba197126277a0 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -31,21 +31,20 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode -: public FusionNode +class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); -protected: +private: void preprocess(DetectedObjects & output_msg) override; void fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override; + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const std::size_t & image_id, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( @@ -53,11 +52,10 @@ class RoiDetectedObjectFusionNode const std::vector & image_rois, const std::map & object_roi_map); - void publish(const DetectedObjects & output_msg) override; + void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override; - bool out_of_scope(const DetectedObject & obj) override; + bool out_of_scope(const DetectedObject & obj); -private: struct { std::vector passthrough_lower_bound_probability_thresholds{}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2f2c8222e196f..77a1745faa7e5 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -25,32 +25,29 @@ namespace autoware::image_projection_based_fusion { -class RoiPointCloudFusionNode -: public FusionNode +class RoiPointCloudFusionNode : public FusionNode { +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - int min_cluster_size_{1}; - int max_cluster_size_{20}; - bool fuse_unknown_only_{true}; - double cluster_2d_tolerance_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; - rclcpp::Publisher::SharedPtr pub_objects_ptr_; - std::vector output_fused_objects_; - rclcpp::Publisher::SharedPtr cluster_debug_pub_; + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; - /* data */ -public: - explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override; -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void publish(const ClusterMsgType & output_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + int min_cluster_size_{1}; + int max_cluster_size_{20}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + std::vector output_fused_objects_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 7454cb7bcd173..5414f98e142cd 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -34,10 +34,32 @@ namespace autoware::image_projection_based_fusion { -class SegmentPointCloudFusionNode : public FusionNode +class SegmentPointCloudFusionNode : public FusionNode { +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + void preprocess(PointCloudMsgType & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; + + inline void copyPointCloud( + const PointCloudMsgType & input, const int point_step, const size_t global_offset, + PointCloudMsgType & output, size_t & output_pointcloud_size) + { + std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); + output_pointcloud_size += point_step; + } + + void postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) override; + + // debug + image_transport::Publisher pub_debug_mask_ptr_; + std::vector filter_semantic_label_target_; float filter_distance_threshold_; // declare list of semantic label target, depend on trained data of yolox segmentation model @@ -47,30 +69,8 @@ class SegmentPointCloudFusionNode : public FusionNode filter_global_offset_set_; - -public: - explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); - -protected: - void preprocess(PointCloud2 & pointcloud_msg) override; - - void postprocess(PointCloud2 & pointcloud_msg) override; - - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, - PointCloud2 & output_pointcloud_msg) override; - - bool out_of_scope(const PointCloud2 & filtered_cloud) override; - inline void copyPointCloud( - const PointCloud2 & input, const int point_step, const size_t global_offset, - PointCloud2 & output, size_t & output_pointcloud_size) - { - std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); - output_pointcloud_size += point_step; - } }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 12577081713be..044a71ab57533 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -56,7 +56,7 @@ namespace autoware::image_projection_based_fusion { using PointCloud = pcl::PointCloud; -using PointCloud2 = sensor_msgs::msg::PointCloud2; + struct PointData { float distance; @@ -72,17 +72,17 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster); + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster); void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects); -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud); } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index ea0904215cb86..310c68f12db88 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -45,79 +46,66 @@ namespace autoware::image_projection_based_fusion { using autoware::universe_utils::ScopedTimeTrack; -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number")); - if (rois_number_ < 1) { - RCLCPP_WARN( - this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); - rois_number_ = 1; + const std::size_t rois_number = + static_cast(declare_parameter("rois_number")); + if (rois_number < 1) { + RCLCPP_ERROR( + this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number); } - if (rois_number_ > 8) { + if (rois_number > 8) { RCLCPP_WARN( this->get_logger(), - "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number_); + "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number); } // Set parameters match_threshold_ms_ = declare_parameter("match_threshold_ms"); timeout_ms_ = declare_parameter("timeout_ms"); - input_rois_topics_.resize(rois_number_); - input_camera_topics_.resize(rois_number_); - input_camera_info_topics_.resize(rois_number_); + std::vector input_rois_topics; + std::vector input_camera_info_topics; + + input_rois_topics.resize(rois_number); + input_camera_info_topics.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - input_rois_topics_.at(roi_i) = declare_parameter( + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_rois_topics.at(roi_i) = declare_parameter( "input/rois" + std::to_string(roi_i), "/perception/object_recognition/detection/rois" + std::to_string(roi_i)); - input_camera_info_topics_.at(roi_i) = declare_parameter( + input_camera_info_topics.at(roi_i) = declare_parameter( "input/camera_info" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); - - input_camera_topics_.at(roi_i) = declare_parameter( - "input/image" + std::to_string(roi_i), - "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { - throw std::runtime_error("The number of offsets does not match the number of topics."); - } - - // sub camera info - camera_info_subs_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe camera info + camera_info_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function fnc = std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( - input_camera_info_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } - // sub rois - rois_subs_.resize(rois_number_); - cached_roi_msgs_.resize(rois_number_); - is_fused_.resize(rois_number_, false); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe rois + rois_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( - input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } - // subscribers - std::function sub_callback = + // subscribe 3d detection + std::function sub_callback = std::bind(&FusionNode::subCallback, this, std::placeholders::_1); - sub_ = - this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - - // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -125,40 +113,34 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); - // camera projection settings - camera_projectors_.resize(rois_number_); - point_project_to_unrectified_image_ = - declare_parameter>("point_project_to_unrectified_image"); + // initialization on each 2d detections + setDet2DStatus(rois_number); - if (rois_number_ > point_project_to_unrectified_image_.size()) { - throw std::runtime_error( - "The number of point_project_to_unrectified_image does not match the number of rois topics."); - } - approx_camera_projection_ = declare_parameter>("approximate_camera_projection"); - if (rois_number_ != approx_camera_projection_.size()) { - const std::size_t current_size = approx_camera_projection_.size(); - RCLCPP_WARN( - this->get_logger(), - "The number of elements in approximate_camera_projection should be the same as in " - "rois_number. " - "It has %zu elements.", - current_size); - if (current_size < rois_number_) { - approx_camera_projection_.resize(rois_number_); - for (std::size_t i = current_size; i < rois_number_; i++) { - approx_camera_projection_.at(i) = true; - } - } - } + // parameters for approximation grid approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + // parameters for out_of_scope filter + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + // debugger if (declare_parameter("debug_mode", false)) { + std::vector input_camera_topics; + input_camera_topics.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_camera_topics.at(roi_i) = declare_parameter( + "input/image" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); + } std::size_t image_buffer_size = static_cast(declare_parameter("image_buffer_size")); debugger_ = - std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); + std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); } // time keeper @@ -180,77 +162,127 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } +} - filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); - filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); - filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); - filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); - filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); - filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); +template +void FusionNode::setDet2DStatus(std::size_t rois_number) +{ + // camera offset settings + std::vector input_offset_ms = declare_parameter>("input_offset_ms"); + if (!input_offset_ms.empty() && rois_number > input_offset_ms.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } + + // camera projection settings + std::vector point_project_to_unrectified_image = + declare_parameter>("point_project_to_unrectified_image"); + if (rois_number > point_project_to_unrectified_image.size()) { + throw std::runtime_error( + "The number of point_project_to_unrectified_image does not match the number of rois " + "topics."); + } + std::vector approx_camera_projection = + declare_parameter>("approximate_camera_projection"); + if (rois_number != approx_camera_projection.size()) { + const std::size_t current_size = approx_camera_projection.size(); + RCLCPP_WARN( + get_logger(), + "The number of elements in approximate_camera_projection should be the same as in " + "rois_number. " + "It has %zu elements.", + current_size); + if (current_size < rois_number) { + approx_camera_projection.resize(rois_number); + for (std::size_t i = current_size; i < rois_number; i++) { + approx_camera_projection.at(i) = true; + } + } + } + + // 2d detection status initialization + det2d_list_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + det2d_list_.at(roi_i).id = roi_i; + det2d_list_.at(roi_i).project_to_unrectified_image = + point_project_to_unrectified_image.at(roi_i); + det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); + det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); + } } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { // create the CameraProjection when the camera info arrives for the first time // assuming the camera info does not change while the node is running - if ( - camera_info_map_.find(camera_id) == camera_info_map_.end() && - checkCameraInfo(*input_camera_info_msg)) { - camera_projectors_.at(camera_id) = CameraProjection( + auto & det2d = det2d_list_.at(camera_id); + if (!det2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { + det2d.camera_projector_ptr = std::make_unique( *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, - point_project_to_unrectified_image_.at(camera_id), approx_camera_projection_.at(camera_id)); - camera_projectors_.at(camera_id).initialize(); - - camera_info_map_[camera_id] = *input_camera_info_msg; + det2d.project_to_unrectified_image, det2d.approximate_camera_projection); + det2d.camera_projector_ptr->initialize(); } } -template -void FusionNode::preprocess(TargetMsg3D & ouput_msg - __attribute__((unused))) +template +void FusionNode::preprocess(Msg3D & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback( - const typename TargetMsg3D::ConstSharedPtr input_msg) +template +void FusionNode::exportProcess() +{ + timer_->cancel(); + + ExportObj output_msg; + postprocess(*(cached_det3d_msg_ptr_), output_msg); + publish(output_msg); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + processing_time_ms = 0; + } + cached_det3d_msg_ptr_ = nullptr; +} + +template +void FusionNode::subCallback( + const typename Msg3D::ConstSharedPtr det3d_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (cached_msg_.second != nullptr) { + if (cached_det3d_msg_ptr_ != nullptr) { + // PROCESS: if the main message is remained (and roi is not collected all) publish the main + // message may processed partially with arrived 2d rois stop_watch_ptr_->toc("processing_time", true); - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - processing_time_ms = 0; - } + exportProcess(); - cached_msg_.second = nullptr; + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } std::lock_guard lock(mutex_cached_msgs_); + + // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); try { @@ -262,154 +294,141 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); - typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*input_msg); - + // PROCESS: preprocess the main message + typename Msg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); preprocess(*output_msg); + // PROCESS: fuse the main message with the cached roi messages + // (please ask maintainers before parallelize this loop because debugger is not thread safe) int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; + // for loop for each roi + for (auto & det2d : det2d_list_) { + const auto roi_i = det2d.id; - // if matching rois exist, fuseOnSingle - // please ask maintainers before parallelize this loop because debugger is not thread safe - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } + auto & det2d_msgs = det2d.cached_det2d_msgs; + + // check if the roi is collected + if (det2d_msgs.size() == 0) continue; + + // MATCH: get the closest roi message, and remove outdated messages + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + for (const auto & [roi_stamp, value] : det2d_msgs) { + int64_t new_stamp = timestamp_nsec + det2d.input_offset_ms * static_cast(1e6); + int64_t interval = abs(static_cast(roi_stamp) - new_stamp); + + if (interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { + min_interval = interval; + matched_stamp = roi_stamp; + } else if ( + static_cast(roi_stamp) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(roi_stamp)); } + } + for (auto stamp : outdate_stamps) { + det2d_msgs.erase(stamp); + } - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // PROCESS: if matched, fuse the main message with the roi message + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } + fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); + det2d_msgs.erase(matched_stamp); + det2d.is_fused = true; - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; - - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); - } + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - det2d.input_offset_ms); } } } - // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current - // Msg - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*output_msg); - publish(*output_msg); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_ms = 0; + // PROCESS: check if the fused message is ready to publish + cached_det3d_msg_timestamp_ = timestamp_nsec; + cached_det3d_msg_ptr_ = output_msg; + if (checkAllDet2dFused()) { + // if all camera fused, postprocess and publish the main message + exportProcess(); + + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } else { - cached_msg_.first = timestamp_nsec; - cached_msg_.second = output_msg; + // if all of rois are not collected, publish the old Msg(if exists) and cache the + // current Msg processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } -template -void FusionNode::roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); stop_watch_ptr_->toc("processing_time", true); - int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + - (*input_roi_msg).header.stamp.nanosec; + auto & det2d = det2d_list_.at(roi_i); + int64_t timestamp_nsec = + (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; // if cached Msg exist, try to match - if (cached_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * static_cast(1e6); + if (cached_det3d_msg_ptr_ != nullptr) { + int64_t new_stamp = + cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); - if ( - interval < match_threshold_ms_ * static_cast(1e6) && is_fused_.at(roi_i) == false) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // PROCESS: if matched, fuse the main message with the roi message + if (interval < match_threshold_ms_ * static_cast(1e6) && det2d.is_fused == false) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; return; } + if (debugger_) { debugger_->clear(); } - - fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); - is_fused_.at(roi_i) = true; + // PROCESS: fuse the main message with the roi message + fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); + det2d.is_fused = true; if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; + double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + timestamp_interval_ms - det2d.input_offset_ms); } - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; + // PROCESS: if all camera fused, postprocess and publish the main message + if (checkAllDet2dFused()) { + exportProcess(); + // reset flags + for (auto & status : det2d_list_) { + status.is_fused = false; } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); @@ -417,18 +436,11 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } -template -void FusionNode::postprocess(TargetMsg3D & output_msg - __attribute__((unused))) -{ - // do nothing by default -} - -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -436,29 +448,20 @@ void FusionNode::timer_callback() using std::chrono_literals::operator""ms; timer_->cancel(); if (mutex_cached_msgs_.try_lock()) { - // timeout, postprocess cached msg - if (cached_msg_.second != nullptr) { + // PROCESS: if timeout, postprocess cached msg + if (cached_det3d_msg_ptr_ != nullptr) { stop_watch_ptr_->toc("processing_time", true); + exportProcess(); + } - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; - } + // reset flags whether the message is fused or not + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; mutex_cached_msgs_.unlock(); } else { + // TIMING: retry the process after 10ms try { std::chrono::nanoseconds period = 10ms; setPeriod(period.count()); @@ -469,8 +472,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -486,8 +489,16 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const TargetMsg3D & output_msg) +template +void FusionNode::postprocess( + const Msg3D & processing_msg __attribute__((unused)), + ExportObj & output_msg __attribute__((unused))) +{ + // do nothing by default +} + +template +void FusionNode::publish(const ExportObj & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -495,10 +506,21 @@ void FusionNode::publish(const TargetMsg3D & output_msg pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; -template class FusionNode; -template class FusionNode; -template class FusionNode; +// Explicit instantiation for the supported types + +// pointpainting fusion +template class FusionNode; + +// roi cluster fusion +template class FusionNode; + +// roi detected-object fusion +template class FusionNode; + +// roi pointcloud fusion +template class FusionNode; + +// segment pointcloud fusion +template class FusionNode; + } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 2f6f853d4281e..bcd62383319c1 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -91,8 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "pointpainting_fusion", options) +: FusionNode("pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = @@ -156,11 +155,16 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - std::function sub_callback = + // subscriber + std::function sub_callback = std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); - sub_ = this->create_subscription( + sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); + // publisher + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -189,15 +193,13 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt diagnostics_interface_ptr_ = std::make_unique(this, "pointpainting_trt"); - obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); - if (this->declare_parameter("build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } } -void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -254,9 +256,9 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted } void PointPaintingFusionNode::fuseOnSingleImage( - __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) + __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, + const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, + PointCloudMsgType & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -288,10 +290,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } - // transform - // sensor_msgs::msg::PointCloud2 transformed_pointcloud; - // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - const auto x_offset = painted_pointcloud_msg.fields .at(static_cast(autoware::point_types::PointXYZIRCIndex::X)) .offset; @@ -337,13 +335,13 @@ dc | dc dc dc dc ||zc| p_y = point_camera.y(); p_z = point_camera.z(); - if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { + if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { continue; } // project Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(p_x, p_y, p_z), projected_point)) { // iterate 2d bbox for (const auto & feature_object : objects) { @@ -360,41 +358,45 @@ dc | dc dc dc dc ||zc| *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } } - } - if (debugger_) { - int thread_id = omp_get_thread_num(); - local_vectors[thread_id].push_back(projected_point); + if (debugger_) { + int thread_id = omp_get_thread_num(); + local_vectors[thread_id].push_back(projected_point); + } } } } - } - if (debugger_) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - for (const auto & feature_object : input_roi_msg.feature_objects) { - debug_image_rois.push_back(feature_object.feature.roi); - } + for (const auto & feature_object : input_roi_msg.feature_objects) { + debug_image_rois.push_back(feature_object.feature.roi); + } - for (const auto & local_vec : local_vectors) { - debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); - } + for (const auto & local_vec : local_vectors) { + debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); + } - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } } } -void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); diagnostics_interface_ptr_->clear(); + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects.clear(); + const auto objects_sub_count = - obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { return; } @@ -430,22 +432,18 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte raw_objects.emplace_back(obj); } - autoware_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = painted_pointcloud_msg.header; + // prepare output message output_msg.objects = iou_bev_nms_.apply(raw_objects); detection_class_remapper_.mapClasses(output_msg); - if (objects_sub_count > 0) { - obj_pub_ptr_->publish(output_msg); + // publish debug message: painted pointcloud + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(painted_pointcloud_msg); } diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp); } -bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 32db5319bb487..0a9d9e3391882 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -41,8 +41,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_cluster_fusion", options) +: FusionNode("roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); @@ -54,9 +53,12 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) remove_unknown_ = declare_parameter("remove_unknown"); fusion_distance_ = declare_parameter("fusion_distance"); trust_object_distance_ = declare_parameter("trust_object_distance"); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) +void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -71,36 +73,14 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste } } -void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - if (!remove_unknown_) { - return; - } - DetectedObjectsWithFeature known_objects; - known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); - for (auto & feature_object : output_cluster_msg.feature_objects) { - bool is_roi_label_known = feature_object.object.classification.front().label != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; - if ( - is_roi_label_known || - feature_object.object.existence_probability >= min_roi_existence_prob_) { - known_objects.feature_objects.push_back(feature_object); - } - } - output_cluster_msg.feature_objects = known_objects.feature_objects; -} - void RoiClusterFusionNode::fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & output_cluster_msg) + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -149,7 +129,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { const int px = static_cast(projected_point.x()); const int py = static_cast(projected_point.y()); @@ -168,7 +148,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; roi.x_offset = min_x; roi.y_offset = min_y; roi.width = max_x - min_x; @@ -231,7 +210,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } @@ -291,6 +270,28 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } +void RoiClusterFusionNode::postprocess( + const ClusterMsgType & processing_msg, ClusterMsgType & output_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg = processing_msg; + + if (remove_unknown_) { + // filter by object classification and existence probability + output_msg.feature_objects.clear(); + for (const auto & feature_object : processing_msg.feature_objects) { + if ( + feature_object.object.classification.front().label != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN || + feature_object.object.existence_probability >= min_roi_existence_prob_) { + output_msg.feature_objects.push_back(feature_object); + } + } + } +} + } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 7be18d59fdbf1..37769ad73e8c7 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -31,8 +31,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_detected_object_fusion", options) +: FusionNode("roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); @@ -48,6 +47,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio can_assign_vector.data(), label_num, label_num); fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); } + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) @@ -82,9 +84,8 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) } void RoiDetectedObjectFusionNode::fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - DetectedObjects & output_object_msg __attribute__((unused))) + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -101,7 +102,7 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } const auto object_roi_map = - generateDetectedObjectRoIs(input_object_msg, image_id, object2camera_affine); + generateDetectedObjectRoIs(input_object_msg, det2d, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -109,13 +110,13 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); } - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const std::size_t & image_id, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; @@ -131,7 +132,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( return object_roi_map; } const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); const double image_width = static_cast(camera_info.width); const double image_height = static_cast(camera_info.height); @@ -162,7 +163,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( } Eigen::Vector2d proj_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { const double px = proj_point.x(); const double py = proj_point.y(); @@ -289,14 +290,15 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return is_out; } -void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +void RoiDetectedObjectFusionNode::postprocess( + const DetectedObjects & processing_msg, DetectedObjects & output_msg) { - if (pub_ptr_->get_subscription_count() < 1) { - return; - } + output_msg.header = processing_msg.header; + output_msg.objects.clear(); - int64_t timestamp_nsec = - output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; + // filter out ignored objects + int64_t timestamp_nsec = processing_msg.header.stamp.sec * static_cast(1e9) + + processing_msg.header.stamp.nanosec; if ( passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { @@ -308,33 +310,34 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) ignored_object_flags_map_.count(timestamp_nsec) == 0) { return; } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); - auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); - - DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; - output_objects_msg.header = output_msg.header; - debug_fused_objects_msg.header = output_msg.header; - debug_ignored_objects_msg.header = output_msg.header; - for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); + if (passthrough_object_flags.at(obj_i) || fused_object_flags.at(obj_i)) { + output_msg.objects.emplace_back(obj); } + } + + // debug messages + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects debug_fused_objects_msg, debug_ignored_objects_msg; + debug_fused_objects_msg.header = processing_msg.header; + debug_ignored_objects_msg.header = processing_msg.header; + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); if (fused_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); } if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } - - pub_ptr_->publish(output_objects_msg); - debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + // clear flags passthrough_object_flags_map_.erase(timestamp_nsec); fused_object_flags_map_.erase(timestamp_nsec); ignored_object_flags_map_.erase(timestamp_nsec); diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 998072d87774e..b1eef00053946 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -37,58 +37,23 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_pointcloud_fusion", options) +: FusionNode("roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); - pub_objects_ptr_ = - this->create_publisher("output_clusters", rclcpp::QoS{1}); - cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); -} - -void RoiPointCloudFusionNode::preprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - return; + // publisher + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::postprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + - pub_objects_ptr_->get_intra_process_subscription_count(); - if (objects_sub_count < 1) { - return; - } - - DetectedObjectsWithFeature output_msg; - output_msg.header = pointcloud_msg.header; - output_msg.feature_objects = output_fused_objects_; - - if (objects_sub_count > 0) { - pub_objects_ptr_->publish(output_msg); - } - output_fused_objects_.clear(); - // publish debug cluster - if (cluster_debug_pub_->get_subscription_count() > 0) { - sensor_msgs::msg::PointCloud2 debug_cluster_msg; - autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); - cluster_debug_pub_->publish(debug_cluster_msg); - } -} void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, + __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -137,10 +102,10 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( const int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; - sensor_msgs::msg::PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); - std::vector clusters; + std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); for (auto & cluster : clusters) { @@ -162,7 +127,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); @@ -202,14 +167,40 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (debugger_) { debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } -bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const DetectedObjectWithFeature & obj) +void RoiPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + output_fused_objects_.clear(); + + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + PointCloudMsgType debug_cluster_msg; + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(pointcloud_msg); + } +} + +void RoiPointCloudFusionNode::publish(const ClusterMsgType & output_msg) +{ + const auto objects_sub_count = + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + pub_ptr_->publish(output_msg); } } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 486ae291abc6a..7d63cac7273c6 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -36,7 +36,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("segmentation_pointcloud_fusion", options) +: FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); for (auto & item : filter_semantic_label_target_list_) { @@ -48,47 +48,24 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio } is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); -} - -void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - return; + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) + PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - auto original_cloud = std::make_shared(pointcloud_msg); - - int point_step = original_cloud->point_step; - size_t output_pointcloud_size = 0; - pointcloud_msg.data.clear(); - pointcloud_msg.data.resize(original_cloud->data.size()); - - for (size_t global_offset = 0; global_offset < original_cloud->data.size(); - global_offset += point_step) { - if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { - copyPointCloud( - *original_cloud, point_step, global_offset, pointcloud_msg, output_pointcloud_size); - } - } - - pointcloud_msg.data.resize(output_pointcloud_size); - pointcloud_msg.row_step = output_pointcloud_size / pointcloud_msg.height; - pointcloud_msg.width = output_pointcloud_size / pointcloud_msg.point_step / pointcloud_msg.height; - - filter_global_offset_set_.clear(); return; } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + [[maybe_unused]] const Image & input_mask, + __attribute__((unused)) PointCloudMsgType & output_cloud) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -100,7 +77,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( return; } - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -128,7 +105,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( transform_stamped = transform_stamped_optional.value(); } - PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); int point_step = input_pointcloud_msg.point_step; @@ -150,7 +127,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (!camera_projectors_[image_id].calcImageProjectedPoint( + if (!det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } @@ -168,11 +145,33 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } -bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const PointCloud2 & filtered_cloud) +void SegmentPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.data.clear(); + output_msg.data.resize(pointcloud_msg.data.size()); + const int point_step = pointcloud_msg.point_step; + + size_t output_pointcloud_size = 0; + for (size_t global_offset = 0; global_offset < pointcloud_msg.data.size(); + global_offset += point_step) { + if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { + copyPointCloud(pointcloud_msg, point_step, global_offset, output_msg, output_pointcloud_size); + } + } + + output_msg.data.resize(output_pointcloud_size); + output_msg.row_step = output_pointcloud_size / output_msg.height; + output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height; + + filter_global_offset_set_.clear(); + return; } + } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 4456d25b18167..81424d4c23a34 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -67,8 +67,8 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) } void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster) + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster) { // sort point by distance to camera origin @@ -123,8 +123,8 @@ void closest_cluster( } void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects) @@ -162,7 +162,7 @@ void updateOutputFusedObjects( // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest // to output refine cluster and centroid - sensor_msgs::msg::PointCloud2 refine_cluster; + PointCloudMsgType refine_cluster; closest_cluster( cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame, refine_cluster); if ( @@ -184,7 +184,7 @@ void updateOutputFusedObjects( } } -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud) { geometry_msgs::msg::Point centroid; centroid.x = 0.0f; From 811f59d696d39c99e8731b70214f01bdcc64378e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 8 Jan 2025 18:39:53 +0900 Subject: [PATCH 42/45] docs(bpp): revise explanation for Failure modules (#9863) Signed-off-by: Zulfaqar Azmi --- .../docs/behavior_path_planner_manager_design.md | 4 ++-- .../autoware_behavior_path_planner/package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 1e8532eba2f3a..3b787303d8d20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -533,9 +533,9 @@ If this case happened in the slot, `is_upstream_waiting_approved` is set to true ### Failure modules -The failure modules return the status `ModuleStatus::FAILURE`. The manager removes the module from approved modules stack as well as waiting approval modules, but the failure module is not moved to candidate modules stack. +If a module returns `ModuleStatus::FAILURE`, the manager removes the failed module. Additionally, all modules after the failed module are removed, even if they did not return `ModuleStatus::FAILURE`. These modules are not added back to the candidate modules stack and will instead run again from the beginning. Once these modules are removed, the output of the module prior to the failed module will be used as the planner's output. -As a result, the module A's output is used as approved modules stack. +As shown in the example below, modules B, A, and C are running. When module A returns `ModuleStatus::FAILURE`, both module A and C are removed from the approved modules stack. Module B's output is then used as the final output of the planner. ![failure_modules](../image/manager/failure_modules.drawio.svg) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 7b7e2438a3fd6..f9c736b9d5dde 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -21,6 +21,7 @@ Kyoichi Sugahara Takayuki Murooka Go Sakayori + Mamoru Sobue Apache License 2.0 @@ -29,7 +30,6 @@ Fumiya Watanabe Zulfaqar Azmi Kosuke Takeuchi - Yutaka Shimizu Takayuki Murooka Ryohsuke Mitsudome From b5c008a2b6726458e7b2c304fce7b3e5c159cb5f Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Thu, 9 Jan 2025 08:01:43 +0800 Subject: [PATCH 43/45] feat(autoware_point_types): move autoware_point_types to autoware.core (#9864) Signed-off-by: liu cui --- common/autoware_point_types/CHANGELOG.rst | 150 -------------- common/autoware_point_types/CMakeLists.txt | 25 --- common/autoware_point_types/README.md | 1 - .../include/autoware/point_types/types.hpp | 188 ------------------ common/autoware_point_types/package.xml | 34 ---- .../test/test_point_types.cpp | 66 ------ 6 files changed, 464 deletions(-) delete mode 100644 common/autoware_point_types/CHANGELOG.rst delete mode 100644 common/autoware_point_types/CMakeLists.txt delete mode 100644 common/autoware_point_types/README.md delete mode 100644 common/autoware_point_types/include/autoware/point_types/types.hpp delete mode 100644 common/autoware_point_types/package.xml delete mode 100644 common/autoware_point_types/test/test_point_types.cpp diff --git a/common/autoware_point_types/CHANGELOG.rst b/common/autoware_point_types/CHANGELOG.rst deleted file mode 100644 index 5e26002677a36..0000000000000 --- a/common/autoware_point_types/CHANGELOG.rst +++ /dev/null @@ -1,150 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_point_types -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(autoware_point_types): prefix namespace with autoware::point_types (`#9169 `_) -* feat: migrating pointcloud types (`#6996 `_) - * feat: changed most of sensing to the new type - * chore: started applying changes to the perception stack - * feat: confirmed operation until centerpoint - * feat: reverted to the original implementation of pointpainting - * chore: forgot to push a header - * feat: also implemented the changes for the subsample filters that were out of scope before - * fix: some point type changes were missing from the latest merge from main - * chore: removed unused code, added comments, and brought back a removed publish - * chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers - * feat: added memory layout checks - * chore: updated documentation regarding the point types - * chore: added hyperlinks to the point definitions. will be valid only once the PR is merged - * fix: fixed compilation due to moving the utilities files to the base library - * chore: separated the utilities functions due to a dependency issue - * chore: forgot that perception also uses the filter class - * feature: adapted the undistortion tests to the new point type - --------- - Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> - Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> -* chore: updated maintainers for the autoware_point_types package (`#7797 `_) -* docs(common): adding .pages file (`#7148 `_) - * docs(common): adding .pages file - * fix naming - * fix naming - * fix naming - * include main page plus explanation to autoware tools - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Kenzo Lobos Tsunekawa, Yutaka Kondo, Zulfaqar Azmi - -0.26.0 (2024-04-03) -------------------- -* build: mark autoware_cmake as (`#3616 `_) - * build: mark autoware_cmake as - with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) - * style(pre-commit): autofix - * chore: fix pre-commit errors - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Kenji Miyake -* feat: isolate gtests in all packages (`#693 `_) -* chore: upgrade cmake_minimum_required to 3.14 (`#856 `_) -* refactor: use autoware cmake (`#849 `_) - * remove autoware_auto_cmake - * add build_depend of autoware_cmake - * use autoware_cmake in CMakeLists.txt - * fix bugs - * fix cmake lint errors -* feat: add blockage diagnostics (`#461 `_) - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * chore: rearrange header file - * chore: fix typo - * chore: rearrange header - * fix: revert accident change - * chore: fix typo - * docs: add limits - * chore: check overflow - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* ci: check include guard (`#438 `_) - * ci: check include guard - * apply pre-commit - * Update .pre-commit-config.yaml - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> - * fix: pre-commit - Co-authored-by: Kenji Miyake - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> -* feat: add point_types for wrapper (`#784 `_) (`#215 `_) - * add point_types - * Revert "add point_types" - This reverts commit 5810000cd1cbd876bc22372e2bb74ccaca06187b. - * create autoware_point_types pkg - * add include - * add cmath - * fix author - * fix bug - * define epsilon as argument - * add test - * remove unnamed namespace - * update test - * fix test name - * use std::max - * change comparison method - * remove unnencessary line - * fix test - * fix comparison method name - Co-authored-by: Taichi Higashide -* Contributors: Daisuke Nishimatsu, Kenji Miyake, Maxime CLEMENT, Takagi, Isamu, Vincent Richard, badai nguyen diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt deleted file mode 100644 index c149f79dab71f..0000000000000 --- a/common/autoware_point_types/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_point_types) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - include - SYSTEM - ${PCL_INCLUDE_DIRS} -) - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_autoware_point_types - test/test_point_types.cpp - ) - target_include_directories(test_autoware_point_types - PRIVATE include - ) - ament_target_dependencies(test_autoware_point_types - point_cloud_msg_wrapper - ) -endif() - -ament_auto_package() diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md deleted file mode 100644 index 92f19d2bc353a..0000000000000 --- a/common/autoware_point_types/README.md +++ /dev/null @@ -1 +0,0 @@ -# Autoware Point Types diff --git a/common/autoware_point_types/include/autoware/point_types/types.hpp b/common/autoware_point_types/include/autoware/point_types/types.hpp deleted file mode 100644 index 0fde62222e276..0000000000000 --- a/common/autoware_point_types/include/autoware/point_types/types.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__POINT_TYPES__TYPES_HPP_ -#define AUTOWARE__POINT_TYPES__TYPES_HPP_ - -#include - -#include - -#include -#include - -namespace autoware::point_types -{ -template -bool float_eq(const T a, const T b, const T eps = 10e-6) -{ - return std::fabs(a - b) < eps; -} - -struct PointXYZI -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity); - } -}; - -enum ReturnType : uint8_t { - INVALID = 0, - SINGLE_STRONGEST, - SINGLE_LAST, - DUAL_STRONGEST_FIRST, - DUAL_STRONGEST_LAST, - DUAL_WEAK_FIRST, - DUAL_WEAK_LAST, - DUAL_ONLY, -}; - -struct PointXYZIRC -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - - friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel; - } -}; - -struct PointXYZIRADRT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - uint16_t ring{0U}; - float azimuth{0.0F}; - float distance{0.0F}; - uint8_t return_type{0U}; - double time_stamp{0.0}; - friend bool operator==(const PointXYZIRADRT & p1, const PointXYZIRADRT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity) && - p1.ring == p2.ring && float_eq(p1.azimuth, p2.azimuth) && - float_eq(p1.distance, p2.distance) && p1.return_type == p2.return_type && - float_eq(p1.time_stamp, p2.time_stamp); - } -}; - -struct PointXYZIRCAEDT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - float azimuth{0.0F}; - float elevation{0.0F}; - float distance{0.0F}; - std::uint32_t time_stamp{0U}; - - friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel && - float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && - p1.time_stamp == p2.time_stamp; - } -}; - -enum class PointXYZIIndex { X, Y, Z, Intensity }; -enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; -enum class PointXYZIRADRTIndex { - X, - Y, - Z, - Intensity, - Ring, - Azimuth, - Distance, - ReturnType, - TimeStamp -}; -enum class PointXYZIRCAEDTIndex { - X, - Y, - Z, - Intensity, - ReturnType, - Channel, - Azimuth, - Elevation, - Distance, - TimeStamp -}; - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); - -using PointXYZIRCGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator>; - -using PointXYZIRADRTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, - field_return_type_generator, field_time_stamp_generator>; - -using PointXYZIRCAEDTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator, field_azimuth_generator, - field_elevation_generator, field_distance_generator, field_time_stamp_generator>; - -} // namespace autoware::point_types - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRC, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRADRT, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( - float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( - double, time_stamp, time_stamp)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRCAEDT, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, - return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( - float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) -#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml deleted file mode 100644 index e35e6a63de648..0000000000000 --- a/common/autoware_point_types/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_point_types - 0.40.0 - The point types definition to use point_cloud_msg_wrapper - David Wong - Max Schmeller - Apache License 2.0 - - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test - autoware_cmake - - ament_cmake_core - ament_cmake_test - - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_lint_cmake - ament_cmake_xmllint - pcl_ros - point_cloud_msg_wrapper - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - point_cloud_msg_wrapper - - - ament_cmake - - diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp deleted file mode 100644 index 08696a9948f60..0000000000000 --- a/common/autoware_point_types/test/test_point_types.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/point_types/types.hpp" - -#include - -#include - -TEST(PointEquality, PointXYZI) -{ - using autoware::point_types::PointXYZI; - - PointXYZI pt0{0, 1, 2, 3}; - PointXYZI pt1{0, 1, 2, 3}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRADRT) -{ - using autoware::point_types::PointXYZIRADRT; - - PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8}; - PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRCAEDT) -{ - using autoware::point_types::PointXYZIRCAEDT; - - PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, FloatEq) -{ - // test template - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - - // test floating point error - EXPECT_TRUE(autoware::point_types::float_eq(1, 1 + std::numeric_limits::epsilon())); - - // test difference of sign - EXPECT_FALSE(autoware::point_types::float_eq(2, -2)); - EXPECT_FALSE(autoware::point_types::float_eq(-2, 2)); - - // small value difference - EXPECT_FALSE(autoware::point_types::float_eq(2, 2 + 10e-6)); - - // expect same value if epsilon is larger than difference - EXPECT_TRUE(autoware::point_types::float_eq(2, 2 + 10e-6, 10e-5)); -} From fd23b61e8c81ef22deacc4ef29970bf1fe460629 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 9 Jan 2025 09:10:40 +0900 Subject: [PATCH 44/45] fix(image_projection_based_fusion): remove mutex (#9862) refactor: Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 2 -- .../src/fusion_node.cpp | 30 +++++-------------- 2 files changed, 8 insertions(+), 24 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index b8881bc6abff7..e05339771f667 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include @@ -127,7 +126,6 @@ class FusionNode : public rclcpp::Node // cache for fusion int64_t cached_det3d_msg_timestamp_; typename Msg3D::SharedPtr cached_det3d_msg_ptr_; - std::mutex mutex_cached_msgs_; protected: void setDet2DStatus(std::size_t rois_number); diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 310c68f12db88..700d249831000 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -280,8 +280,6 @@ void FusionNode::subCallback( } } - std::lock_guard lock(mutex_cached_msgs_); - // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); @@ -447,28 +445,16 @@ void FusionNode::timer_callback() using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_cached_msgs_.try_lock()) { - // PROCESS: if timeout, postprocess cached msg - if (cached_det3d_msg_ptr_ != nullptr) { - stop_watch_ptr_->toc("processing_time", true); - exportProcess(); - } - // reset flags whether the message is fused or not - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } + // PROCESS: if timeout, postprocess cached msg + if (cached_det3d_msg_ptr_ != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + exportProcess(); + } - mutex_cached_msgs_.unlock(); - } else { - // TIMING: retry the process after 10ms - try { - std::chrono::nanoseconds period = 10ms; - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); + // reset flags whether the message is fused or not + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } From 51818f00ec4775510386c7757065bba84474a0a1 Mon Sep 17 00:00:00 2001 From: Vishal Chauhan <40782713+vish0012@users.noreply.github.com> Date: Thu, 9 Jan 2025 13:30:05 +0900 Subject: [PATCH 45/45] feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files localization/autoware_twist2accel (#9868) Signed-off-by: vish0012 Co-authored-by: SakodaShintaro --- localization/autoware_twist2accel/package.xml | 2 +- localization/autoware_twist2accel/src/twist2accel.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml index 75cea94546516..013ef54fe52d1 100644 --- a/localization/autoware_twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -17,13 +17,13 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_signal_processing geometry_msgs nav_msgs rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp index d338b256fec77..d142328f9d498 100644 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -19,11 +19,11 @@ #include +#include #include #include #include #include -#include #include #include