From 8596ff001153d5d1c63762d943f20b192de7c449 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 16 Sep 2023 19:16:30 +0300 Subject: [PATCH 1/7] remove env_manager for refactoring --- .../camera_component/CMakeLists.txt | 68 --- env_components/camera_component/package.xml | 30 -- .../src/ign_camera_component.cpp | 42 -- .../component_interfaces/CMakeLists.txt | 16 - .../component_interfaces/package.xml | 20 - .../component_interfaces/srv/DetectObject.srv | 10 - env_components/scene_component/CMakeLists.txt | 118 ----- env_components/scene_component/package.xml | 31 -- .../src/ign_detect_object_component.cpp | 109 ----- .../src/ign_tf2_broadcaster_component.cpp | 72 --- .../src/moveit_scene_component.cpp | 115 ----- env_manager/CMakeLists.txt | 144 ------ env_manager/LICENSE | 202 -------- env_manager/README.md | 149 ------ env_manager/config/config.lua | 19 - .../config/environments/ground_true.lua | 14 - env_manager/config/environments/simulator.lua | 19 - env_manager/config/nodes.lua | 12 - env_manager/config/utils/utils.lua | 9 - env_manager/docs/assets/env_manager.png | Bin 180593 -> 0 bytes .../component_manager/client_component.hpp | 61 --- .../component_manager/component_manager.hpp | 50 -- .../component_manager/component_template.hpp | 0 .../component_manager/node_component.hpp | 36 -- .../component_manager/publisher_component.hpp | 46 -- .../component_manager/service_component.hpp | 39 -- .../subscriber_component.hpp | 44 -- .../component_manager/visibility_control.h | 35 -- env_manager/include/config/config.hpp.cmake | 15 - .../include/config/config_file_resolver.hpp | 28 -- .../include/config/lua_file_resolver.hpp | 129 ----- env_manager/include/config/options.hpp | 58 --- .../include/env_manager/environment_base.hpp | 0 .../env_manager/environment_manager.hpp | 0 .../env_manager/environment_node_manager.hpp | 30 -- env_manager/package.xml | 21 - .../component_manager/component_manager.cpp | 159 ------- .../src/config/config_file_resolver.cpp | 51 -- env_manager/src/config/lua_file_resolver.cpp | 446 ------------------ env_manager/src/config/options.cpp | 83 ---- .../env_manager/environment_node_manager.cpp | 6 - env_manager/src/main.cpp | 60 --- 42 files changed, 2596 deletions(-) delete mode 100644 env_components/camera_component/CMakeLists.txt delete mode 100644 env_components/camera_component/package.xml delete mode 100644 env_components/camera_component/src/ign_camera_component.cpp delete mode 100644 env_components/component_interfaces/CMakeLists.txt delete mode 100644 env_components/component_interfaces/package.xml delete mode 100644 env_components/component_interfaces/srv/DetectObject.srv delete mode 100644 env_components/scene_component/CMakeLists.txt delete mode 100644 env_components/scene_component/package.xml delete mode 100644 env_components/scene_component/src/ign_detect_object_component.cpp delete mode 100644 env_components/scene_component/src/ign_tf2_broadcaster_component.cpp delete mode 100644 env_components/scene_component/src/moveit_scene_component.cpp delete mode 100644 env_manager/CMakeLists.txt delete mode 100644 env_manager/LICENSE delete mode 100644 env_manager/README.md delete mode 100644 env_manager/config/config.lua delete mode 100644 env_manager/config/environments/ground_true.lua delete mode 100644 env_manager/config/environments/simulator.lua delete mode 100644 env_manager/config/nodes.lua delete mode 100644 env_manager/config/utils/utils.lua delete mode 100644 env_manager/docs/assets/env_manager.png delete mode 100644 env_manager/include/component_manager/client_component.hpp delete mode 100644 env_manager/include/component_manager/component_manager.hpp delete mode 100644 env_manager/include/component_manager/component_template.hpp delete mode 100644 env_manager/include/component_manager/node_component.hpp delete mode 100644 env_manager/include/component_manager/publisher_component.hpp delete mode 100644 env_manager/include/component_manager/service_component.hpp delete mode 100644 env_manager/include/component_manager/subscriber_component.hpp delete mode 100644 env_manager/include/component_manager/visibility_control.h delete mode 100644 env_manager/include/config/config.hpp.cmake delete mode 100644 env_manager/include/config/config_file_resolver.hpp delete mode 100644 env_manager/include/config/lua_file_resolver.hpp delete mode 100644 env_manager/include/config/options.hpp delete mode 100644 env_manager/include/env_manager/environment_base.hpp delete mode 100644 env_manager/include/env_manager/environment_manager.hpp delete mode 100644 env_manager/include/env_manager/environment_node_manager.hpp delete mode 100644 env_manager/package.xml delete mode 100644 env_manager/src/component_manager/component_manager.cpp delete mode 100644 env_manager/src/config/config_file_resolver.cpp delete mode 100644 env_manager/src/config/lua_file_resolver.cpp delete mode 100644 env_manager/src/config/options.cpp delete mode 100644 env_manager/src/env_manager/environment_node_manager.cpp delete mode 100644 env_manager/src/main.cpp diff --git a/env_components/camera_component/CMakeLists.txt b/env_components/camera_component/CMakeLists.txt deleted file mode 100644 index 13e27e7..0000000 --- a/env_components/camera_component/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(camera_component) - -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(image_transport REQUIRED) -find_package(ros_gz_bridge REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(env_manager REQUIRED) - -find_package(ignition-transport11 REQUIRED) -set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - -find_package(ignition-msgs8 REQUIRED) -set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) - -set(GZ_TARGET_PREFIX ignition) - -add_library(ign_camera_component SHARED - src/ign_camera_component.cpp) -target_compile_definitions(ign_camera_component - PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(ign_camera_component - ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core - ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core - ) -ament_target_dependencies(ign_camera_component - "image_transport" - "ros_gz_bridge" - "rclcpp" - "rclcpp_components" - "sensor_msgs" - "env_manager") -rclcpp_components_register_nodes(ign_camera_component "camera_component::IgnCameraComponent") -set(node_plugins "${node_plugins}camera_component::IgnCameraComponent;$\n") - -install(TARGETS ign_camera_component - DESTINATION lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/env_components/camera_component/package.xml b/env_components/camera_component/package.xml deleted file mode 100644 index 5f37451..0000000 --- a/env_components/camera_component/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - camera_component - 0.0.0 - TODO: Package description - splinter1984 - TODO: License declaration - - ament_cmake - pkg-config - - image_transport - ros_gz_bridge - rclcpp - rclcpp_components - sensor_msgs - env_manager - - ignition-msgs8 - ignition-transport11 - ignition-msgs8 - ignition-transport11 - - ament_lint_auto - ament_lint_common - - ament_cmake - - diff --git a/env_components/camera_component/src/ign_camera_component.cpp b/env_components/camera_component/src/ign_camera_component.cpp deleted file mode 100644 index bbf7d69..0000000 --- a/env_components/camera_component/src/ign_camera_component.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -namespace camera_component -{ - -class IgnCameraComponent: public env_manager::component_manager::PublisherComponent -{ -public: - explicit IgnCameraComponent(const rclcpp::NodeOptions &options) - : env_manager::component_manager::PublisherComponent(options) - { - auto topic_name = "/camera"; - _ign_node = std::make_shared(); - _ign_node->Subscribe(topic_name, &IgnCameraComponent::on_image, this); - } - -private: - void on_image(const ignition::msgs::Image& img) - { - sensor_msgs::msg::Image rimg; - ros_gz_bridge::convert_gz_to_ros(img, rimg); - populate_publication(rimg); - } - - std::shared_ptr _ign_node; -}; -} // namespace pub_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(camera_component::IgnCameraComponent) diff --git a/env_components/component_interfaces/CMakeLists.txt b/env_components/component_interfaces/CMakeLists.txt deleted file mode 100644 index 4ad69bf..0000000 --- a/env_components/component_interfaces/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(component_interfaces) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/DetectObject.srv" - DEPENDENCIES std_msgs) - -ament_package() diff --git a/env_components/component_interfaces/package.xml b/env_components/component_interfaces/package.xml deleted file mode 100644 index b9b61c0..0000000 --- a/env_components/component_interfaces/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - component_interfaces - 0.0.0 - TODO: Package description - splinter1984 - Apache-2.0 - - ament_cmake - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - std_msgs - - - ament_cmake - - diff --git a/env_components/component_interfaces/srv/DetectObject.srv b/env_components/component_interfaces/srv/DetectObject.srv deleted file mode 100644 index cab64c2..0000000 --- a/env_components/component_interfaces/srv/DetectObject.srv +++ /dev/null @@ -1,10 +0,0 @@ -uint8 DETECT=0 -uint8 FOLLOW=1 -uint8 CANCEL=2 - -string object_name -uint8 req_kind ---- - -bool call_status -string error_msg diff --git a/env_components/scene_component/CMakeLists.txt b/env_components/scene_component/CMakeLists.txt deleted file mode 100644 index e756164..0000000 --- a/env_components/scene_component/CMakeLists.txt +++ /dev/null @@ -1,118 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(scene_component) - -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(image_transport REQUIRED) -find_package(ros_gz_bridge REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(env_manager REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(moveit_core REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(moveit_ros_planning REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) -find_package(component_interfaces REQUIRED) - -find_package(ignition-transport11 REQUIRED) -set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - -find_package(ignition-msgs8 REQUIRED) -set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) - -set(GZ_TARGET_PREFIX ignition) - -message(STATUS "Compiling against Ignition Fortress") - - -add_library(ign_tf2_broadcaster_component SHARED - src/ign_tf2_broadcaster_component.cpp) -target_compile_definitions(ign_tf2_broadcaster_component - PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(ign_tf2_broadcaster_component - ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core - ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core - ) -ament_target_dependencies(ign_tf2_broadcaster_component - "ros_gz_bridge" - "rclcpp" - "rclcpp_components" - "env_manager" - "geometry_msgs" - "tf2_ros") -rclcpp_components_register_nodes(ign_tf2_broadcaster_component "scene_component::IgnTf2Broadcaster") -set(node_plugins "${node_plugins}scene_component::IgnTf2Broadcaster;$\n") - -add_library(ign_detect_object_component SHARED - src/ign_detect_object_component.cpp) -target_compile_definitions(ign_detect_object_component - PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(ign_detect_object_component - ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core - ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core - ) -ament_target_dependencies(ign_detect_object_component - "ros_gz_bridge" - "rclcpp" - "rclcpp_components" - "env_manager" - "geometry_msgs" - "tf2_ros" - "component_interfaces") - rclcpp_components_register_nodes(ign_detect_object_component "scene_component::IgnDetectOpjectService") - set(node_plugins "${node_plugins}scene_component::IgnDetectObjectService;$\n") - -add_library(moveit_scene_component SHARED - src/moveit_scene_component.cpp) -target_compile_definitions(moveit_scene_component - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(moveit_scene_component - "rclcpp" - "rclcpp_components" - "env_manager" - "geometry_msgs" - "tf2_ros" - "moveit_core" - "moveit_msgs" - "moveit_ros_planning" - "moveit_ros_planning_interface") -rclcpp_components_register_nodes(moveit_scene_component "scene_component::MoveitSceneComponent") -set(node_plugins "${node_plugins}scene_component::MoveitSceneComponent;$\n") - -install( - TARGETS - ign_detect_object_component - ign_tf2_broadcaster_component - moveit_scene_component - DESTINATION - lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/env_components/scene_component/package.xml b/env_components/scene_component/package.xml deleted file mode 100644 index 1dd44bd..0000000 --- a/env_components/scene_component/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - scene_component - 0.0.0 - TODO: Package description - splinter1984 - TODO: License declaration - - ament_cmake - pkg-config - - image_transport - ros_gz_bridge - rclcpp - rclcpp_components - geometry_msgs - env_manager - component_interfaces - - ignition-msgs8 - ignition-transport11 - ignition-msgs8 - ignition-transport11 - - ament_lint_auto - ament_lint_common - - ament_cmake - - diff --git a/env_components/scene_component/src/ign_detect_object_component.cpp b/env_components/scene_component/src/ign_detect_object_component.cpp deleted file mode 100644 index bf908d1..0000000 --- a/env_components/scene_component/src/ign_detect_object_component.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "component_manager/service_component.hpp" -#include "component_interfaces/srv/detect_object.hpp" - -#include -#include -#include -#include - -#include -#include - -#define IGN_DEFAULT_BASE_FRAME "world" -#define IGN_DEFAULT_WORLD_NAME "mir" - -namespace scene_component -{ - -class IgnDetectObjectService: public - env_manager::component_manager::ServiceComponent -{ - public: - IgnDetectObjectService(const rclcpp::NodeOptions &options) - : env_manager::component_manager::ServiceComponent(options) - { - std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info"; - _tf2_broadcaster = std::make_unique(this); - _ign_node = std::make_shared(); - _ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this); - } - - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto kind = static_cast(request->req_kind); - bool status = false; - switch(kind) - { - case DETECT: { - (void) kind; - break; - } case FOLLOW: { - auto ok = std::find(_follow_frames.begin(), _follow_frames.end(), - request->object_name) == _follow_frames.end(); - if (ok) - _follow_frames.push_back(request->object_name); - status = ok; - break; - } case CANCEL: { - auto it = std::find(_follow_frames.begin(), _follow_frames.end(), - request->object_name); - auto ok = it != _follow_frames.end(); - if (ok) - _follow_frames.erase(it); - status = ok; - break; - } - } - response->call_status = status; - } - - private: - enum DetectObjectReqKind - { - DETECT=0, - FOLLOW=1, - CANCEL=2 - }; - - void on_pose(const ignition::msgs::Pose_V &pose_v) - { - auto poses = pose_v.pose(); - for (const auto& obj: _follow_frames) - { - auto it = std::find_if(poses.begin(), poses.end(), - [&obj](const auto& pose){ return pose.name() == obj;}); - if (it == poses.end()) - continue; - - geometry_msgs::msg::PoseStamped rpose; - ros_gz_bridge::convert_gz_to_ros(*it, rpose); - geometry_msgs::msg::TransformStamped t; - - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = IGN_DEFAULT_BASE_FRAME; - t.child_frame_id = it->name(); - t.transform.translation.x = rpose.pose.position.x; - t.transform.translation.y = rpose.pose.position.y; - t.transform.translation.z = rpose.pose.position.z; - t.transform.rotation.x = rpose.pose.orientation.x; - t.transform.rotation.y = rpose.pose.orientation.y; - t.transform.rotation.z = rpose.pose.orientation.z; - t.transform.rotation.w = rpose.pose.orientation.w; - - _tf2_broadcaster->sendTransform(t); - } - } - - std::vector _follow_frames; - std::unique_ptr _tf2_broadcaster; - std::shared_ptr _ign_node; - -}; - -} // namespace scene_component - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService) diff --git a/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp b/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp deleted file mode 100644 index bdd930f..0000000 --- a/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "component_manager/node_component.hpp" - -#include -#include -#include -#include - -#include -#include - -namespace scene_component -{ - -const std::string DEFAULT_NODE_NAME = "ign_tf2_broadcaster"; -const std::string DEFAULT_IGN_WORLD_NAME = "mir"; -const std::string DEFAULT_BASE_FRAME_NAME = "world"; - -class IgnTf2Broadcaster: public env_manager::component_manager::NodeComponent -{ -public: - IgnTf2Broadcaster(const rclcpp::NodeOptions & options) - : env_manager::component_manager::NodeComponent(options) - { - _follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"}; - std::string topic_name = "/world/" + DEFAULT_IGN_WORLD_NAME + "/dynamic_pose/info"; - // init broadcaster - _tf2_broadcaster = std::make_unique(*this); - _ign_node = std::make_shared(); - _ign_node->Subscribe(topic_name, &IgnTf2Broadcaster::on_pose, this); - } - -private: - void on_pose(const ignition::msgs::Pose_V& pose_v) - { - for (const auto &it: pose_v.pose()) - { - if (std::find(_follow_frames.begin(), _follow_frames.end(), it.name()) == _follow_frames.end()) - continue; - - geometry_msgs::msg::PoseStamped rpose; - ros_gz_bridge::convert_gz_to_ros(it, rpose); - - geometry_msgs::msg::TransformStamped t; - - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = DEFAULT_BASE_FRAME_NAME; - t.child_frame_id = it.name(); - - t.transform.translation.x = rpose.pose.position.x; - t.transform.translation.y = rpose.pose.position.y; - t.transform.translation.z = rpose.pose.position.z; - - t.transform.rotation.x = rpose.pose.orientation.x; - t.transform.rotation.y = rpose.pose.orientation.y; - t.transform.rotation.z = rpose.pose.orientation.z; - t.transform.rotation.w = rpose.pose.orientation.w; - - RCLCPP_ERROR(get_logger(), "Provide transform for frame: %s", it.name().c_str()); - - _tf2_broadcaster->sendTransform(t); - } - } - - std::unique_ptr _tf2_broadcaster; - std::shared_ptr _ign_node; - std::vector _follow_frames; - -}; -} // namespace scene_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnTf2Broadcaster) diff --git a/env_components/scene_component/src/moveit_scene_component.cpp b/env_components/scene_component/src/moveit_scene_component.cpp deleted file mode 100644 index 44c3d1f..0000000 --- a/env_components/scene_component/src/moveit_scene_component.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include - -#include - -#include -#include - -#include - -#include -#include -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "tf2/exceptions.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" - -using namespace std::chrono_literals; - -namespace scene_component -{ - -const std::string DEFAULT_PLANNING_GOUP_NAME = "panda_arm"; -const std::string DEFAULT_SUBNODE_NS = "moveit_cpp_node"; -const std::string DEFAULT_BASE_FRAME = "world"; - -class MoveitSceneComponent: public env_manager::component_manager::NodeComponent -{ -public: - MoveitSceneComponent(const rclcpp::NodeOptions& options) - : env_manager::component_manager::NodeComponent(options) - { - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - - node_ = rclcpp::Node::make_shared(DEFAULT_SUBNODE_NS, "", node_options); - - moveit_cpp_ptr_ = std::make_shared(node_); - moveit_cpp_ptr_->getPlanningSceneMonitor()->providePlanningSceneService(); - planning_components_ = std::make_shared( - DEFAULT_PLANNING_GOUP_NAME, moveit_cpp_ptr_); - robot_model_ptr_ = moveit_cpp_ptr_->getRobotModel(); - - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - timer_ = this->create_wall_timer(1s, std::bind(&MoveitSceneComponent::on_timer, this)); - } - -private: - void on_timer() - { - auto available_frames = tf_buffer_->getAllFrameNames(); - auto robot_link_names = robot_model_ptr_->getLinkModelNames(); - - for (const auto &frame: available_frames) - { - if (std::find(robot_link_names.begin(), robot_link_names.end(), frame) != robot_link_names.end()) - continue; - - geometry_msgs::msg::TransformStamped t; - try { - t = tf_buffer_->lookupTransform( - DEFAULT_BASE_FRAME, frame, - tf2::TimePointZero); - } catch (const tf2::TransformException &ex) - { - RCLCPP_WARN(get_logger(), "Could not transform %s to %s: %s", - frame.c_str(), DEFAULT_BASE_FRAME.c_str(), ex.what()); - } - - moveit_msgs::msg::CollisionObject co; - co.header.frame_id = robot_model_ptr_->getRootLink()->getName(); - co.id = frame; - - //TODO: replace with stl model - shape_msgs::msg::SolidPrimitive obj; - obj.type = obj.BOX; - obj.dimensions = {0.1, 0.1, 0.1}; - - geometry_msgs::msg::Pose obj_pose; - obj_pose.position.x = t.transform.translation.x; - obj_pose.position.y = t.transform.translation.y; - obj_pose.position.z = t.transform.translation.z; - - obj_pose.orientation.x = t.transform.rotation.x; - obj_pose.orientation.y = t.transform.rotation.y; - obj_pose.orientation.z = t.transform.rotation.z; - obj_pose.orientation.w = t.transform.rotation.w; - - co.primitives.push_back(obj); - co.primitive_poses.push_back(obj_pose); - co.operation = co.ADD; - - { - planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr_->getPlanningSceneMonitor()); - scene->processCollisionObjectMsg(co); - } - } - } - - rclcpp::Node::SharedPtr node_; - std::shared_ptr moveit_cpp_ptr_; - moveit::core::RobotModelConstPtr robot_model_ptr_; - std::shared_ptr planning_components_; - std::shared_ptr tf_listener_{nullptr}; - std::unique_ptr tf_buffer_; - rclcpp::TimerBase::SharedPtr timer_{nullptr}; - - -}; -} // namesapce scene_component - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::MoveitSceneComponent) diff --git a/env_manager/CMakeLists.txt b/env_manager/CMakeLists.txt deleted file mode 100644 index fa98e56..0000000 --- a/env_manager/CMakeLists.txt +++ /dev/null @@ -1,144 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(env_manager) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY ${PROJECT_SOURCE_DIR}/config - CACHE PATH ".lua configuration files dir") - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(Boost REQUIRED) -find_package(rcl REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rcutils REQUIRED) -find_package(glog 0.4.0 REQUIRED) -find_package(Lua 5.3 REQUIRED) - -add_library(env_manager SHARED - src/component_manager/component_manager.cpp - src/config/config_file_resolver.cpp - src/config/lua_file_resolver.cpp - src/config/options.cpp - ) - -configure_file( - ${PROJECT_SOURCE_DIR}/include/config/config.hpp.cmake - ${PROJECT_BINARY_DIR}/include/config/config.hpp) - -target_include_directories(env_manager PUBLIC - $ - $) -ament_target_dependencies( - env_manager - Boost - rcl - glog - Lua - class_loader - rclcpp - rclcpp_components -) - -target_include_directories(env_manager PUBLIC - $) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(env_manager PRIVATE "COMPONENT_MANAGER_BUILDING_LIBRARY") - -if(NOT WIN32) - ament_environment_hooks( - "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") -endif() - -add_executable(env_manager_node - src/main.cpp - ) -target_link_libraries(env_manager_node env_manager lua glog) -ament_target_dependencies( - env_manager_node - Boost - rcl - rclcpp - glog - Lua -) - -install(DIRECTORY config DESTINATION config) - -install( - DIRECTORY include/ - DESTINATION include -) -install( - TARGETS env_manager - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS env_manager_node - DESTINATION lib/${PROJECT_NAME} - ) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - set(ament_cmake_cppcheck_FOUND TRUE) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_pep257_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - set(ament_cmake_xmllint_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - - - #include_directories(test) - #ament_add_gtest(env_manager_test test/general.cpp) - #target_link_libraries(env_manager_test env_manager lua glog) - - #set(TEST_TEST_FILES_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_files) - #set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) - - #install(DIRECTORY - # ${TEST_TEST_FILES_DIR} - # DESTINATION share/${PROJECT_NAME} - #) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - env_manager -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_export_dependencies(rcutils) - -ament_package() diff --git a/env_manager/LICENSE b/env_manager/LICENSE deleted file mode 100644 index 2431aec..0000000 --- a/env_manager/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright 2022 Robossembler - - 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. - diff --git a/env_manager/README.md b/env_manager/README.md deleted file mode 100644 index 17bff87..0000000 --- a/env_manager/README.md +++ /dev/null @@ -1,149 +0,0 @@ -## Environment Manager -Environment Manager is a package that allow you to create custom environments based on *component*, switch between different environments, synchronization of different environments with each other. - -![](docs/assets/env_manager.png) - -### Getting Started -To get a local copy up and running follow these simple example steps. - -#### Prerequisites -```bash -# install glog -sudo apt-get install libgoogle-glog-dev - -#install lua -wget http://www.lua.org/ftp/lua-5.3.1.tar.gz -tar zxf lua-5.3.1.tar.gz -sudo make -C lua-5.3.1 linux && sudo make -C lua-5.3.1 install -``` -### Config -Configuration is done by lua. -```lua - --- require configuration table -ENV_MANAGER = { - -- dictionary of environments - environments = { - -- environment to upload with manager - example_environment = { - -- namespace in which all components will be - namespace = "", - -- dictionary of components needed to upload for current environment - components = { - -- node match one of node in ENV_MANAGER.nodes - example_node = { - -- example libpub_component.so - lib = "" - -- name of class which implement example_node logic - class = "" - } - }, - }, - }, - -- dictionary of nodes, that will be created by env_manager - nodes = { - -- node for initialization by env_manger - example_node = { - -- node name - name = "", - -- one node can produce only one topic/service - type = "", - -- example "std_msgs/String", - msg_type = "", - }, - }, -} - --- return configuration table -return ENV_MANAGER -``` -You can safely use all the feature of `lua` to write configuration. -### Usage -Create component library. -```c++ -#include "component_manager/publisher_component.hpp" -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include - -namespace pub_component -{ -using namespace std::chrono_literals; - -class Publisher: public env_manager::component_manager::PublisherComponent -{ -public: - explicit Publisher(const rclcpp::NodeOptions &options) - : env_manager::component_manager::PublisherComponent(options) - { - timer_ = create_wall_timer(1s, std::bind(&Publisher::on_timer, this)); - } - -protected: - void on_timer() - { - auto msg = std::make_unique(); - msg->data = "Hello World!"; - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str()); - std::flush(std::cout); - - this->populate_publication(*msg.get()); - } - - -private: - rclcpp::TimerBase::SharedPtr timer_; -}; -} // namespace pub_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(pub_component::Publisher) -``` -```cmake -add_library(pub_component SHARED - src/pub_component.cpp) -target_compile_definitions(pub_component - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(pub_component - "rclcpp" - "env_manager" - "rclcpp_components" - "std_msgs") -rclcpp_components_register_nodes(pub_component "pub_component::Publisher") -set(node_plugins "${node_plugins}pub_component::Publisher;$\n") -``` -Add component_library to your configuration file. -```lua -ENV_MANAGER = { - environments = { - example_environment = { - namespace = "", - components = { - example_node = { - lib = "libpub_component.so" - class = "pub_component::Publisher" - } - }, - }, - }, - nodes = { - example_node = { - name = "", - type = "Publisher", - msg_type = "std_msgs/String", - }, - }, -} -return ENV_MANAGER -``` -Start env_manager -```bash -source install/setup.bash -ros2 run env_manager env_manager_node -``` -### License -Distributed under the Apache 2.0 License. See LICENSE for more information. diff --git a/env_manager/config/config.lua b/env_manager/config/config.lua deleted file mode 100644 index a778f16..0000000 --- a/env_manager/config/config.lua +++ /dev/null @@ -1,19 +0,0 @@ --- env_manager configuraiton file -include "nodes.lua" -include "utils/utils.lua" - --- include environments configs -include "environments/simulator.lua" -include "environments/ground_true.lua" - --- env_manager configuration -ENV_MANAGER = { - environments = { - GROUND_TRUE - }, - nodes = NODES -} - -check_nodes(ENV_MANAGER) - -return ENV_MANAGER diff --git a/env_manager/config/environments/ground_true.lua b/env_manager/config/environments/ground_true.lua deleted file mode 100644 index 86da26e..0000000 --- a/env_manager/config/environments/ground_true.lua +++ /dev/null @@ -1,14 +0,0 @@ --- environment configuraiton -GROUND_TRUE = { - namespace = "ground_true", - components = { - camera_node = { - lib = "libign_camera_component.so", - class = "camera_component::IgnCameraComponent" - }, - scene_component = { - lib = "libign_detect_object_component.so", - class = "scene_component::IgnDetectObjectService" - }, - } -} diff --git a/env_manager/config/environments/simulator.lua b/env_manager/config/environments/simulator.lua deleted file mode 100644 index 3e36699..0000000 --- a/env_manager/config/environments/simulator.lua +++ /dev/null @@ -1,19 +0,0 @@ --- environment configuration -SIMULATOR = { - namespace = "simulator_env", - components = { - --[[ - talker_node = { - lib = "libpub_component.so", - class = "pub_component::Publisher", - }, - service_node = { - lib = "libsrv_component.so", - class = "srv_component::Service" - },]]-- - camera_node = { - lib = "libign_camera_component.so", - class = "camera_component::IgnCameraComponent" - }, - } -} diff --git a/env_manager/config/nodes.lua b/env_manager/config/nodes.lua deleted file mode 100644 index 9ab33b8..0000000 --- a/env_manager/config/nodes.lua +++ /dev/null @@ -1,12 +0,0 @@ -NODES = { - camera_node = { - name = "camera_node", - type = "Publisher", - msg_type = "sensor_msgs/Image" - }, - scene_component = { - name = "scene_component", - type = "Service", - msg_type = "component_interfaces/DetectObject" - }, -} diff --git a/env_manager/config/utils/utils.lua b/env_manager/config/utils/utils.lua deleted file mode 100644 index ff746d1..0000000 --- a/env_manager/config/utils/utils.lua +++ /dev/null @@ -1,9 +0,0 @@ -function check_nodes(config) - for env, cfg in pairs(config.environments) do - for comp, opt in pairs(cfg.components) do - assert(config.nodes[comp] ~= nil, "not all nodes presented.") - assert(opt.lib ~= nil, "not library provided.") - assert(opt.class ~= nil, "not class provided.") - end - end -end diff --git a/env_manager/docs/assets/env_manager.png b/env_manager/docs/assets/env_manager.png deleted file mode 100644 index 834d49238a76faf2a347d13b429b877532cf060c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 180593 zcmeAS@N?(olHy`uVBq!ia0y~yU>9LvVBN*R#=yXEbx!zp1_lPk;vjb?hIQv;UNSH! zFnGE+hE&A8xywE!HFT!ggZCxM2iY#Nads{0P*z$P7~r6y>Bt_iM$524Ny5Q(ib2;5 zP9?{Sk|C3tIvPS#8A?<<1XQgW7A^1)2*^m;_jT5+KK^hZu7#kPmH)GchQqaKA zXvoUaWygotB9^XwSRDW{fsu(tN<3iUjU6-P@VU0i&0)a}}Mg~Tvy?Oy6M~{Afe%`*YqT)wS-1Zk!zH*CLr!GG;8A~1C$jQya z!=qze|L;$7_o5rchvwRNfTA&2w;(cd8ap39f4u2c{crn@v9NUYR#wi!W`vpei`TEC z*K2gGsfyfvc6nA6sG}($m>T)c&c>!jtLK}jfX24>_hv|9>o@Xp@$m4hnfbkVYUpcC+BVsYD{|mX6j6AJw$^(hxqvWjjn+*oSh(Bt->Dsc6N5QE_w52 zcf}*7qk8vZyp=J>04``VF*7q8pWZY_T2TAjsab15;To&8;qHYC7k+SV64&0%Bm z_H#!#vGiS6aDMsv^=+WY(UUHq9JAmt>p2-IsjA@P>ld9CQwVsoiFd0Ema1m2#)bR$ z>-}6T)Ygle-=D=P@WQO|c-r}Swy#@+{VLYn7Q1%xwV+1Xr*|{0u?6x}hx+>e_mz?| zWk6AL;U|-8V2-tgMTO(@^-HS56awDN+q*>wi?z0z1_lNO%XLn_t}6N{=G(x~s3)S) z^=QGtg9nv&6=cfpR%uK+o;SBs7)$?q0w|PrPf4v*;{}=gg0FFtj=!XwTwQhfnq}5u ziUDu>YPWEKq|h|mY8x0BY>1DXcKnM@L#wcY!-CT+U5j=U6ciN9){72v*LPoVCcj_VLH#^8JVTtab8sJZx*zHM^NI z`Nsoxdx@X5e0+Szw(tA8w(akmDfxK>Q#*?s#@CzJ~>II1^f{QTKU%@SHd(1m(E!h2Sh+5%b$Qn=9`Bz9(*+ey(Se&f{IL z*L9!P+pY8L+}!9^2krHHiq>quSGDo$wdmt(A~&x(Ik)Q7%EEU$pPQKl`j^i=V7~8T zZ$#G0rH^(z?u$-YFr)jtfuZx!KkLMn>C1V)HiBKl;N8hr+%7FdtruyapR+1uh&Vl%hwc$s@<#q|F`f;p!?Ca+wWPuVwo11HuFaM z{MuDrTX^L7ZNFUjaa6zV;*AvByP-_NydwX9p05u({ygn;{<9B<`PcV3%C1iJs63(A zex!K*?>nsLpPFk0yjgYrc+xR&;D{>) z)xX{RwJ)4MK4|9e`~7Lp@prcGEB$kJKAje7FlAx?wcQ{Gyx;YD-HuPEw38DO796sA zzvuI+#l`N~KYL{h4lsVaG=1NeUxjkZe;qU5XE-gsuCg)deO=bx9Yu{vv)}G=*m&0L zNC{dh2dCu>pM*l@qq6094DbEAwmnS$bV9+Rt8J0C-}=h;DEnEz-BR&lVY}F@oUlc? zdp;i9cvLL(#2j)t5Ihy**=me$6rKXETx?&B|Wa zIp?u<{LfP-9_w#zGW!4S&F1rc`~N)E=f1c5=d;O|9#{5s{Qwq`)!rpx7-2csoS~Rd(-WI-wa*)>~8seS^Iw<`yad8 z{|fx`SpL66x$(5t)J|E)N?~%^mQ+VZHe%0mKkG`(2uT8ly z?YqFbO{cW3?q9$E->;8TmfI}-^y}*Sy3n=CcaLT7eD`+SZN0cJi^Pv!p8xO5sc-)E zzk*YzW}nzlks|$af@q8N+bzL+{yf$12W9Sm-?r}$t^5D?{(sr)@%4X&ckO?os^7_9 z|Ko77pXUDVJBxF+T8k9V{r!G_ea!=A`4xLM&#n1%^5bFoe+3H*?|quK{mARM@4H^T z-~a#b`{SC+=LEI(C$n}{{(s&kohPARagcjezQXq>7v1I8^5pM$*tX-vqV7k1*6$)- z*edVe{zq^7t*q6rqE8e)J3l}Et-sv=>VvG}J-^P{{XFTvuIc=mFK4q(O%03EOgl3p zuy+1W+1SFPqM!tK43yyB$$?_*>a6Q=)o))#pSS%z|NqbV7SHDtuX?|7{rdG>q5ppz zw+}lm+<#5%x%CS36R)29NsIf~6&|v`*z%tE{vStw{5)TO??(Luo4ZmU)SA8o1H6GrRKbq$60Ux zpHJ$4*4TNR-naYxzTZs^+Rj^eST;N9-F{#H|M#ni>pUfH*_PYC@BjZdw81p&w1JlP z8>1I%^1n7d3g7o>>W>rdcAFm5o%;3M=z`6s6UxVVO5=Xtx_)f-{XfsNj!3Od-SF+H ze*Ml>d~5Q*@=1P5T`{xqRmR&jo6mJET)2?cKI)Ex;r)+G)NM8G{=W2gZ)=deAzOAM z(Vt_J@toTC)%Tce(k$}GmlG;bBPr1-}mq9di@)lFFu=p-^uQCv;3cgGw=S`~qYNo3fquR3O|2&f}#wA*QxAgi6TZ#3chVRFl=j+O@B$)qB{jl%*z3TA9 zvTZ!dmo*Qc;@NXqf%)`DrlUvy^0~FG+WC*?c((5M^ppiQ?~?v*n99U-G%Radf=*)+ zw*W7;X5WD?oH~9BW*ncBHesqR^ZGT*vbWj3&GcX0bZE_n+2;9iq7PE8hKEI`UJY#% zy`8_mR%hpvNuggO*!sUtZTtH0*!#NgyHBiIYkbZk*->KKNwwJ_4ozG{thJEn z>+sNG$vdeAvp%obTGjq~-R^ZyR&=EhO>2HEHIU9Y>s-*7dsPCaJ)I=D z(b*+ONKpI#>+@Qr|Mz(G@icwPIO2ATO*$ul)$*p%gp*pU(xT;5XDNEHB}6*TczEu_ zv_@Au^XzMDe%w4?7Z#d#HL7;UM8*I%1Lgk2vyXkh-%Ovs%9|}ma?#wnUoZVDeAy;e zrev}8e?6Z6%=xQh)W6^1`+r@{>Y1q9Kkp2K*7GUH`_gWNXIp;PuX)IQRd9}^l5E|N zhoJ^$n^FyCPEEh*XZ!Vv#rHeK;x<9tBA@k*PIyk9cswahVQOQ>w#R+edZ2a}>)*{n zCaW!qdzGJuS8Ft{Y38$95mnqauQDX9ty@{vXv^t>@|2?)R%s4%4rcAMer~YIw5UBv zH{gunrY6(x=7&%4tlbo4=6rh1w=)SA&*v7e+xn5|=*NAxc{(<4GH||U<7Co&=HkBX ze|G;#a|S-68qvlR95$NvxnV6@o0w7~?R1s4 z{k=Zl-2KS^snxn^(Q_O&{>wPIegEILA3q%C*MD~V|N3Y)J;@U(rynbSez#*|lw4{Q zcbbvGX|EMIUmqSj@nN}HSXlksibtKHGp0|?0A+0Vz4P@}@7we%`-#QZ#-w}sGeirM zx7zTTu0C{0P5Wq8qW`_?GeRdwX4HLXmalR8U|O_>=i?`j(`y!-F4!2hG(uh@?Z>RC zjmpvMO#8LZ8od1T`TY88vDuY>Qtz#vetc6}KzRJ$2zx&^zyCky|Bnnkn`$sk?&d4m zPZpQgoRMDp^k2$`ncpqaOkbSXwxcnrZ}RWSj%nv~pU?k4d*Lf}`=2MZR;g-%t;t)CQeMg56dsu$a3f8-`c}0w6P8Y138OD9WjV*m~?@UFvw&cj(Y3{S? ze&4>oPHr{7`LWegPBG|db9|OKvF3x++N;kdPn{kq8J7LkG3wh|*~^xs_MN!ciGUt*DRNr-@fSA;&p-BY=2K&CQ%@6IrX5ACMa8_ehTY==NRe>v6 zU~2)=KGs5U(siCHaUFrDuerTuXZS}w#gRV z8y;W#wJASkP3@WA4x5uoQWo5JQy!3SuxZxzng8Wp+!kVNO!Ah9Ht%rQSUSy330t#4 zrg4+bW}Y{^Yu%#eI24|nV|i6@^3ABiCz)Zdp6;!WoH;|Zt2;~n--mYP_w!ftN4`4T zd$Di+n%t|Ex#KY`r&F+?SyUb_x%nl|K58&%Q39>K;-Y; zf9`)iXPqDDy|yp*?1t6drS8ot_UsPnssT-yW6GLW5ugnH#a8n{kZpipZTVwvOQiWKQbL%6Xtu~hR?iT zLw5bs^6Bnp(pLY!_vy!tJ>~^lwq5el*0p%IbzlA4o4w}ucIQ9d(n^mn;pb2vazxCX`cS{KX|iiX~&9H+xzVQ z{dje9jo2Aw)7hDoB`>UAui1Rz;+*8(NbZTRoTshi6HyPEIrD03{SgFYZgZz=Mvd0Wzg?&_JYakocdpv=O6PmzZ-Iwedg6omD^Lp zG@fl-eDl?67xiCzpWb^a2g<8yr7!ucpNOsql#QN!|FdlQoxr`n53Vagxp+=woxf#!XN|jL1RoENj)=y!eV}&IE8b7h=d(Q5=CZJKX1v>V%EAvQ{4jwLVzOx9Gl; z`Bb-e`u;yph1Uhxyjalu=%{{O;g$9nVcv?U(1kzFmG9fkReLP^|JV5cqLus)UoM|7 zcmMCZ^40Ff4QH)Cszoh~SpDeqV)yz#kMDe1q4g^6|F7%%A&!M3ZFW7T`0R<_|M{%nY|wI$xmmP6>%0FpqkuaJ&AeiFEN8e+iYvRB zx>!&3y{C+BfX&w{!K)Vs7(Y3CzxiX79QW+whx^t9uQFU_6>#R%w)nrV!jsoo z9-e0tWb{0=etUe_%)K94_1CO=m{qRRXL>>^dyU}yzi-l4%cl!eeLia*{{K$Sf(Y(Q zMn_H=tiE`Sw=wA+_e{myZT*_nXcBqLWf)zjF8p>b^d)Elb{-yZx@&rml~maUY&m zxvCcnUtN5={r*0yW1r;z|8P%9eYW@eJ?m!@JNNCmtv~-{*5RlNw{jNDxY}#!c5nWS z$IlAq_ioAK5V;aS23vQ%ICs|<$s$H%NbWZ1|8Q$vny2Zrj%tmqZfHV8f z*S;%W{pyIi?Mu&4g~_48l`O9^bl)$!U4Fk-xxea6f{ymrO;)R~Ww}pG+c8x%x^nuR z6Vr|dr>&TLwZ1Ywnr+rrv#X7d{`~yB+F?~4@7A4bHXP!*dI!|w2aSekPml6$U*mJ; z)b@{kw`#7gjtdRVPWyf9`aUuH9|!rxdM@THnDP1as*^me_5VK44=+Aj&F7x|)92qx z)5!7XDoEAV-<`Xxe_} z#z%|x?aBPR)1ffxr+D2?oMSL0%?^>%PP-nFS5o@F{hD2{$WiyGz5CLbj?Ovl9sldn zDyOWOsnb?2n|9D$ZfWIt+wUQw1(J8x?Em)*G#aB*k(S~x?eC^ZTFq;D3ZI>w9nP8| zc`P-+vijQ_&|u`_tLy7ZL7lg&_fxM%)TZ7IVehj4ee?XPgtuER`*p7gt9i5W_^Pkx zXBsc(VhWEbJPI1HOy2+ZTl9)wy1#3`ESB$G<8b=b;XUz&{rzvttzS8*rSq@AvOVzb%lQ}zAd zadV2#S#sO$4P))9oRcm0dY0ms!sD{xp}EUW)_%VmzBk*5`KXNj;X`Xe&U}}xd@`}{ z_uK8^@6TF-ddS^-stw+U*4+C%FZ^$({^cRagJC?fd&SKYV}jO(PcH{adfcS+CpqY}Tu4DRA?TLnQid z{NrhT>H#^L>7Sc9MP{3uFo6fg(3*bApi(f~B;KlA@0c1MXUS*rprP~khS zYxUb{v1K>A6#5sPIj}}7sJGxO751}O$aU*gv^LXG8U4eDY^IsM z-zu|yZs9S>(1_6NwOk^`3ip!xZRdS>#bah%aL(*zV^Uk5q*hwTTwUi6HdZA|r-rRs zWwhDL?0w`JuLGc@TfApav&m|K&8Y%28t(_s`!q!><~-MpU_Z;HR_{!jk4_Zrx~Z-$ zP~3BU&ok4j5NeYRrfDX%q`r{+?cfbfA*bA zA6dF|pXSsVG$ws+b3+R)whbKUqm#bK*bz?sU_&~Ul6J5H+2 zKJt0~|34?RR<9{Jt+kgYZPTLnlf3mJ0~}4SMQ!rG7d>6HWcAu@QKB3EJk_u7y#N2- z_ri}y#gB)E$A;eZw%Fubx?qN5#Jb$oYqzcX_e5}tW&6(J=V6U6E1%B|58d8--N@(6 zzDVBbiCJupk}l5zS5$7l=WBk~B80><=wUV7b9}?&B>_g4uy|4F&*uUuH0nH zGVjk5^>EQ?mET`@s?QAxwacz_Ky{|d<2@v!Z&p33^omeY}UqCt5zRd6S?`(#^Z9`>~a+fb$?&SA8!?plPJGi8vb1OOwVi~ zjjl(x4jw!>-NdxxoGxfGdCKJka27>NQBzWXp6vMHGWW>xu0<#I&z@oiok#RJ^?*&) zp@E@M6Xb(K2M-?1UH0c_oW-&Rhl|It4KX@+vo2E%xU*E}v~lzLE)2di=}IkMMgovhmOqI4dUD)wanO@ z@3Zbe2z%GgS^xh1+wXeh(&XJni$N2s2SPYRTsOXb@ZbS!@JGG(GiEe!9qqKN>_VR- z0(p>=%O`k2g`dkCgM*q?Js=gg!XIqBxjFs3)x#EHx$OIz-|n4Z>Ds&fd6*Ac;0Y)= zBm}W8Qx16Z`t|L_8eNmsOB31D9U2%KR|;4ZXJowa4gRR~e#VXlhr&l;^Hk6qnjq~= zOqT^U_;`5E=$tOfb6I22s9DtlaYMrHg$oyczFl6m*V4E#DgWK>P>|0+a-i^mx@3k) z<4X>aV#C_nT6Qtl!Y4~~&l~`Slfg}<`|?s!MKxbjwDphuU^;r#;c_dQUQhwvxRG_4 zcEA})Ug)BLZ{P`1E58RHXBwxUvwXc~bKT8CzkXdCE^<^- zaAVo~cTH146WGNpr#9WGt*H2M<=o7|_h&;E+^DY3I|}j}S}>}GChTH98gu8sfdiXB zOE|V^blIwxZiK``8jGIx=}!k9K76=(mZZ!5li~sze)~3>W0@~}q1jl*A(H*z@87?( z-HzN7{P?V6#>HUi#EmQ*0vW-Li!}7++1LN;Jbd_Yr0=@Yh6aa?%w{J=u&h@ondD&2 z+GV=^E@CB%5%aOqQ=4KIYjjOjO4`{m2qjeLR{%8xE~?9f+wr*rz#GD+jK zn7QBY@bU58yQi3KaJHV4L!_1qiU<1`UYE>n2xUe`ppc?Lk-DTeT9HGzjU`bfaG2nGBzgl=k9na1WG2TZKN4F zjE#>{xTWWVQ-jai28Y6|y?cAXJd}`ixX#TXvb(NZcLGSR%Ew{Bjb!P(ov023J6SE- zVS&ZXL&rdwhmk2*TOnXhRDM3Tl_X!8n2xTypTrHyO^tHG0vg|Lv_yubJ)0keg57>8k=6F=FSGY<*k0ew5RV5A3nT% z+kcI1( zP+COufx%R!%c2_JzJ8UB5IH(AcxTvq(70#gG_IQZjEs!-X?+|r1&dj?NlQst?Oz;zp>?vjM%lw}1o?p*2V8RP79nEBQoz z|Nh5^{ceXcsPn$#$t3TjdwVKv z_Z^-VufO5VrqjnJdFx5?+k80SWKjKXXL`>5zuy)&+5UR5xbS}M_oG{{$4Nh*Utf2p z`u*MnPJtiwpq@o7FKD%pz-3TAMsv-DmrQddG}0P&T#u_h`fBxhJH7w$|9@S#c)4Wq zqfe*x-+#$9`Y*=uX8ZlRjThadkA6HZAK!Xb`F2HP(%;SC1uaK}{d>Ii_ika`Y{b&I z4!qjT4Ch)YhvnQQQ44n5Oq+f5_r34?W>^-dO-Yr!9$Ow8Q+6}eu>9SfiYF7@lb%iu z@B4Z!T7TQUs@F42v%|L9E}vJGHK+RB&bj+SpWR=AvRX>oJYdi7ce@`8&;N0xY5Un3 zhRXAPU0EJ>|Eiz$TanYc+he}Im92WQ@MCxUpG7AlU-~W31`UaC>%|)+A{y&;-kOc@5{T(mWJG^dcAgW-a(_&I>Ml#&CrQze_noBzs2++*#v-8*ObMIwn1Wi7>gF27cmr2dlyP(~Xp3oRkHV zzu&KY;Vj<;TD%qWZ%_J1_!=qIn~Lo+mtNjZ{pZWEMJeFS_4gYN^L^a5eXp)u{hyB& zr=#x${ax<{PP)0@7(puqv99|A#e~;_9gq90k44}Ab*;;~`CzA<=}h;SeUD||cU<85@pJ($Q zS>ONZccq;BW!&b9e{^ZYt3)6W&>GJekGwBA-@1Y!yESa&Xpv^Sv*9KmNMD-!5xv z_3Aa7yuQAFy?%e4%~MVPm3!*Gu8t3V?Xmr1)Vk{3Z?|=W#wb31S#G~~)8EVU|Aj<8 zl`_(iyXj{2=kxskYx?ABf1Zw)(_XhD>CfBz`qh8_{oeoI8Z;fCUQ&P3J5C5RH+Ota zP@VWr_nTSe8hzmlXPnR7aFFfe#QvHkvwnjX+RU)8|0jH`{?p|7GWxYIy_L1I=KsF) z{Fs;aT9L|}tUC6c@!>IruBHDUCHLE!z1mY0zx{sQ?-P$}K?`Nv>K20rbXS`$3$B}- z{54kQzTf}+89%qTHaKk5-S+UmI_^~s;DnVFrk=Lr6LdY&n%cH3c9{YPo9CA2C5u|Np#^wR&yRxjB~Hy~XeMem}O{?(51QprvQ;tFG@(nw`5X z^2JP^W&3^{)j#%hdi=kZ`B&L9ZD5nTryQ5BuMsV|w*8mGNA38ZUZTsczFWSgUXS^#a=(pH-M`QC`+s#^ z-}h~6D8uxJw=&*dbeHedum5?v5L70-{Lsi>v%sp)g!$^lTaoE=MV+0Ux$8_M8Ce!NMy-yP8K3%qve*Tw#0x3bsUrhWf2?UUTaiMfTf zv;W#&n(xq}?0@^Mwt&XA+kG?m(TYb)BzvWp4?P#{RNZ;!@r>#F2X`JEO z4VsVRulvxv>XWwilTC*VR^KFdw@BZg;F&A~-_?IA*KOVR_xJw))uJ-#r)$r>SKU8#&78m6>wj-QW%d0R zsF)Z2W%~H`oA;Ccv9NT#{m#oFay#GuOb58`6+mttIknJjmowB>uO4Oj1;m0LL_gDq6DA(i#v z-12)nFHAUJeed(CtP?yF>%MKCAL_X(1Jl$Xab#?sOV-hzv&3gM-{(pt&yr&x%OMbf=b?z^B z{0}ElkJKHdKOVLpSMImjbU{MW=iT1#_paXW=Q)+tnDkGl-`#ieMEAV3fN70L3w_?7 z_@E14=)=4Xw$LZCG2Gf~%RR;B>pbP|VLi)|*^WqVd%C-rPx-dX`+zg24&Msgyw%Xi zqV^znoWS3|e?w=?SolKFU1np%clBo*cT~UK8gBaK#CyyB^s^2|{bpyg&R>jTJNYso z`|K~X-@BLCGd3puu4QIA`t7!`F{ovamTN9tWD@2SIlZaI*(K%XmDRIj-#lXJs(gRH z`u^{Gude3T|GxeBVY_@@mQ>`!X^L^%KyzkS=awaJwK-*?{cclgLK$f0=YNd6#=rCb z|M-jQ_2xuQEqm0d9+nsv_I2;?yYJ<`zT3^rZ?j=nasCsFuZ~go@+~%Lt^HRUd3D;= z+|0@4ZfUBC+xDzCT&-Yy;m-qh`-s-k-gG-Ijg7HrCDIIri_f{r}CYo_U}Daw6>ggyS=xZCti^>z(;L zX2-3%N)|OfT4cA5TW`mLRYfwD<&PHqx>`5ywe`am;YTa|YqLuEpMUu9p=q7uDo}aZ zw|f0Pt3In&D>!*?u6f@Q_ehcHG4~GVjc4}>;2Vkk$aHj*j=hNBZ}%gYqqm+*V?N4r z+-Oerva3v+CLX+*k(^dA<6*=-+Zl*eI3oUbKcD<~$X{>qYKl?AX|6*yNygf&?%7*^ zraVmzTLW4mGOafIcuAj`n_TRy?>=d#mA$jCPWpL1d+pY1JKnGFonG#F&1ub=3#Wf; zZNBE-L&=J-t$%0Hm_RjHUDEzdWieH zXM(#UduJ~EHhKP^C#|bAv%edsy_gkOX_G5=(mF2t+V+|ERckr_csne(kv6{x&lvfE zjK(CF8at(Z;)2mzs%pE1HO|d6UcO6QKU%xb=B(G=J>7cR!ke861J{=Er2Y+@^|di6 z%sQ^}>D0x1lTnt^OuZS4vXti7-SYdk??N_b>8)0L&3x|G{ipxumEWuEj>;;1lXcHB zqCO{6H$Z0fr}btTYY%z?9j z{nzg|t&v|G<~!@^jNR{cy>>e6ZxSH0lRq+WhWpAig&&8+_Ze)vowxhM2h;1HUi#Nt zUh~X@Ioo)IKhIoq?Tr0m+3?$I-YeO2@e4$3 zX>cgK=ob8|#i7t^=Uyz`FNg2kJwh63l6TJuetcsX_CLVo32UW&SI2zUBejBs^?@Sa z{avyX5+2Ohr1Rh2bhg9to14>*gBt&~GeZ+Am!ui%_VdN2&n*?RnpM2U@biwVHf3Aw z{>*&kP^dI@W@PLP$8foYGu<=OPCs@`KQklnZeqpHr_=Q(z4ea&6=VaNH9b_@x7DFA z<$_t!hKqAvJ>PI*`gPlai>~6XatmjzwF{6rzp%_CZ#DDLiJ#By{eOVnPC!yB?ZoW= zG4(y!VTJED9Oet%ZX;*5f423z9gD5Jz8z$jU*mKLw1s2a&9vD&9=1uZ3fQzSwPB|F z`DYTV*YEq)B>!~pRLL#RWM_Uo{_oniio#mym(L@al0j1;Z!Uvc=U8{VH2R54s0REA zc3ER`YFC|%V0>3czPRhh8;{mi7cw2K31>?JP1%8#dvx~KeOcW7``4;?HV06HMLlL= z#O~|s?4Hbe{Ymc7WqDJ^6^y5`!cQ|oiuWy>OB>i>Qfp5-TZe={ezlYr7-=PCH)m@5^%g*n;DaBAVB1JSGKN0>156q1;v28kp83x%yqt zW@U#xjB3m2-B!jUviX&yaGBSLBUS_qgNx+)-|3TYB ztc1+JTimxbF$wv*Bl5o6_rsrA+yp_foQ|)lyrIM4V_(OORT^Elqqm+r$}2KGDr@Bl zW!v!B(yLk=XSmj~%T*-Y$$WZoqU491Z8y{WEuxIxZ(V(N?v53U9k$=AdVQn%{od6V z3g_2;>$Ly(vHz;|a;tzp7yIi(z@4F(_uFpgNk<<2@#As7ds`^K?G*LPZlM1A)sM6O z|2S^H>eQL)_j}9F7#7ve&fi!0)kiKq%KF&mn+_jAJ32(Yp3QBxd>8+JmgLtX(bjBT zk(M(YleQ`MSuEOh-{AIf`+pxJ;Oky|-k+HIti1BeMfc;I=l^{Z*l#22ul8QxZoy&R z)z;RZ-<``j`^&FJ-NrM)2RuL^gReLIn5C<7%}ejDMLQNw&5pU_VEFvQo#%GjSNYXk znmjM0wM%lvmA+Xrmd7JErybq*`(3oG_{@V5VXsoJMyyR`y*J0QcvXS6_Q_9uyC>?d zer5MW(Uh^oSScN&u95$g8G_;X46H(A{NfNU36Oad&>gN`gwey z?r`awm!LMosr6+cX@9HYzpe@oy*lgWzHeLC9|g6(S?%-80?zzQy_(W?2(%sh04*6Y$9ca!TA*GAqC`&TEuI-M1? z740j5Fq7Bldc?gpF5b8?>6~hRZ4GF1*lC^3KBilFQa)*%t$w$2`KpHMUzg|0fx4wR z8;^={&y$p#kh*`l?c2=I4O{Xk<>yrlx~umYt(aq5U6!-!<+6{j!uMNEizzz! z>ixI-_5amFuKAWO__2BZ-!)vpFB3yo>sK7)UM0Ug^LLsR!?a z_6MD^c0Mh$+Ad&C=*+YmRX?9jKfd*PTr}(5pYOHTZVCEod+nR%;ivI`9*J|`yWNw& zy8d|Y^~p@2I4Hwcq!}_DwGl7gx=7>S#aCZVr8S<_7}cD5Y;8P2a((L6h_FJ?ZlT3? z(~gJ96&zrE+*ka1?e?O{Y8=Du<;eBXG+UQ!`##tg*>wwTj;yK}Z~`~80Z{$2k+ ze7o&#KiACp+L{|@?2@ zc&uhR%JbygjpTmyJyy+UdFE`s%5=1|?z4D&O<~Q?)A4aH_|ENlZ46qqa$@fBCw8Dw z3a2+~Ol+S{310Qs&p2WYzsTq3yWj7t&e?QQE%btA*y{Pke~r6eTVMQI)px)G6cWE} zKNApf>^B{Z+){F)Z&0HOZfv|mNK#NVrV$FMJcuuED?)R%wGjA`}+Buf#L6cZ&LNU zd4e~eGHyO$!)5yZ&+eDYX1guT3SBUxIXz=qc~sic%&^I6tr=`fcJ{4_IAi&{>0mHPen&)?~?Dj<#N8zS^mH zKB)WuYToIIjY+v{_NbfeKVEt*Qhgq%z4?2}!wR{@RR>wcL#B2oZ$0{PZqNPV-;1zSuSJ%&aw~oFeLa+5vNROjYL;m=VU<_-M-~{Xd}1jM(>fg4bqz z-n47(>uZI@fg<`du$sUva=Yg~wr!9l^Bp3m-S6+Xer4^ORf{yt=B=5^h3NqN+Qr?D zXul5o_IQ!Twi#=te*6AiURLn8|Fs`?P1ZSVoLzP~ilbz`L!gK`FCX8!yL+|Y?3rr$ zY0KwtXL>+0P@ti5q>=e8TOA^&O?N$#Ryujl292&9U`ZNySV%nT>f0#bz`h|e31E#0|yS&rAEpb zM?RLG&cMjTvMg+YMi)0bJG=VZ|H`MXtXpP%PYvhZ@?2#D-o{6Isgd(;nlY$>S0wqu z*LBR6a(O8m9#}WPfLfB83EO z?BbSN!YbyYF%^(4_%F9P8VG_+kH~3Uq_Is_MrKdKM)nIEpMvIACQE@Q7E!~(A)gz% zsN=%zPhtKI42|mqDssNPxw#p%QRMipB-7YU9F0lSbC#sStVA*(LmafI;`gs#-!!@& zJt;FZ;D=gn+}_r9Z_ACnrE^3W8y_v%ATY}PrwQ*dL_ z`%}@DpzZNakOdvPlT>53a5N@~=Pp}{&2Dy(-NkpI3p!G)C1x;#B35VGf$!~YZ8GtD zBT6RU_FQ1GbDtZQNzV(#plL_7F9!}BSehD{cdz(`m`?*k<2iBUnah7cyYg9?j=sAc ztc_*p!eA}aWf_fil7ix(<0Ud(KNNv{pAiB%UPAiqlbmimJprx5$ZG*dwN$biI5)y9y{QO)# zTIA@w;N4Z=#Q=?Li1i#&FCJga6cEtxW8M{xWs$;*G|(h;_CA#L9B1EMb8BE|^b^$R zdi3b$=jZb+Y;5LSeGzyvRJS%-sMi@w5PttX=ip=Dd|bc$n2?owfu4KSq=`7*%vQ^%XQR}h@r8S<(6i^ zoA>X}GZen>s3>+hvmq9g@(lihrl@V||NRkeZ*M<8^rCSCN8?LAk>9_5UDNJT zS{-+7TO`OW7yiM^h2Q75l&5P1yt%t~JGjU}O9PXY4CaED2h6Ip509L7e%6$ulHl@J z%^OlQsb?2FHd9v!$T{bq4=S?I^s~5e_!KXw$c&se`?d&3{)I*3@$_?ZET6C0eC}83 z@w0o_)pP>hH2Q9H0m-824%Re?ZA^lmePcN_`>P&EQN~Jesa%%vvV$$Vv%z7bY7ZNh zDQ1TSY6h}kx1ZUh^W5!7R~dLYfmIZwO0ceCjkTz3OcKvqrHW-m#tc7Dl>n=bUdC)M z2L-Q#Hw)w#9B(%xkJBtHUAl`+1hB=Prh#l@Qhoh@OTmXTZ00L4GO<|2Bdu?I{by;( zc`lZ&yPKznfh%`VtRM=h1-dNDGz0!byXf2%V^DJh6;HmLBG9#!zPyWyy;*TD2MCr0 zt-m>=dHR#pCy=ohWSLw8WlStADqPC(A812qqvOtcozuxDz~Ka5Rq^n_g9m1J@t-%GfP4iDtQoHyHtyg5|3$}* z`G|!ikOm+z3rQ>t3<`WAr~TdvTS&5*X}_G5R8`{U<`?&^7#ovb>-4&y+X-rLxNwNn zvVk0OVM&9-M$ZFb<>DIKqOMecgkWxirsgWp!M=~>8=itzSXMHE*757gI^tZ9qY&_g z7gQKHfO?r~jL~;v7u?7Se2gWHz9?+;;}n4%YU6%)?QywkoqvCx+aI4>eoxZ==SlxZ zr?l7ifTli!%Dzo-=Ih*gElRuY$6@(nC)MZQnN%*~Dkz<^!4cHI-}r1+_OUMQbrRk> z8(nPvd^lWqT(Gc@l=`lr~XJ(tPSAB2nQO@zieA6W_?W1?g@86Z>-qhs>+Aw+Z z9nN!p4zRF%`X9S~&Vk~~pFGT2x|AP+dPqMWiSL&HZFPvsyY9|*}MbU@xO1~sC+(m^@g829{0W8!#bmIJ11y~fjjz^ zx&^^3Y;0`vqy?kT&$s`-)HQIAfzmu#L2b7qla&wsUwctgI&&T8Ozl3MP~Q0I zl=kte*K5T=)2L@=nXZmH1{%eDKCe1YdwR+mJ`oGpj>gcPS^;kk@V?on$t`j_$N#VV zPt>!P7TjfJV`IA)GwrFha?zQAcur?<7Y*Bi>R%@xyl_Ob8x zyWQ_+gh=P@NR%l!z!>ULepEF4$aA~zJEv{hrWF91Rek)RnSb50%Kv}A9}jew72SSs zR^;bHxieFzb=H2nsScWO(W4nr`2HEw&M#d$jV-!{vGLKP+b=COnV61xCdU48 zWbNwCU#<=>1RLiGy?Fh)Ix$og2Thy^;Q&-3#0zqh~Y{bHMg;op0( z6Iy22*VmQK$2ME zgxC82r5|6tE1~i1>X@)#g&k+FBkXQW%1knSn&7L zY5m-~h%Ihh;imDqysIaKJr0@rd_~~mLsBYR-xT`HG*Vr2KPr1|>fY+_+dL<$wSK%` zeBSo#d*5}98TX`fw@m!|RKI?sM_4m&b1vIsbU358sy($Z3E%jZS^rzqw!r`d2QG%zr6 zC`?fKu&6lCL2dF+-qT}>I&D9lP+lA6f3*`dYrFU3G3mAEn%}$$(hj*Cw!T|ulgBx|d-eZotzWO% z?DRr3zi@r=dE4u3JQ4|ie|^oau6l9_wie*(`TzgE+k@t6-*oHm+pzcBtH`(9r=A-Pw>btvNuiHI~$&kB*kIT2`#)=B4nfAUtz|3#b+O>1m zxkY>4Y&z|9K{Eeip-jz(gNc7dGSs$CRCbR_PELMw9dv%u%|7dQ8FzP;djI+5Z~s^1 z+W(i!=j+}0%qe31`{jduLL34b(-?Lt_bUXv=@7fF1P&lD5s=Kp%gcMZ@_oF?=69xM zrOUcOVb|bb%4K3^W@a`eDe0<&#;R3*iIZkEt(+cTXZfdCk@@KJdDZ7Yd+q{S%dUoo z-(0qm|BvsN{q66H*tq%Lo)cH~a_Pf(Q@7B|rCZP1{eGjU zF(-9eWX`82C(FLyEpN|q>Gc&`weNPt#dY%9LMGO1?e~PWYTyTyK)mkMKlJ2in6LO&P_1f*XZfs2UWt`^} z)jB6IV5MoG>&k12Ssx4c{CL!@=(Fe9tn5R3o-Ej`yES#)os#+UzJ9x%zdd46MeE8{ z@u6pbJZzV5D*!E2+rD=Dy<1|jB^Nh5U3bsEcX7b^_cu4Ezwr9U6((Q*r*QY5Pp1#x z*SvC$FJSktyh3}XN-hD7XPlv{PIHKOtDY@}re#oqUg6NVYuB#bbBbq&J-8rg!~{vs zDjXKi%g&`3x!$i_pxn6Vd!3na^z}2>j5n`n@l`F{dHT_&XUjf3>$86M;=`=>8;{E= zid^0{gI%^HVD|TH?$ZaG+1s@o=Ps0Bjc(-;F=fbAU3K8)arycjL%GmNvgLOQ6=QyV zI<0@WVzI7Gctj%W@8|aaCD&cQ+4+9&_qcZ5P5Bqh*3Fu|%`ueqp}TzTl~w1Dibd;$ zIBQJ^l32CCIP0TBpyDj24L{7*@AA7n8#L1WP^>kK>mq28quBXCft9+!qBdFCeyy#6 z-M3%A-?DVguM>G(neDvNVux&V=U4sxdfhyJ(Z>1p|9&QFU*CSO>a?Hb(*|}LvuCw4MX}Cal@~fS`$`DsDlNaWhSfC}O>b3& zrORZU$O;3!VG zRM~thFNiVx3YV+~Xno{DE4`JOuP+*(vrrDXl*x1P+H|Y$`TPHVQnMe56BA z@yz9$lbzdmHpbWg-FmgOUA}I|+Fh?!Ez~l;=YJ>exwKZn*{fS*U!~2?70W3}ODTD` z_*`~~=n|&0&U}_CEu}kWIoZrrUHQtrwL5TXQjN^5b0N29shR##A3;Ud?@bQ`YV)6!6k#Dr^C9Ri#WdYRb2?{!fiK0mL>#sA6tCN`=|QN*KuDbuFiZNG}n0LGF|V*S)PACU-H(! z`|R^#JJA>ODxb}K7#})w-;Eyq4?ln{{kmKYP;u{P?Ii z;nclfn4qjH`~@{2saI^||<<{hd1+tysQQZo&Gzp~3edjZ$J6-VE0WnHsvUGD0a zF%F^5$C%H(c>iVp_q*5sJ(Iar7JB=9*6E|6GCiV8}>glm%H&Y*)g>v7k|Npm=OC^3;*4cfvzuW4r#8>;?Uc1WcQ>%EKL1@6L zsa64JmKz0MZCz!Qla;jz)VS*jzCF3@^)=l&i7QI;*2mU-JZkoQPVqL2^M?1!6;`R7 zje zuV42YZ4S;#+2tb)3Im45S;7V9&Q3jE_wl!_f*d4+;<$mq=*hg$BI3a?Rx{9xzJV)l|&x z&(Fx%kh}fvw$QEfm^QMCMH~RN=s;&lS{b#5U4K=xAJjj5&vjIzqpLk;DId6NDZsG( zs@|RtPM0lPuE&usA&wtjuFxZh6d*NdzL;-Q|$ zw)ea}!BqD3=f%w&BG!w}5%l_cqs_4>UVm|I?d@CUcS|k{ADvadFcROe)z_+vJ_gkX3-OjUKy?#Caf4h_u0-x_a zf1_2`_UP;N`26j6ioADhyVCgR<8k@?xyHd_W2)=2>WkIofdiQ z)6KkX{`2k1e!X1Yo}RJ#+V*=@+JB0}nfER!HvAm9`^79!Drk4usQfA>nhBH%6w03n zvUJH_d=!OLR%W>@*tf54%FNsV<;$tF!1;pX%AAJcyx%@*#+xF9^VI|XTsU*>^@(ZA z-u7;u)A)$T_)J20TxBZ9D!sj5f~r3pWEXDVeUMGM<$+)H%6Hpt=jon1)y?Eu>w9}f z-QTa*J8QqZxoK?Uthiua<@dYg=JCr{S?0d9e81;&n?k^Isb{a$cb&9MXP2v3uy*ga zTi;%-UO(%Q=~hPQ9Qn45DYpLa*WCHF-#8aOW@ML1h_CtR`eN~w#zl8hC$Hv} zGC2`)RO9j9Z?{(OoK*6-*PJi<{=ZMB^_QLU$yJ@bI zr>#=6{}W(YaC+O1N8PtK9+%5r^{@Kb%yg#(l3Rn=yMmYdnOYuuF3_9WXZegHd3NQq znQvb%pUNY-@kN-QIk^-~Qi?taYGH`-*EJr}>I^OTEafSv4y}cdeVgDl#auKh-a)P>TDX5PKmWV1cw=f zn84Qf>({Tsv~49)#|}wATa^nwFx~sT(AU@3)tl!QhX{Y+xw+vlB&S*jT)El0d~R73 zN0r^*FTv85MN2@von;e1n=&@M-G2X`SX74MY@<}K8(N?De!rLPC~*H%$aQUz%VJCB z*M5uKk@xLp`gYL#dC2jVpmh#q@ArOJGt#H+cR@v0N*?8P3epSQl;&Ya7 z*Vq4Dy>|DzU1zVSu%}!!7R@wg2;>nOD|oOJ(+Tj)3nk z?|6phtojx3Ts!M`2okI7|zr-K?e3kQjZuvIdZ8wzGEWVc>`NH;+wXgF6 z9wwHqREPZ66Ir@6qXpWU!A%`7;ULArA#!;GR}UkkV1L9}@t!|7E6Zwmg#QUa4Zo6e zXTy5Zr&$MFnFHF2wV`%}*1buyw9bKAk_BP$W!+4!tu0&(p_VJOoLfC_bpda{nP!rrcA3A=xr#+DIr~~frdPu7(7dWwE04Sm39P)4*uM4V`Q7jLNo)Pv zw4F=0bish{`#W%RY&D@wHz=E&hM_`~ANEoYhWYJ73R>c{|Zv zZsWtl?d_(W+nrdexVX3KuGWgWon?Nfpcz!2o}Di8>StAai0tYF(=7R#4~{DiEL99( zV`MsNv|#&J56sRuFB3~wW(4aoXtVkw%PA3!c@O4T?~~Z<<-A};W#((IxmUM;I;Cyi z-oUjjC?F1e}~iqghu zJ{Hprc2Bhm0G+~e_zoM7guuTSG9JATKcBbfZ(R9j@;0O1{^-O-?_zVeUOn_}%c^t1 z0-B!#7rWiuQF!af;!^F91=gS>18l?9V^{=H|7m1jEIR21fu&eA2iIfO@~@f?SMMw#2% zj_(2*VuI7x2|`-SAh)duVPrb$6>E8{7m~H=m{eB;$mFMSA94Gko%I7qQ_h+N*G z)x!yK^C8ZV=mins;pLNx-@Q1*c~KY?3=Wp8bBgs-jAl+fWyv=0Zcu~6L~|yvdMFXa zDWLHw2vXz*+-15dreS7c!t?ds{MfszkG&CaU}(H0Sa7cQREiOEcKWfG^EDfjeqHYn zL{0`pj7&!-P5$fvX&HEF1++FkD!cbv+Pmw-VQ{(KrS@R&{#7eiW^P|(+t6pNoiD|0_!SstP`*>Y`FTF6gh z#eg+`OjFn(kpp(SNKAvn#s^DEIv_!_ie;5<59c-TPRT@cK4Zl~i+ zpJ$DJchC5`3MvHXaB0s)_R38rmaeIL>w4rs${8BJv8*x)*mL-dS$_Vz8d;E-!dIsK z-(Fo^{rtnj!}Y#@J+dfuXU6l|$t7)JIm30MQCwAjN!i*REZ6 z=N5;DJnJ&Z0*N=yIMCbM+iR^bNsp1~Xvtdk#f}i&AafK%nOM43MzFCpItaYDws!W{ zx3{lfi>uC^ZIU@Dw({xJ6ABFu^O!FC827&PUwM0qX0T8Fr8}z(19CDlUcB4;|IV4J z2ipPfh6;5yC5b;O=6%K7}ZA@RM)ti^!DQu4|K5P2(en6Y{YQq4L zla+6WiyzyxV6T9N*p$iVz=;7utl(p0I{GTYJuUDj z)ABh*UAh~O2%QxM$7?FvQc;C~GjC(pHZDrLbK$~=j_>>GGw++tYXik?07K*bUFGlZ z`Loi3=`0gV*B%}Yg#ew#EIyIcu<-Ehdy3z&8Aor6X!`;lbqugzeE)Zyudi>o z_&WD0tt||=OPEkahXoO$r%$K|cr10;%HHLapOG;m^VOA=&n=&S^LVOcpaDuQjGQ*l z_S)}RxiXVS=B1bdv@k@t?vzr1XX7I$ZrzmEYc}`k?tZh$?0(JXo1g~w{y(3(Yd;

I@GsWEOzdL+ACt~+d40d;^VvC%GqQP%&zekLyZ_&> zZO5dtZ`>(9FZ=87v)TDh_58M9ChYxmO1pBltGM32ACD4sr^Que&Q70Kne*%A^6e)l ztKWV;zh3TF?Vpdw->%($FKhdKVLuDSHR)I8)qL`_{(h%;ZT0te3;#1Rv)y3g))6@N z=kNFXw|Bqam%Zy*r}{jD_qDtKzGiwJ6Y<#eddy{$cX4~GZkk;7Nw)v{B{)KPul22U z0q+B&*GeSseY5HGp^8tL!OQ)&f{y>6?_C@>x%Swcd+Y1}JeGg>OEfIvpxOPJ;^>l# zu7|7qW*8idtNG}<`|~;Lx2xCh%euJyTKa?FAhn$p3zVoqClys2%_N?e_bJqUwLWTy9qOCZhVwMfbP$|9{8NzPr_Q zJ!mqgW|>%Y&c+XtS9aC^`FQ-{=f4M-`M3Q3d9C^D--Y@2_ieipfd9x4~6>z54#W@7-@YGyZLgNt>CP18V8Y*ZolJ@xOom%evFf=XZa(eXP2@5{3q~XD;)$6$IU*xY|yZv6(w%6-+N1dN%dwb8j$i2c-ITOevj*g8Fvrf&flL4Itq*<=5`jB%@($(?r-fs_0b;D382&X6e~i5clmn;$vlj=Lu2g01nsr@o_Z#p|;dAaU{$9LO)OufPK2u(6$4`^mHc%cCsMvn} z$&T;$s<;2F_1#}ma4j;ub)A)vhS;aQ{#+uad9Al^=WO=-n|M+V20WUwU1JQ`iksrh?UAEeV@(H*E4p!{ai^@Av=T2Rhf>^6-%r{H+a9(v@mk@0Q(O zd-j9POSw?3SH)AWOkz6fqjou__U5%aA`P#qeb#Y>vR1uiy>#<$e9=kOht+QDmc^$8 zgl^uqf9v&-xs|svm$#k|-1Br*fX{;5pjj{`ao4z zgF`B}fJWH2UxroSA{|0>DNT6h5Ewn-Rr|A@+gh&hZ%Ka^?cE4HHrJDPC z#+607CdHw?cT2CwF8pP4OLL2A(v?mBZGTmVI%w$x>=59WumAV+`4I)5t4Zu~ z6$z_X?O$T3@#o65d0D&s7tWivTvvCp*1C|uiccri)o-1#d_E^pw6>s&KRTupK`@VkmYMaOLm#r5oXuQ(?y>HvG)m^@8 zJ-K!fj$G-1Z*r`Wt&ak=p{gn6ZhP3T( zDl>g%wXb5h`X%nAI8VylFF&u%nqT#5WoPnLeYT9(cUPSBzC9`HU6uaiUzaz3wLH7^ z>*}Vw+pH3)8vlwZw-?QoO{r`XCSIa-jwhj*EeD%lm?}nJ? zJ71nLZRNOWZ*kph_IcayG2M2P4%x&m*x(kO0UFs=yb!Fh-1u0@y!bt?2j@F+^4uEweDirb>1!TWR~UhMy2AHE*LJT;-TIB|T8Y@A zZP%_&+wpPP`RJgji_RSlc>o%!YqPhKHojF9{(iX@(^omr%!%%=;MOOC?lKpwQgT` zp3eLA=4H>-ipk5j#q~yOMqHh-X@9)Sch%ak?vMALZ@OB&=zSzEfMcemtAyeSGuZdqu5xS8bY-d*w^5*T4C7zcN?5Yj3!c?s@+$t_jbmJQtB3!H+`|6v;>L47KvUUWe`n}@^N{MoCavoE_3f=R)6$B|ku9q>Wv$NaKIvb99a8Z40dok1Xdlp3K5$i&vfecBlBf*mSjC$Hp$EKvuV%vknEa zF8?@9_C`v3-79}b-$}(+`ma5lQ+zISop&y(;hhscE}Iak zm~tg#<_uXjtto+1Q`T*}=qA0@D&K$M%yXGNQHIB)txBjgdp!*vy4|( z8Se^bthM=2`>0dh=U~-7--C1R$+qskwJv1p%CBF)>gM_1`evf((REJUx9VHX=dy%zm;*Xwns%U3Lv3QoTIKdU=p?(dngfh#{ph3=I$4gHxl_wm}5D^KR_`FL#3 z`K!h$H{whSH{ScL65X24b@9?Q4w2&ZCmI-;-`y|2UmHE`nOHgF{=#!#wcaVe0C$%e z7+Jbh9Tr6BME(M|55XO&6;+Jy_wpUS?a^wGwr%gszm*pvThH6|HZEFK^o9BL5B>|G zHru0jz3H{Yuv z*6;Te&psSlurzB=#pPZ7a<;oxB=_2{k7-=A>Z{%VKc8LZzcF^-y7cL~-S1`@FFE(m zwP8uttsl)_;$D8bl%RTN8{C5J-OzIu6mwz>yc@l*>h%o zn}oyd{O2q8GWms?W=W^za?XBTbFPAY)wfX7{3}~}SKICWxa_=V_5Joq+l$(^m`^RSYgZ)dRHvpGt^VI@f4p`(cGict}L%BkhhojP1ks( zb999&YuCezjM|(+dZF7X-C&s zRo1qg7X9kka#ch@+oLaPow4>;ZoXfsETLlB zA#XfR-#ZpDb?>T_o>{97Z$2OF^UD6^bb(FGN0at`zq#!v%NJS0N4G9J|xo)|= zA~WrHFr+SUSa5-frK?qJsXe@j(6~zE!@c~hH0v2N|80M@{k2!}f*p5?PT$;+csOcL z{H>~wbIb3&{9;{x$y2>`)uI))zu#DrWUJq7JUd;zwK8|78f(|8 z1%JL?j~8yAAflng(v_b&%cRYj-}Z{#uNRBU&Tcwi5*#{j)#ocEWzjY3e(ip<)}h!| zL9-`h!mm@>>rL)YlGWI@_3P@qS#`f&Mz?z2Y+TfI<;`d5+pZov9Mb>4*)CT7=M>DjgK(*E3g;}H0K-mA}(?|!?H%nw?>WW8AjGzT(k6*nj> zv-0D1g&(Y#Z&y2Owi%JExtgclVLMIPT={hWkcqjVcCnSM_2JlvjeYm$iIqmn%n`M8x%Tr}^S7Xd zZ~FWHTr#Q4ZJf0?HLGmF3|T4ruUCTCuG%=+&i__bgv z@A=+tW@@@s=db>Q4ST;oKAY1RF+D^>?85YQko2Unm7y_d<rQSm%clOZ={)mFuSHs(x3>BJ z=ksYHjaO@2Si3$h`|SMx^ZfrRVZ!IlZs(kxKl{bJHx3_nJnq}PulBcD`OSNhlP|V< zKCA&(=JOmj9=>lCRLR_!q&2SO>F5C=rZ}=XU{(FT|V13mS ziNnlkW%K`8Jde{pZqfbsj0szp)#f*|ve#|QTt0VO<*$CbUn^n@4zkXDb|bM}HfMJ3 zw#a0~2i>6(32)ymIYS!wt%Zxeo_x~xHTzO^P z?sry~ExWR!nAf`gdChP4BVl#c$|dP3`SmNajB~F_-M_U}Lh)|i?zd^P^H#}r*XLKQ zJ9in>`(J)v{%%Ci#$)Uu+>M~SL$g6QI(C~DW|r`c03k?(iQH< z2P9#Lg2o61PajbL53PdJPM7Y3z2TW>4C`4sH>=zD+w0n$KC`ai+}_wXAE)g8Z^G7P zbn;%>m+A3!8y7max9PIo&g$0L)KUB6Vf)%Thy8Choo;i`xHoB=--E{`%S;2V%$&R`Dr+^bjKu}eh@sf@6P{Cp1Fir4c=L)>MrRxW=glYOardhEg(Un)=k ztDE$f;jdBiHomZ}1<~(IHr}gdI(n{VyT^T7mado4!fv3U421@VS)jR)Jyt!u;Bg8N zu|S3C-tVJl-WVC*RyfXV%q2Xx`23U<2h9qPC}!8($eFTjd%xeSF1zHZe)IeN`uQ5$ zFL`OV>YByZ{d^j5c7^ecMa$$*t-h3bukv~BKkHD|msWEnXI!}!nJ#Pnpn*9mMEzBb zd1%N{?XF8w8d-kgu_Y62|9m*y`FVEXG0Emmt<{T`b!oEpeAHU=@9%H+_KM49Q@IY* z{rPx2toHkxo5ne!JC6u?Z}~CnTLm8HU7QLujjtHDDd+04A2zs$;s;K z=YrqQy|=$YJ9NQ{%}U`Bj@C*4@7>~h@%3JY_Jbdh{kzny1KwO+9loOC-L~qicJKGvImN{@Z=Ol}mL6GWDc+W@+`zzeRds=`c%9K^{j+bB-~Zi_cX!vz z|Lf)Eo=my7e%;Ej|2aq#J&;DaG}FD`rDxw18D9=te)H{5`=%q(*}R5p8y8)3TsY;N zVfS`WshhX+sTgQ!Le8Tjo!1t-_Xq6Ue2`80M%C-J*Jb{#tnw4>{V30~e$S^<3qKj& z6UhMW000j)xy#pz?AvmHiTj4y?3^Uf()ZQZ9j7{-x4r&}4_t@!obUF$!S4qo!s zznit@X5%6~iw_5wefO^s$p7_kv2NGq?RIxF7WevuUizpzeev%}tMa-(-U|;}FvIrR zxw?QYro~%ZyS9pGT)Tc<+u_dfGi4^5?Jxg6w@=^>xX*hbmGSu<=iudjvKOyh`SN_H zVC=i8d|jFj3nF}Ce?dB5pfN&^tqtGf>_c{GJzR4n|KE*!Uvi$`IlpqA%S5*6&m007 zuY!&tWSYNZ*#XtMZSj9*#XSF$I^S4qzxuDlx=Z};OYhz-zn=?Q9JG4j64QVyHH&s; zbccoJY5fRJSoI-vYRkuE4xx?Nmm-^Q9SUJ|`(}IbZ|>To{`P-U+GWck9*M0y%Js73 z+v07@V=@-D7S$D%Ok2Ea=BDe>2Oq!N{r=voeM!BhRTIwDUim%qu84x0Xl^X8)hStx zSK=x!bpF<(EAJKmWjeI`{XXe) z;-L!bCxrZ;W9!#zsu^(Q`{v?vmdmfEzij5W+W?wsZ{Za7QCw=bIPk&x_<0w;t^YVp zL__THUiAQ-OaDLW7H;1v0GePuV^aF+%I7)7)l+6nKMt-9pXh&>@%`V2Wy_Ws{=9!^ z>Qm6Hd%M+n7f{GTs>a<7-{YPdELZ1ASu%6(gE`XS-yEkEGGy|J9NqNc5O;g7&t~m= zx0EhN)~+%LPMC66L?P4rJR>t(iN{~i$_Q`$y(We^-q{OQoI0ZD+_uHyxWV-$`+q*0 z9Z)M8nyD7MoJ(h}uf%<&t+p<#id_DW(*2IT*G%&;KQ2>z19Vg=)0IB6+^s)-Y@^Yp(|$9py| z(yDnJy6^4HXTNrxtM~o2Uw-aeectb()u0LP?R$#-LiJBCJ2=}A)O6(7qWfU){jDok zzRa3)N!ww;jC)%XQxGjnSD_F0{zuyj`@Q&A^!&~|*Qy-X(%Ih}0%usg&OdMaoyT6| zT&V4e*C7+j|Ns4d`)YXn-FN)WqFHY?oz`3Ue%q|N8TDsPueY397^AzkanZI5b85fc zJY3e3+uC3?JLut&@c7!R?{=@Q6WOG*|IeqlajT6PO5K(oIu*0_hT_~qJ7vr7Tx8>u znX!46sh!!{?{Bx?&jU@J{{2&YdePIQ{?Nw}rB_4WPH^TsS;-e;%F;FK`L*c$T+r3J z-J4l9-=7FtqOQ05%_f0mQ?&!GTw4ulW~`WT=4wc2<=xWjHy`)g=PA4OWE4iOa|je) z`77Y|{B2)P3*CAY_Hf!g@7igPd~ZKY%ik|myLZC-T2Z5p z?|1HNAB2ocSp*yw2(gxB<>c&HC&V7dovaqHrZqIJ4-txIneP3b{k8Zt%cSDD#m)e;o(((zZx3c!1I^p89rdwiP z4)fcao4uB?6k zeq~=So2gY8*<_H_Wb+La(hMXUhfTb-C%b)3v@d4O{Sw6d#t2Gq9^EWyOEUWTe5E+ z(~s$KRVUs0C3-veKJ3#tX>K$ z1o+&aWuS!UX)1AU5mcM^VvhAXkN&kM^0YMr)_6P+;@!1)VOnR~(PS>21zfSM0kXMP zzx~tyT+|9qxUj%+p_lc&l5@+?{a&Itq2FOETUTjmX}k6Fb0>~BWP{t0L7Jd>`p3^! z%0%|^cR6f47SFH@zR2WCSHt&zF2>eoiTj%87GJ;C<-G5+O2C z_xE?ZZo2iYEvnB!`$jI_-eIT8q&}}=k?!U*MrRiXvM%0cv`lfrdIwXsE?!>VyL-P| zz1dd2k1qyXwM_78?3I$1e((SEL{>!rOPA@MA3m^wBY_p7jPGlkXWpG+aDVFD;`7(L zoZX*!F1R7M_Dh^MD3@zKIl&>qy)ts&zAdqvx5-}%+x>Ryl2=D==j}ebZtt}F{h8As!a?hCO7}>zbY0!p_x5a)!$#YMr+)mJmAx*rU8bmG@Be?lAIiARwYIAJ zQ!(43Fv;R@aO2{0Gp^iRZnfL+-j>?8pxMTA1xuho@b&B0@)?r1pFI67R3Ho5+7;Ir zU;Xvf)#4u?9`0XO-m%r(i0SC9=;E{<7&OcL&et5)voZksH=G` zxWTza28Q>(-Y-LV{iS*_xRQ7_1mo9?=ilAdM96_Lcr3-M=Hj% z=c$5g>J>GN&+jboQJehb^uek2JGtgEHYT;|$s7htg6o-#=mXXFkDrk<-TZU&x|pMu z(?yJ&q=Xd$*7RL(bl7NhZDQn=wgVp?9?t!CGyVOC6j2qefF+6-0vofqMWm&qepSAY zf3t%5I;bVcu_PF}YDahO;?-vZ1vRcc+hv>$iUV*1eHH7T@4q_VtWCRSpS`H`+|Jpn zt3I*Ll4(q8R5*Qyw=rp^c*Ak!3CkOb&xJ*0F5NKq<)_p7`ImjoV|SO=eLAULc9_@v z#tg&cTYIN*3wQ{vIOI@h{r+$78Z%Iz$)S{^;{E!ptgKsWt%&6f9gV+M{}(CSNvh&5cypENDEXFUSPVryL|gmvFI~maTSTP%kNb>UwzbLd~U^C z&9}eb@85pj?)RBveX`m4d%uSLu$E$)D+;oE#rJ=2&Q%r2gKgi%GN*X4(abXkN_tmq zb9Y`4ar^c8H?YoI<0|0~_vG`_j;H#*iFtnK!m8Ohg6b{{B3xp2L83u{6|@fP+SRLb zpUkngV%@?6UWd7Wf$7{%aif`MyjWj!>2awzEQnC>*o9a%q_Mx@d!4OuwMMpz@4217 zKJ~BLNa3uQ|mfM+2aOSmkaA4@fwnR?lB*DV>* z#-!YJ)|(;j04=-U-M+5I_i zMy${GtMc=hKliy;UASVeMjf9G$w6qeJKwy{Sy~t72n5Glk(>j@9i?pW;&X5@{&Fi zheCiEBh%4M7uD@&I2fjKA1VNw1}3cZCM2)E9Bkpf{PDDlvMgP5w?FoSBs(p~1uTt^ zPN!|FIg`{?yv}`N4u?p$s`q}7BX6>Bh@5uwUogX{3TZWLfJS4>TEF8NuY;f8dE#Wd z_}VYUfHhl1)AxbX(pI&ACWnuj#?iK?OIe@a@jkjiEkLJDt5&IjfhiTV+)BItb$jr_ z8Jk!WEx;PUge)ilq^BKEzq_7T7sp;u|@3%EMl$!?3 z$=mfZ=fVO<(D3Pvn$Ks`jnCU`_MK^TRLyTr#`}H0*V+F6^VuxzjKpk{%pk{O((gfy z;tjdmZXP?g#X#V)g{p7~0FE-EbDE|C@ zXsRb@S(G@~gB&5-9cD5e&7WJmS=@P>cH^TxpU-6%PsQte=h3nd@@Nu zM{JX*RRE|>^;Z7>k9N>@uw1^++wa$H29@G&eKMdWIJ!@!yjab|3f>zEY95`p`@JT% zQa(6Y&a(t1n+alN0;=&UH-3HGHqj2oAoy^XROl-|aF_2iv|A zcz&m{wJtB6+4cFHxy9!!w;f>OZc*I(;Sl$?Bf|bSKy=8Cul+I+GC)!&cDMI@XPqlPH+0&8KzG?x?R7gAfkqHmx~6U|xoQ)|bacvEy+*V} z@lVtr?7dzVdF!3Yy=}$k?oNs4kGVPxy1{_S3lj1Q0dt|-3-olay}899vfp$wW2>F_ z#?6gM_xIJl-SN2YzrR=LD^oMGXDX3-JD=tpYT^8LR6Jf}%G!X&MNUU9``g>LbngH6 zYc;RDolT3gr>|pHZS7uD6O$SLc0%WZn$C5f)ADX`*tqH1YBx9U1v56ass;GmY%-96 zWHZpZ(QlyQUG!7>*_?Sdt}4$duHIU29{xl*AU@vy)EToA&!g5s%dW;#!UgAY=bub* z6U%;*%hFZ#guT9{>(~9JE$7&}r1SS|e7o_uT;l2Tp!NAIg-i2VxkUKw|9seRcIC68 zN1f`RU9Y$2o@!uZ&H=5!&V6o`azfzu&*$@R?``K^72M#k(dzovL+P%d#H)hMk?J#V zUfigC-+1%aImM4}@1L#xNpQvb_4`kqDLaYcNTe-3QkB+?i{w^&9lblJc)M5<&s<&+ zc3!EJ>UTTSIUbhXt9;JKo^^M3dH%XLm$xnSxBpvm?qx>yRaWts3wir~rtN;%COvEY zX+9Cq!mryYlYMj6ZoRf?=d)SbEw#6HeOb5r-KOxkO4W`3LCXtXxomy&@AhX|n~Dv# zU#|pzJ7@h~C2rT7O{c+=g<&g;<A*&N9&q$$TXj>hSNwVgAnVe_zM{-?Vgk+^u(e zzsK!<)TMoB+eWM9{%pIR_nP0^5S_R4sNJ6rhoAi}{cCnRC%IdH-;R@E(OX~DTTSk2U*oeTiJ%iB@@_DdUyDrN zn!o?=HPEWS>K6;!4_(_}RlaF&;R`F)tySBv^xs?i@$vE7$^Euzppy}jPByKcAsbe_siwm_5Xk0pQE$M z`s zTYl~Ob@|k^N&&(8T^!`k)S`uU{q&(0J# z%;gmUHT|vM@44S!0=hBf?J4c`XFhlCj}Lp{@Nvs!KkjsebLRJJ9{M^NDt89r*Qhdi)#t|3BP0{_eW<^0gbFHeu++>TblF#@AvI_cXw^wrI(qz>8)s)mdLfgMHjArb@!D^pHtYj z_uH(#nEbmX^PVKO%cg;M@9*6A|KIP#HMhQ*-Fx&nAY+X|Kw?2q_0Ol%ogJo&`2732 zzJ4oc!Dia5%wecV%=_jD8ADlYlB_;~vhXWq^Ho^1wO3$XgoN<&p>jyI@YzHwt= zXOfc&OIQEQn@(xl3i_V!OG*2+I3X~4Vcm;4r>niMoMY>f>UP@v^?JNL&+DJh=Wowm zxAPci4rxyXm!!t4nV^-J9KxZd-(D>4Z%gn=oPKDB#*$rszuk@wof=#5u=U}t%Y6Zd_~w5v9;>+Yl>D(x)M2Ul@Dmq%Hchw=a$ZDvS95G?7rPE zWvUhOP4mW{f4^S)KAit-!=o1o0OjBy;&y_|S z!q>+gjE#Q0Dk=c9`Cr{{&J2q_@%S3U8yS47JVNSMy}6UO`)$ri)#*InW2)b7Edwp8 zmT1<}e!A3hrBYD5ym?*>#}v`3{4E#V7FGqlT{b&!laE^PhUeL~S4Gmky}qu$$C+#G z)nz_2FFjm!S8sn!NyCBppc=4RnnNUbn*t9zIQ~F{!gkPb+>c8)J2xL&x5#QEhrX|~ z^WiP`s^7+6UCeR!vMMw$ObI+t4PIF6Vc0%FK;vG1+U=P$w}t+XUO0cPzz5Ja{E+Rp zUIi^o`v33m;lHc?RlQs~-Kpc-8RPRU0s7(EDGOGlYkc_y+GW4Z;%r`^=pv>)|9|iQ zpS>?_*UM$xjEjrG`@J^oO%I>FO*V9fdN(LPE2f;v<9g6%`Ah=5gtlxEXn%kAsS_Fj zS89EIea}W8cKdtA_`J=su6=U0RV^316d!;VATO%&&tAO2e__$do*PjfTISX%G_2>OA?%3e4kz+;d%SjF!wX5!cmwba3+baYFF~0wMGtJE8Tfi#e z?j4}xJj|`0e`&Utf`)d;^aIO!dwai&6nl9#E-KDbw!K_kzlUqlraI3x-iKd$Es(IS zxEdP1_3ific5^<8n}+hNy5u98%+<6hdLgrG=)9Fox+^s;LUt@)wev-cQ|$5uZyt5) z^KI9t2`-HdpY^QdX6p1-V;0adyk}Wkm)M2+t_%qH9#i#l>B26xoeUgy2aZ;s)&1W=fA<0iqFaxql+-QUR(B=q0J(E-Js4_TA1soQzFrC{u z*Ldcc`tH9gYM72@@Z9@-^GwcO`+r}4KYlO%(&IwJ3J1e?i@FY!UXL|rsVF{c8vbL_ zxd@xTUoOvXUhc%!Wi{Xb_N;AJOVk!L$#0b{zhfBk_vm&T-Il`(9GeeWY+-#NSN-PV zyVaYHibdZzXZ`+8miBp#fS&N+wDN6N!=i73F6|C5Tyg5)l5Jlud7HCO1uY!4Xb#lP zv#~%Yr9R=;_51F}w>*%*xt2Dx+GQn5OJ8w<+W^vB;drbAG zlj_2|FItP-6<&WVWXFmRQMdP<+OyW|vX3!n1H^?xR_`v%1fOn@lmGQ+l_aQ}*KqHe zGACMzxd1eQvEOz5t0S4o@k^~Xvgms|J0C8(_gnm{@oXWRK6h{&x5jUW?A( zyCHNrlVNCp+uA9ONp@3b-ZBfly?o!tW720uXWW_>BD(VX{rdV9b~)=a=grFhV{^Rb zeq)7drcFT4u?zP$KaT(Zs9PV@vUq>NddBOYf%{pyWFr~Z=}u&5OzK?lxt6CfX(kBTX2Z;;jD_S^UgiFow2z0&Gh&>&gY*&n@%pDwkddUV8h>xeWG2_c{@`r zZX3uIy?nToN94Tqdz)%dtAEbsZ6=@%-fx#qkJ~ia-_9~-^??%5a@chBXTb|4TJsjW z_iyvo+o|&9;#CL3Rg=!#Gcvo+`sUT=#m@yao@t*eRTWVP&`~V2>sJamv6=?x495k3?6lUVZzhWqD|){OQMF6#@>R!5B+b-P@@~GySc@qqVudzLiQdk- zwW{Rl)bMT7n`kH$5!R}Jn7lq3f79cFFH&|`&Ja^cCv7Yc(crXpUi=@bq&;w z+r&}v{_Et4hm-l2TW!?QP6X|CnpOPH$2fYqkMo0#&`#$CN5=PmEq#4`_fNk(bwjAv zf)%!^q5O>)o%=cf8SsFru_A?w;o${ zSI%nE&7CQ^RU_ca{_Uk~^Y+QLmYlcw+%vaw*}u(i&X<8o1gqC;gq;jot5;QIuXkl! zswGvLw`iMgs6*MyrPD#1)~>w^TL3!gDsy+>-Qtf&#czWaPyIO&#MU*x_FLqNcV|R7 zRvDSx;tI?A6b#zIG+SHv_U7=?#j9k#_Av8VD1@Y}Dm4v=$y&K|R&&J6r{DNhbg1R4n-Mbb9

uc+qfS`JF(8$h&!go8L^Zku4JSzI_K{LNvTh6Ujp;s%T^Y?DG zeC|{_pJx%M9B$R}X-(&H5}lT29vUrIe8zCw38n5E;qkSiOt!3(0<}{YtVqv#9hs{> z`|h%tX;RW=IWs`x`ieZ)EISJSXK3GkzyH5oj{5J1?eg1RIZy39E?2$gti|IV#h#w_ zovj>SuEalC@#=!cnlo}8*H)Fkzjro0{nhK%ihaK>n(bS?eqWX2xraK(zst;voC<2h zMu(vfhL;U9gLFi}|zid%yMP6u+N!Q~Ta;^SDgwLuXPi3sQqw|{p=nt zHGk{3TMFzJ9r9$2K5zSd&fhPW{g-js%{2s_uGjKf^wZty_j`4xsd0&@`^*sdb-mcV zMjcYCV4294}Pv7&G#g=?NXPy6J(>Cp^V)IuW`t$8}zVp?E zhYaUVUXkGC`<#81m44ja34mV{jV2`73YMgE^ylVbXxQ|5vx!RJ*v|NZMwk8=2< z`C#wxa<_GR$1{|zu6F=I%D85Cuqlh`AV%1Yq#IyN*7%9$#CUS z=A#Rp+i&?8pG}#ayDjoYUC_ZrtDc##eO-8JXUub!wC{J9`_B)!xa^c!Ysjs2p|vX? z`8@x!vdGX{##?IsDiQNL1&0MHRw@Ou-I^3Uwb$sP**ed)R~i@PfL6x8F$uHG;=TzQ zuMv5+YS*T<%hm>_t}BQXZRVOSIs>%EDREx%u^!H=Gj6U8Xq@-!s`Zkrh`r9M%zE!R z=U!d){l&-Q^8Afue`GbDd=7oO^X<0VZQsMs2TV;V`IZv7f8mD@QKAX~J~vMZqnEPp zneP1#KJ#Y9jN`fIcCP&Z9X(`Z5%AdWu#~myrFd$~T;5L;oEaLsz$RQ0L^HvGLwM`` zD-p)gO3w^$SeEPBK`XBY2Bw$d3IRUFwbanie zdp3yGX$t&|@Bf}n+gAGKlJVSP_a}wg)1j?P1}2Ur4h;^5tla@mofQJsd}2+j;+P?* zvFwd5C|Hn&SQlJl-ShqQ%$wSge5);=&s$k4{R}>H8`9;llKJSbU%xh=d2_+*W8$1x zafN_4yA*8|CZs`*71*3J>v%+VvNFa%Wc+?aUp`JCGpPYC%K3ohcPF*Ukt?yyvpIxDhk= zna{ZCyRot^Ev>%rTy5~%;`BPWiKd)W6a(&@tFBKrl{69uP3KKx-Sa)%)YSBC*lD)R z+18Cox58RJ!UNk>=fU3p>u0_dQQmRDvOIxzvH@r{1f(CO#Cl3VWP4@0(}bv`#Zi=Si}Cen;7q_wW`t|FVXPQ|pYms3JNMoZ8WIpyYg8?=Jg|=DOp0nKU<-6FR5~Li|$6Me8njX5B zm^M3HLlQD^`y=?l^|-xNUuEoSeym@x#J$16&^G&^2RMR2ZMa!tAMW)VY_2~d;8%D~ zHukcHj~vJ(kS|vRG#0Ud;&iw5^K~l%jM~8GRD4)|0DL+D1Lo-jSwRP??dQ(io14ac zjDgMKx!gKW0X48DP!A$xhl3^4(YCg>clUmuKf9*9^|N6OC=#x*nq}qW?Ad=!@RC_Q z)6p$!IUd9FZ&tv8>iw-V@5;^oW%2yZCR2}tQ{bVtN9?+bG-UN>WfzWwt0yaMn>v5@uye!acD*F%qotjZT- z>AJdo*%=Oam}@Y;|EplUTi@)(M=tTX#rs39xJc;0e9+*q*};<8m3Ps)b$OM~@9?G_ zGyAn?Iw)_la#Xx8&&|r(wTr1OYO}LSz?z#&(;vZIV#TuOd;08~7j9&%D>(O8S$1)< z2uzNFX|A9~`GOUO<|ZcawPz)1e0RWwSAbCC@jc($%}q^vHBVly@L0~uA!6-&yceOn zRp7(D{aIUPSk;OBwU%$RSGwt_`n|*25*4il9h=Noo z=slQIo$>0*O66IzW`+3rt!7|kI?5HH(gr!GgMo=7WOc*$efwwH3eCQ6p0;g9vEP4h z@NyVXJPWJ{Xf$E%5`6gJU~{$KJe$f1-{TmQH2^yaEe_Ld0_TU z|FmyQXF@kKZ%;2dm#a3p(*7)Sdd^x_4v|W2Sax7!0rl|gZ=U%lDSg1>`JHd6(gOVO z8E*lN4hJdbqfI_Neh>{S0vVZ(O2t+zgD3JOjt8p$&zZSaMEbP-oZ|2$i8;`dl0ZIQ z@RWum zc+6%%FYS~b?A?F!%(gz?@)gDJ?nuqFyX*og1wetlDyqR@x3U&P#El*6^5V?k1kjSA6Ny2xc!+kejU9}rJmo> z%ao8#200lLK`(YOG$vi=eFJDWI_|(^P^R6o4QL0+gBV{r-I>Cm?5+^PJ+t z*(txECl)fY2&|}LWIDPkK1mIn9s`n@Sh`HNJ@63+#Uv-7w6gr1UF)r$ zPg_6nrLiw8x)!K`b}S~%oT&nGsRJ*kfX1~8Nyectd2m!;KJ(^(Qu59#mfQWMjva%Y zgT}zf!lmr6AVR{u3KG5wx}ejlA~}yWf*k@{H`LMa{hy(6w6)pYw{we?QGLzXXu{Uj z%+CMLvhvdt$9um`&#qbjeP4fZo}TgSn`d@iKXS;x6I3*AVY+t)Wi7XDa@u0!nP+}4 zmF4n(y^5hRDV%jDWTgfu#1!%w-~a7N`_?@3=zr@HzUOzYSC$`Yg{6E3rj_Cz!3!ed zXico3udlE9l>;TKle$>CLO1sKfID=cm{`HX z`2KHNT3LF|7JuvK*H-jgH1>jp6hmVT>m}5MwNI?`)`80rjhzRQ@BKbJd)BODQ~sQ^ z+2G8?(k1)oQ4&J$I*y9>pyQX4Yv;V1W4%ecwg_HhG&uZXIVG!c?dnzC_j7`;p5FBG za-s{^KR21~{f?bIYt}k9wY^L%T`yy$-N5J5fD(Vm+=lP}DvhHJ%{KDRE#?oq>oQXS z6vd#Z33$x(ln1m@O6>a2n0vojvuh-0*nwI-nM~HEW@d5Wna%5_eiYKUcFN1xALhmd zXPEB&jy#i-lp|qp{d}4J=M5D$FasML1X)hWXxKg4`#!!U^P(z9+@X*asY<(Z{6cj) z3x|mH;$ya8w}S~;u@CqDpFNY;d$(eKao%IT@*})paWJt`Tq7K`)V6#_@w=WY%!c5z zof>19%1zD8^7#CJfs*-lwH`!`<^>x5Nh|ZnDQdTTes0~VlSZ0w#~))oB?wydT=M;1 zeO}t$>^bvaf-=xly$#@#iKg0hW=Y#OCT(4-@fnhs85miDv>xo0PflBXtZn|EJ@brQ4&%JcoK^!A~be2eBakT^RH0# z1^;t9=egD!@FPifgW4>=e*LmO)06w@)TT!^B8)5o8Y>R0-&^(dm1WJ35Aoc$wC9$B zdUM`2ci=1Y6ejdTS6r37)p&mAK+U_SQZP?5G@cUH2wq@u^31aJHo9(LJri^v%#ogQ zGDXfo%@&koBDvil#RjMrT-)&d-~6<)qMRmmi|2joZ*dwM!pvH5jMYmP6o6ko-`hO5 zu6KXAk31vGD!fNxIlN-s^SylL&1D-K^PbFk-0=0vdKqSL;R^D%m3lxMWVPH}Y29*g zz%KDUP+cDy9DF%1Q#xF?zujTuwD9>mLGc7m5UN5S?$u|fZ9XPvKDqeaaUt7z$H7qn zCZj)p=_DkX$#Zp*Sx@ZSunvQDZoed6f3oqxN21z3U35Q9pt zZulN|`}T2d?|CKX)`qTeyn6lm{M58|F=VszV$>!_ZrkGjG&7e|K;u&#*q2~JB=kUa z{nnXpWvWlypIdxg{OdyK=@B4r1%xra|C^h(Ej33#-|Bgo&b0+nQc~x>-Q(8{`wu-A zs;JRiN?Lll=Fi~D1V2V5*El{{EHgAN;;MLm-*EHMB>u=FmfsTt<-MLm0t94ps_2J% z{U)1plbe5Bu-xvkk|)*X{rQZWiW?*4-psMyzbpML=Hm@>mwoz)A?;MMZ_iDe#f(G<*&&)71%e=Itsb{;)+&3Ez z^KCn8cDu!$U(V)+n*Y3*zQiYA&w$Q=+Vy%}_TOJ$H-nCB04b_@+eLTztLIu)*DnJdXqc>pI)}7Ejq&~8%W2!9a{N|XJfF9E znI*Un3Jwy6#%oNLOh?zB&9Qra=X|T#Yp(?s@ArJZwIT8Fn)3Jea`|3^jynR)Sg>?Y zy`iNT@aE#;_T1fXw}H0%%>1`=EojdD#?t9=S@-Mz*Q)9r|G&9h{Xv<3xcu?!ULA^^ zib@*y#2j6CRJ&R_G(&hF-U~K8GG$dzGKZ3gY5SH{OIt2%aeZ`0+w{Cp(z3Lqd9EB@ zyqpu)e2?e9rs2d3Nic{rg|N{_oBId-|VE-)n9v9+1$?Yc`>o z-!8(c^py%|!wqBgveic10vgjQ8E5hPFPO1`bs-`$qgc+Y+1&9X)^g^z-c@>Wx}THf z5a~{rdy9a^FYd#kUr#yR-fA8Y68iJ+`+EJ=Yd-xoz3*@LbBWE52hB#s&(3U6&YPJ! zZKCP52xqg~ImRFF`paqDtA4+?Rd2t_->>iQ$Ad0=F}M~Q9;;gX{G9H|9Y6UlZOsms zG|if_X79IK52u0_8WdfREkAi$f4|P=w6jA0cdoy@?RK7a?XNGK=g-VARBk!AZ}OC| z-AudR?aIDU`uaX+V^U}>J4+X5v>(&a7F~{~pqvMG`ceMuSM+M-{rP9CGB2rQW^}CoAGhJq*K5)GGCO}h zY?l|?RPswmkiX2|@~KE+a{l(aWwU)|7&tyVJA3-o6XiW z!p4U;xq*5GWe+(;gtu)0xliB}ysBK~df<$o(8SG)e$H9BYI(@%KQq`7xlax-^YdJuYf-q! z#LR4)Rq1`hs&6;byYJV(Onl<4zxT>3Yny<%Oe|ejcdk=%S}s+kT*1pO%&Xjkv@vSD|p`>Fxaeu^BPtpBAUw&Hwe~ zW#aLSeY<(td8Jf9OSHO=U-zQyhEZVp3V|RSZ){n81K?P34boF^Z?tlGoufDc+?~mHLySqxccSpY8|Nq~In9q+> zt;^rZGzI7Hv)kvl?;hw{d5ce1`^&#t{pGkfJAa?0%>P-dcQ(qO-hDMJx-~g*dR$fJ z&o`UTKl%Ip{&daYWi8eAo8NxRiEFoip8Y=UUjP4n19Gtk-3`?ltst=In@FSH;ObhzQ6Y5fj{iQG=m zBPSFZ9JX@InE8MGz0^fNb(XG*$4HftWg5RM7iyMH=VP3 zz2(OC_Qjz4yu`T5Y<_&)#l|CX;n8lnyq~pG-|yM{;8;lg@#9~=?Y{q4k7Yw^hMd>i zz2EOWQq0`;?BR^t_ew7Na{sm96Z!w^`uT?8`9z8qTf8y)nqT20C*cZyJGR+5V z%bXD|6K2l;XLC&XMo$8y{7_-z#3m zlfLi&4uSuBA6f_g{qC?#P($odx!0oK3IRShJ_%^3FHqcX{;x=4om)-6t+9J<7baTItcsgX*h(?f;^1e8XE_ zk-ev#`E5hC-F)_Y^}hdK*VmuC|L>dm$Jo-JX7?kXEnLuUEH~?J!C~IS<#X9RvsEHdijMZHl4}!{=pa44?#Y&pJOI8}c0XWlOBcRQRx=c5Za{IqSocmKjd z=c9+U`M2$PW^v`qfwS*TzihaY{e5m_-Ss8=L5pm?Vq;@(m_E0D`{!oG@z`Vc%Zq>U zovJBUe-<_0N8!c!AAu|6q^edtYn=N&k+2`$T-)8&J(Qn(_w4Yxd z9e1r-@$`6lT2sgJ`E^z;N&U9pBCaJ{8UOv|x61zC$Nop|*;N;wCAxoptot{TKlP7= z55Fbo9*>!p#c9ErGaWA0KU~-@$MrY$cH@iPcWeqi9ynEgI5m4;uwLNKeeU_g-DZC|0`1MNmETM4 zJCW&-{CC|DW?<+UL9T|E+0n`ZtGt z-r3&S*C*<(`L?*M_U)hBpBipI4_WixzW7b%=llcLPwl_ov+}y2`MZEOFW%2Ovd zn<}E!ro2pPW{xi3x}R;Dzv|!n^-sbMtIT9ZRPEhNHXG&PC@v9Op05Iz zvq2Yl9lG13YkL3XwLfx4eE!Pa1D))syYKaG2iqNg-|c>XXz$ecx}T{P-zItMaW>w} z-F{aMw6WWIf5ZR1&vt+K^TO}`!N}P5J^t6b z!}0$v?@bG$ehW?ECxA=iGc6apL{`=E(ngMvkR6(?Q3cXtsP)F1M}d zmu^gIufMIbm6_>i%UYhsq(fpYMX)^3m?dCf{9pTQwo7rObLjmQZ95Iw5&g7ltVOI{ z|F+F66`!c=azy|9>zh^mJLhY-EO?<@vEthL`28DRpJTSM54t$JJwZ<5XW@j8dYk@! zzaJ0U7f|58@P7D=)v0BXru=zEjdR|=`FyuZ*VMl1^P_J4Y4dEW+b*0xynn|}K9m2Z z_dQ>fGynSAU6`#I@MZ4gea)Acb@@v+z1@(ra+&J%n4mv>j^&>#e{7tqYkGg_jFmV0 zQ@_rwzh~5#?ts=H^c~F6mHArt*WxTQpM^rplDhUpwVE?5U2^Z_N-j7? zdVfexsFl0-Xo6my1>c*R>GS1RzFzxt;_|cCjov8>fBBVn^!57vaqISeyY=C*Pp_O@ zb-?V`D^x5Y8wtphd`BXf9*u~zzZ}+MG zxo58andh`Wdi~Fh6K6VHe6RcEv1PI8{^JFp>p&j=%sWyMKKuI4_xpaQ{hj&!X3&C) zD}nAO#iDZ*>&~o}Uw?C&*!xMJf3w+EG$Se7CY~iEk0xT>0b5wr!{Ba2cMDI{L1==(e41_#JTt9S|lCmSX29BZ2`4`&q^z{q(G9N8`e5|!vz1VdA=>|sTCllS}GOaF|ea_xze6d_(+qIhQ&yGpw z%iOtpKRU*J+uZt@c7K1L-}CtI58L_oxPJcKzU}Z={raEYKk7EUWm65v$^EMR?K!Ah zU%UY6+9S%3e@wZYBA?%HeX1V3@=Ap9>3M%j3K;CA zE|+iQl{8ZM^2GX6-p;4fKAf+czVLlUjyLE$@TVUR^XFHo3Txb}dcC&jYH0Y=H>aFW zf{qq=ylYvosEtma1UGmm% z{Vnh~fBjYkz01?(8(){Z9kWh+>|hu!t7|Fx>34o^dsdpdc4JcZe(qJTz~O-2eFKF< zva8C!!*c{z{c87`Fa8xivc>U4Kft#!DGhY0$TVK_I|g@RXY4#}u-W@e|F16z8*JC= zHZJ-;f2aDz=98dWhozwI|Kk%Al@Hth?mK)&wzAMxm$j=d=J~YfyqoJ}yFT_g@b9wu zkd+NuzB{{&aa&nVR(9?G2kiD7-yN$?r?)?n%3gD^OtCZ6^~>wRBhJ@ZCT@TSSE<7umYWh_14{R=W11ghUZiLW^3aIw7Or=Q{e{PX>? zU!S@6{0dwQI)9Pl*R9L9q%@8l?@4?2^z-gNfBg2x9B&hQ%;&m4`BJ&puZi>a6@!n9 zGH!g&@U(Uwx1NW!q5K zvgZH&XXP5x9-X*cVxB-5& zJ$-Clt$+S;iroGay<98~)S%T8Mr**NFdBcZ-#_h9&*e#QuySxRrWx+(Q_=+hG z7t2@t>^r=(`1v*~PyRVJ4PA#1Z@nHDea`0dnGbo@<=3~G27K9QvCSDPLCCHT0QNS(PHhy0-rBhIl)IO)FeRX$#?$+H*Sf?PqtSN@VT)GORz|*D1CP=-r*n09{y?0 z3ixh%hXrq$rV42MvTYHMui1EJo^5o^^c&9nwkr2)zsFuXxo+F7EX6gSWh?*1s69B@ zVq^d7V*jDSnQ~uJ56yZeTbXcubGrXb)9hpI^7DqT|+{pXc28W2VOxofL4Z(wLbk%PYeD^KZFI0O$hT)E^%n zKD_;Dw{d2uvID3|rh?WaJIR!0=H+s(?^!|Ls(B$Nix-t4ng!cH$C|%zZ?7_V<~Y-w zU-{383!t4pEp>H0&x=m$ioctCyX3O(;XFP2cUPR}Y`<5vn)7J+gr64|%LP;^JU^aq z_w&i*PZ!##>N(%R5eOHHAPHZ zJAA>7wbAC1#%VpE!$%a4e73A8c*PgHyKE}2*$sy_K3OeLm)GXskH-&h?x?(ATJhm~ zegB1i-Mz2Z?S2AU?6`0-d!+rJhx|+H|NndB_~4}a{5=j8(_f}{s!kKwFMeU~-b#;W z2Ooik&p(-8!mUER(Tp-t4?xp7&}#pA|6rbwNS~Obz>@_LRCQYqxJcXQh2-p@exnKR?Ic9XBkCpPg8{ z{hn68%_k2FwyTG4GcvQC@UQ=Mc|-T*`m^QvyRw&wDh23V`F?+*L*e6I^J&8VHikZ@ z%V+(XyX@cHU+>PJG2E8>|9{e+i42TPM_F{7Vwlid8e#^UXS+U0zbI1{b~Re7((el7 zz-Q2q{e`kdFAkB~FmK(hQ~o>>-_MaaL%wn5>dFg`6J0+C9KZi@aj-2Iaf{ok)l z{S^jJzk5~r%H7&}#b&}+hpkJKBmdsrwae;ea=&d~Yj3N5zqg#D zZL=x=tUo($MCV2Rs9!h5DBw$SxNX7L-vWQ%dE9^bYMvZN)o0VM)@sXrXHP4Ylhcwf zzf<^VcS>}Y+`C^EXUmU^!9waH^W|>6T^fH_8+OPR9ANad&bKQ;?Oe%kzv|P}@VJSfk+PPFXZx9R z^B26Z{`Tjbv;DWr{W*Kz?Rs6b&Of_x(f&6NXTKM?Q}udnb8~ytzS9@_FYWn$ulgxy zb@NAy;QIUJwZ9A(yKKo~d z|NWIOtUdP1o9C|m`M1Nljfb<`=GQy#?cW2=HpklYZC&~}(_2>K*SbS>e}A1kU-xaZ zfZy)f=dU(j2MwIP*>d{opNuPK&2H<=*g2j3shrX7-Syl1?S5rkn_Jb&C9-#0U)_IM zPLbN0`P>=vJHFZN@0{=Uq&#t)yL1T8$gaSMI)C4|xVwYHMJ%h&xYR^6X} z-Wfba2C9z*R%kYQaf@VU70$@H$@+=+)2TQOR)v5#My8{me(9T7p{Ha~gU!vZDKR1c zpRW3~+G{RRSpHtopV7BCONH$dcvL;IQ$S*qoQ!JR2M~`o`GA>;@&5=xX%DqLq-gzRTnN ziho-}uGaU$=e-r+>nFDRGO={!K6>=<79>D~(Q0oOM&sx>7rU;k{~n>?D~{|8Fh;aX zkAr4AuU)?$ub_0iY)?^@!-5}7EL}&<(^^i62V8ymvSe5O)ALKJ(i|2<*k+&ffjPf% z6X%SZ@05yaLk`%7qPnV?X(^w`>sPPNgTwpdx~y@PNWfL~%P+qi3$@(IHCF_D z3Q8(GG(Ck3Hb1LZu33T>KmiM`G$v_Hl}b!Y&Ymz!6Qn*=bHS?e<(FT+U0qZpYNg3^ zlq=Ta6nxRlB$b3T{_YcLfxgUWLDkHZ$|3UIWOFCNMFyMqo0*x(MNFN%`husRM%ar( zf#AbTKn2wV6-MLDE98Q&`gyMEkAiuck;O|ppv|Gs*|>Vumdz<9pg3+!;#~1+MTu2y z-o{_c?{DK4(Aajj#~h*K(U%K36FTmQ|GmV#YL}`9xWxn}wrU5wa=5tk_3PK^-6tMx zU48BhXlceP!GNoZFH3%{do?rg>G>9iji0Vhkc1EGr-~YE7Iu9yXGMqMsw$_*?xX^c z6TtI!jhncpXa?-@bScw&eMC|QB(^|+skr*fi;Jaqb`%zKUzDsxT|NU^wX&vRrs@CY z*>^*}9-GCQ2w&XBz!cQyuyNhGcM74h{NPTE!%=RZoQ#Ycs~@EYrh?9=*~FS^1@;GM zsoYA|GjDHgf>Bh4)m24*~JF0@Uy%o5zaN)v- z=gRgR*LDT%4N}sJ1w}Ae@~U1!TCMAo){J@jtFz{3%{Tvy$ipXe1Ns~;YOh+oT6*QG zW1vmUG;t;M7 z*f0HDOVCnblf=4J;Yr|ieGQ<56L6a;l~1I)`^4U~ZCSbNnKpw;buGVy`)zG)tF_m? zlG2>vu+dFyRwmdYP{bY;FxVXKl5%#%d%IP?iVTk}g0DkiXgnpTk-p%CS7~YK>(#He zDm`-lav7Ymg!B^9*iFpL{5&dVn#RgCCi&{yPX^fpmc6B)kaoKB#PLA>`75%{fBSx7 zHaJkh#8st$cMcb2gI9)Z|M~fO{E3Nwu6_zN2}zd zHEmKS$c12yOGOPf-*tWRE91)Skkady!VYGDg~5cUh=V`uzS;0AYNmi!fPsQcCGfx*J%NeIi*NabT>U&LLfaI) zLIP~@lrRO)#-y}ohue5xTcw|yqnP%LG1=ZYIySVr)1zGQWlWT!8z@e)oDR4=dHsK0 z-j}|azVENByqsKjKG$k)-DWkGRT-05x`zcqf5dX;%>QMH!NH;b?iv-Yc5#HuRDyOo zK0dbsA|T@3VE;vPn$BN%M2G4mq(yg~_xq8=MU#Upt6GtQYZWaM&0y z=?gq-TgLby8rUJ~K(5YmQs``av?1~Eu5&Yu(>rGJ3QVj^+qP@f zH=&8|lrHU56KnGXrPQUO27*7MTO~TJASjPIzMV<(FS(FV$_mXr<+_;6|Y1 zJ$Olz<#FJQoY2Ixi#7SfuKv#IXjlm{0}^~s6elck*tl=sJ?10Mt||F~FQ>Twv;n&{ zAcZk>u2KEJKc}UorPu3z3Mh$JU}QSV zG@5a#Cf#cKxb-ub1j~w}J}u2{nwy)(bXnEOh#$z4k5_ zr$~1B+nJUyH(pRf5`JHsh5dAC^zyiif`I&j8LaH3pb;N8%#r(Hxn z4PoKHz|<)KS#M|_6nb2RsZj}1YbY0ZJ3Ajf_qK9kd6%}sf{2*-DtJV%ayoG4n$X1Q zi%;%ZxGFp3=#v>N2*(`+tvFm3wDMePKYAO%@K3)j5tc5$hm$7>#!ax$yP;vI_~-*F6^T1r*fK()1^&_3`@k>wF=>zLTr} zoNos=rV!1dg&utjjY&nOA zfL8KthXe_OzGM7Q^@^7BmDC<4ww zAXh0&;9xYi_H#Mc-zB+v)xT`l%+k`*^i{83EGqF^4)(CBIJ9@5DnElUT0|k>$zw=T z0QvZYW(?)CmnS!pzWResmt$aB zDhlnrWV)wriDCMaC~zNXp-wkb+B0k6+V-Snf4xJ0J0qgE!9kDFSXxP`+~xe8#j9?M zdh1U0>NVXgc=Xl$I&frr>3s-tc0O#SDnAv&sTqL>&iDyTEMCm67j$*80;pvIP67fB z3ey{AuGLpM?{a?6f>nRJmb#%YCw#+<)@k#Y)G+g|g3@j`)3+;ET~}S{mW8OWJ?1sc zT&tjDy!ez|=+&oPp_+|J8&h6_R?%yK_8{DR!6_o_=8sq%@`?4#n`FVx{|}b$UR5{q z FHpo$R`xDE^MFr9t>NNl3ws;Y3qS4wE7>HR;{yxAFbrAZLWnKj!x_DEg*ylvG# z4b88CNTIrk^~{?r!JF2rikeo*1@x@q7S&j_>d%kk_R|=d**xCg+q*RV{Jc#kl)6t$ zkFV3r-~V?TXtd0z^i{}Ae4>d06eO(V)!IXIJ)4m^%xOYD4(tfhA zT`uauPA`5D(8)(PYro%h&Zzo$RQ$O~MA^;Mr_=ZUS^DRRy8X$YclK0%e$pnLH^FnV z+Q~mZKXdPW^CdiyDfj0wt(>eW(K3uoM<4CjwmY2ZXxZhDO0Z-faF;2~EY5|mE3IWz zg8ff!$-JEM?99whSHt5^a%rzoc)foAzMRcx%`R=tz8-M7g;V$m=nR2X>$f@-UXLy3 z-Tfo&>?~E#3a{+Ar8OB>Rs?Rk=qBANJ7Xu4D}Ri8znrd|R#o-uwcDS(+x>o8`MpZ_ ze}CWCd-u!Pru3R#n;=_uLvcrC^_Pq8hu8UTUwU<&!@m0wZ@COVr-24MeyebZ2yauE z0&gj*xE?qYCp2;E;;L(5SHmX-$(JIEIF_s`#<^f`TX?j@%6UZm32#=3%biF&d%B7$#MLg&1av=Z#UBqZ(hsV)jB8N@YgBz zHy;H)?)iL(V&9tlZe6QwXRR@ z>GX@6gyiHe*zx00_vu#gIE~Y~+vn8$db#{z-r@6hzjdC!v5~QUv*9pkd6lBim)(MU zuR1NTD0*_D;LJ+-_+Pv4|MiQjc-R`Yzh&+AdsY=c{mwh{S-R}|`D}LB{fCXOL=*zn zoGaCRw1z{ZyW{q}b(|u%k()nLxo6zWQ~Gp1V^;afR}-}6n_Wf}P#R4QGw13mE#6cT zyCGoX-o{6IyWc4Ne!D$h<81T6GkJ%vNr3J{Eogsm|Np=8pXclM{jaWA%i85GSGgo- z=hJBq`wkfE60-_q$!2{(L&!ec$m1Xo$W1-OlIp1ggyP&mRB!*wyxk zv{6b&MZMg=y}#e>e#mis_q$!%UuI^?t@tGql?7UpI;Z%Y<-_Bk6ANz4eew3!>-E#K z*YDkCx@_5eerYowxw;>Tpan~t_FdkdAAhs%_uIpPE9Ksun`avhI%n@g-}{C?pHAy< z-uFEF+H!ySoqxaGPW|)abCN@s+UU(_Pn{BzyI!+efIm0%T;&W zpZT(^E@buo{|}n^^-S92i>ps*E^mpqf3cwX$oum$1_=%u{_g+(d;f=jJ(9*x{{4Q> z|N8UW?f0LVM5au1b?S}I-5MG(d+GEzE0!CeTQ7ZP8mY?gO0V}{w)t5p=mwHU?l1Uc zECQ;&y^-8==i9d1d6PkFdeV78W8pcw-)`IVa@lOJ^>KTbcu&`RxV!4z&gX|@?o~YQ z-Bg=gSs!2%p$*C_(V0xBD`h=FODBZ;CI;#*U-{~#=Zan_XpO_b$fD(c;EbI3#D9@m z`{P~yDJq4pes%rr+{kKX*ZXsKd^)Aw{yHGh<%2hs@$XX}Y4_b3&$KonylA3+q=Ccn6hw;MDXr`mh{B{;BnR2r#XS`AjbD!N` z|6*Z#+k|yXpDE5;AG~dYIiJYif5-NIyOq5#?@z`xZoM52HZorwuD?HZCavc6+U=)8 z!(+D|_+2>R`|9|=UVIi08kR-X{)^lHKH}@Qd)4n-l?(pfxRZ1C{bZYWJ03rAxBnH` zU0nP5thw`AIi=&e>mI*Y+&@iF*{#LFI2v?Ow~2{K%HNx8(m4wM>bB?A?R|OZZ2!lv z3}ugwbaLM*k6e|%X7PRD_u+FH8j}u9n}W7{Qi$cu8%Dv-{~}4!p>rppbT1bev7UJo zD;T-7^;PZbTUV8gf?e!HCVu~Gzv|Wh?N_hObGTT3D7Nb5(uMC&>+SY=x9|76u;VAc zI=C|(tysR~y}{?XY5P{6Yu3N-x&8OM-KVowuN5nkuF812`=pVs-1n8s=Lzi>zT~WW zf1*YDxj7raXXH=WdM!$O&!_(SKhAkO9=0v}zPs%2zcgn4S!Q<%oGTwRvOnq4UdOTj zne{^>_bH%dnPU7$|2zj>m292zRR8xKKijV%AMbYFGur*f?#Bb>kF#pzpj%9D9DKf8^l!&C(3H`eC&lvjXM!hY z1vbU+uUi<;A8lK4W{bs%&+n6WzuhLxFPFdP<1qo5Uvp=H?!@J2s$G8n%uM5_HrXTHU%on}qd2?~GJ4ZwLox-@^s;X`<%aCtzNH}u=UxjY&Pfmd*v>_%nfRPec7;2Zq@H|5%+Ch zf^N~9T=J{_`kZO6q+fqMOz|0frat&e^DvZvwybURuOUM1=o^bLR32; z?Re*jpRMy6+O^H^mITk<_4i|cy^i|alAwy?@BPk{T}h6%%E@2wLh-KrzYpw@B^O-} zzuS<#ZfDxU#J?+i_Sc_dUKhDJtu?kxZc}CV@vr~w%HG^)Ei=0Twfiq` zs61V|f3Ez;SMG~{{nwdckl1vm=JVOyg(rWlk+*)8IPbrIf4AN)4VI~AXR{Z8PDkVj zu4R7|=q|hUz_}AX3tqf8H8X3QTUrR35j$F*`|FRb#b?=u<#Wq!F&*#PwYI%hZq-ru zJyrIF3ts&HdrEsfPh$S(XA58C*Z;n~LG0$W3+4N(;_GbQy<9$D?$DWMXPe{e&mF!3 zIxYTX#m#4>KhAmZ%glbC^rhg+^M?zU$O~70JSuL^`|$NM#SQk46`$NppRc>Gcys0F z2Rrxevs=?CfAKYA=E{8=`RW5o0(!xvQ7Bqz^i<5i_<#0Fe)m$FrK_Z;?$$d7??DQz z;A1rYJ+mW5LuoI!>EdrJpbe4J?*DmazL4eq$!nKvYkyw5anIqS`2HWQSFT=dRW|;s zaXPJj`})iZe!2f&mfN=moIiXJbUeqy^p870mk!+cI(_ZWy_KL=*x~*^CoWa6&-ov< z&f((w;yq6$c^}#>7xnYqu0E6XH_Kap$u^bWDRjTwcOs|uA82*R!Ug*uTYLSTx8LV9 zOV>W-j}x5vPD)MM`SRK9e7QKwk4J=`e!U(apD_b;2yPAYzUm9_tIuq)2so4a`uy+0 z752YxoRNEt8CtRO>{_y+OoRvTCPM^8qI&Dm(3F)U$bh z_ZweW`x&1tH>i5>()6eK{Yx9y{L8Hrc=`G*gH-JTHc?uKfPb=k@z7uk=67+2^fy z-@))U--&Izjxt8d%-MS7{Kw<+`k>a+4)=@BM?e8pw?0K}OZL6v=j;C!doB%1Y+ro8 zdB)nKUn>^8IJjv0kL$JV@!Mjj%=}l=zWDyv7iV9ey7TkJZi8C$W$hJp@hRSog=xB= z1G-+`+gtrZ?9c3sIQMmTx5#BwZFsqG8T*o%+y4LiefpgBdmWIX8y{z`u6*(4VX@4D zWB1E8?%TMn&eUBO78dEUv@v+GxhU{V0*XA%qskie=JVNoPGc3v$EA=W!=Xcj`#c3zF644 zsqEtF@b$}*4K}~Ma{Bv;%iki*^)pUPP%QdzkiA{mp`Bm;-m70}?`GuG#bm1me5n>b z-Y-9Y>wM5nV82=adPK)R&spm~-_F-DRYvc4<0AR(R>{YD=4}0Sck;g1m$E<3+5=kE zW|VNCVFPqITCUl>ipQ+iEY>OCes{@d{!H1vx;?M={eE}bWD5Iv%v+C>D z&COo$qBf<>y(;15!BqEI_b)&H{?*ie*5;*l#>X$4@q^ZtJ=@%UwY=e%--RD`?)FXF z;`i4*{Qc?unyt^=C;h5<{96XPq%1WnBlpdZ=kj$w5_hcLJmr2w^sC1=XMqkuzhQ*d zC)QZrFmtV%Qof7np5?1nPur|^%njU$1rrNQ8^5u1-CNF{yn4+huQ+$dZDG#Y2jiaA z?LIDFAM7ng9Mgx9`4RTKV;A_-Xt9KhJ+y zb3fIh;K6}C|)g=T3bkeR{jxyw#G7pfqcWHp5)RHRGnE zlF)on6VK54z;m-Cq~RTNhQ@0wx1=?G?JcU?KL@mm!+T!cub0=_xa{L=J{~>#e%5S* zM5i6I&qrr3^!9FiWchrK^4{w2(=snFJLxJOJLUS1kIpbp-v2y<)5%un^97yz&vw6( ztNCzH;7-+N+3J1S=Vr6#?F%*x|Ge1i_xJbupcCNcR6d*O+ds`Hz~=3i%THRx<1#j$ z0WW|35f$J6W1s8&&X?9kf2}Q_OmP0R&-vr5>~#}QPF8RKXeFib>*Mm>Z@0~^`So)7 z!nSSOf@+g<*LFW&4Vson^!7n3*XA&#Jxg5F-Zisq%BpRD{Y_6D z+h+VQoT|NcOAtrRAB_P1*do(VQGWLAH`g@F zlZozPFP1HJDD1cWHev6#TiS1ae15FA|Hq?lv$gS>hv%J3!hDkN}Koj+1cz{ zwJ*=F1Fae>SAVwa@um59(`M&Bx!}ytyYR7~yNqC2O4aL(1v|do$~I@+m^bbGuA`rI zo|<2~_5H~CY%fq5f7%x_EWSdTAL@Y z`?&+O^k{LPl~>)L$MWqLjvqeqe184Dmm52U)%CVG>wZ2wjp^vqY0-LbGCmhvE`LyY z;N`;TZO_jCsQOkDp4LovXIawJ-J0j+_iNQ3AMY3EzjbF( z<>zX}+VXqvcE7(TvTfPrm&=ZW7CHTXv)RAm`up@Rva``0KeVud`q1-Q8Wz@3^Vnxlr1+YD>W-qBvs$HN-uC-l>}%!4AlDujp^2~S_x_VlCb}I(*Mba!~Ej> zXZHL&=ghwYv@vW$@r8cwy9v#_DXZ6R%c^>|YW2EF(fNBz8-?_DzcG3VDjDxxuK)M* z`F!)gdzLy}l$WrnC;)X#7x&w7{RM5Qd3bK0t5~E<&GdQgUd?R0lce)@IOgqoxolY- zx2VRu?f2_8oz~mka^dxz;`6+Xld8|mFidW=tkSJxS^56emuJVP-mCq7_tQD+_bpYm z*B7=Y*ZD7E555Q5cvBFcDq~Ys@buudU9Ze0f*?WOx( zpKspr^73;10B=p^qw4c2l-3*spZ5Cv&dy?IT{)3ohKua3>+k;)eaTVtJ({Zs-F|#3l=vn>VNDw^ZxVayB`*r@>l8Y|MN*;l4<_F=5zf{ zU!R@-dAD)qH0^*d={6@mDm(tII~McyR`&X-pwX(A6P;RZ?eh04Uu@pz0~&ftd2(VR z_teLY^Y1I}{P}G5^EFqS9rov6>~H!iTX%MLdvx8A=LZ^@qu(5NzR9h>$Kdt313_$E zcJcrF**oOaz~`vBCvDHW>-2WtXV4w-lR-CNy^g5~e_nW8cKYYvRe!%;Km6`kMfrmA zunvcdzax`-O%Ivxd(@@ZIx&%ZCQfLGpb&&!S1m|W{Gly9n3cxkOx8@}Xy#^(=N&(6$DJie&^tz!Hg zF41ol^Z)+-K7IcGKhHlk%l}b$qxMBMXW!-xOSk=cwYpv5O8K@*kEe+%-XHiJ+gs;9 z|9wo3Twm(z)8(3=%R?1E6cvB}s%&Yy<*T*u{$HGhY(|7;COUsmyd+MkHX`x9STvrUi6TDhV7>)!A8xIfo`hW+x+JOf>K@%^>pyJfTU zq?}^-FaDkV_5Ho~e6>%fhCgX!mz%JBeqB}L-6c=|9Q*Y5Kch|fRB%%-3{-7$Aa({n z6*Ab&?xON<^}*XKUQH>wdUyx8I|m`2Xa}rtFueb)U?qn~aX8!;(?w(*_^ zRRPF*H!WGtL1tpxy&l!i=gnE5x@rL0?R>-tr3v7wVwj35C7k`2`kh>x#t z?2w7xDx8v(DyR^k!)@KHB)}324x#l8!my$31_q|7N)x=6y3M$G!)xVqD{v4Oaj$rF_hm^{^_)qd z6~c42EiyMv)I$w{oq2+|C8U}!AjoN-fB$;d|PzeMQX4KZI0^pKpDE9$W{NP5=I z;N05&=U`_|(p<1=@uinvcAi|b&TO?(z?!qA>DOS^J4|kv=_@|bPVnTdRlm3($JapQ zLMDMOkI4~?+%qX_Sr{k`8{aUkvQnArcfZviGt^@aocY5a^M^mqD0kheGR;aw9{8FW zg$4(9)>oie&53iZZ}6@M%?NW$iBHJum$Ti~CvBdW_tSe}Qx8L<(tReFPXZ1zrF}CN z+-a$_!#|Y$x&db}Qr|qF5i;Le(>l%nCfK$Ag=gHjaAAYE!bu^80H14B0f@QzAV%ZQ zzd8Im*leRi-KTHiU_*=}gHCE*8M12g%(EgB=dr%%(*p<+p1SHxaKh}1$l9+9%zc1`FObc z2W^D_p9hNq5C_p|F&fX-SMvSuw_??w--#7 z%wYO#RsL?z!zG@R=jlIF_;S~d@!x?J(EJ2)7e~mRhM8Z*Cmvti37WCfs!Q?#_cFl5 zf@Q2hD?{=HcV0gewl zB2_ZKkWz_)CKIkDU2~>btpu&dS)s%zJaN72lVril<7bwYzq<0$aQ}0&xqka2T|cks zTq&S&?TC>v=+rE5Kqfdc8sCn0`6u=FRZ8exX87t`uq2F_#Vw%m$rt1{a0)loNJtCs z@Ti%7rf`+jHa5)}Mz9b86?#x*4xnN88YQp>&@?w_4N14sCO?y}TSLBHm0FT;3MK_A z`d}Jax@wpa;u{-gmI_YHUlc!k{i-{XhtuH&E7T5<&IzFnvRyS2Fij44CE3Cu4*0=OIM}SM+un2!AC-@>zE^O_PI@{?Z&8Q$8Eszk4A8TmUl$> zSHWXMNFS05S6o;dTAKBUd$|LsbV4LkFspGB=tS<;xsKp65Y(#v#5v=pjo`~ED;+|A zmLK}0%dQN{r7+ilyw$+K)G7Xezh7v-iwL~*67f55#!P5py{qlMC97VGsq6447=h&VI7;Jv*^5oBpi62+}^1guPDF_#NK^MpnMQCQxe7|~C_;mrs zV90VUSdxHhTFlUxBz>>~ylNICwIT(YC7xXf4c)))iDjTQEQn!I3`z`ztQ;cUuf5;H zeC&|KawaELuu@;^MRI8G;iE^ccqzi>Fh4GtT_ z!sk^&W5?kv=Zu@+MEntyh-Wn?90EHFjmQ;J2*?o@=ZETIV4A9#kk;LC;?9n$(5LrV zwTmrRf=_7$1sKR0h}9Y^8R|Pig7OfR!YSp1G-sD5eHmZcLrN1WS0%!i^&#nO^a3ru zTXJ;~xLyRMw*_;V(#rZe=1A%Gy^~f%$aVv;R8PZl7W#WEBwHj zcj6Oul^a1x`E8d$F}9>!18Uh#ePjufThPRMCTCtp$tj=4-@@7zhMI^X74Arlji75B z4odQYTn7@>*xE2NS3$|&{qeq4tFou=;yVV

`?yhsp{cMy8{uV$4^;OVv+e2Ac(y zjLfPYu3uG^a8LGw0xTUM;<~}Xms>#N+Luqpu#;$4ureC&j&mvNf3^$MsN*h9N4hW= z5p}YllZSUG+QQ40PBDYc{H{yFLsG&+|5{yi4F(@|2@gPsl@3WP93sM-MMU6blStfw zGhsp#B?Hg@54$SYD&Hvw%l0tugM7CNl(eG4r&Yq&>?$Rt{q8tnuhcU;v{bwRDI7q` zA>0$;4GxC!n)BeHr4?}C%(2cB2LoRoU;XN*sD-E(Jggu(z|a6!pUwq|Zo<>e3TU8wvL`1(nn*;SWcwKFV;7@0hyZcwaszW(o6dsHm z3IU)+#7s$-pbM%w6efr;8fS}4v|Uv7E%0hK>go=JzK}kK#-yifL?H{$K;o6$Gj93_ zMk-b1-S;N zXu%9acA=`6LV%8;#bLNBj*1v;Hg-)ZF*zn5`t?OppXYsS+OFz4EQq-K`t%!U&@VW~ za^{VlprnXz^#;^!Jx~RpbesVSz}+`0IAK}BV_Cz@v!WASjjk5fCmCJuT!o|eROnrGN7W!M zSmeXwSE0dyl}kV)?7^Zyc-wG-?}0N?t||R5(todbWtssGC78LOpu8Z+$aJ*t#YcFX zZGr+LX!nb@i>mI*RhxxFl$K(PuMp57z^r;ws_+{3EbEyy%RA0=)&12DecL92#eob= zvZ4wBZ&oP0hil>#FxYJCnvxW9YVN9C&P-m5SuhI$hDI)Ng@835SeL+8@E>43^TtT< zXXui`J*(#R!&lcL1qKegp1Y>pT`|Qp^y^lZDQ=v|;Q-Sss^_pE;vu&O{P^Mq?in+W z3avV||M|yEY#|}y(qNyV#i2YGF8Ng0VDmvGBh^(8G(+#Ms1Y@UM+nRkP=m~rQ$Qo^ zau)m)z!jd1#=qxw%n|ba*T1Tc(x6I)m`aSLdO&6&anE!h|vFLBMd86tR7OR99k zs(azLv?LH$34oFRI;#XRZlO6kmMre(2Tz+UGL9V9})z(9OiswROjRm}%PK5qssZ-Kr|z-_=jx=Oe?FfP5zgT7=sT zVu3TDR>Xlbb^;Sc7i;RQT(w!d!fOh6#XYLm6ebve2j-DR>wa?0h~yXisZv$fw@Po~ z``eVrU?Hy@+C2U~aoxXI{pWU%vb&ZX&^k7cu9gw%0iW2y{8)otf zP5iyce170n-rM(t^RZc_Fd@3ZVWK{KAaR8Qqp`KGi(jwT{(#W_l?PS^Ag6s*K?$>* zi0hWc&v*x;`D?yx?WoaFdOzdl7r(hywrWb> zmtB5op549ZQ@Zzph@aUfc%WglfQ|J`PFDv{-!i+1(8sdfrYjDCc5p(Ioj^py0;DY+ zErOt=@9>i~r1TVM%ppG~XV11jR|6a|V$^`q_;;9#*x4yZ|8{alDirS7VBNaB+R(4D`n87Odf8)N!%W!=H*Q!c{+xO(ua%{1>+Wx7U|l_} zkOOD_>{#Z#=+os@zdm!#sTZ5b=#6v=k$EuL(e`3RWl6^Pr0sKz1kac z)o`jcvle*hDDJ_uwzjsp*>Q@E3}_o6ql64Lr@B13fAReO(5puU;H_@~hXvO_<5`dm zn%Q@oeuAnJQ9a0*ov2;=D((3W8(&SE!U^`jLW6@T_l%ozN|z+g7Oq;A7kjoiEf3~g z1}2Ut%Af(^39I_fKaZRTiGVE!j`-w zQMXfp%!-Uzx9ZdTV$m&iQxL^+fJ|eE*4BkHZA0hWdkHpw!4;-eR$6oY=10FOlw6<6 z_>ZA$J!lXMT!5|+U^JH2P~vxaYPWvX$GoFlGdWY?qhuTk0TCe6me2In$yxse+;$1j zV>JGqot*r*@u}4OLUA#LfG4ZLCV>bomjh?&w3U{-KaE?l>UfC9&nd{$1s7R@R$k#S zmX6-S_(B4dXBRACT2)nUHTT}WCCx?=pfix|1VH%dWF7UKjD+6Fi04Z zl^PduUb%X8?#fk*yUiG6K`DY`N{B<~e4pi)rKgt!uqg*Tkp{U3oQzjF9yqg4aH8JK zodxwtrfal{;W7dmEuc7LRVvC+{Iv*l0SH6mC+-!mzLZ$q-Tr54faB%)&W`ZtQvmI_ ziVk*(>(u%i5E|}N_cR2lmGAJ8HE89P2xIBMKNBUu&DsSnO!}rKCToHhZIn>sXY8{% z%iQS>b`dDLi@0apR8%U;3~`SNtw&ut#n7n2A)uj_C<@+4sIh{fG0D-=`krKC(s9_$ z2B7q}t6`?E&_oM|JJF%*b$=b;L8`eNSUChV!VV|}f)?jr0F6~`s}%j4$#is)urWAu zf`|)+jK;GSm9$-LLG3>4d&Y=vIfp{PPbQYGsr%c`fC4-NynfNY32l<9n9(@e%f+t0 zu6Euky(W|fV2%ETdk%#^K0Ms?xX=38*&nY~uRrCfK6lFZ&qwP%^cbI;U|0L=gmnHM zMfEuaP8-`7|7YU(bN~O}`FlQ}v*!MKWp8!)&QGVb53jfX{bqB~x0~sw_f&qqWs)>q zFZRio%l^~v?X6C)i{J9HR?ZF9V?>*ZJIi_|XGMokUtR62ReVlJgW8NN`@|IjR04c{ zy>A5_jlyU5BjM$hm6K)4ZX|L~m1ghycNlu)>HP0oLPGoOeq~4+Bsg5QUKyhG{(GfB zBMTSkNQXcU)NKJK;s%?$U7mEjxbD8{6{`odFaia0!-4Nivb-YcXJ#lapI4Q&dd~gY z?{jN@KApbSE_@Yx+1=9c$ikzdsn^y-8dVnkDHre#WqkZZ`$4v8&W#Nl!hW)J-Fq(@ z9-|0aA{giVX6N&HLGkhX4{T=Xs=W9ypa?v(ZH6|pJ&SwB&CN=i;=a7z8~U|$2`tTl z^3Q|n#y+;L_xpaY>ya>I@?+^zyD-h6@ZFuAn_evHZd3OE`y$w$$ut~v=lhcti~G3x zQ||qJ|GsgM>2s|@zuioq|3SxE zWb(hzfOyalH|KAcABR_<9KyV@VP>wL62I$yv)Itzi$I&X!BrfH_%EyxzM$fmWV*+E zyV_arcE7&9e*T+nxAQg~lgd7k)UB%~U;AaE&Br6cMUQ*UPl?CZTns7r|L-s8h$fEz zwO?Lb{B%lt{gW6!`)gkob?XWJ-`!fbe%}4{&)=Rm;EY5v>yZwHi*7dXBZzS`- zyt{jQ$-6s|_bQ*yP5AVnng3Y{=xWN8+qv7PqMeZZ<^N4<{sQY&tLoR+HVe%1(CBJV zaBUK4R;ZB=nyTh{w0iv)B_=7)C5vkoaV%1}B>XaHNn5dgJ&TAS<4fCxTnpRN4s@6( z3Z409|GuSj^2sxEENjo*xnCtG_h8;k+wA?-=Vsde-hMsm+X?0V8@=ZDGS1um&Uy5+ z_sd7n_Mx>K4smt9{rCI+|8245cTIojL^*#vBJ9ueZtmiKJF6Z20>Xl#g5ry}{dm-U zyYRSddbiFdkAnI_pNQ2HA3th(B;>6l_@#F1)v(R}wXcF36FjBvo8PawJg@SZFnJ1Zp&pq#?J<`rdLn;|GcW8_HnQIn~lfixQ}rq8lRNVfA=>uEb{2I$h4UY z&cEOHJFh@5x!|&|`B}qrp1C@pL))jWuQ|HIf5-GhqsmVyzwf@U6aUg!-Td*;zTbD> z-vXUmx^q_M7rP&a<^Lsg?2E~sox5%0)bO~Qc{`tq?S8ZA^q0+dVwi+PW}n{t@S2K- zSx*3OM*{~GIlShmcwe8AcHCuML09p+1J_~$b-UrR4ICW~-vu?A7W}y6tY3t))<<=&@WeOgK8pto&J(-|rkultuNwb2r^SU;pp3gUm6B?a6(X$3Q0;JBS=P4qD*64Ro=y z-S0QTGQ#2_{kGq3IJBJIbUSbN*}A{4FYlJ$uPv)zx9gSG?kAJHcfCKp`G%Z#fXlDg zEH2(5*_Hd%V8Ox@JlwCFs^j`AC#;W3F*y_ROy zru^BVC%DM^-Hzk~@{QujJRKJfKN9TX=!(eC&o7W;n_Ya)Qe9wUM?q}$+pW#}l^KLw zyHYv3tlw;C7L4q;;clXKaps+H1C>|(FC2S3c5AQSQ^d!%aDm4atwZvUi(f9C&L-H} zv4!IjXwiJ`yvk=Ax8JKez3=YPt7qNCexKsE0EpwrW8fd{= z(}^o93i&~Yw>9p+GynO#>UTMf(*M8T%3iM!C3Ii#%*KkAq|NV`Zz$${y8Ed5>z~i( z*_o0}xGX=OUSogF%b_W0?UAUOJuFAXn8Q9RDKJ6N1rEmdw$c61d2a{&J>r(}Dh3+; zpjd8H68dnD-`qH4``I&ZxYM@Hn5p~ep*{EWk26=Owd{%N?Qm+5V=-&ls^k1Z!8GNH zlVQ((#U?>TQF9mGOF2m$Ivo5Ghd7)Rn}nIfoWdqOl&@2I^k)10y5c1TXOuOB?z)$# zJaWI|5!o5wa^GX6*Ort!0yTXWk2)M29KYPD|Npo7#D)t}i4M{iKeMy5H**|Zq1d3P z5~wV|_+jVsdE8lSZ4;|xx;tE1lpiT1DJ@FM=*V|zP_j~NQ+jpm^4=nyr0Dqg_ybJq zf|vW{M#*Qcm?qV=#D55 z!cRlv)Bii(cY+K1T>2kri-}sUjy3;dXOP$PkeS~`U`bK0)XGNpUa8c}%Y3 z&Fa|ZQe5}%^L+ky>^fQ+X20!rd_HI0{F`fG>@_1MA(7~|ZTs3(HLe}^P49%4at^0C zE8c5v5v+ave|K@-$!YNs;-FB6=d(sLfe-iUCkxf5eVgj0e|^P*8P9*Y$P2QHZVljZ zexYta4 zwpi-$XTgMXRH?^>}%gnx&H0-LE{BxfCmOR3d6I&!Nu`y{)dRY~)|~qW=H% zPoL62r`!o24GcOYbk?HwSIO<%?RV3DyH8OS%=c`{*|L9w8_VKz3Xe>_hiv}kVcpBo z)v>Z;#r&UV(wPn#`>d{hzxO+vQpKXlr@1cGoNrhzr)9C zx@s~)(t*Wg!C{s?-|Y>Ii|q6+>V#YAYN)w}xc)AC*ecGZ#C|dLdTjaKH~k&6oHaNX zFMlN1y8Co`yq!RSdyj;m%bQ2c znfV(tO!6f>yBpdc%n%jr(Aax3jm7%BriR&PyAJn*Y4<{YZ4hJ<5|Lhby{TALV;jS& zSt5v}m?rSyUVK{G=C0H;o950f{^P3#D?q>{Nv!*UYJaKA_h;TbcD8I^pyS7*;_*C} zRIg<%ybz)*)xAQuWkI*zt`qC_{d#4oC%mkv=F>^_^zQbDj2#`>Yqw5QtvzN~$>O?5 zVP&TH_JE2<#(gW(Zs%^F`*z>&caEpRj?B9tl`1fA{xqX$uWSCkj^7?zc2jl5Z+F>! zD$xzSq zbojqM4rx;*pO4MI=5kDl6x$!}>^Mg6a$9gvQn%}EfcV+XO1E!hji+jzu zf^MWmnh2G?xv}w}rAx}cSK<3l+2l9#nsHqElys5fXxcsYt4dX?Yo~vy%wrM~DPEj) zzEMcz_Tk8IK}7h?;;eWdueRI!$&sIPi#Kn(c?z0Rq3N}oqvE}NZd$$J=E#Tc#nN5k z^Cx?Mt}~vUmi8ZXDW$*p@87@uzuddNhNDf>U0PIR^AUqT_J6-zZuVHC;%F$<+7Z!~ z(I#l=7VEs|sE2vg_1N;WtxEk1JYIQZJv!Q3)p&(>jhaB~^W04*)f{=FqIl$tRVknwoAaQ$2l8UvP5Vc&og%x}{oG?x_3h zCAqOB7hRb*P8ZqNZ~t${-fy?E9b*ID1uBWG)b4T(Uiay~%YrcBcb55EuZD52Yj$M? z1t;i+i8p^fpI`rS-lc=3f)f@*h;O?#SH*E-Y0lM6uoh$k#}(fL)%zFB+$)@WptaTL zRbHri7rbn3;OKDp%~|pOx$)-TXL5R5R^RWsmzEZ8^*rs$>sWK{u8UTkOJ*$TlXtUq zc*UgjP2rNt(#jl{fIBsx&o+NqSg9=H?6~G;QncL0b%Kr`4>0p@`FcIReqG4gt#`63 zYnJTQdAK98DamyIWfoVL2kUF!W;YhJJG2|jNLlhAw{=0qn~leBd1YoqB+G9GH5&@FwKUo?L-O~2y>|1f$7Kr_)?X_Y+&J^wp($zW?hTpqy#sQjPCYYG zaolJrbF~ZRSBG>CgXtgV?kve(|7!35SM4ts9OAzFaL(-oTf;+NLo0G{K2*9;!*uVr zx5s_??4P;Dx35~7r~f-|rGNiylTz%xtkr9qwk$SIJ7bV_;A6hhR*(HNr=Hs?=s0o3 z-VJ^VnmU~!XN(r9Uyn?et$wkvy=lwrg{E5Do?7k~bTkyX51ogIY-P3u2Rp3gF|({q}dM%%t>|3 zTRoqoed~EvAUmR#g{1^@6XQh!i_MOnF)KZ1dtIl0S8RZcUCj=~A37TjFqNHDoqprX z%gfA-6YG_mK>OJ{S4atptXy9HY-YNYlHb;lN#6Qb^4T$fBks z*nO+eRofu`IdO-YS*``%fR8(z%ixgz~ys0 zxk39Og8l9Pez6Q{FLSov`<;L0%~^u+;YH_cFZ1p@?T*yQV-)1zta#t)a7WLnWy`W` z>*sQrYD@FPMG_s?9?so%Q*DL%_k-;6H;&u?E8P8V*XuWXKA-b;z1p$BWAVa(=X?2; zPUP)=yKU#XttXVaS+;(k7GL-C!1ll2@7u3fED^OhDSF;#X+e>X!V@*W%wOSiX`7m4 z)0Icv`gx#Rrk?+>= zY>)JPzx{sQ>#twF{5f^z#zj?OSmC{*v7tC`)=b&DTbaw}Mjy_UwZHP&P2TXT|C=8Z zPQVm_8g7kJoE7iwlhY74YgEkGizk>ifC6 z{`{Yny-t$jw#@k(bF1I&+}wJv^+C7!ttl-&W^(5T?^0O`~CHd&Mw)h zA?sF`G{6582y5qoYfmSpd%u;>zF97))KaVacuw{Hn^UAjMY2DtYXzA&XLQ9`uL(YJ z|K0BQ`y47xZgOLFz2%ky+QL-(u{+*puSj6&@;OCawxE8c>}8E+o9E_Q8jnnzM7Juf zd_Fz(qtry1*~i`cP78@VpIaVh{dUV`rb4qUmt{&zwnhc0^lCmbTxoCEQg}?%P}K3= z+wJ%H-kHB+Z8@68-Q^;8XrhM;kD_Q{@k!O`Jns%_1}|f2`o*b!X4Mx#agm?5+oba( zez?aAd=rk;?K18OLEtv#%|Xf_Ka)o<-+psiMl=K4u!4>_Y6eEP)oYkWs9XX${}&#{duX=SyHv6H=_-VG3ATHYgV{_cQM z*|F;VduEWB`35BJ`urS0$i^D^rBowBt}DvldNQ?9P# z5NT$38C6|Xm2-^e8;82|%9!JQveGr+7SNGS;qHff_N>r| zAM4q9#&UbIa_$TGXhWk9)B7F?!=#Ig`>b9yFDm?W?|o|8{>~qzt;Komi+*a>qqLV5 zx*NXN*&9pCTRsbUeur`GA~m>c4y*`V+%s$8m1xk=aqg)pnvQQzS3aA0-0t7U{@ZG^ zbA+yhEm3>6#B(x}qhTEPW(nJg;-&%<7DO+s6I;8Z`L3a5q!G4xU z|9#b;b|dw!&jHKt6-$GD=9qt9n6nQwMAre@fjIm7moI--Cgh!jjnru$oCXXWPtg9FieXwC(|WuYw+j;|D59E+gCB~-Mg1#ntfL2@2qeZ zmj$0$rbT6GMyJm$&3STS;x^E*m{PZnkc;BJuh*i#eYxy^8#Kty(Y2{apx~VEnh>q* z>5I*uzG!7R+SL1ahGBBui#f@M5S3WziXx^>(|xi&$K=N8h9L1Qix1%D9-El zTI$3wZH)qpiiTK>%qJZXeikuJZlIKw+9KEPC=15;s;B#PJrkt7wS5VVkfZVgutuXoM_P3ovl}$et#) zmbYM2@w){<{0&y>NLeu;ifOKZ$nW33+biEMpLO%mQr6Xdpinw+L1Dww0|ySwZ*vRZ zy?nZm2)9`nh)^hUhuC|Cg-cB%E#oZVZmFfTX<(g^EhPMPMQ&f=Qz?b|oG zS97fQZArLl2uesA_Ko-Xd3ZExOBs(xHYFWoa|IDD&>){O%i$o)(U)mv@fPMTpD&6r5*_{ zP!fWU**69WYjiDmvG@O!nZBI|>MmP;-xzc_{5d4GLr5>*1&5oIww}oUbWcAz>%^-$ zpeUNse_$OmGxPK*>!Y8(m5~@peEnm4;~}6K7ZTteZatZ-iSsFVirsDfI&f z54i2+vfv7fm8J&Mw(Z-?eV^Y+P5X8;s5uO5>jD*)J>RSO`1s-?CcT@oNg5QiYd{2Y z_^N2crKIh4OL+fe&h9-|K3uf~1t2u(HM((bSod5ycS-2;@~D6%@qhpR#+P@yEp+#XM)s7J z1MF$vrhWex`uvV@+P4kg|MktAH}AFf)Km3VH;*fEbS3`({p;5?^UW92wr!sIwPaHF zI$f~S47wYN-#wdYoIcOk!otI3=NfK~7pnUcnpGS(CNL|N!y|>GY0>vOJLB2THYMjW z`@&vXum5d*{r>&^o4?n|8{b~?yLz!4BD?-MeMaxwz4>+)7C#Eqt0Gh!4d46+^}rr{ zd3kyJhkMEf>)!2+KWMpq$xPjxw8K^hJ+FgC@fs%yD@^(q3mQ<;uXr#2{^iS^)2+5o z^?JH<()Yb3cP}S}9&8IiOQw!2N7v7|DX+BPeZjfhfUFO%Vy+9lc>6Z?yybUoS*AY(%Aq?Q_IlOR1-&+`i#hu z#^(ah?{qF@TYs)pwejA)d$x0m#Y>icE{cMtur>?E=XYe&wmB~is@xf?p`x*^d&@dy zaNsI1y;W3j+-N(eINr4>DKEq-0IEkyEkMAraM8@QqA$;Mo!gl%{cetwK%*MS>36SM zmIn&Y4he?mxVP#CCoQ*UE)A;OEIWPLf*X9NcC|rWzNNo`<>8HN~->O-Xt_Rvu7i&vIID8I%rkjd$B$n)!TAv9`(5;w%@ISuP8< zZ~y-N+|ISLugsYRt?3vAUl=exzw>UY*V3mm*;O>kDi)Mh!W?MC$l2w~b}OS;NJLt7 z#b;<7DcCnMadfrMy7^JC-@ojf?3DfNRzWOL-V4^R&*wJYy>DxHAYvpbSM9@z0|ySw zTDxx<%h535u;0)CyAa1DBy#)o?pIe;G{Rh$Fhi2C%YsZ6E(MLa^t5PKt1T0Y-)&n~ z8o=A>(9BX*U9De!?(KyHW2;!GKV247uxyi)kVw$GWi)xg4Wlht5l|T`Cx@ma_YKSA z(Gwyg%hB&=aw<3?LFMJ{(%9#}e*b>{_1^yEH2IfS&!0kuh#ELL96;BI-)wGfzMrY8 z#@V%Y$?9!T&oB53Di!8eJ`+{d5X*6ZR!$BpIZJ{9zTDGKPSbZ|*#CG=^o}bBK({n6 zC|MqB8dDSoby$|_hc5>X95}UgUdZW>LL%70j#b>@bzGUv>BA9<&6vP>y-*@QQeCr-7aCVhCT-Jn!bU*?VsI0UA z5l985m5xSSLRz&81EjzNyY|BM>*D*}cb}@agp`O1OiB}c7c5_%-mJZD$D_Wm!buBm zXl(*D^1wy5m9s-rQu>bNaiM7Wy`9zd(%ebkOtw5cx3hk_)DElbp)9|C{rZ=a7XMCb z`O`Jfe9_^M$8>I|toGDXEqBzfR4iaQD%JS10GjwTEEqxk4FwP(uoIF~1F}Gs+TQQ- zhMT`HE|^<9Yx>k*d9U}K`}*~(ugT`EyUrI^>Ok{`(uDSg?{!}uJa{m(vTyD48A@DT zyWeiR-E?`m|NTSN`}fUU`)+T1^ zJm=uSgQsWR*)l6zkdd=Xmnk}bvyjMZ)#aaIS*46gRz#%Ve6zE%ocyCX)*HXcz2Fhd zkYz-f!dk$=vgbQHKOf(`D_Zeg+q@3_f~^Hm(D?TKyS_>BvopbSitp!z+Jib0ptOIO zCGYb+c`FNx3jf2pD;uW?O;~UvXwO!2J;#k@X;#kAr2I1STxF!L=$Y7^>-pe2Gc)t; zvgNOzev4{KI(RG`JzKFHy+8ZrHNg$ime2FFO*2Z@X>)o6yg8LP`7*=^r3tegBw3IC z`}Z&L-tU!Z+bZ$|pq1Hz4wh{)5)wax1!bjA*@2pTFF?&~*fNdAAmJAi7W`N@^Q?SR zC@8>_V()?i+_t6&(sFiSStX>gc)^X^w|7r3erFzPy&hU(clkZ|)Y#ZKx9i=LfY1I- zNeA6x&|JNn)%E>f^|NoDa$Na7rj6;M8IRWnFF(jmw_ulCzbW3IPP1&aMZhNIiS&T=&N?3+a#S2)k@l=bF~oclV8 zNl0XO*^4JTpe4-{rGOyEkG96r^SB%MAl0gd3FGrSVpF}IwmDf_9+@+FLBs~e=^z5p z8Mwgc_|eArcNohnzjHfJ<1*OHapV5|^;5pr1+6>x8dA469uh7%CmXjiq)&U#V&@Z% zpfW56L?9JpvH~K@XWU$_6wq${{9kO=2~eA8!h#!Hoz-D&AqDY9A+9d#iuYS1^{>Bx z7_DK)`2H_zb93`W-@r*<+elyqzL_4 z&T#=!+HJ79eiamzUq76LH5yiUEyw}qc*Ao$@vx zgk-9_P7gX88yiD+AHBKo!E#U&bQ4+=)Qr2!jw=|{q{yDSs8~qE+py{F6Eh8jhS7ODpEkYMumfE(XIA>^$^*HCm|K~D(sVZ-5jyx@;=9@R z<7Z|XH{ag~I>h$i`y0~vdoKD-Z^|x=*V+AM)4_;WghZap z2#D=4zmdQ!ZJHILa4AKS>E7agyR7nimFWsEu4cvmI3%94{cf4J4d{?r<9`R|6mPp% z_4;Po>|8O9P|r4xCX0gGx!Z5vEx$ikQ$f*4ru0f+8^h$3i`mz2srR{SSbVwQ9PDrV ze$VG9ErlA;^zGt)JFAbs{O$j4nOk-%bF=;LGwF88@xO0fXPQ5m$MD^?==`nL?{+8) zBwgzHod#M9x%FDq>1Q+3=k4T6u9;W+Epqq2U$5VGs?S@&o?XC~bT!@ncO>XU*xXjp zFoR!FX9YHbS8x9R_nY}$N6yv=T^rE#-?y_?ugzIHHH_=sZI$28?f*;u0IkJkb4uRZ zmT~;YL8gyFf*WTkiulj7vHZ~oI@;?h-}%3mhaR>`=Vko=z5oC0U$U+@=9pv#74R!O zYi)b*{jvOii|c=9U01AFv;W_(ZJ^b4PQ}v{XW9SxaJcOE+wJU!ve$0A>}SpT*KgBg zKda1d`O4o9?|Qv1dv?yIPTjpBipg_p*i6d{%hga9;8IRrr1>mwl#5F1m4*Pp39+n0o!w z2N#`JZ{Ke1HM^yezuEDLf{cK8wqeU|{o0q_jXQ322pU!#6%D`9t-tSvhjC+v&;7$I z9%l6lI##{idi|EK`CSni(EOQN&Hu32t=~f4*QmeW`CM-IuUD&^GcJLAr(#v~{cid8 z(6GqVb!n;^&s1Z77vB4weJ1DjiJ740!s@-Ma}}B1LVAp)s^H*c6nw$L$l0}d#pZK| z)9&m6O?}+_|M&j?r?z{Z9{>9$-G1{#Wp_5|<@Fz$<+uE)+U4+=O zUTzVc6Y-^2L%&Vm|3~%uk5FNe&IeX67Bn}Nv~HD3;tjOV42Kx*oVWbCwte5!=NDp*3zS+~Sj@;)n$ey*yF1;Sxd{(emV&ZksiHaW& z%{sn{v&&^+pRBc+sZvx^lH94UvXA$=OU{#BwCY#q*TO52Z&ei>H}2h2Y6=bN40BLW zcX$Wne4lGxvi^9~LD1seDTTl8ecxv;-l5}YSoh&^zkS~RKTq|uZ?)KzoLv1p)(154 z!Uk$qv8Alw?z(dB_rLG^^Ie$CP78>1GObR#%Hk^bDsy7p)mzVs7xna6zuR%;RxL*e zN2sS+;Ukyb51P0?#iaEm|9f8l?>K1n@{!LTdpir9e0PRLnKJgohJNiTUgBW;@5kfj z7fy@Vj`v7T);?soa(_n^Gc)tffE3q_PqP#rDzwD1+{?P=H7{m%$%c;AK0co%UYZJS zoRu0Q|8ee@qm{gdh0|lpID;z8l6n|lmPm_=T#rnj>)`W*b)n?3XB_+fS7u44adv?g zEjPdL&Dy_gCd<*Zo*Q>^E8g4Zr3EX`|8v`N`ku>+P4!lU3a;pO*v`@A(lYtbOjfB& z7RLQ4lLcllQ%`X^gh2h|KFG83*PBQic~z?__6nXxsk^Nv$?$K zo-Z%|{`D&dw1#f+UeFnLJL^v0Ofjwc^1|}m-WQ9yn^t_S>g~;5lF8pI#Hc>Mrf9>= zx2NO(6@B1;Q}HYyd8X|9C9M6|rQ28|r7E6H`ZfQoLrUL+ic4QMcdnbYu`(UBPESeG zqnJN9sOhqp^s%_gr=rzwHXd&>`F=IaHvMaNZ`$mH0t9yH^&e-l`@6dVh(`VJEsOML{%>Uix!*<0uO<3^F%>D<;Hap}# zZ(8y9Rrr1*qnss@pBHVt9%t>b-7ieki!p$y!Pm7%v{>g}{|xtZ$=SyiNq_xeSNq)h zKBr$J_rCR@DgW=ytraslIvmP{CM>YXzT@OLG5x|sce%(?gPG|ccE^f5zImov?)=&R z-~a!M54*i{zSXAJx{9}c_;o6&5rej$tS`KmYT>;Go2s}5^cWI{=d z8r)rbUIf2hAGP*KRhy3gmC$JmPB71!{?CoIY0c59tn+X8|Nqx{H+fUl5kdDWF^}IJ zVCLs}byHQ+DX!+>8o6y(&z%%HFvG!6F-dVz-0#Qo|0Q%P4lcjAWj1JMQqrl;9r-#} z6!@O66prxT|H)-h+Q*c{j@E;2`Z$2Tff-bOk=eS+V-T@@wAtu z%w9Y0_AS0o@`7@G`&8d*ac6jnt^YZ*geTJ*beF)DZ?7L7wA9h+lef2v;5fTU$8n;_ z!K?d}g+yMAzwdBr$Z z-k$T-dX|dDq@xeo?f)b`d#}Do>5=mOs|ytc<`y24EKZrfAnik^`n-tJ1_8O1-ZE0> zlUrn1Hdg6+R#?AYv$<)fc~j&hMZ1qSe8FcvpZR~%ziv{LvDC4YV<&_dU$Q?+jmmw# zLjD8j9`-kzPV0I5o}Yh3|5#IVsT5RQv~Jc$55wGBTO4AXJCedbD|PEks+8Kj*5B&o z5}%D4-T^)4=eFLw`}OqsF$PLa2% z&1U=b@~9}k|5S`zva{# z+0R=)q$u`zIG64Ie1FNc*7SK>_Uhad%ndjjaVGVk_PllCfkI+E>M=j~glZ4`J*yPH zwe`rJrte(ZYXmqxM!x+R$?v9pJ9Ixp>|EzDNv~&n<4gbi_~^f7 z)A@V9izAnJ#3Z{vzta*H7o6MeV9X&B8SrFh@d?FtiNJ@Iz3GbQe{LyxU+DPp5V!so z&=!@7zHs9@Yr##6+E$46&U4>!v;2OoajWAze~!q&-9Mkr?)7!=jhOV|n9R1Q68qw7 z&HQ#2H(ZV`3ft@v$xeCkg_o5?I>nyEE}B;I`bt8R;^%8SR5`oO z>=YDUtme4!mdC4NK@BrdU3bjADQV;T%SM&gbsQ&F%6yJ`mL{y`b7F4cY9F1e(-lF3 zhX>oDKppy&6)K$4=Y5+&`(uB7_%cbx!1Jmicsbu-XtOG_aB$e%H99>+re{1 zEY>a(N7Puhm^_~M;K$o-xAlS}RH~Y`UDDC_zolR+^>W7}x2g!1KWj2fkK8CouQ3&C zmo1Ah=zO{_>A3xMzuO=Eqd8_r=j~iNu}kNyz`wdLi{;OLwe7C|zOyXs%K={VI|=*$ z|9u~6rIjhVm=EeKOn;a&xx(3AtH(Q>pm}hM7 zGb>z4OX-EmQq5lOu0LUIOj6G`ZcJX-D&}=Ipkan^ZCk&TsaC}_*WO(_K9Ql#1D({PlC$s{C_{xLr!KxNVVHDM_MN=ddg+moI3rEMc- z@A&`kcXRp9jAM*uZXf3ypJBXV+l15{ne?*__mkHh{%SDKc2E4|M{_qBES#8Rqw{j( z^Lf>5(!JYnWvxyq)|FoOvb-jlSMcAGt;fo*@l^Ot=I#6*VK!|^s?e(GjxVzR)Lgq> zVXx-QbUo#ro7HvA;ALmt$)Ab*{V4KeabLQo&#F|O2bO-vTE1S3)}Qfs!_V23a@n41 zZZ18$Tk56q>&i8;YLc(zr}!RRWGb>*NyPHojpTmbU-MKn4mB4sObL4=VODPZJz2M@ z_x>I2H5)fwv^+awa?Zx1Vh8sgxYJTSeQW45T?4=9?<=S0r`PkAeq|B{&0!y$2pYSR zT-Bu1?PPOea?kWKvG`T3?~NBGZF0Oj)8w?5=1lj1l%z>pIgXSHW`Db#zyFGcx|)6F z%XKp{j2u0M_e3}-+*&njV}X-(=2nfBOU)`jJ;^$2^7OL}lf-1nvU$dh-bTmD*6;ns zrRIFKhf{CznJpUjK^=y6nUB7zPLC0~616H#N5g1i-!k*nWqk8DU!HriZq|!4OL|Tw z%zFCpq|0oL&NWk(`h4h^`>&H9cy+ZLeJk|%@*j(N( z@nGsZsjOl2bN`=Dr?c064G~S5E_Wfz@nZG)nCvu>{?%fOZZFk&TK1DeBs3t!QDx14 z@u26DF1+?;aT_?h*Gry{S@cj~j5Ey<8y?Hw;uH@%Y?xdeXOn%h z_`Gd;j#5{2)P4{CV>U`Y*S(f;)pRX)^j#f)q_kprLZO)b$)tlfT0bj^y^L7m>e1~k zZS-AXmua5Py-8xWb^4n=NXPC-u{*R!WlgV0zPe|?lOI3aoc6Jw(X4Dwx_98t)Yj~0 zTp!PRC+@xQV(X)(q#u%xPbFz{pWZWl!-?qEH9O{2M}Q_lwzM^{9F2^bt*J3dvE$pM zWq)kSEAn~f`^y}T;&>$YwZh-_>yNZD5BZve2sD5(S;vnxUch8 zh%ap`4)e;JcXRoiBCidCHhF;?*NU+vT3`HpQ{c#Ft-K$n*rcv;R=92U)YLpI@Ve|w z{-mBtne)l>x;>BG={HrqH}i3}Oz)SCH=C6HsyR*kI%7$1_?qcUQgyk1pS9w9JvpVg zEBe|q+rHG?Cd=HK<|5^q=+D~A!@M29eWgHfeWJnDr4#Yzk@$WO(TIp6ij&m(4JDJA zj_55_Sv;fg61Uy)Fkz9KFOM{v^=C{Hl|8|2?&KG>SEy)#dwGNV?}IFprSh-#uyT~` zP?zMdE;HPEdKG8Fhm=b(xzBBxB)Z))cdvfQpL-!}t6@@lT+|We-S=l&Y`fHPPI-~p z$s0R8{{1#teQdHgm)g$CMLLljx2xap4PJk=n^&-Xtw|iiqS8f+uCG#C_ep5Nw!{6c z?r|ln?s7l;8Wvl6^}%-K)0S)}JVM`^L6O;z{(A4{V8 zbRKNHA#VE2^vr*$ttt<7b)TvSa^~c9N+&(K=q_)2U1r)F{w43bZcTKTo4Lbc>T>xr z&pl^LiHmkh^juH(d3C*+-%esr^pW|cF7gIxA3rg9zJ2y_a`KI}{RhNFdoR0qy}S5A zNw3(<O3iP* z*t}=5TB_uu2o`%5l}i(jZ)SWVC{k(mK}M;Y-|m6*d4+k?JA!S@5A~X-JWKk_@!CBn zME$m~^pBf9N1r8Mlev*2yfq+abvxtRmgk!r+j|{y1k!@-mDE(*1cGlKIPE6(OnHj% zBiXrUjn`cNSo6HrVh!)vUEwv&MvM$G?ay_j^B+}1Qdvav_u*sJ*XnNYh}{qHvp%DGBjQN$?0-s3pZ=uRIWFTr;$dbV?8Y|H%EZju z@cw}W4^FjrtiK$lsbBNw z5Dd?X{PXOgSbPs>`%F)!z;K_2M`6AJDGygh{p#G{oAtIOQ*~0FVNmW;*T)-;b#v9N z^gp`G6uP86k-UCdD09=1j<6cpRh$VvDVNLoj{Z$#m;0J{sfV?1iR;WCAAKgxEKrqX zii(;$FYROE$Ig@avn)zAG(dPRBJ< z7l%hXXQkQXugn#%y>~RYsW7kRVh5|O*tbiI)<1B*5_x5j_1~Y?+@>A27t`y4_PX|^ zzLOB05b+>!-=uAvYo`@@3f;TpVqcS*cdRM(%8Yp7!UPvAt19=<)X#P_{_RXVUr?G?LBNe<+?<0&C~liJu0h=_3AbyYR_WpVcquPk>T~E zLcMHZ>FWu_A$!=$V**kdt@PM7HO$zm*~5DI$1)=`8zrOmX(}2*b5Akt^kH#XV8dc| zzvgo^XaVs5XItKVT6gugwJ2BD8ubq~XN;~bf3#B0d12@0LtB=AG^0jpx#ur8ud7SW`s5@e+IE?m zKlA7aSQFTKjf44C$GrJpdQ|Rer}muqc4W8i?2gFu9p^6Jh&lXB@rYx~h8Jm7*X(bs zX#LP3ajMu-bG7j73p1JzFs-|I$*lL=#JX!i&J!*5R~9)hu3sZLskXVgbKhltmpf6t zK5}zT+bI9ryD;SZyWQ{QI(C9C4tW$ak^A9$nK~ zaY>}b@?o~5mfWYzl`@xImg_i9WUYuhGr8r2$^IEPmRxm84llUW1Lju;i4Y7^;g-&;v0$6#g<0uI&FxW zAK8+8U4C+oTGg8ekK|ochC1c=IwpSJ9=7u31-CpWRUUb-}=`rXd0Nt%B@3W<2zA9ED`Rz!npz)NyokTM;Nw}>LdS_;Rz{jGpX?$j7W-&xM#nbi60Y9+g?-)U zj{mr;9-CG2FU+@KRcglLL(xJ17r#AvxhT|o@2Ni~S`KPgKXq<9zj@l1*>CdHCrQki zCJkE)psXdZ{HwNRl*v8)vh$mNOiw#=@li30%kDK%-e0=kC|)_D;Ccs7FNgPk_PvP!{$YXO<_)aEyoo*T4zsxJUivk>*cv=Np%N4 zcNNLnZ2s|n#Zy6%%ULp;+YZ*WmzSDpr0+E@s&%nFmpEBFuxFv^DFqw(eK~6WQQN!D zt-NGpw;|8PM0xg>lZynT0;jEMS3Go-^`?e*gt2;Z-XS%qXBKB)DZjm*;F-$hNR z;ovX#y<(qSCaAnJ-g3cx-NDWF{#l>XKCIm?Z?*mls3^_p08f9mEn0BmbjhB1o)$W9 z`qY}%hMo3moe?%&*E`_JL$BP4Q9bQ>GqM!x;Yc^g1i9-gdpRp66&z7gn0zLRr?BVG7oDz&Qc7#>lJ1$^ zoGalkFEnrVQDg50@7-pBr5(RK-E_77tvni$l6-NBt9hK+r6W7{8{GpWc{DXJ@otYVa|>P6&GE_S+@#b?s;CfM=mMo*O`Jw)mwfx zQ!{r++u5-F&(PLg?0T=R?fB7F^HU5PPU@YV!Tqp2BXVnpMH_EY&Jr%(ybs<+EwMqL zmtU3j`>eV0Y{mXZ7dbv|^U#(%-*a%0;#d90$2QNHvQ=l*(Z!~#KlU$=?Cw65Bh!67 z*e6T)qUpY!T6!8r>IwI*t`O8cDWkPz$ATH|D{`)|&(&vAx=;t|@)_n&TM+T~=P5Of zMW4=Fzu#kdZ)@kQXI^-1p>#TJ~Ltj<1hTBMlU z*kLg zrhM<*H&qqv_nU2|J2}$$$F~3C0pV|iz!Q!E;0Z_PzfyRQ$IkP$>s~1Z z#Oa;UT-2>!=s5Ad@4VJbx3gJg$K7>31A3Msdc{S9omc)QMcSXCE~s)wtJJ#OZQx`K z=UK@IoHUXpq&^pmeR5hoZJD`^p5`JWJq@J|*I%b^k>8^%7%3GM6X0@3Ey+cVNhv@N zlw^9-H)m-qTWZSxUTWU=J=W^EQC#L7DyggvE-AvrQO6o=nv`l)9_i#wxvj8j(s_$z zvszyoJXQ+P*mm`_f{ndys!rWT$ysNQf{xwEGFv=R+5McDRi_HC`S-S)S~oV#Iih$g z-%PlL*fB@a&wr#bS!8hS9>r z^fKLJexEC1=YIH;)uePh^=g3k>dDhZZ=PtjpS5b?w0$N;Inx-zyk`Z~oDdQbXYk^Me#6BVEW)WEj-{($z|W>xplp6X=*J zte5V;(#sp~{SH0eFaO_Vbrk1NEE&sWa;%KMr%di95ET#kk@EIr>ES8T6- zxAVAM^_z_fN=>Y*%I{UGpNKnH+?~U|aBFaYPuH*HH6Nz>Hs(A^T5k7m@?6C@XQO=# z9=%GhG}_9p$Ck6sYUXr+f|9YX^1Cu8SHji!LAjZX2{T z==y|kj(M^+&X1L+!z&j*KYVL%p`cd3!i=K|=YRHcbe&myX;a72 zyZScoH9bY5wU<~l z4$TW;nQNG-=6@^OC1A;a7xPWV>q3vMt5*A1m~7Z9B3rmx{cZLZ-b-7Tx~^Ep+2!K3 zNqYUJ)nPCx&A-u}Ly`R+$ixdI}bmR--=Z=SF$j}B*;G}(EUgCOhCsbNu>>#R=b zWL?>n_~`HV``dRk-0*UoSSkCpb5Zd<1EroX_g=5t{q9Y5FK^J&Der?LvIo@5n)aU(p|4)ju>#Cdk{B8$J4v&C2rhTpgB8y9fJdd7rE9t3f zxWekfkz_3N$p4O%por#7=U*502zD)0xR$d~w?E=~$AXFnjqJC6ymF<L3ni`LUtq&|dUnj=qnRslD-mbJZDB|8PmwRO`U++$V z&nMSZxywi|Ti9*X#ohJLsknltZqlMR&HQ#No`=n>?PEFG_TgoU;0q~G3C|lU$m06v zM%L=JOz{=rYG+OUt8vZyGTWtPVvx;ADM69WTimZ%o_Uu0e^GfIlJlZ#+P{pDoYTd1 z%1uh=kAaR|IQYwFujZc5KTiDE5*)zux?uCYLp7|*qFRjX9d%dCd}a$j5fxed{mvzC zec3fLC%7u^a$F@@v-??laNoAs4J=2?gx43@f|mGASg4_)B(dT4(%5O!{5EwoDg8Wm z^XcT2D{+a_12e9ye>`8s`v0HLxtErBzIC!WUV2JJV^hb;Bb`dGJddUP-*!8XJ7|K6 z>s8nDH;Yc|=FZOD=9#lxn#DEit4nbG5$i)vlDmI;ZuNe(NoVogEw}75o07s-rR+1# z?5?}IkHy7l3Fjn`P)PFD*H?d6MrlQGc5M~ToxI@3gqdq4zxrkRXUl8Py;#TODk8G* z02^o4@fNo)UlZG9!$Rk*blY&XNhx#jqrl87iyyvUxY4jpgVF4s$z>~DjkfOC#QBr5 zzPkKAeXdH$YOz!H?x1Fpn$CO15zvzEadG*q*O`a28xZGo3 zBqlC0d7ZrdvU$~>O-Z&_O8PiTRJakrnsDy90XW`=69<@p=^;v|K2g?S9m8=i~g6;KtY$SYs?D-OTdg zWFEsN_5|a%6W!%Dt_)s&?rMr4tLvPU#+;d{(`KHIXPK$o$48|6MYk@~#NtW_bN$v$-ua)>@f4&x-?;Ddd zZ9znC%kzJGe48F+rkRiPxfCI&kZl!#JaNFxIBZp)ZSjn z+q$45f61COL%p*rZk~19kQTk1)m4e<#!fk@iIpjj5*kfT9y+@5MfzJIkxKK(?6q6d z;%7atk6N1&v;(v;zj#U7(<3_;oRFF8ab?3lf#Vb6rbJE64D32%ohY(>N5qLo#S(jk z&VAJB94ObD@sp1OICU^bNzi(U!W7}CVFX}oDm|Rc}-J$@`4jj zCVG}k3RIcPx_r;IvrS21Q2}K>LBEZ2pQi}TuVOj6rfT7mL&~!j$Srr9czBgut& z@1HG+de`E|-F0U3O%-v|XB(%-mfe)8Pu?SN(c`P+_A<_{hfaAFwZ~dJ+A36-xqExW zzlYtKB*t*~=)D&kugBNdZs63`(=zf;DYRZW#WCoM;`#b#27$_14Yxk#Tu3?Cf3{;w z`;L=LDy8C;=O65zu*J};DM|Offvh&uU2lh`M{726v$_Tqezu!^{Kgi+ygPYq7ewNO z83hCNYuG;YJdm8U#n8@Xd7z`=ohAEjWvyoQ-PHK?m~=kRGZh!_pkJ(p{U+0G{AYx_ zcz-DHTg>xm&ixe&PNdb|yE`*KU{c$2gKYLC+(#?6ZQkd0M85v=Py zan)sEDz9XW_9yspXWwvRbzKzt_~x-^bIaqRi%zOG&uYu5NKY0#U$aTIMY=Pie?_p} z;@W3tIc7-IKDSYl(7e3)QJ3~PMGd3;G-jFavNJEf+xPoj!ny;d5&X$^XXiBg27TNa zv$*DS&HIuw6I2u&Hzpj`11(_&4^)NBebdpYZ?YxpM3|~%?nNc&HJ>=?^Q0J7`8)f zu^+Rbvgb6lW2vs)pPU5Nub+BceqZ(#MPKiOb5u2iJU4zirL7*<7A3CpZ0lhajca~8 zeVdf*FYq7RGyCD3?rWiI?pDrKWYSd&2yvX4xB0^1{QOz1E7vd9G(Mbs^Xpl^|2wui z2c$H%OLf{5KN4H-dgaW+w_6r?)ORjYUS*$Uv-zdDpv01msA1!F|`g9Rhx*;>`YUEI8?;CO!L5$OoN& zN6j~@a(4YVXnk5J2P(DarblQg+waTls_n*bL-{xO;PCP>Z*L_?s7!f&qDD=fVk<&n=#u{XHtZ!=W0qex!wgTXy%`ZP7_0;bMxHDmG3K-d@Jh6=SS9SG|6D zT-<821)w%|ef+OW(}Rq&jvRl~CY@)nNM%K?Ow?|cqiYPkcCEj0m*9KW7bpigHG7_d@QzK*gq`4&xXHQ=pybG4D&PbJ5VQl_JOrHuBL3j#Zpc|6=f&o^FJVhPY(G0YV!8Jwx1-x^o|`)d z94T)xReCGn-+3bWZWpil`u+c^6wYj%=2jB-M^vxBf6IJTjZc>6v!3zgBqjMBP%AaE zR5%AZ)GqPVj62ou_c~5Vy)|RQJuYLTk_{`nO(&(#uQii!KezOMN1vyuh~+$AKNfwR z?RScf-QxYu6xa*8#@H~N+x)}P_w7|br#%i2-J8J(+Hcix%VV+-+O%Ygd*waXH(A zmecY2z@N>u{0P)yn5qjTqzNL&0JkW|c8DACJ22y}x6tcYum(n$2d$&!#~Q z%dcL}E-C0%(742Ic5-5pcCNDE@d;_YBJmxWbA?1AIg)K|+?}815*oC7va9mdn?`e zhUxVfA8D z?cQ&g7{sx$Dc0i6f|Hw+xVwJ%zTzy}`Fv}cn``RN5HUx?)$*5@`)`;3`@-E#Yh{*O z6=%`ySyGYonLl&qa-W%#xYoxuDe);- zxLyw4#L1SpJTy$iapJ$g{rm^Bzg`XJfAuqQ$?GJ(!qd9j&-5L#_ z5ukIe;)JvyXiHAxP7wEouOhex3t@GA#r*Hbar<*3krfC3osRz}bS3g(s>Rug#w^`nwjKSaf>d?{~9*_t`$~F@B~sY3}bQYH|@`j)E#zmU#*@ zb)HhT(mx$xpwy+LyGqYy`->(eCYhJJF7;%bt@!uv#YUY!n@+fvB!#swJx}56x>{#0 zAPPE|wRNY-1viucGleq z;jc|gYF=|p2&$c5`)%f9rs}s_rw4uy0G~v07StM)6uH_4+91;5z|H9qkfV5~YSy#` z5&T=l<{5{ux=xt1zh-)D*-9_*i3zv1WIFa0mT_+OIHXt7A}99n=ldOk3q4L<(9zzt z;DnQq<(J2;vnT&@(JL)8-maA!cGa;-<{N)+P~*3qxFztQbYVyA zTaFCSV&&q6+PfBbsIPb|;3j&+`lw&|k@eN@cBc2a`nXuT7`-xdS!}dvK?-NroZ}S+ zdXeAm6rX3c{`=E>E31IgN>v9bV>5Xd-usu29L;J{dYf`r`Qqyh2fWuRDH;8ceQft| zdp8GD-SJ6cz3P5*BwQ@hX0jZOiQj0+m$i7oj^3HEvmZPRJhyZHwRbzu2{c|4p0MCX z)v?O;4##@8+&UPUJ7Lo2Rjb#Xs+N=uH(XcHcvog^DB+E z27yxvjW7H|)RHPrq2Zzfhm$vFxdPFnO#q%42i z#H8LSrIBim8_Qnhiip@}rs=C!{Fqdnck`C*bxEZQRZK!6(dRARi>hcmQw@Fdc!uz% z!j376uj*)8eJtPqJ2rqh1d>XW0?ZsYu3!J2#n^gBWX%=G*(46>oEFdJ)`w`#f4fF} zSJf0y>2{ajIW|DWEB(Cf_bq{o-Arp!&RM_Tv*4cRtkUKM9wKViQ#~KgN_)TWcb>pW zkGoTqB4)C9;e+=UHA(pZsR!@_CyIA0G>StG89+`|W)D-$&H#0>9pf;->j%mtV*VI(O*koZ|J@?=1pFK4`;l)V{4-^c**e zMIE&*`}_6!c4mGXgA=n3RG)lua`M}w;_)VCvnHxC9-JsOIp@xzvg9i_T8{o{;&n3u zl`R=!jhi?@r>`8hcwTo!$1V$c3d&}dImN=-Q%~KR`_(DDAKaGuow$O#i-&!stzZ;r zl6%$HtDpGp_edHy-Q6=;&DYCzsWgl0ob}U!KDJ5cZOC4?Q%!%?H2H8d$HIG+&$k*T zA7kmZ{VMUY+~4Y@%Hz7*Y0^dt=W_2VXgvBl$y-m-<*{IAWWVBtxplu@npJ&y(Z+K0 zLpk$2mycnqPfL8CdcPes7W{7i|9=Xb1Qwc{6lgqe^?FTAY^vjg_gba`vs3R_gYJ%) z@XqoqsGYU@Y}2E?Y2R8|^!ba={asb|zO;>F%d`fTqt`yIU0TZ7we^D5yNw)8g$oVV zZx@fNSQuNK8}#hW9^DrfpfH&++uu2s$vaY3}~WYr0KIVYhsP%z|#e>ovc( zVs_+af1TM2IOJVil|(;ZbK#k3_xsJ}V@U!l%eUVtI=xO@Cof7xCiJ@=m6 z{eItVHa0dML;gmyxYGtH)6|^|tImJ=>k!d%P5RWX<^O+O-_Q0{F24Hh)&~Z!VwYW2 z+x33m@1`j$wn_v}1|6lVyYYz7);VU!H@e8)uXx;Bc3O9PNUzPb1rgTI%(%PKXWc9p z+B4_P9O=m2-$5lYXhtLZ=c=y@-5jpPCcikj^;*>Fu!zJ~ zg|8ZcQ+{~vJr*8cYwD4E>U*nHe`P)UzfaTmvph;(>vqL-e|+}(z1w<09dq@_vmYj# zWc>K?VsZa1cKMosw^JSzgUGIn$ zonfEBJ>gu*S<~w#uk^w@SYZZ9NKtMF zEXymF23f946%ROU3R)3ryy@CJ1xLf#`imGADP6QX+|JLhbhP7C^2fOv^Hv5gXG>zN zpWNxNn*(%a>9)4Uh~2QY0C$Q`pWUY~$h%X0x$kVTj@x%;J0ErHkw`qPw|mXU{C)FX zIQM@(XYI(Mp!10gtcstj*X`00aF#X{6YlhRP~KDj+1kxF#LTJS z{DH2chp)$nTkdtUxL%YuoAqevS)JU_3j61GUT*!dF4=VfXcO_aSsQN2uVPv8-zSXa zS--dL)(`{r1iO*EgPU0%8B zV3?4@8|z0M%0ZVW&#U=#5>&Zvy~MipE+bQ}n23E++WX}t#fyslT8~*5@V}V!l0`-1 z)jLo-icwIYu}D~B<${XdnYq#LCI-I;okNh|Auz)*=|~6bdAr|lB%K#+kGDnd^~u>8*%};`6=g z=idd%2R&(%5>vEVU+gFRwCHY9>|JS$4a|#)-=d$wu|wIhkb~!wz>7yVpU+$lunN7@ zqu4aLNay5=J`QBe$}%215X+xax7*Yw&3P}v}$dO7FR6wR_no$Ax( zJi72Uj-BOb{*0RmEkAarZFBhXzb|Q?k)Q?#BWD+@B*#wJ={>qi0b!1UF@OL5-45E~ zzuxtG-z-s3Y7!IraehPMVK?6y1`jWa#X0+4_jKqED{acYnkgg_?HcxWnc##4E2mc#%{Hei+t%MaGp+AW{r=*-1)o!% zG;+-F47d^tIq^+_>8V;kkz=8%@$Qq=`*U`>2ksRH*?&p!MrlIAgGraZu2PYmv>;+b zBk24(MoFf39sx2;%`u>UPUtbWVCDT~O-No?X-fxbM z@H1~FPyGLXaq&CtR%X@qHWfzBF6W{fS0Tr1G%C6*IMbwb_{QzqyO$NeGcx}DMnEd8 z6nuQ;F3?Gs=RpU9?B2%ge4r4NVNyT@`0Rv$3?@*=?`^vM@0q67-cNcYjo*Aat-l?# zH}JyW>BYU^b1W9jtbOPF{=WHU$+T?=H>8)^=GMeqbt(q!OyzUms>B3IF0QHpWsZXH zkPidvw&H`P>H~SuGf?^?L_qU9O}+h~gbO~*Ov9v+RXpaxwi`*@(dBnbd;jOPfvy}W zyXY!@^R)it&Y-X|CRrho^X8jvm2(2u?HBm@f4#-?e-UNhUd4ap^a#-TCvnml)F=>O z6wGL8`2KIf!Gi}UOJ^#-j(PvrGp#J)$&BW?#edemy>|Wj_I)#Z%{I?HyJq=Yz86gp zhZhSLoV#mN{On9I8#{Za+VoOJP|}iCfrrusQI)~^{8s6|{fjxTv-gx| zIkHG8U$}nV{oM1dQrWKQ@T+x1ros-`S^H_dM^n;`<9Ew$=N^1{d3pZBz3vGycdPd| z73X;xN8dQpwzaGE7C3Dy`ZTz1D?ghvYv$eud+YZsU;aGq)txY{sn6RCrmuUm3lzk& zK?FFQUwAQnK4+b;prr>o6~Mk*Z`X>m_W%Ew)%+-MuBmyy_xqdp|G%d11)VAhx}qdW zseUrAEhyy8O*Tg>Z(08*AmES35A&tAvQ^)c1gq~{zy3Yl_;-Vuy1)%Xrv{3fe{y8B3HZZtPN0v$@p z@-D;JTA3?4)8D)Hb*by~U%!5BGuZrk_n}Bj@Q4j4yaKcvm$S36g{}L#B&u)vf(UM1 zS=cdAN7WS^Hzp*bA4k{R^yp(+Sq95HzcjOJXRh`C$<2L!=SUELgH<^A#JtZ{@9!IK zzWLRb`|XA~pt1*CeV_0>P+kA+!Gj0oMe_rWHwuZoZdexV0zPrdWx)aPGKT{25gJH` zZv_OnIP5WD`tD&Iec();z|Y%~=XSoITE%c>=ON)2Z{O~{W4Yb=q3qOVkUzks-&Ua; zRS5|Xc18KGdRhT$ykFV~l?$i=PY=3*s|dsaj#e5P>ypy^T^=aueLBB&;{LM*=gvmI zVhniBVo=;#Tzww{QQhetzc~N}_tA^kB~Iy+JEuro4W@#y4rf4F;RJTF{b>6+Ai7 ztb`WVhnpTfPBZ)DeBjE;$NTvWH!~|;@Gm&$nyovn%I3<>B%v4Y-^U-Wwm*5MP4u#J zr7F}gr2yte5cnt6xBhnkXIJUs%bl>G@&E^w4th}OXiR!EC8}#-?aeb{rkfj2+`m_F z?xXokj*=-3`T6hHo!dFh-Kb>Cr+)Uo*i1!3`+-^ELJ)iLcKp$d${jQPn%vp<6Ftr6ZgZ*&e`6b!naybFrxuv zmE5_V+)^1%%i&qEq@$tu9iQ>-g+VL#+|d&!@)*DiiH;LHj@Y|f6?PTBb9nz(GVR;l z6ZiXz&i&o;@Z2g!!3-wQk%M376mvYh@@ESCV9ydp#+JPY4<2m3{dN^-g~0W%FxP|5 zKiW1e|Lj`tfHjgc{b23LU7RHm0Wyhc|2r(|=N9K>zyEtKt<3YudwuKY(^l@DY!%27 z<*;D?{`&Oyf9=w?d2-JRp9~EqP(k)%`pnojbAFdD4XRx7@mW^>!j7g#Nas1O&re%j z{XV?loNi_s_v*Nkx5;aaLGDptXUtD~KkKH6arMFK{}ulJ=l7Uo&yX8)NtAEj+;nE5hxv9^1tAEYuVk!<8Y^G5gm-__@Krk{C}`2Me~t*!0# zaILBTOE&4d3W@yw^-Dm&eYWK6?^RYOU9UsVRGZ;?z_R@FnVH7tC1qsx`1tH_tUh>Gf9e+JrvLSCZ~I*PT4i7fyH1s%aT3d;_g@!%TC5dq%F=cA?AhQxNn^D> zu^T(JCQ68aZ%XF4^ZuRlty{O`S{LtP$PlW~sre5t0aoO3g-}YMneKp6jF=^^rZnOyC6p2ZRtZ=Zw+ps!%_gM%;orIi)dau{*52skWQ$lA7j+qPxL515#^ zZS|SwurVrZQLPN<;3)66qMV^-Ms3aD>rfs3a(;PzJ^!HbIg7_v_syBPTVCnqjTD}G zqs`s7n2*+~&Ye~BtzqWcOVb}ey$SX^gb-1_Al~@sKqGVhlp|)9M|A|QgS>QA*I-xu z|9`=t6Ll7^uAb3rYQ=O^EBd`-Gn=hu=jVy5a$=#$x0b6%(m3th=JR&H=kA*`cXz(V z%lRiI{0uiMzHNLo>Fw=#ep74gT4&bYpPu{-+1?WM3+|1N_$R5o&C96)Uo0Q+mnm&q zp11dD*{k-Kj!7zl0`ltu$L2@UDQ~r;#URPqVL=@0GD)+X8`o#WW;&!vb&rx37}>9KV{PF`Nzp}X_fMG3G$?pvPJFjMx*rAtLg zp5}#x>$wCpzG>yZoXam|@*>F7dP<#oIVc=B6dD{fncn@o8ks)#>*}C)FV|+@{p;>2 zy!HG4RWo&GR8QM2uko_w$<6IEZ~oeAD)tL}zcl0?58oZuHZ1!E7JE_6 zSiXPF+O@gI_sMfApWqbG2)ns^!}EF7av5na;DU&?+d2h2_ zy-CnW22zy1a$AsjV?*Ngu){fbYQsIBt0(`oH@3F?e*azFk|oNM&aXG!ybOy2-s`V? z{C&$LPiw=8uK|%;rKr=9XRr-+o z|KH!`wy#z!u3Oc{x%2m?NpjxDiZ;%CJLz|RdA#$b*q0G!d+r|(J^aX!4dP%>@zuoi zZr|o5-B!PC^OoM6HG6jZuHSW2-%bAqj*(wJ52jwbcCGc-o0m2fGK~L}R&m95IX1Jc z)qc56OGe7s33LUt!-6;#KY6p98~d&rNtVx#xKuy+qfJ8E>Lrhhzq6~Xol}#WYuvpy zAQO^V!AUY?Zo}?;y_LF>H}ibbr5|*2|qPPuM+!XWg7}FF+~a4R7c#7Y0U_Um*<+8~uMCueAV` zdLDTept4En-M^kCoU2MYU>463DfnIsx=+w}Rqls(b&HquPA*)Vk=CzLueiB!$?VI8 zo8M+`U90pMTxo$5m`C0JTmF}}Pg3&^5i{RxlCnlZqA}^H#!C)H7J(IFj7&$fB0Iko zg7Q$)lEacF85hhZty}X*(Fl53a+dFdKZ%Fi-iGxymwjKXDm$$wm)&@`%}?%`KNJ_c ztLn~;*}Q7z-T2=eG>g$|~|td@%{QQ~7-E)_=cVi`y=_9$z2J zE?-k{@%r~~t;c1Gd&FW296_B^s|O9tRlf_r#4|SDOrIb7e&6qR7Ph{oS3`C^ndH4` zJ!o20%BmzI&>wU_+m@ftW`mY?UjWSqE_(N}x0{V$ZVl*WV}9E&6a1{-Zt1dB`qi^< zaj%4-)7rIbRlnPw*4g}`McD6xNU+bs>2Xz_pnaPsYYUv`a|mdB!7tjwAJWY*_1 zIp4G93O|M$@()|}|R(mhva-;YPXF33$VOM`?$}E&~#?ADP(C)+<9J21SZyvd7qkjxsnm2%SUD(0&Zr_Y0>7QB3 z%r?iL+Mspg6t4*Aq`F0`rq%s=sb2d#_sx?i*|+8Uf6q;uSD9A(>*exY?{>Xjl^oEs zXo16;t7W%yr>_k)Y<$FT|7Swzv!h>Qx4LnO9DaCQzW&O?cKLPY_iHxaHqUM1RJ*XS zU2c`DSmct&QA|hkZmxRC#B?+(iq{j|Zi(ehzp%hD+VfZNy)Z#j6Xv!qkn0)_RESm_ zKBM>h$6FRxEG-+cVoY8`$= z^Uq;{2V;4E@qx!SA7Z-Ye?%WUb5Cbe5-|zTMkH$wYm(O44*vzJ>#sB!ug~NQ-I;$geu9)4*P=34NFyBpg&`PguSKoMP zuRStt`Qx{}>rXUuh-e>**y+b1qMh#P56K$}2LD(eeK(r>f5o#O^8S(jCEsuS$~?Yg zGVj?p)3yYc_sepx^V|1r=}h0~Yt1_)!38LoXp9pn_^z#zJNtXbdX?O%|9{_|$G3Pr z)6wKUOE+%49SidIehmX1o^tbA7wD|8XEW2~e2q@3{CYCk|H~0!|CGiH=J#tZ8}3b= z7OA#+Llfll`n#F77`n z7Ole_y~uUNr_bl@v;Tg*e*Kj8`ZY7tW_en_-%}j6aq6~K@wkZBYc~6teS5iVw%^95 z&p_wUyxFs>`rXdum)FKsXHBRs`}bFS%?775i-ZFWZ)UFF^T})5+}GCc|EynOddB>I zP4QXl_j@9De0jI~eVQ7--H(LneZl^=TdzfBuhm&IyYQH#>Bl3&-~#OPdHeO&@AnvU zujJZV^z6s={r^IX&)b$Cto#4_{{O7ox%EGrXCG zPR;)K_xt_Vr}g)rx!1e<-LC94-}UE4TfTSuBK$so&ib8CCV8j5FP`&N;`QgR*W=TV zecUU1zJFuM)kpj8|Nr;B>;2;mz2ASGRG)w5n)Y_gfJvq7e>pz0 z>*rfGPxjf7-#gvJlh?O7fP8koX&1-ePtkLl#Tn!O9Ie~3Bkjh9#K;}OiKisf=UjYx z>dlVFeYXk@^KNEP+g8}4pcQ4<_-Nnnce7cIK2HC=bKBQz(d8^%f^l#5{eBmD<5Fa| zfyt`F^QzzNEY$O4o*KkcB)jyxyMZWUWL8R?o2&HO&C6(7}?$-bRYxV!nXX|^b|GZZ6IKw*sW8|LtjbAg?91M@IT?$(F$-UW% zTSPggzgT&9)p5HY518jxy;}L0W%q&?jqYsDBVw_Ljx)+=GSDEY+Z^LD%YY`;bL zn%zvf`ExS6Y)OFGu^)CCAqyqm{|6=3ru*A>`YBK7&wljxRQ}UF(ig*7Izg5H^!xt> z^^Wbn5cy&Et*q6l_YVucbM5|l?3a(-&nGwejpkH5>WnJ6=(?Lnr25z2iELel$9^nq zjo2Jx{cgwOFK3Kx|Gnm7I;wZS=i`6H#z#NQpLasbq6UZb`i5A`vSd#?oAYNbz4hJP z&lO@Mmz(B(08)Q|%F70atz4jrFsb{0e5ui~iIX1X&z(R0%dRCJ_w39!#|!Pqi;8FO z68xvL<$_aJf?Bwsh-ksaw@D#wuGPO@E-(G_@p$Oy&ki8+O%n1XAQ6E&y!}12^_}P z?&{j-%x`PLy?fCq({1)=^J7`N1ch8#bMDALy!G{Z{Ce^D8pGzb|2# z7icRU+TVK4O-DrIShcic;vH$-L+@7j_1Y!B8Vy4^N&u-;E^QPWDug3_|O9Cg

l!gK6`cr zEmGCC-NRw2bV7UUQ?0p53Ec^6SBo_|{aIRft0>W_MtA$$%sP3{;^b|ACUh|$1fA_-M@D2+Q!< zWW9A!Z~vd7yCs)>Wk2^-Ki=EnpcRuGefNs3Ue5=;J&$i?{A_%*YW2EVbE_p9OG2`r zweKzT&$%cWS|Yf}^@haN-8;iH0(#CT`4}#b_#AP*qJP`XwAnf>e5+sR*M@~RCLPS* z^G*NQ_slc;^Ms>2-1cW`fB*ab)y@VG-G-H{@5A&{%+z| zJ)4`h|4sUpdv|KDS?D=kYIt&*d-l!K49ej4G?)l@%k-{p#S)E`Gj|0g>zW7Xq{#$s z6?y0fie;AA!gbM7Tb{1#cF+-@aHKL{aL1D ze4nYU7I0>L)|tEVjt)7+l8uLRx8Gd`n(zy5tEg+KDm-8LeD3s6!)H-D*>?UuY_9c? ztF`v`+wJjXWpP_>{MsB~Ar*bd&Ghk#z{OkcmfbGx(O>m!(WW&43bXwz)_|5^=qPj- zSlwF*iqDss3nUzaSSkfGTqR_$_P(sX7P&w|F=fyCk9lr0rg3lL)-B3Wl)jzjHZ^Wf z`%bN+z2^5KZs+f>4NDYTm@wyb_zG9iyq3EcKQSM@^ULk&%U1EY8!xS=ahoMaU;h!; z)paFfE^F7v$Q((T#Q2b(M`sk@=8Z4AnfmLw{eR1Ao2#TW+|&aPN=>axluVzrEc*Pk z-~YrvXZyeV+pl$L#ig@bR~Xzay&k*y{mh8@ccz{CS?jhm>ra5gmOl;~@4W9f38?w| z_4@9+`?k*bsdimJV;aY`+tYPP*oq7kTZ>yc6#a-CaLvOX;Vt z|BW|K_O1|_t^|%ZFmWNB>75+=F5XF7@;@<`+RFLIvv;*>O}})~+jmL;*9o{e;?ZISM1zrxq3nm!_oln=faIi zEZrBC1iBO6Tn&$Z%MofdJAK_OlT{3tyRH^JmpK1AbAiP7V`h^AQ^g*hwf%l)GVA=E z*CH3pkS;b%FF9xVe2&%6C-MI*B;UK_MW0{XXQg$UZ~N-4oz;t~Otda9nx)Sx@_254 zf&RVs68;N~Es8&PXdUI)vDj5LE$6J%+SLw*cKsiZ&RG0Q@79Tl%GSR;ukQqvHLZ*H zf9OcCJSI}ym396`pLtYNs?7h`&T?h*b1N?NtewoxBQe3&UW@QJwk(6IaXKTUR(ySd3};m^z8zuWC;W&RU9$lu!LXbvt|Aw-WkV|l#SC8M|hyYFB2 zQ`h|F-}B$~!;M^MH%}zgVd=J{)$7mMe!rt5pz%fgouGtUfyC{gXNL~ou~2e}DLSdj zT_<*3L|DU3^vU)3dfRhXuEmz$T_dDp*PXo!d@TFFAII(cgtMk(PuRVGvu<*7OkrP4 zPWsPl(fO&*?}}z|>+MKzzaTlaGVr5>uGTN5DP6BNIq_x22m4v3iYYv0H-CE0a9*I_ z0*UX(%hqhv&ECTCU&*Bll*2@q2i8jc@8P=gE^5zg!4<9%k_%I}+?ZtUw9aDs`)9NB z-)-ui6DXzO#xb3{Q<=w4`}Zg7?hA(d+!rlPexVo8^ZI&3a<5KCw{C8~?YA3+TD6a7 ztz$Y`z;*n_JkvuL*G1K}=BVE5z2RqZ?>z^5ap<(xV*>9#Y`dLz`(t1T_wlf0N7wj= z{6AJX16~k@p5eEbnXMVRMo=THXOoevphnoSiE+?6kfE`QV@LHL&ccdIH!r4{olDev zXXpQW+j3)->*tcw@jUF`7Dz;!h*??diwD$xILLl0eSYn(*td7K zP5Zgf&9E^cobBkHc)tY_Q`@?AHhFCS?5%%-KekIUx%_oZ-U5k(zAdSsl@Vgo+#JNx ztIEVQ`XmgU{{H$J{LgInTt|^A_jZ2yd7-XimyiGZ{eJz^>G5@IxQ?#a=Dq!ePMKg* z_X7)W!zjbXq!{t^-h1``|FYiy`eV;#2g7Zjd%I3>_#HOgCRX?J>GZEB)#s-~cO3if z7oy>|LrQn;gP#9--|u$(Jo-J};`sNZkY+d6kLmMkr{x;{{`>uYI$L2`<{aml+*2Rl zT)M`8-6`|`clEyiR^}DCykPf@t?!;$F&*V{3|l45bkxi95hDAST+iVxs-;4>#kCt9+@U-`}@u2$fYssrb=%U zEv~n0Ov(}5`EuFpw5xH6wEUkf_N{pRtFZ}W2YOY3SSKepLFJ8ze#U|i`{C5hRY%Vr*170|Ol=Uung z$s6-NpR<0iGjqQ=^U)PdeGI0z9=6NR3sn=#+AFHDZtJzEu43!@KOc|3S#$o}wwAX# z0X=V1lA~`XT~q)5cecC+IEW%XX~$kR7tpx&<&$9rEQs70%j-QaIlV3Z?RaTV+PB1i zF?au(Pm-HtQu^s8gK_nKx!4)PNI^B1^GekMiG!zqKA*q-r270A&|x!fy;4(Idp@4$ zIGA+%u+!Q(4u(w)9FJT+>*$a7D_(SOW{j_VI`z$c@7%qT z8q1#Nz1#JAU01Oj_tp}gscQmqAfpB=`|W;dSo3e)Jni-@%l%G<=S+Gx`zB{!1CLpJ z?R*>a`0kv7L!8G~2IzeM-R!XOiutZ>>(v9+y#1Q|1yYMB1hg}~t6RDxchbtW*=gk> zvsUfCuadd=$>!KIZys5tBzi%Wff@-VS^=*clYDf}S>4H4+`Fkjp=tJtZ=hzurd$E* z?58YU7t{4tbTepbZGFVE>yOxNBe|*<3uhIdv)sJ#Xk?yX%G0QwiP47NrweNwvq+wO z;m(i6TE?rkaBaO&XQuV<)8Xw&$0YuiifTM#+Gp{-V0vts`cw`_rHGr~m!^o&MZu z+3mV-X$vHt&pI~CwR^39vh+=-_w{G=x6SSiJLzCpu9q32;=bVL+wJ%JKL6qP^Jwne zj76QtZhkUZgHA2l=kiSKs5 z-DMtdp+du_oNI%V+29(s^R}YQ^GXH^nx^G$*~&HxHPTyX|JlqfYg-be4Y~ z4)gDNyY2R-4=*PB+nszStC*C-FBMw#aqAAV+@n2)>)vYxfHsGaODhnjbpWeUnL$!Zdnns z=W(C)muIu{)1HeSs}?($nzMZOqb}{V$F;B5Znt`Nb#`Ntjmq|IfvuI7Jk?*m-G2Yv zJlV9spOWeP$1I-DOrQ7W^;8~_jQdTKl;3)q(|(v^uhc+k1r~*^nQ)3c-Z>&$K(F=>{yKeo#(%s9SYaPYrXns&mr=< zu5+dWG>)#!YS_JAQ`0skX~s;^JO7W|ot}69Tgju8dB(FMpZ~sjMT+Aw@{F%^V-fSw zpoYh1Hm|tS9oD%(v$pKroXtO?SQrEgO0J&b6 z@qAJdZ`aQKcN`4aWE*{Q{z^W7R(kc*`HnSLto&_09*Im4d6;u+k&!9W>LBK$FO*lg zf>r^i-7PJ1d-AATe_pKk-|}nk_I|(j#^%@!In({F2_g$aTB0v1{pl$%YC5oT`Mjw6 zHJ^RI&2uZwZK-c%I{HGnY3B_)Q}G$%Qro=#^Jp*6;tjomhy)0dUf)zg= zwx?&8J(=j9b~Pn-s~1bx&VP%&47cs?Nl%Gq<9lLQ=rZ_K98K1~GEv#R zFSs-+W3KXyp4{l?^Qzyi*}ucSX&q<)`}g;Q&Ftk;8qa>suKjj%>#eNSsZ-Z)+?$zm zQ|juZz{c-iF8jZpSN%@%xy<2vd?Lv?f@*o&Zl*;}P3h4-r`H>xcj)4ZWjxRCNG_hr zC-V5`#@L)aj{`j=)Z**^mWC~rybn6rJ>uIwH;xF60H6K!$+gdB=kL>b(feKT(8q}R zD~j%AE}y&QAe;1ytKsoyS2=KRomW#Ypka38|Isp^Ne&w~UYmUr+H-X1<=FB2`p4({ zzCGNRoK{?BR#I-S^lzWy&p&3y(fi$&oiY%FwAMg<8;$4%%Nvt!@-JfsEmmP;;j?@q z0NOqjnNfF4GX2Vdj?IeuF5SAdD_||pHo;?_3vO_%J|hFwzu+0moxgIPt5uof@0^*Z zZ6a9SzqeEF#P1v_c#YXbKd%tOvAggJZ|<%t@D;dAAvC&bGQ-`2Dx)-=d0R`fvHut~IGa^Eaq< z->cvNbCg27!Zyub-QwL@h3{TRWv`t&ce%uWx#w}XZID-uotGh#mv+2l+2eP|y{$BU z{)sZqb`pkG1)wzoZUPDcIW3`SeH;oC^f#O}yPdJ~>9o`5ta^>ZAKRR=Xf|9W^n6A# zU+$)pYEfA$mu|YfZr3ZV*!sU;x3+jJh)7-S3YsWAQ~>X`2o!vGRb3iACoVPZcA22Y zI@QdSd(S7&ys6;=4OLLAtSE16WM-eIlv%L%{N$gYJht)xBfHFk)M=5+yhX~d^P1mT zuzLMItAFbjrZJp-^K{y@X|3652Y3wvn2xeVq%4EX!ZI*z)qIfr-|v5+arC;?Gk5%c zJn53X=bw9L@>ZO^X`BUZt2r<*vUG`OKvsWf{^MH*S`_fgIpH9y_!S{wE3YysXU3$X(Ca;*K+`1*~~|G&Kv_ zLJ7Fd^zL8VlFD1@AGa>qdG^icb7@cCE%&-*{__u)arau3lHtl$hvMSTb?@x9tW!Vv z1QhmcLI#^B>gwvgHn}imb=@kKuF(B1JOa?jkqSL(w8MmEb0xfn;C45UR~0~16hm#c1L3uhl#nl z{=;|v;Y*CI96^S>C}uRaUa@rP(<65B9Lz5S6at>4f(-@{t3nRky*|Cf+i1__GdcU$ zwY`3q?)}TV=xk-$w(6;V6B9vk4GTL%P$KxJka?-A)WaPtSgV$h_IlfvEq5-aGe0=^ zm_tD0QxGT?Te$)p4n%0wwb=&va@>fl503*C%wK8Et_d>z%(co-ZXkX3xG^dY7f^ zw#P1`ozJw5e}~Q$GSo#%`&KNoX3c7wano}GI3%yEIba_h9es1(F0SeQ4jZ||TNUAP zV#!#3-}O>!=LyHOvLy4yM?90}O)#l0x|@IYjbjQTwF#^!Z+!ObS@qr8t5x&WKZ6zl zyb5^moKtN^LZ_gz+n#{-;?+Mj1vRey5i$ZNBG8JMbjI>{w@Z&t%IF(!mfbyj^X_=n z%-2tLC7*fo;oZB(#Yhg$ie0dCr{y`>{ipA;U-=ayVGmj-nZR^=+qP}T?x$Z^70`g` z?D-Lw>V+q^pUqkNuH{Z`x~I76_1cWI&1L2C^TF8x>~)0zeo)kP&%AkGRX2wJb0Qxo zkoR)$_|2D_n>+pbZ=3AOI+m`jd#ZY5q3)J)I&k;?WRvLMDf=gwET36>`RbF~^-|W` zZ08#8%!@ScUVjl9AE1n}q8bz|n#X%2i+9aD%6}Uow<`ESUAg?FYuCPIl=AflK4sw$ zsT7ClYSa=h_+IaEOGWb8skCna<%i3@hk5#*EN!24v+(ZS?frLe z_p`hR_b*P?137;!*NmHYlaKZ6+^}}7?y8fSrV0)VBAC062tea&!A90Qe?2_iRk*BY z-&DQ3a>sAEN$0%QRo^}H?EO1CJ9m)7K^a%TVSya$w%d8THQjopwyxXleqcw(?(?~6 z8y6oBnWVPjxTUc=s0Qt4jk)xFlHKGl>vr$Ivp#c)lrh|AB6jPRr zOs@A>VM$`aDb_o+ex5J=-`!0}+pnK?<6Yh4CB2if+h^R=JbLG}DMB9-tA(La={+ON z^Z;h2cXGE|p4`$-+ZJGWxa|AENtYNtN*YIRno+%z4dz@(FhByN5mc<%!bP$i4&0qT zX$m)!a!k+XxPMpgUbtHu=-KZ*OJ!%7qjB}t-PJ$gzJ_Z8MKY7?J-G1;+*t1XRr0h} zKYaJdnI!AWd9!!lSIm6-ahuIkBoiDC+zp@h#lh$b_t~85Np0`mo%X8Id4FqX z*G;3XGyY)>jRpHyIYeGp*UprM2I!T}hTZ!{!PyYHt@2~&NSmiAn{ z+WtDcKtYNSlXVP@Nsrgr8becDqm*#LcYl?$7wq2Dd(XOircNhK>YZHlykpmv?=0Ir zb8YyUy)lrL!_blxYSx7tOe|f!d;gu0gN9F*$AP=~Q>U~woSt=4_^8jpyWtZ{;_q$U zJnLrr(L3j{rjA~1hXprwWS>6+%Ol5F?)+u(WLG6Pc*UE`OnJu z_8wBq&{udlHRZ+Hv}-YMLhjVcdTxInYrl79ZCs9gOgm~=G=R7lDnJ3U{n;4_s3Wr? z4&1es{P#I+TZHE2!tc*jbZ6b!+&eQi)=NI78>9oQ4MALp02#3L-Wf5N0Wk;e#!vV% z`&;H8jZ8n|-z#op?))7&>6^#8{fEx{i+J;wcz3LyuqD^1_3PO;+me<)c{khRm;1f1 z!831eKl<%pDKtV64ryRu>Q!}Ea3kXN*&KMxu->V)_jI0|`6f9nUBp_qe7;LiPPJ8i z(Rl~sZ0&9Q>p-;+ah!8C?OC4CmC5efmxOz-|JU6OgL{qMe+yc(%o@8rH8+qQhO`uD%* z&gAJVm0yR{UkCXFobfYwK;DT+PXjeJK;>hp%7b_1-chzQFD0dYtI-TD`L3;+JNZuW zwwZ4uaYeEOD3UjxJ0k>*dKl3Jh>6e3B zvBr*~!-5+Uug~bfVyBPgPHnVjxJt0qsWW{#sa@~vCND`>J!)T^YiqoF@02?1A^Q(h zn$-8sRD=f0m6;8@^{>gXJxepYb7q@X>8yA6lrj^F?ypQU+i@yFF$}gA;7Z*NSf)pb zDU8x-Wy9`jqpi!&=Bx}_{_0)*q!P!X`|W9F*(?7R*h1|D1tZLh4z-}}+xK5a*3i7Y zBABtfKK9a{x0iqWU6M)rR;hWp^t|@>{mOZRXwJS0#7#y1yBi=B0fm$&9b6cPIIL^fA7@8)OF4qyleNgvfBCG(fD^RPR|G_1gu&AHN_0(83(3!`+AlfpR5U5*8YaGakswy z%ZYoc#SJ%$S6`CE8y+20v6T@7w5e)g{4D@t!? z<%XGaZ!JEzgAB#R z@7N03EBhE4lisdZG=?P_HnD>5EBt!2j>br5rTzXs<;dN~XO7fP{+(~M*_R|kcB4Y+Y=ue{J8}molF6)-!HC zwS7DJH_W-n5t|j>;IOgmdf!BNF*mbe_j;X|jV+IbjBoFhkt^Tta;eW#K795~v!`E9 z?!^`!TQwaPM9hADYz4H&VQA!IET8Xn={o1h=Cp6Oi#qPqa(f=0Y*#nUq%h-BxVl`w?))}XIp_wk=?##`$@%v=&b`{s*zK|6nMnG`q4r1;YD zu(WAyJ+^P>VfU3sgTux)vF?S?(MSfSxtu$G>rT4nZx?^;%sd_UGk4cd+Y)-|cxu|T zyyhss?{{F05M=)aC^NBinQjW;fe*#K@H%j}e|kwk(OJK=vP{dYcl-7(IX%fPDf(*U znK!?Sem#wYMFx`R8gGG$leJ%yw!jQ`aAYjcpH|}g>0$iYHwTJ#+^L=RE!V$(>X*$s zXU2y6?Z@t#XeO4fty|-I;0>V{Isq{bm$Xl|U!QsN+0&w(zqd{@^ZsRd|4f!0n#9MZ1n4aHBtm#gdMm$fBF`W->vrEQVD5-4BvL_{2c?@wl}L})5e*9Hy@u{ zqzsExgh34szM!)DN|9j%EYuc&DzDi~-cHiKX1w|KY188GvsH5^d@(+A=9?BSFY$uB zgfvRTQKJ3e-ER*o-JJ!XqV;X>-IxdOs=cjjcfJAT@jG=pD?KCNF%9MZi3T2I@Bg2m0fejzQq7@Wa0m7g(v(bkK z5jt@P?k=DHC9~+MZQ3@s@8xCnt-J55YQB9fVk~{>@7x$zfr?_vYfu>PYwCeJ)q~~E zU;9aQSN|>TKlA4LREDDO%qnwdeEGU(rfyj0KEyI1h;P8DZvl9taCNN&tV5^}Fv+3O z>yYc&H?5PbGz&KF*4KQwa!-Gl>E_?3-_HIG%e}~5*P_Q8 z)5;=F%8Kqdc(;E_iOZ*p!fD%bRDUhwhouRyUm!%|D^Z1jHTSkAroaNr;Uv>Lx#v=L zv+vwUN~_n{xtPg(M|Ft0+tj36VFsJswJ+M&!i#N~9tI|kkmU@GNu}$!7egC7j4V|; z58lnY^JmUX-FuJEeab8OZsBftF8It=gYbJ1l)6XM|EN0gbTdy9~5o zo?dXA>78B7B{5I->=`$0ZQmK}ym154iRuuO{M%>JrkG4! z@?)VT*e-`u)|j9_2R^4yx>x#r`tPr=zt32*J1Q#b-nuu7U##8E${|u6J>wrNgEiU- z6nsydbg5XFeO6@j(zZw7J$38%SDt-y*2}gTIZ9TvfW{M*9qzlFqXt*!fxwG<=Ve+v#d1)W3lOet;<$|3$u=dw`UQ4}nCKm&2LLY|8!J+#YAI~`K=(;NU?BrRQH8dO+oalxZPFGeo?Ede4 zN#^b4-(i=|qvq4_csXgw`Sq%2)6?dMe6sw~3aW@8>K)3N&YnFhyyN%rNz2~9i-L?KugGfr z8WkO#zy6h=*@FihBGLsHXBt7n(xk0ncfQukUn$#`&%U{D>YTm1&)?ej_GOCa{(6(m z+FCzlP@>hkaoMtEb8pn`w^juko#m8Jk)8cI%+&A_o0Ljp60@#ZFm$k_@t9D-_nj(x zr=IyGW_)|YC9ZdJ3sYOB?<`w7GdKL@^9#sr+*nQ?8H*2Yo;j0`XM+|{UfFlx#lOG5 z{RAEvHp{`Z{H5LD>WHx={zh|_$QtRxKce}l<)L(wT{4Lji z|Ee#WU+}|X#et#m919zt%#Y?Jo`T>cx}uiRIC}firBC~A&G-C!2DPum!1R~{G-o-@ z(+$1A+_KiNKe#!cU;&x*cBtFE2?rMh$G+ZePE zX=Rx;d-nE%?+;&qjODn}4x2cHZOKQPbz*32Vq)EMZVRkGPvLvE&{zmPh793^5J>vUc= zJ~@2)Y)-pZ@y_4*DtjkffB*jMn>H`|ZpeOhNSZzYo5^(q58E%WV>$C?<+N$jmcGVj zu82s%_m!%(Q)_;7%v^gzI_6zn|B|bd_MJD~y#LFw%`f1MZwH3Pkc0;Z8kc)W^>D%6 z3*MT4{dmr+WHFYmuN(W$!1LdWkOOz^Cz%xgX8k|WWbw?uA(=+y_8yl$dhXBnoOS-p zv5;SpUtnWEph+4~4!5~-_q#JID6K5WV~NYm&5d1=mln(}tQfGS*)@d=9H}6+;Y{!9 zHZS3wls&C)t+UqJXYV$Oo)oV9o0pNc9m80M1t)%ka^sb`44}>JnjivtunMTs*3y0O z?zorN`N=mgp3z(VrFfBjdA-Y}PoDP=pUGK&uR^RA-iP5(m>{eW@FW(b>y=-F!^Tt7 zrZ{XoCD!r~oG3uVRI!5Z_8w9bzf^b4B_(e zn&M&yO>pl5HL5l(1#kaGTKlk+^-is$r@s2%e}~VQtpyu(RMmF!b@SWvzOk7hxvGTY z%ND1?=X1-i{d&DVe#?{B>-Vo~=C`xhUAOV5*yY4_*=c`&cdE}@p+3K6)8hHbeW?3e zUBAblXZ8y;u2M*7nzn#Z?~;vYS?Yd6BehZD+RHj07fL zc{>~F*B2H#e?6hR(UFUnk>wTW?0{2Z)6jxqui}Gu^S=CEHPiLP4ehgc-%r?5cFFSg zyl-75a}go>goQ(ddq1~)?U#w?Y(D$!{di2e^y4N0jcZ4Zj5t}kL=^&b!m?~)-4|HQ zTTltkB%reO!gQv0|K==loosj}DNX+Su^D%2^F5cV*KU5BneT8}9h5df${HNn9dwf2!-CeW!+^Sx)TN=N=uW+gPez)BEoxHqgSNYx2>7YyJ zHXgINnKC)^+0687psl%A!{cNB{`z{k{C@59(9mNt*?R>wuKnUNs$=mIQV7uLT>V{N zE#OUU@;p!~1Em6vFFPA{$E#)jPEmM0^XA*Vh6d&Hqc5$Vq~`N0{r2U12bLj{L8H%t zI}RWBe!q9Q?)Tep!?Pa+HOjWneRYUie~mMrrHin?%|^NEH->X-KAjYu9UounvfxJe zCmZobJ3)njH^Se}$$OzKEx9L9@V!=L?zAs^_s`^g+neq>A97aBznkB-Y~KJrVgi(Y z1sob2Hcl>2=+@nK;Xotvg&_*!COzO z1PZ>Js@|Q+^DN2u_R{Ta&)->l?NWI;Ki9vSdv+a686(Rtm4KeLuI;?i(_ZpR8o7WD zEVc?;G=uBt%H{K-ve#~%HZ8uc@~p+LpU- zXkff)wpu*Crf~Xd`}cc3ziJWoJF(94K?5__H_O{On=|)*y>>aj?z8k+gTp+(emZx8-BQ!@HlH&-om9VWe!ph2rOn?jmqE$;>(%gh(LJSy zIMuIkX|GvuH7wfqKOB!-lDthMp9Ju+pVVG?-Z}D|Nk#iKsx%E-G>9ryT0G6{`$Dz zzVCAVo#Gpb?Wyti6We7seO$l)->;kBHXENcnOt_SGQC&McGmka76Fa828WFmt3MX0 zHa@CIe5MF5?(TE#`2A&)ov+(R(CI(Xi;KQXs_a!-S^tS=o9$cO0>qIuQZWnkx7p~b z2gKBTJi7I~-S3Q3n#*-|6a@xW+%3JH`QyXG$kN1HUYsK7^J~j^Wq+MMrXpCj_mkHj zvA^}XmwnBzUW?A3J9lfuirW8wzgvC3Q~dRWa(~a~k7l=X7K3(3&)rukxa;ly|Nnly zSloYXm)fDNKYqyKH^d zCY?8fwYC1==lNgn|Nm#cyKa)F+Qs;PpTyU0x#TtXp8Ue}HI7wjX)$xyx>NuM?>}zQ%le@W4jV%@#_gNuuyK0hOZXhe3Ngm=etF$_+iGp*&$#Kc*V3$f zf6OKM$>|&3X683tHis1k7Y;CO7120ne!r%R>lnvzr8&DEwn=~abXq@s;k7lJ&&>+u zcKaH)C41dYH(g!bWPReB8FURfweY*WMeO{&8_B%!0 z_bQ*y)rsjjbqu^K^sMRhV?C0KllyJWR?j)Jm4mrB-N^3imEf%G?9)w$J=N!$>{izj zU;N2vzgGUO%;jgB4y`yRW12N3Z{N>nYorWUDJ&K;SoL$!D*aWai<1nSj&C`&akZ|$ z`K^q_eWwoGEx#Z8|L6I7t~&Q468bwHF!`F@Oquz2YIxku!q^Z?x2?%7J3zOjyxaZW z?(gsW|Ie+9P3ly=a^=dD&|tUwg}J|aOaf{?pEciXVEC&e>+E^^|2C@&&hWQO|A?Np zQaPZ9p)twO>+we^rlTJ_&MtZ1IO)>n;yO8N*s^ zLFb-K|L!g^r7hZaWk8SDt`+lUr%sFfa)?|1#gE7R`Vm{F&8r2i<$U?L-~OFM^v1?V zC6|4t=ax$;-t*SqYjW++qDk+!T=u){BJ4XQ)G4m^>($3+Y`3Y}zF5$_>C|q2`@bQe z+Ze?3+)lSn+;}vTWm@Yt^33ZWoux<4Qphw*(2roR*EPQESTARQFW&+kQK7XkP~CaxpeusVQd- z=Sezd)^%U_d_>s)jE{28ncPb0>q{GxLX?ilC`Bw$Y;foY^*ep8Tbm1MeCyNR3$OJ; z)gQc5_r7H>dE;8zx9|6)R_yjydKv0jta2U{tW3)h!3vsk=P$f7X9@2q*{Yq5Nrvb9 zKjbW*TlQ+l7WKwQi@Nnz9p<;MnPZ^+|EHkMyxMO!Z_aEJoO0|>5opgG(;*I_#i#N< zpEbW866`xQ)b5z^Wn)Ruar0|#z5Dz9{`I=wZ?9)QjbZ5*oOQH9$)ozyN%hT%f8Lev zzYID=g|!)Ue!@Wq!+O7$-&Py8x69Y<2>xbzEkgNrl}QxKs-3yPySYRj-->aNnc5t< zbcTG%F&jVoza@WvUEhE1Tt!d)`K@<$PTzjNZnxf^4~P7YZ+kW?JFWKrm&^WjD85`? zE%W>n$(-t2vmbncuQc|_-K4HYYLb4_3=ip~8mc`&i1O0eVDkEA&lcX5j> z{$#A>P&{=(;k=|T~J*J%MM03VE40O7t8SM5N}kqqSSBybs(J?|&RP z=^yA=t$S5*UtYg^?YYaelI`}qZ)z5`@Ck)bod@qW_P*qu6t?<{!D(KR`_=FFt_fN{ z<=SMfqX(PW)1NE5T}=r2UV3%eoT5{2-t4=PXA-_v{JOZb#<7_lg2$fbTT9itFi0fE zvh=mu`5za(U487^^6z(w{liRWW(O|a@pjwoG{u-4VX1|wIe}7)kJabb7@fQF?QVHI zsAP{!O**r1ciDqR_N94Qfo%i;de|X#Sy}f?_;Wg=0&Kn^eR_cdQ~dIrAxwsYp+q64w!Yj^IfT(NulG!C`sl)m*2hUYS4QYK!iL zeuL{pjgMl>?=IC|zo$rNrbW5Ix&Pf=yDgXv&IRaPaqE#-Soil=>2a%T&p++~ouaa4 zz3!e5ha#^g<~*JCT3?HQe(krLIkT@7oZWio&hFbmCgrtr@^}1twYuN5KZ)t+7OsyC z%zQ6yyfS5aV0_-jnA_p=_L$g=g{>uDF1k;DA}H?uHEqsP30+>1@*9clwcqcStA0Pu z%K=_Lku-I$0Qlgd3dS@;1_lKNPZ!4!=)rJ|ECMfl8O!TkFEPD6-yy$y?!`0zE}uSc z6L-n+r13e^&GCFL3lW{@D|;P&-@MiD9kt~^igK@|d(fMChP#$aY!%XYw(3ig_t_g! zo7LB@HWpdD|LgU5duwllTaeB`f6wO`-Fmw`F8dgtjkw+UEk$Q%^`cvaACHP(|8!b^ zed_dBv)i-29G9;@vyJ6!uJ$fa-w&dkuKeb(<* z@S5K-xIN4C(5V%x&TNfLpS$(3&C$br);{a^2-W9ktrgUm_U!0Y4iV5~Ahs>?{j7Iv z`8;kKNgjKZR+gX7clvJllrI6Fde%Q;uQ>bW?!6j4L>A!)0d?qAGhgqySfiX7ZT#C^ z)h$vRwDWG)%Vo1)b?ff~AM_>XyLY#Pp%yD>N5`9$Uw>)@q%Mo~*t3`#Nj37v(QM3T=kxUF{c)vNL#0f!Lc+F6{_kn{`1}2S`SeYKO>PMqDn~zrg-uVSQeh3bR3)`9A$%%H%R$1JU>pR_Svqk#uq_wx0j>cG+7CkwkbHnTQ zN%i?_ET7K_o?riO=cAZ|F%i@H1s~r0a@l{qzy05mHT_+jB9GhdmfiNvUb{6+?9|b3 z3-kB=Omnvnw5>QMnVvDpQ|;tA>mI|u-}nF5-62=#`{(`Z^^a$qj*kkFvitMl@UI8W z{9N~h6O&rHK33+G&dP3=De8FCbC^SFanZ-Qm^($Rf$Uvf0g0gZ_4zruWnik+d+_eFca>?r!tWV3C8x^8Y-Q~NUD*{DnJQWf z+86iMRoUcBYw+f?X3^VJGh0dm6)SG%ZhyIQ`MgW&^J^w;bm`b}!HGNbYH0Y`ePY*_ zt-ax3c+Rxvg6==?e)fe)Re29ytzJJbRNbxn`JI~2XRBT=o&M$N^!Thpoa!Mtu~LeW zJ&)U@^Dgw7- z<)%}6XQt2l`Tyo_euJH>KJ`C-Kl_5UN8m0K#UNHj=l8A-3FVoW1O*sh&b4p4v|B^V zmx)8=oZbQx&7h^KPA~SFFKynUFzNgMKY2ydXPBtwy`;u@=kMpvAEjz z^7EFdnp>(2?kbu7O<3Bv$Zr1I;~j#^Z+^XAKYxjBYwPa!`>Hp&X6#%P5b$upsVTvJ znYp>S+g>i4t#&Zp|LM&1d7j%!b>n`nSn&QqGrwP0=C=@$N15K2A6=dKYtOlsn9{4E zlf5@bubQ^%^@hWI-b@Zt4U>=ESh@VwdKFvowKab}_Ser@v(BgaM9_Rt1wET(Z@=xg zuTIfSN3;Hi1j{qAbXD$V=Med9ru+pw;{h7=U9r)@~586tkX8 zu96w8i^4vy-E>OpLDfoKU(uM2x{{43QZ6mwf zi;q#3nk=$rIT4#41*I#03z-&`rD^?aM)D+kyKgs=--1R1UwoTg|L8}k2HpX}@z8LX+>1T?~2PPYrZ=w{Fs`MoH#{k)LIHO850@NS_Er^^w`v~Bsz z{Bz&gJuH1JHN9iUtutolO+Htb|BvjQy%e#&tkH_w<%md>Lt5I+rTN`EeqU$*8M@K` zFR#ez8?37!69Ax9&!x8PG185X8aP*iVhKD%m7&+zaY1{Jcv@BMoZac$7p=P4pp=wt zEIl*RfqkCN+jsHr|A%b8wi33aSHNLG9FyS2Ni*-x&wL+SK7Vo8TVaiBUel@s9T*zh zI0ZD!diIqZX6dq=EwHN@JU+<4#Bn7`L1|xp+G69kJw@Mhv+m|a)w{e{Fj3X`t?a{h z>epA--a^c*uGs6~@+Ki|dRo-3b9c?xhMs2W+B#_|XvxWfM9{jX#rk&*8k5ecwY&ol z0)mLGVjUd0)?X`^&27J1yEAJ0>}&FZ?z3;MH+pOP?436j<8#yYpWX5$yZrwCFkK>C z@2sE{XSA7BecRH4!m{%JYy5KclhXQU-@G#OTiu&?;a3*d-U0_Dm;kw7CNWJtEz0WN z-T&*ubhmHcK0hyw{pzZ^SKwKq73&*z+lOk1v|ZgE8l|o7u;9dB_=FvgQ)9=DQ)jlF z;Q7n`Zl3MqWy`b#4G*0uJ7rR|cXxi!Ww(`xwfzh7m;?(a&6J&&`N5#veruR)Ms{{^ zLS@L>SIa@6#nAYRYsYWrR;R?DzqWa0gEo8Yw1+1xM-2hND(gSfXD)pI^4;_6-AYPk zMw_L(ZbhHH`#*5K-393B${Y#}4sThM9{CtsD_lP0_-@_()t)b3ys$7(I=8~}X$W|M zO+Yzg`Tnf|8f(J8w`Fc{XJm5CP(GJ$qrKP2BgWi{Au1{xVa(#zt zvQyeOr?eO5yMMENUDDA}d-ly7pV|M7H^1NcJNfGy_FP1BJ0yMqW8) z-e30p@9UfVx4?M{65;zC_AX&rt33f25|4$VwQv(M&Ce%04< z3+zTIg%3}2HlH>73p&L?e4(bLphCczlP(u{K|TY8=z@Dpf*;w9ZyR5Jx9nYAtnF{Z z!l{oWjAz?ley91auEO@WV>F^^aaA~o5hDN3B@HHI<9;`}_($mWF(tfz_`tAAkSx1NR**7gdyWh{a`RU!isJ*|Ps*y|= z1et#EqM?b>zudIV;prk*nn2~k1%*alD{Jfgynx##x0^v0uz~CaNr5a#3N_wsZT!c6 z=Wjlc1vk(5tQHYJwWW5>Zu#i<`!2}A)HXPjvPcPPta_fCo7=wQ_sXl1tXn~40*8si zgJ0*)`E6UD$54~+E2QyF?FeE6madk7pz+L`?lT|u7Je6%HoVheapjEP$t`QIc1A5- zakUG6tRzFDns7$Q0*z&TeSOx-Nnh2J7w-o9EiIl`)~e)Y;9@shKDU1?93r31)xU6n z!XA{GSIl;B*^`-ef7Xo$^=IBlzx%g;S7}0$v+!ZxwLvjDm%lxHH;-5H^L!?^x(RtY5f1{XKN5|_k zZ`x-n)}6n5+$`yYVBvz9b0=p0wlof|x65>?&p=EoF8I&X%Oi62^|o!>mfrpAWh{N| ze_#Q48JNRf7Nw-;>(;IFesxvu?U&dFhmG&k5DT`p^fd`#c{JMXHj z>{f7g3<-SjuES!<>ea6swXS)FnhGcctYOp&1?P8Ayq#4N5R9IAbA?a+`I7I=Qdj?X z{Mb1&mHEhVwRJ&jew(f7HOg34W^HA)FE=f_U*5jXwLJdNnP=vkZ=UseXi$GWW7)|i z>%kFpR%^j~kN=$=+wc5kHLf;MdM9a=a>BRO>EUWOJ}v&nB&&blzI}U`wr%>%-1IbO zsh3W-!PW(pR@5A;RIBdUJms@W1WVd3SyZE^H_~*%5K#?sJpPl4o-=l;?nBFr$!h?UMgz z&fGKI9IpImrJ{3N&o#^KtxgY@ipf`>=IwXb_;lG6MyMSOOtr!t9dE)S&+q=dsCMWYWl@3wE&R#wxxIyTPHHo1{D?;|*UZdu`wo}QlW zZhOpaK?Hv)w9;Qt$Hdb0b@MV+SlB2`XA<1lH}meSmv=bI?=QVLx3}Z{?3;Jf%u1K( zx9|AoxA)xNW{;r9?MoHJ8)^2^iJY1?+~wx8MlZ)=tOt=qS+ zn{S>SRXe8#vMQv}QSC!SdiwKATjh)!lh(Gbgsi%3`~_NJeSP*dNI*F-G{$hb9NC@r zt+#V-_4&K$=bhb^lWujc7MXtj%X6c>k@F#@-@4_t>-V*_c7Deoiwh1Yh}_t*dGqIE zQGK%=Hcs1h1u|yr@RyZC);;TU(2-Rpo0;(k)#n4NBS!jlV#faPH3$gV^D~(0GT_)j4T2f zph=+QJDH+jyBJvnPJqgjt)(x&8fMZ62nhQ741+Ays0pph7@SkM&weqH17Z+9}k* zQL8UA|K*j-@AkQ@v&u{pEZjYF?aY_gozs>DeEhrT>(-mUFW>3f3|Y5;Ks(8f7h2?V%mn2a zP{1zu#UxnRIx~0n%1Yky{i}cJ@~%17aber?TmMX=zZHF-Wh)>Z(Lek1t8d@F&U&=k%Fvm-E(dl`{Sc4%HC72k*A`%iI5J zTDNYUw)Xxmf3u(lhm8||ZGohE1}2Ut9tui)#+xS_$KJYrw|@3wJ+;_Q4qKL8N6%b4 z_vP*4&gl7lC!_0UZ?$LXI%`$@1)A_qv6R@AhKFCDIkk4z`}wDw7u>jb(eUt(kB`rn zeZTw5j%l3(xN$N0z+J&YkMQv8%%xfVX}>fb7ToZ+(1SN6qI?y=t$Cxzr{0Dw65ROb zNI>B=pZa{`&0p{ST^%-iwp&lT!$!eZTOgbG9R9LI*#_ifXGh<6z)~>qc|;p1yniMD}y#q;g~H{VVF1 z@BXb=`L@{jYq!J3nQ7hoz(J;Pp}aASPsG~FO6=m#7(v7M`>8v?YnT=^GTkmOD~r2# z;9KS{)OKcr!*&Oknw&IapFh3Z@BE!z`K+_!zON0x`t7rz(qi84yF9hZ0Xc1vi~1pv z_a%P8QRbtdX4=2k(6&u8%RVWqk{6p6yY*J>aeR9@M-tROK`gs&%xmn}ar#Wy=?OZo z-kpCwRYOUn?v>5asm8T6rH?AR&Oh6F|Lg^QrlV;yRDVH^`f&Kq8f6L!F6$$A{on5A z2?zVnBm&ghJm)KQhkcg5qQim{xv-qdp>RQ0Kybg==K1L!x99t6MvBqi@{gJIy7a7N~|YqPy~6 z)$1pyw{kw_f-pR@K4cO!?44OV@8yZ)cl$~XTZcKaD<%0EU!JCMT0kp|;o50;4iWY| z4_Rc8ESV%7T!y%yYEM4yfd|~DU?#7N6CY!hUq=%J#e`B^{=Zkdz zxf;S+eMR4Mi|+oEDeZGGbf4@P4vDW7jSk#wUD57GPDlj=LYm;rEJ{f(;o;Zycr#|H zRf9JCUjyYai24~W3QB#3n;#qP-`T!9{(_HBs?*W?yMLdbm8sMFO1m*>@smu*R-?vW z99t3=R3x~}36!^u0uQOI*xIoBew2pDwbiB8SK@avG$uU_tAX#@kPu-K+_-aQ>|Dt= z`FCsYr%Bh$4EV@WZolj1vEcXrgfy;2@3DJ(*8G0WwyR;$zL#!*7Lru|`}zF!Wk!hc zCwd)Lvv%d?<{sbuJ8&oCGG%bEGYb`bZ|7<~^oe;@OZRFmhXoO#ep>M2X;!>~Qc2yS zbAdjm@87Mxn3WzA+bx%KzPz}|{GP*90aVGDOa-~^ z?c2A@XWw-6HRUV;Zz);;IxlfmfCf*qhCLHY*Uy>URp8_g((%MiL1~}C=9Ow?FG|bj z>zmJgo$P+9rOSWDO{sVPN^e~{zAO6YW0tP7hP!X)?OvO^^XW9(vTt9n$8Trmvsm!^ zJg3O)->lhuZqq{N z_HPj@!!$tG3f!&tTM%Jep9LC{m<8&--AteG50sR3^&J+>*sQ4*aOP&q1zA{bQ{ab< z4NUjF^Y@p{m$mzAXGmzxOHO+|Wkqs)`TyMd*Lg=@bBKJlUmcyZu@!Ve-Q?0Qm;LRx zDmu5_*mBt~`}zF(eRqpa>#}tHobOi!Di^PS?0xsd1jJUjV9v;NbYJDwKbKj$Ox2fx z>SIv7<*?u<3uxfr+t*EMo0aeWUGhx&u3EkB)?E*m8z*uXZbeSg=HYm zH7*lT2*^2=x%e`NLc@UsA%%c5H*6_rgyX zZq|KKRQfhcLdP$*fZnVe)CFaRr!mTJLu2vl{PyTNd@!G};#A z?~RlFtWJtWrc8X-cdz>W-jg$7BO+RY3ITH#Tv^qa^wIW1fBm1sdOIF4{eH9gyw*vg-&x%J6p#@Bwmnj*ilbis|; zl8s4=-z~bc#e2bw*(+yU5M(+!$Cu+SxZDDjexH;C1lP~LX(Rmm^U9fL-@KdrU)NY) z_MN-EQqs9jZBg&F`(*Od_Ro~i*>|J=)$CuB(zryP&#$kG&fR)7=f;Lav+s9`)zjJk zec%6o>-Kw9+LfPwZM&W49VQzRRkC-RP;A}Lr@04N#kYV4{da~$x9|IOO8eyOo1imJ zH_gr1x#((hlOl{4hPW4k^E6opI zkIuJ^PU=+6xfYqe)!*)?O6BI*ZCplQ8D2@>|IzmMA-`4K*Vl`-x%JD%W|sey0oOvZ ztQ;ch3EQ0(tPt&}|26qu`|n*DSEv8mz5gJaw2Jng5V_hffz}UOgmeB>)Sg`$675iI z@byaY?VZo(&F*#xyl;8=YOB<1kzk)hYlkfk!eg9wD`hCB$LT^?X-_F^5_RZ}3f6_kP{qbt``gK{iL#~N1frk3k zu*BT;SvS>Y{?mE>j{o`6`m1l;U1UEGH#=LdSS`tP^yRYI z=f2%3_LsJ+sfaYpeP41~cl!;{4A*J>{W`yInqH5&Tz0QAeRkcim#6FAfeIpTy`4uu zH)hD$Uk_gJ<86NZ?YeK9=ii(X>=*ed%J#(07mK>zfEHQ4IsaN#Lo4R*OaJ<)YZ1|F z=hgjs=^4){QvBhtH|XS}MlMirI&5`Xu;RVt^ZDx^A5hnSmv=jN``p}pKcAhHkE?jt zT6WpjJhWo^hmHFW_?_Un?J+3WXS)7$&yQlrV; z=h>f=u08K7cH9fv*RC3yXwA|k>h#eNR78P_!h{W+E-g%Etht#tPu_j~+x(2xBKIT9 z(!K>7-`>%lHh-RkPT9`iw@cqdInO#(yjTQbLGSSm~H>JCS<{i&3)f)=i3KY?-k9BUv;(qX3%+u zz=f;A^iS>DzG(Zg(skz~)8`nTbdOwkW426v9|IGIiC=?5VN>nWhu)9VRz06~*;wQ> zYuB!*g^TtvgI3<2^y9o`*cepXxTht#cJK0jt5+*hjxH*jbHO|RtIdBy{j{t5rB`h9 zVq)nEZCVp%%F=ap(h@nCmlqsmfy^mMUXy%RSJ2H{yy>Bf%k3ix8<(fN6gK|-`$+x2 zSMSzuS>!p7*)`X3)q<>TH`BB|*N1l4uKjm;TEhOnZ}Zi6=PjRGw#nD*q~3|Yzm%gM z&amszzWU(n_4xT~>U8v$G9B%=|92y8PT{d9i{ooPx^C)yrL*}=(5C&r)`YRAF$c8H z>(<+qQM7m4f=kjK`+nIn9S!@lojKWwp)m|p3K~qV<&EF;dH%fj%hoDv-RpxV>uY`YfMqm>L$PSt;mwd&lej>#G7^F7CJ6Bpz3x`1Ab_XMWp|O|QI|U1Qfw zU$$j)^Qr#&KabzN7ytGCN0;`x896K0#fm(eQ|u>gl;Uye-|YN-ncwf0&wu$lcx9~I zw!3A~w~J2ezI{49ej8}z+s?52%cn)>SysQ_`+e8;y6fE`r*$^_eER)s-G{aLwWqgO z`Q2~+vePj6n2LU8?T^H79N#&euWhxu@9(Ow_i{7n+O4&l&sm);Klr68_v1Rd<*uvW zfvyLeU4E}}^81+cHlKYqr7qjIeu}o0aX=Wi?v{X{{bfrhG@n~A#rb1*<}IcF9}e@s z-+1cL+vk`0zipL`4PRLv|335V?0xdu75n~tI{kK5_PUpH{oADfp3N`eYEdm-vr{kh z$vpTzm)}Y%of;bEd2GRB-je+-+3I0vgY9m^ej>%@j-EGuJ1& z9b9x0)BMwZ6;IxsKV$m8tGna{H?E!eHZ^Va*Gt!I)5`YFjD7m<{FPntUsu_mZ%q0n zR(kEx-SYceSFhWpRhh70f5z*BO>#MF_I|r{(!R=J(W2u2UoNQsp8i@iEaD)a^_vZz z8KQGuGPZ1Ex%{*H*ZT2yxF_33dXr!<$}u>b$_yyq(oQH@v3RR&9Q zPkw3e`|4+UHN;b1G;7uUn?dh*~P3xRTH?Q10 z`zwD>*o0K?cqUiYh?kXBQLWSVwQTwO?e^{dx-W}2nNRq4d;NRaquSRuFE=>@P@;!i(98weSbdB zZ-4l!IlgzVXTH=8xZ+*&i*NO2t7@(M{eR0g+5Yp^-#evdiGJE%)~;J?U3O2Kay!uZ z?W)?Z%F&zbg@4_ve$dFSwy-MYYrT?zqwO_GdUR%A&`o=|JXELA0YBIpBA1ihp-rwQbI&*)Nu!TD5vzR@IJ)ZBu@o0UaoxqJ8;Izt2jC!1{H$ z-dmj(?$8LildYe6BYt|3VxlgytL=u%()Tha&Xf~+#ryZsB6gW74~^%KoEF^pAfIXl z>c@Q1a9Ci`xXk{ccUM%eL*eVS+s#_d`fWaWe7e-3hHQcf~neDrddA@Ib)9IbpvtO_%Mne^KMUR_ijCbM5>~ zr^7Q(t_!&G%3|kb>dHI5sK#Yi z7ewt;ZGUui!mo82ufM#>UCT4I^_df6T6A>urQ`f|KNNn(t^6Mv_~?pI=9=c{`EU4OfTTd-^B|8c2Dn-HrLzv{R;bEp3KsfaT6}@T|52zm3>Xe-)LRS zS+GLZLPYHqkLO+YSBt~mzDjp3eYx*~cQ4ne%}eF%?N>Iw`BL8cNSyo9zm)&;Lh?*A zC;v^K=wO&G?6zRWX3mAy;Jz_vMtvTOQqr24squ{SvJK|QM`ZCSC6ycBK6J+H)Mc5g ztMsb`7Ps40YQJ=zu_eCBV{tn(*OybGwhmhN_(JAiXYPyt^GJMh%&Od8(DfwK*81}= z{nDTk9pe9O?XRH5MNYo9f3!ot1U%!f|8ZC)dgrUgIdXARw2n3=`QP4|wtj6&>ms>b zb1NQoPBvM&ec|P|$GpC}?gQ;p`E)=nnnUFCH({50&|Kbn)@#xEdv|}kk=%bZ`lyTJ zfvwl$Za;tcMZLyTdZpWf8`15T4IC9`|C+@s{7XnqM_)^+RE zMP2*aQ_HnHnDze7+-IJ}x@23z`|QYsM%GF1<$lQp+VtOa2wd3bbxbAJa z?E3L^=H8i8xkXC%Yl>WX6}Hyz>^d3W-y1(1;#U8@^c>suJf&Ipx1=z;vY!2A?fOy6 z?XEeOZfSnlCTWk4KURPIyg0$^?z_@k7s+Y7R17xT z^6F)y-_q}`_cHD$aX&pG+$A^f#o741nepo~elG-V4Vt_r=*lX_y@vCr1vM@TbN;sF zQ{eT?)v9|l=Zo2vT%7%DX_s8qOGVG9RiYZNc>m;XyO}mg?W@8@bK%_DUC(A^`xVJH zCM_*jnhzR{d8O~Lz~bN@`-k3E(O)@4wk9`}@dX4LOuGAY!6k=4eaDxn8lrQ*t}vF@ z*5zv161sa^;FZ&^H+97~Gfro6T>I%&zUO{s+iy3LznzZ%w`upRoCQA~H1n%1j4KS3 z7tP3IUC87+S2Xg~8TYlfE>%uE`gO_Y8@J6SzRVMf>RkExrQJ^#_to{WRoutfZuu>} z_jl*=ud;skpN4FS3e^4=QpF{b`fvCBzj~1;SH0i%tM7L3#q_OBTd&Odl6Eb4!HTOb zTpgibiWf}(R;6`IE&TS)RV(&=aXpf=V8vUXt!pz^PWtA!u=3+XhP`*+U;Xv-Vt03y zU{HFaTOlTBA22(f48I`g$$WbK9AiJ&sS6SK@0ENuAKH|^zR$9Hj6m7(*j;+F4o zIg*pM?dOqtv$esSOEcB_8yB53(22XdoIU6Jz3Tk^zFpxquX;ptkB4{$MohC^zx2o2 z=Ru8&?ulKCK5RSn(bWE^(3y4&n){Yn1guesS)0ARe>48ETVMXd6B|&GJ z8ZxnT3E%lGyl;td*jb0bXcxV0Hvj z4=%l0H@E!W%`J!4Z(k%@=dH8RrO0e+nMFXr%6+SB{|5iLt1hiEjoW#vQ|^roiK&Oz zZrLA|I&u&_0d|Z%MKeS?auh};^N7!O>cg-ye!pTzh_hK)~jKjC&JfdXkP$b zUHe-(bH(Rhuh-As7OfM$C3kU?R;$I$;uYQN0|gf88eN{mE3#Ed*VcOD535|k^_%&& zzFk|kMI-c5tVGGN`Ed(^POdAtELj>=dn4HM{Io0c#oKrP+FSH6!t~ZkNS)Xfnpj-6 zylWoQ(bDU&+pASqJlh(xApG6F7x@cj!vg2?RvTF3D0>kEtW3J**@q)0V)E5 zT!-(S`;xh3_xdTz?JiBf#j$1kcKg({?(56G{ks~~@366AYU0j$+p-p{&^=w!mlUN{ z8?bn)h!wL@G>6FV^7X}{OdJXU{EZzJD^{;woo4Ot((>xuG9S~sFBdId{J5uM#ss0S z3qkeLYWa)>E2dW7+E?)LsJMR^@2mYgqg+?$O5U&ie)ma^#afZz2@67-k4E=R;}#LE z(@?m4j)P0#OH;tgO_%+wZ>r7CFt7K%6_)Yil=ga+=(+Vj9=4ynHr*&HfA7~d71ozb z9#s8&I^Fkn=eH1%h*wLsTAOeEbXd6H*HXiM|Ns34rT3Y3i*xU7Jr$V#W9iasjf>(I zoyyx7^>XcEW09>!Cef!_ldonh4|@Oa&gH1ao%>o|s(+kZ$efbNcZ`4LPy=Bo+Q+M}ob-k^*e2&pc zcMg%+Z`J4jQ-Tby&M~eKf9 zf7kjNpEX(b;dy`kpX8Ie;o*^~Q&mdVlrLBz8V_o!7`=OBto8d ze4u^s&FS2ij33Ugez)`8{O9}b?Y8`916o{JdPn%~3ea^!5$~%hOMj0IW{^1ba(yT=Cvv=+Uk?-W_a2!emm>SvGw;(>4$HT^5<9H6kQfu zdNuUqPMyq^)yS#k*&L* z&#MlrX@JfHM=j*lTR7Q~{nZMUP3IL7c zPPTPe{Y3ZwmxsyQ0@uE&{&aHv=5J}dB3qZvl#`hz9c{Qx&LOjO(K+{1VoXP0{fh|# zEnf`q1&z+nV&)XtYLxx?ths-j%caAAgY@se*gx?l^M6n;Ygw_t%+etL=y_qE?k){^YBf75`qcfZ*K3y*Yz_(hFWU5q=fJ<5%2|26D|2H+ z4}1x^RJr0im+Mz|ZDCP8(fRFnug~9p@6g7^ZNaM@4EOSX*&DT*Z*z40HxZ3j`!39! zv-!MTb2FK&u=z1{b_ z<@ew1H|H@4SabZm|HlPsvoe>d{J6HCf6Jz@;-$Y|eLio$9W?&9MK`-!6jbPHfYL6~ zs_07Ljt$%a=AHlPb)rjI=_0vFFYnBM(|n|gy=$-S znTtQPA{VY;pI-H0CHtJri_B)WCNDFmK3XybBeiQV^V3o-b!B3!TZY?8XvKo zZ3ORra`@hL_|eqZmocf~ALQ$PJp6WuTffAAG1E-TXEP3WH6Q!Jd~2?wzF(&360fby zN58~a&P|W_^H=L_^SSUVr<|t#W9fSNv-p-&u)a>}{`RH0HP5Z@8}6O4bixwB#zi%k zjTLt9+BHdZv2E@1i{Dk|#x6J3@9y4K99mFzkX2me_HME1joX6XIt1>n)5wShZGc#p zHFeFgdAhc&UBAwNM)`NY+2sA{Be(vZ3tPVC7hbeoWA;V%UyHEcj3@uDr&k-l)(p6^ z)m^r9itg?=n@-+m+;S*PpI=N|mVd#4++8o1ZN9kJefGZD%TgCKCfU~2dHuDH*ETAh z_XyOXp5wUcvwQXJvYjEezMrZ=*+T;qd&u)1Y660%XWhKvvwvOjcVB3EvxlwgW;^>$ z!TrDA?LPU{%&|EPL`O{y=``&4D zORqh#?ugrUv0nN8{{MB>pk%(%j<;hA?}{+Rtf`^D?nkmJT1oq+t7}b}nK~_Um7GQz zvuNuqE*;T7D>JxV*Y3M=j@9aRpuXx?^?*5bzg|uj4ZZd0^F?=g-)-sF%T1@QTt3fg zW&P~jZ5yx0*WVS3t9aPP()G&Qs{hy4xFx%uPK%!O!Mk{^f!D*0`;7lRV7K3}xZm#9 z7pLgLi{BmRYW_WAeE!DH=kv0oJ}%#R{ck|c{x8et?qloPwOsVdGS}LRKdX05H~D@m zEsSyHsl)vCIqLIkKBYKj?sN$J3fkY?+1>T7aBbPum{&))xQfSKF}t0!IVJ9^?4JkC z{M*jk{mv2EcJIt7)~;WYm&<*7J?*EP-zjju#J;xW%8oXE`FS?C%>^q3N>X8<*?Wl~>!NT30N}72evrXIp4U#Nw+>%5FU#+pI%BT%Yd{ z_%)05>c8Lb=hxU@{=UWhUWGEJu&LkQd1a9m9}colzAGoS(J8Pt#C}WbTAiw2=a!$> zR0;W@*8OPFhpg+d<#V5G@7}M@w*(ZIQ~&&S`n=HYRnEbn=h4%+MJ`uYJfB-`w)V)| zl^N>%tnIVD@0&H1TO{;)O!3*NEK|AmRBLgyi9XZ5a%Ng1Ti4fLKQ&GD)|Oq<)6s&B zZcgUaWn0uS?EB z%jU|q6*m{PFR=RD240*n!5&nd6&qb}Fq|97+Vx9rp{&rCrrtviYv1gCy>9oU`l?$_ z!HkpRvZ89gUY)FaLv)4akAMTqMEkiUx7t>(Wpe#mzjSKYCeU8rm%E+(w*EXK?5|>N znxQ&xpE9pXK#26JXl=RDx~+#UfmRfH-i^ArQdVtCl;6VS#Y{&_|Ihzx6Qv*`~B`@_JF7NRIVQu zo|{~JQg!+i9^uVN+3WX~eOk8fm0tLlfG3x{^?k!$e+_vht8uURyzR+v?oYi`(>2o8 zUw`FndqPwE&a1?Wk?C`%+Gt$5Ir-+N)$xC|Zrk*0 zroj!%w|`$gx9paRMaYGv=IbwSj?rQboU!5;Xy!^)Tk&$P%~!MU+UxfuftN{`m4Dop zdi3jctAHz6-gA8?Pm5ydS{0pNd)hgj?d99gC83Syo?h(Y-dnkD``xSJUp1pk%u-hx zw`tAgwgWFk3$O;AI(sw77}NpNW?Y+fO>EugGsgWd-&{Vew|h;P@6&~GOh;|bI4$D` z)nmxrjz9&ak{x%x&fGWm{k!UC8`pnXX~lF_L?bNy$Q7di#<_DBi!Q#osJLazQ)yPv zoTx%QXf4ic;i?t}CXOjW7tGdG%?#B4mAm<@*~!THRgFdSYrk#$e!qVI)Qg~2e5zlc2^!}3K4EK9;r_qhq2I= zE}Nxi)y=@v9PyxmnQz0x!|nb`1wWrozYUsmex(JnCJ~gpkZWfx0YUkhH+_VEe_yxz z{KV_~F0F6m7SQ0kv*Bj;g}>9yCcxGLT-#`6{qgDa_;;lmOT*OvJ=L$@X#e-6|K7OM zpaaPmS)`ONWHlyLSO0#QRu+(K0$vcjV(x)!(fPTr*KFxRt%s}fTARUzz9tukD3^tn@$;TbJ8^7X3lHHXOCYCN= z^VHDy8}2n-yL~}_@0Uroe?A=k2C4(I*Y8aO9SBzY@u>L8$f?|VI~J_n`|Z{?&>-QZ zf8p`9snzfIZr}U;UUleuaZ&IZ#}M}gSDB7(+qUiT-M=edy$Ik2FGyI-dgt##r-d78 z7XNnq9S_>(gmn6Th>C#V^BFhy&E(I2`7S)X#85uV3KZ(IH?1mDqwqfS?>J3=Cp7_9Pg@Sr|P%_G`f33U!7j3ZQnf6gggKyf*ca?KMtCh}Ly7BFFj!$e3P&2JRr z0q-{Z8MeF}JTgC(%jHPNx^?Sfwf@@7U%eM}ihFM+BPex(7B6LRHg-f@JR^64XW8ky zx1*B^c#@15nT{T;4c@U^cmJPHC(D+3>+jv-Yy0)e-ljbtk4cw3of>|NnLp*f>BZNn zH6SA#7C18LN@(aw3noh+@!SlyG*qnMyE0enp_8HOroCcQa9Ci$V50}#CJ2gIw#g1I zKFMkG(~tdGvb(-xcdz90iy*XXr(@7~z^p0PHr1CpUu#ibazE2WWQ5m8^@N6 z1r=*&=3WvAkODh?#j*p_H=i}T{pI=m`hU}}u9>onP0e9}g@OG#H*iQWFtV^sb#N(} z6&am&L3`)#7mlq49&FwVEIia^C4v(hPgG+VkI3y?x5`StceA}+VpIScA1#G$O0zng z**g(^ONt(oU}4Wp+3A@%<>mV~==EOqh-Kvvd40+^8RDHQu?s|*kM{NTEfoBiZ#;YN zy9JQtY5~5GooMUQTCbQEFftu&ln!DBr8)3kvlwR@A!S5o%eu(lt$wt zg;_amU0`_))+ob(D=yo%Zk2uXZaLp6&(sT`fXn8HxqtKK$!}LJ=Xf(6mGR?>1=R-N z^!iClK+wuQ@VUmar+nq{+Ux%=jsGjI5b$QdEMzUQ!UZ0twNe_V_AOrg_*_TCUdd;T zcHl7aQTlN8!b0civZh&A9`AVK-{4?aU+mNmHWbuUi{W-TVqrYHDoOqS(|5(scO5QS z2RbnKZ_;BW$V!Y^ZlDFy&^>0EYAtCN{*E^Cg0?;vwrdx51{Js_vaa}i>P z321gL-}ra#GJp3SzfV@aQm&QU!O-~V0PjjjW^T0Mb~(}*8ylN`bydG$;j)>s6IT9L zD8K&x>fSBexAU8C-g_p;+E_a1JKqHbkS`CUFukk${o~_f{THuZUCJu*66O%lFgvJn z(G?P?0uBoPOoAI{&AdDH-#{~p_Xow$!D2DBek(KtHqfoUKtF+B-XQ2O@j)vHVA zgiSRoxkN5AzT0q^?{dGidEVcSfCD>HTy&niE1m`3^`IsI-d)R+*J!Z${Fyhg#?_`u z^Q4SYPGrn2j@A%~le%Ma*1U@8=(E|zRq!L)O+plu(#$sV8_%sca(8-e#`26kubAGx zeY<_uO{>uLGq|APuOJTF+Ohf^XejV%#7gJ_;)9?PNFr^fAvj@xl0c|fN5}OuIrloR z_8D(}dAGJ^>g&?l9Nj3z#Up8yauqZH_5I2!PP}_Ye02o` z!3)G3K?}spzb<(3M+M|i_Zc@kH-M}KXCbbP)#sfSe(+oC=Dr|8I});e+F=1`lBBnA zc_=LS7p!DedciZ1{b642^Z1E=P{0Z7Y8EsPyjN_kUOQbQft~xOj16h)a*@=7l@o zs>nj^ZS-QbvbMIi*82Ow!Onewg@Elka82QW()0sIy-BNsi%ddVdYV=Bg5C44ip*U; zxmG9xyxA{%-=u~9a~~asIWxR*0Y>YZQJDC{V&Q{V_=cSD_ros`b^Gir-dKf z_F6eEh|tf39#62qj)|q~YvD3chz$Y`3!buo_9;|r8*gC0TiZHKT7&tOSVnes^vS#a z#+$FglixFefDPBLUM&sSTkX9dBEJy2?RUX8CYG+PUzdVcFM}Ey3zo7fIW;b7a-Hgb zr}o%1IfG_z7Aq^OZQUJr&*u1gfTK^qL18-6{8=~W-}!sTY2k+m<*FIS9SYlJ%VyNw z*-`k~{YZgvw2_inQF-n3tK|{ry7P|oI?d)#xbTW8F>U_Lo4eD#Wx3dxrkt2?F|90m zVL-*otL*o)9xDgrY?Vy4frphWtI{Ji<7oTKPYlZISM|j%Sjh(3&suZ)%r!atx<6g- z_O;ABo1eCM_RW&CWA{y^EX+-M!PQ{~%fIxr{};XE-u-KuX)7i8ok!B>$ug&fA3~Ky zk@r&>Z&vIrS(pn7$wp430(O_s1%R^Rda;?8#_bD_xW13%^yg1~uReCX63KMb(c~r%sEY!wE>~=GaCyNq>&;Ia{awEg zUCnF%1#>#WR@WQ~7ebf> z7wZ4If3~Dbto;9qm28gb&`N=Uk!6+Eg%zNEL2m`h?YFLKUI^KOUd07!$?WvBm0q&; z6bpxlcJZTNm|q!~Y&l$xh^B4Jebygd{Jow5vJVKH5-M1w1VOuKczQePXWx9&96k?x z{f@5U2FLXD=Vx^dCy7b1G$wKTsyRc;RYn#mMFGL~**86BZu?&L{l4iUedr!Fkbx8U z8{Jqydrfxl`tABtV1YQ;5zGP-H*emYSzB1i5L%eI71Zlf^@Q!gY&f7G+R@Q__RX&| zAI=wlpU)S>*VYR%3mlONX}ugGw{PEGZ)&{l12?2}JM;c63ypeY4f)@%Np-KeUy!xgi3uNb$k~hmXa^v(>~IO2FIO z5*(n->r{uS1%;uYq;kqL73^}*CWQwg9Uc5fXclABr10TXb0At1D|L*nAqt6RN8wU=&U*()B z_yyW?lkC;K^H-__k>uExJ1{dH1$E61t~|Q}bn44B;ezkc+^vVI)|zj6CM(#O^!c2u zEoe3ztf@#D+B1{=)w=t?k-6*=$R;6h7?g^3bO@h$bHZn_`_A8;TVJ_EN8kSVt}bLP z5oee%vasn02x`x~`FCcQ)!n_x2SkJ6x2!fe1ar9@Iha-!wd^x!KjKy?A81D##pK{t zr^JZb=%rhWm{_{*$b+2(CYZ%KIx^3`+3e$be#h@e%SQX-@3Kj+vzN!L((X%4Mmt8L3^?TM*$6tu=~(aS|L6W`uiY{!cjHkp&@PP#mmAXgdnSJQyKHveE4M#a*Vm+jpK_s64d&~o6=(yXw!%2ewY3z~n0JTJSQn+`gOnpf89id&z|%$Vx8Td&_*Iz8@I z?D=iC^RlD!b}rrf?N)Z#gT|}2W}9Q?r>^+@VDI;Po89GWue{sy*{}NR)$rdY`}WjZ z?3MrbZTtS4uHql_GVVRk7WmTdV3-l-l&3Bk|_N9vfO?vt60Q>oM`#_KO18~I}_gT z{T@~=5xUx6@zKomd6~{_84F)re+pd~zT~$#hlslBdvA_a4Iht5=WhXRnmE?K{-oc& z(B1M{Ydah^25H)bt1~hkwQ>x<#nYIyG^hq~z3hVU#*Pa&EiU`~d2sukUHE$OkMNR- zp>di>N5|5$Zze7}`1aYm`ZeDg*rhbq?RvEeGzX%h{a8cM z*oXK1teaf#{(1e+p8MxiBTE-(lMv|eW6v2Y-s<>Uzcs0T*edQ-20D;>Ywh>D*Z2MZ z`(FLv7UcgYv!>n)Aq-MX6Z|Na~AA)y>9ocu(GhlSF{6kuBYcV z@BaVq_v~#`LocrKU;8iTo6~|DZ;Wpg{RJ(9nj(ajjn6O%7S5abcB16rWAEycjr%+I zA*Hc+1*I_a&DPyn7tDUX-cfy(Z^7&Q1r}#bF5kGBK0o*Vk7MR<{(isje@@UR{Nz`Y zthMug-#lNJmc4Fgnvj}S%gN}#7dwBwTCJ|F>u(cieg4}1Ekz47&Vu&v{QJ;uKf@_) zZOGNGt7-ZyU1#l@b+6Ym9i8K=i8j`KR!2avdiKqH<7A6Fcl|S;8D1=B0T-^I!GT6j zmm{fZW{LmLu0QRbwk>ySoZajE1v_3Yn|-U#`rV8#UWr~ySMx5PSCw^H>i~Fz)#v&D zeb|WNHhd=GGW>8x7%+s^V=kV_nlqOociR%MCHl%?H~6TPdX2p zi`n+?*K75!FV^h49#^e9)~jKg zA&2A5i~4jczxH+Xwx7>tt4G(KGrOJR`Qg`tX8v1SugA?^^CBlZdvor#o5#9d%VjL6 zxL5gH^=+?&;h}Z=|NVNmFLakvVDy!ByWj2FcEO2T^=roKTjuv`F8}-3Uw;F%Pw@ZW z_x0-1`SHJNo=kMVRdiZ69W*WTc1p0?|LpTV#%EJrui5OkX;R@i%ja8GE}NzGvHH!% z<8R*A|KANNQ-|zeV=Hqes_aCFq zUafk)Hl164Pl3?1`hP#4pZxP;m-V|Hi;ure_h#*qYHqr$xqQy0bvqt)#ce+p`k?OJ z&gZveiqA~U30?i$BKlYMq;GHAr1Lg(>+icE7GG2N`yqdQ255(k(fshZ%BKhZxgNb* zDFUho)_}HIAZ@|Ca0WVBB6%1zT9PIwTnle3fGY!^+_c4KebTkWrthC|bK<*yPaL9N z<}X-rwe#~CV}GGbfA0VPd*5N#h1#{}qOX`O=GNPh(EoMsmrLGdzg{j^XAIny!h95T zurKGLu*lS@lfK$~I-v|Ye7dx~Yb%F{Xve#)*W*r>ZP1dh{SqjolUX`%n}O}mCzE}n zk3G{22)VIZbjjg%{`VK-53Z|;uPwdgx~Y~|#Ps~E#X;Jk(<9SnrbLI#jg4HDaXMyh z^h$=v{pZA5`)~P`UyDqCTgbEhZrSX=|Gw{^|K$AEM`52s^WMxdzx#dZq0jTeG~Vs~ ze(z*g4C{$ap&RZ7M;0CxovfP>+GqDGqx#v*^j-E=0+VwVtT49!s?>1EdusN&oyYX{ z|M|4)&z~UAbu1hr#ip`pSjwg|Non#a$3D#1J-w>n%9+DUkV>aokq(Zf*~w>p?u&a* zjXwJ((YX4|nR&*Wzkli9{kpzd_b;c&)*^51^&wlN-d&5n+S(|}>cGXdl`H$sj>2VE zL!wewgl0ymE-xrJ%xmrwwkq@JA|}n&lFXBAUANYSB;H*SxOkFi=PG~gbKCA!Wk=_1 zJUYo6R7u^8|F_8J+N1^kZ&$teW6x^MZ~v#@yTsSuQBSAcy|ncX*U=nWK-8-@4=f zC`Ed%T2z=V`UEtD4%%`!x%#EM{jb1F@vKd+;y-E~pU33d`q(Nu#8sPb>!%IN7lm%( z+CCq;J$O~xq;I~ft-$-$p3ljuD=k~fbS!J`MavEK%mt-WLCt?2s|&9|B?HDE}dCc_T4jn=TZ12f({86I9-k`N?XQ~vNMbSA82B7+k-dW7sICB z+7#q^Flgsnb?vp`t7SyDWlCRp+-IG)Nwi><`@#iw%O@psuEUsAtBzj0C1 z-EZH%J&8J7Ds?XN>YO)QLf6#!r$^NNT>3A{B-3+!_=lC1Kfdq(fA=qAYe!UW#Gbp^ z+t~i@bUL;rXXn#t%ibo0-nV={XEMv)qpppc!+5nO1W(v?zwWo{!qli!a~J+N9slpr zw1^~5&^h{(A8ZP_Xgf2na8>98&{651HfGIOQX56fblehF)2al7X} zU9su3-tI3_6<@EN&6%<2tx0KItiS7XrDcu_4ymkF3O>o!^|VQP?&88!P zd9}>f>db%}mv7c(?yuq!DgAd(uKLYHmPD=>TdtglcMi?z!7t$WPxmP`h< z8g1i@zTYWUzrFXB)Y=PIwT^9>!Y#7d>}c`Sxc$%PRjWM@%Q_=fEhP|sSmo{PbNjCB zJ>PZY-n*`8KZ-#isEl5b>j?;|zmS=8#-gm``)RabtYQ*0Ts-q_b{hMPn{SxcRxW5< zw2ebH!(rjo#3#}|ZlaXkpx z;`&_J-)7<;E9qIGMgCc=^TI6B!o%Kex$L*?q}ptiboOP5+5V-P62TTRGIH5|SErx1 z`>nInUnFW;bno)ejV~n^+-%sjyzH`WX!rR#9dV5;@6g@>ye?CkEBb3S zms9Dyh38CtyiI*B?aREn@*Lk%9xg^u_!wgepW8ETPMuje=h?e-mFU@xdeEt&Le7t-0#;1_mRebVxHjy(|h$ifvdYIq- z4(Ljs-H-dMCoOPOmDb3*I=g8}Q0h6$=Wq78eSKlGGe;7%4m$hRsV$R4i(anU;x>Qb zj9A50Z*8{z+OT}lg}{R|y55+wYfWYAS`}aX=gVb(|7$K%Y_ImXmbU?Y8Lr?ejp(-L^gMv-Z1| zx6>hTD(IG#Z;$2wTUc@jeg5}y`TVl&OFv6)zhC!zQhgZjMXYKTUA5Z~C>*JrHs=)k@&2D`m{S0C8o`^=&1 z+|=lzld6-eLgMpXqn)yDUwzZHI%h$|t3OA!oCB>Utoy$EzUu$&F$-2Ocs*zR7E*QV zQ^}JP6FqlC>Rs`9eJ5-6+BJ(8FMh*y^xqnWx38xyx4Q5D{Lj~|8LbP?1%;es>)Q15 z$z*@Em41!4(q`vQTVr?i=j`+L|No?%UHaWI{#T&Pu~$*7yh8PngA1GvZ22Olp~Vn-V@+ts z^q3;gOYBqMd|SJeX+NkayDYYWYnNuTQ-gzH`epQW$#dd()UquL_<7LPc7-}}BV9oAa$Uv=Jl z!HNwY(H*PeU&ZfSB$~~2OKPuE;k?Rc8#kY~vwpd1+kc&{RU5Wj>Xf%FS{FKDnc0?I zprs9dVcW#=U1zNe(Y@!lW&5`3U&XDZ-1>Vq=x)DLwC$Kw_6^VqtC}zWe!n+wJ*V|@ z=JcuS0zx!)f$kSDOW(`dm8HE~DO5XAbRDRfS$0CPU1k5h{QZA-ooDeq9>OckboAdn zN8ei6#-wx0K%HsiMcH8j9UbgveWJscruvoFFBd&_Oaj({U}X8Ee&N1DVEWycHRe-= z{VWd3)%|$b#b$oiB!`hSiIv)?3vFYA1iq7G=F1wKC%599vL> z^v}oR<@H-8IT-HVx+ZLYmgt&u3qhB{%>J|MO4p6783Gz%pZ&6C?E-a4L(wwHmL><6 z9)r!C{%LAcO7HCbv-<2>g97-uY8(m|1RDRbbww4H-O609ntoN2R@w9iL>FegzN)!w#-hB&MNC__Z8NKWyY>2;1I+4opN44*Pmim5nNqr_?(X#< z&jl;2w%%PC#*=kAfVJZNzTbJ}!S{cjE8pd8)tV5sGUDu_-~Q2ce)fOAlwEWczj^=P zx9v~%1$J1S*4g}J*VA=jMOp^eOlR%BoxgwY?&_&5UB3!;wXS6P0a|iYy>!X8IaklS z6s~OJY)mqJzi8RYZU@8kU{|zJ7Htk6oBe5r{#gs3Vti+p_+JrxP!wpCw1I&sRP06m zf)#l+zi;373sd*Ad^*K*#flA}gVHr`I=5Qfx)hi=>5v1fM!)^Pitp+DROKYMAu0hyraAN6iS+uCmRlyQ>2J%9&<`^lQ78wt=>F zq{P=Qy)30R#f#ZB*Lu~3i0^0Y|Nr#2emW&s?PdG3l7p<`K1`9V`I!kZIV%bRZZF^W z`P}u{J9T!wSaecsIq2FUm0IuhS1sGb<0>Aeq{n1kU9@ff)!LQCitqRRes}U&&a;{6 z+ZOlR?b@BcYr1IHGF9G`{JmepE=x|co%+81|L?LF3)|IlZNFX#291<}2DjcWXy$wM zaaQAu6+ui#wf=i=wp^tgaOS2B+Rm#BTug!+*Uh{;N%Hj3cl)xndwLDQBU)hMiGPAZ z<07}k3SNqLwp{j`&EvOvjmQa&Ex-PJKA#IZDgkus8|TYQ=QF0>04-AFYzup%X%Sdp zoOfr(lk=t5<~Dh;MjUSE-`;C>>%_8IS*xCW7dfC&P=tsyHi?r=kNbp_DQg{!&_%#%i4Xv zUY&fOx}DSay!4g7pbI3`zAt*`Ec!JzS8vM&r;~qG*?YZRA1*oMoolg|26 zqI#=WE}iDJ&Fk+m^L-Ovr~71c%@SF0&hq)3lmEk4Olga;+U{SM`O@1WoGVECiO8Wo*w9D+8Qbr7%D&x9 zS7)5%wd-yAFGk6%rSr}m_puLNRrfXCF|Mrp+`cc7tA)3E1%6&TwL@yR)4nYkQy12~ z2Vbvnd)2491NV2aa7SEBVD&c9(rRK&a-P`HC7SN0lfd{mUdd2Pu}gK9OJ}T(%h5Kr z5DQ~B)-@{xHcAH^N$9*Qp|f#;lGeSi_4}Bg?|Q$}xIFFM%uVO*-dD-VC7!#Nz5eUE z;`ce<^Ve@(SAF+&-q+tId-@a41)SMnc0PVZio%am;roiZ-zTum*g550?E9+gtmn^0 z&U|@&PNcQP5B>k2^v!R7H)MW4r_v%m^PT;y(r2Z!55B8;p0nEXnrcAK{rkJKFDNxW zl2XurF2r>7X@=!A21X_p4gm#+gp*8LuW>6svYI~s!|b3_eBO398^IC{AQjUTHwZN* z#pyAxh&g1OzCLxsJP;KeHH%qsdsVZY0x=7mwooO&YJN&#bT=Q)1tI%SJ#zo z{r71WXut69>T9*njOWiRw>&+SON4#%m!kgNG3QDbZrUo@)m&V3>~`(@>ihfLzbSv8 zaYo?Hm0bu36_YqQcZ?ogOK~0RJo@cXJXS}ogY@BZNtmoJtzrTN;+rMXD zuzYS=R6YO6R{FFTxImz<2f%Aty|9I0(`R32RzaNx1>+jAC*BhYz3m*(HdH5Js*>)f8r&M6XYwutHIG~SM90tyb`@ZF^pko`Bn z<<9Pl^P{H*?YlJf-%ECA#56cmvxI2{6<)AK9LaU3~n0ok=BE_=3qJS;vX=-&=D zA$^32+A0bGF^VVLOm~_*&9DD`du`aCBkFb=_y5Uw=RV{Ayx-nhEF1z6ij0kqxE$Ao zm@*xmIw^!7=C=cBOk1znDm}V$jQ@VLSEYd(lDWDH0Wp~uJKSww1#Y|^SN-+--QS<4 z@4s`u`op4by{$QsW)%S!!K2!c5tLV^hz>9NAklu^0RlC=Q)V`98by#4rYoQb<3xUFyMNDAD z)_L-R+w*E_z6PCgJph;BShM87l`B_1duyJLI%#!PwD5HFJ)>_olKXeuJewzWefPbu zNylZ&dqCOcBdG7b@6*)vMKqF@9y5KJ7>={P7(F|Ghb&wP1^v9mvomS`%UzITwgln`npKwm=36%LxK`ZT3Q<4 zs;&Eqm8@1zX5kQ+!O$4>nzOjLxPNxTx;|fV)~;mZ5GgOH^%C}tQLq0hKicAUIzHUX zvgS|KqNC7AVPwh`n=y0d&BfQM@@C&&kqZ*uP|_H5>f+wG_pRRchbOKEC4=0=ufour zq872>+AeRGn(dAsP6nNdRiEKp^HtyG4M>d;`y65ZlJ*0FoV{tK5*vDnKE!;ee3lt0daMQ28PB!)_2C%*1s19Y8U=H#l;~Kek~Xr znBd~YnQ7~_{aqc8rS1Q<2c5ESRsxHH!{-uHaB#5u)S$RMm%gs*2YLRQ&x0>FZ$_RD zUoZLnlD4tw@ zqy_Q~Sbj!Cj4dWKsZGWuF1DeVr z${V9zUsqO=f5UTXs+Z>e*zawbApd|J=b+BIY}v9h{iS=9mC9bHu$Oi*Ffz#suh?o= zmY4T#t?cWt<-3#v-drho#RIln!C}E%rmfd@c2s06F0Gy#WOx1CwJcGnRD**!CdKuvchaqH?LSxIz2L^og;xY&ky_1~wf1~($8*!D zA3uUl&AxRzNmm&b5ihz9eEISvR_W1)j*1kQoEKjEzce#2GCkG_@cMb(P6cEtpJE)MU1a(Tn9oDS1>~2SPOZ}}#)qJY?_@4e-cnC_!H#+lKrX2Uy+nMq| zDx0lm`^v5NwoKVn>=I+Dv`u>cybMi7CYEDv3*3$z75q54<44}wuT8wtW;JHtcfS2v zbzf?BTt-H`!-5yxFlQ=kc*V5!+8ukt?}0K?`lklH+xkkp4C)kUG=SrMA}dRmvhXUn zP=Y4Y)@$+ZNBE}8|E^y;C0qZg&2dScpCcP`3a`|Sct;{i)GBqZ-SCDC>9EZS8Uy9r!?oWAw zotSWmg{5n<*`r_HP$x#HH%7e{6#V$=*wguOUX>rMOpm9+6FuC8(=-$UVw9#Fh6}g} zuh{xdSTNl*%W~zEq7NBct!6{r4i7PhgsrSBU8{B1)Irk$L*qV&wO^$KKW=6_KHW=m z_hUtbTBvI>wl_Fzy!&`@Da=oPoL8du3k&X!ySK7;>6EutR~_eK=qd?tSa9Q@<5g9t zJr2>VS!>d^h3|$t9L`W;>DtE%wT6)?TP+~lR!yldFXqF9pi`aM z*BHbRWd+Qc5!)IZ3U8+PfnpjQus6aQqh23Yek9xbIX=ki=kMIL=PThx!Wpae6#`;v zlf&WKcEl{Wwkz7DW+&sP#-LMw8Q>)xENVHnggY#_ac|>ABbb-eS+mwocRNxxz5E^f z(kapDhmFe2E2W_}Hx>%6*g8*Gu-fd_@8wIU%&8rEE!}n zF&*`kF8r$s_2rt)4r{-*bX52(mi#?6$ZprUvqhCqS0MB@va+yr?URC9#K@Gb6_8yU z<+5i@;l<@sg5q{A>n$+E)TGMIAu`?PsRK-=ajL+It@o^z?5?bMeRk=T!(of^O5s5O zXRO)N;86I~_bRNoQ`jKd81=eT>CvHMiu3iC-rR=q z{Bu}Fd#xOhUF+jA=faL(pjJ}k{A)Y3;8wsG2d;s_TpsR%Y`uW&yT(fAGGl)1T{?w* zeWi2E1Vju$btMEcHa=S90sGb<<+a?@X5j|H8IFI_wT7ravd^Mgb=W8U??q1A6_me|9D@)g0*(h0<%eTZYxK@?u60`i`C0TDx@$LJcM#0l1%%&MK zjE#?;^lY66E3%{11F~!DUG8kZxB*n7Ufb1{B>)dKC?lbXm8DDA=Ep8r(={T$G3qt9 z(xaKZWiqi|l@F`7pZ*FDSa>P+PeUQ#OD;S(Bg`A4UYEFhk>af=*L-@QG#0biD)>hV>6B;CvQXTg-BX6%|cL~7aLM%+>j$o^aD60`T>s#tH$``O{zTiei+ zp21G0G!4IZH(pypeYilBY3sFqw zD!yXtJ~gFzS29j;E}e3GZS`RjLRvt!$cYK&=WKb>6y)`8?Fx9Q181z++#uQY+J-^T zA7+?9Ah;xv+_WQ2^Xc;%cs#=y*TNkZ?AZ5j9X#|4b~>!x%9rRfHRv8|2Rv9{jD%WN zmaf;Y-@k$faXpX7eR09>*$pz`UY1N4slVaCFD?#|>(}pJg-6vt35~RhAFtRtYDH?T z;SPZ@bOi)7wj~IdG{UT0u)qJ&S>;FB&SK_zOKaNU{cMi*ie+=pVwUK?EY?85nxqa+6Rvl7$e~=D@#{){<|)C zsC2V;fxCh2w!5c%Vk&@(!x`D2MpbojxC$)jBWwcZ^mW8N-oQUC=p8qac38U~*{=NP zRIhWn_R<)Jewce;G2xJq3v$@&w^?v|y+u}R-KVeg?z+XFmrJLJ>whe1#}Wh+8XO8+ z6+)e125r~`>OD9ceh-qF(my@uTjb5vD|#_)Gh}Rhl;mUw@5e-lH%7hoReqE$xG{Dy zWA*ePyWM)cMS-xi1&`SlE)Eg(k_V-*Hn;&FC^5JlVV2IVtANS*3^JwJ2?cfSk7%GT_O}VQuO4)6;GD zCOJRW_tunO2=_89UKBQ%F)T)!nuDl&F_cCU-n5L-^ z5TiEZFg&$h3tMpQ*N=`0=f|t=cXUKg3HrBTn{J^g%v5+_q<{jW_|8|*$OS0c49u9e zUV9zC@6F4;UN`v&ugdQ?N}QbG#=>k))=>zMd3^A43Dm#?OE|7X`FD2AbgRD~;&pOn zH=@4}w@QnfLnK@>;w#7+uzweGUx|{J7R=AQ@@4Vq>9&7=zSz1NRzw+0WBM7hzAyE( zDb!^W){KphZt%`zgH_RL{tK?{`uz0tytus=e>;0?o}EzmDdF1A`OFf53$!MoLipYE*rM*U8#t=T~p7OFG=fEC1$e z)wkL6I@UM%fmI2rBnX8uetW}wt|7N@#hmA z6*X&1@8??F)r5HPz$@-QR!V+4OJ%OV{Mq1Rh~wV4_w~oN2}?n%t2L{C?o#Q3tp#XsD2)9Xix}>*WZHUdvC^ZXy~TBsg1e^& z?b|!2N@llB*!1&3dy*C>_iHTOvpHBc z*D-zB^5x(AJNUOP(3}NM$1EIc<{X%wmY!Z7zw^}{iCK8<*XoWNb-ns;i=c^6i~CAc z|NPmrf7h?Bk%R@KtdM|4S%yJrE7+w13I?7`Td&2r_?Z9tb1LXmu-eH4)8AP;FJHY9 zY7UO1i1Nn&^XAUo`*YWJCuT@iXxu0$pwV|WAX5XZ0@RHRUU2Q#fsTs&$LaQ69oEx= z_PJ-3G`@WKa~);xHSfyI=46O{qfwK*=A55y%iSF*p~0J1>QJH=!7+Y z568!PRelokFTWAS0vc)U=s4~Gt>JfRZb&@bwtH^TDb4vW(%ygRf_Nt)ldJ@N) z_ZHMxT9fnmSa1G}*|V#U2lbVo`Nhp4a=o4h(cVsQX^eWkKknazjDXALMc0xvpB@ri zJ@3Y4)-GY|FIHejFULz0$)L&8l^+r-=Odx*@GU~f(Ss4`bSL^tfkHizTPM>l7!{asSc z@)gvqnk(|-aM|tL?Z;25&#$RduQq=+S4gApCqtGU*a->_4F}{|v(}dHu6cP`?R4VO zDdxpd^BNoqE93eg?dZp97p^)KW@Wuv5p=5ic&^Qs=^&?=an7-`w*Ji^hnaI&K#}Eg z#8@gcMnsT*!^l;H87RJU$LK9@1!1>}>#Dea|M^~;~0nc1~ z+>ikt7m88)aPH>Ko9+kD2QC_>X@P~{;WeO%A_aqk zOtCy7%aM3JwsMuS5FcVpD+Y^6t@&hmQ} zPWi+V)6LBxB5q#90`Yr#V;@`B{>~2V-p@NStU+QiY5`t1_g{ZqJHx%2g{AAQB)2X! z7jp@tQwP^w}rHW%Yy@HOtIV|>1k=-R&1S@Jp)wE zFOX(hI%RS7-h14;FV4>U;_*ohM&*{IwO;*jC@O1wL+-0E+5b zrAN%pp?{W5*>srWYT3V##-!)%5$qtxBphP3lF+bQd+paIPWiCA>JAMK=`3EF>0he$ zR+lexmVMFTu#vfW1vjLoU|?j57C*tBCiwBv!o_y6UOSC<|72z9+9wH?UT_=a*X7HW z&5QS{4BPJR`yQ03Et!^1DX!XkPgZUD@we|+I&A!#yrdPZ9+W7+QC#f8qd&bUPV;G* zc0ld(RRS7ic^;*ZVm?Rh!d8dEsw%5^xzp26fJew)G&pEJZHimJJ(_*jrI~shBG$%J ztRN{2G-Ex(VeMCA!Hw516xw%mSWgMki+S_M^HYf7@95tzBW#7UcU%23z zx!9pF$Sukq6nG#Hu>1l|N4Os8bC%iP-?4j2kX`>vBgV!>`G>qXWGWss79L<^KVrJB zu(z%>eO_gnO!=Kc?ZB;8pu~~j!r1tzqrnf=rO1= z$#VJg)5f+EG=KN_{^5(&3IQ^A19$~Ni=^-Td1ij!MKR#ZT2S?hoIV>XSy{Ste zM4Kthg7+defEw@RE=LYO-+4=L>6Fg-PO1Shv)?CPU#}GK<`>6ndC*jmLV(QkiF&!=I zJ-%a`+X9Pq%FFMvaDZDlHg1*Q@0RyJw|Q>)_t?GxhLlX|_7 z_4_@=e;%;!tljO@a6pKarOUe|YV~23u58t}R`8%$E3#s%ote@-Bmc+yiYr(AxD{qr zGgVqOASU$z=gPPR7K+SkXR~nhENyTov}yBQU?KN-%hoWv9|!sS?u7p;p8tE!-X+KD zExtl~pnG3jW$Ee`vIIvWC^{CrX4-m<-R+2`VBylrU%&pHZa%ts*SiaE?(bmfy1wh$ z)<^Tc?@7P&u55d^^qmK68z1*sA6q&-P6~7a)??}WKiq1b8P9)dzvt&+`F{rQKm0v= z|Bvq*(D9hH-&*x+6yrWLiR)P0;jcX5{3x)$>Pp)D|38k~_wE0^dw+Mg{=SOt&t95^ z?bde`-xW>YedKIj+2$LW%jYWH4?3Ux|9SmC^>g9p8NjJ z{pBD0|9|m+%oV=l(6XxYme1$>IN@$5BpzFGvFYo#s=a6JsxQyJCnEpj0K1rtU2nhb zw;N~e{8yVuPr1JT-`Br(dP{Te>@eiBda+=w+^jUvqKC)Rw(m)e`M&FVFKER{;Yro$ zM_R?>BtVOE9>3XqzHfc)+wAVRck=%~$|rBXTXwtT&T;#HA1kh|j=TE*{q6hzzJac{ zJKCF9viQfl^8M2KbsuYlr;G1x0ksGX!kCzjew0}E=Wg}?-}m?5zwt+Q?*8RdJ_=d< zda?NTcd2iaUq+4v?I?3BRwO|V4zWpD2^JUy+3SI11+=^Pc_1Mbg z^Q7Ka9Jl_}_##PFUhm)A`~Pz9JW(^h`D!6(SIpzw{OahM7tZptzV9$&?+V}l>*`}q z|Ei!jJD<<%F8soL_Hm!JUfsXX^Yts<)HEist+2E!-}6}3eD3y$1$#1*WkB3W@R|9&cKm#(zHSwwvUTK}LY`0=}7;Sr{_Co%(e?tk=h z>6H9+@!sM8Pv!E782g;L*>F}M!*Jr9g%y839zQQFbnlVa_Kx~r%j>&%rOk3yJu~0` z)7R$fO8?&fzxV&Y{blo>ymQBXZ?^xv`K(^~Db3|a3Jj*R-}!ZIdtXgM$x-dN!i~>z zfBl(#|4&-V2h;mL#}7J|?|mKn_`_lT_^`e2Yu{HtzW?v-{cRcU(ac9>CM);b7_F;3 zo2_?K@9>f7d*7sfX4@TkvS7iFlm2x^>&mvSKAM|XJp1fA#ef*2oy?p962**-k7n>M zc-G(B;82*;=H+}_T_GUn<_z>=>mSqBYt`;Y0tF}jw~jyg)!2XkpZVLT2I)oq4_lwZ zeM?5;S<&L1Z(h`>rgJ|NTKw~T!O4J+mFKJP?P(0w+ihuUyZ5H%;e(g1TnVxH^N|1k zn;CPa&e$ycdDq!V-gVE;9A6#xb=Bwk#(y8$Pc#(mo8&zeTujZ`Y7vuHx;pyn?+ZNk zvycDz_kI8UoNp|njSza~xJvt)nG+dcI^wk3Uk9sl2|&+65R6RH{QrwbO$NH&|l*r71>zh0m; zBhy<^0gX5_v;K-B%8wTE*JjkbufE^C-1cqe-<$t`{)d_ty5mP*F8lX`Zk3wN zW?UcswANYAwr=X`7*)`wO|j@N(bKYjn`Vb%KQ&o^4WwwwNG-@UKvuE!s;p00X+ z`t+j8Z!waI`(}RIdFT7;_`k2}rv9{k2g#&%P_~SKs;T7_;1mhJWRH`;#mGuMm)M zW^7D4&E~t{#wNy#GVqqxH;yY&#mkb;MVy)0%{}#wiT>fEJYxRY{;%D$J)@bA)|{Bs z7;4;enq3#PjQBTyWRd?N9^uc97a7@Q1itS$Zfn+jf?aE8{-Y0v_pQ-S*ZlW${{Nq8 z$N8t(o&6Q}Ws!Jp+1<6R;X6ap-@Uz|b938de>=;4KaT2uOILWtd}2=OO#X(p?=m%q zrSEMxYP3|+?-|dYhmqAk8{UC7gFfE>@AZDA^sr~lYibtE_^oT2e*f(D{PS6$9mw*< zXAJ-Tm1R%exVO1T=F`KB&vR_dzfZ5ZzWbgiDE@BNUzlOcT^xUkr*eJq+}AnJ{(N0u zZ#(bvoaZ?o4-{Dh$jq6-A94TtzVAnOKA(5{{&d6RY5NSyx7Y`sQ*&r=_|3x7HNDr< z*u?5#i}0hD{`FP${*N9SSzF)!Z@cgHy4~NJH~xt=i7E2Gzh{2NKAXwMQ&xOFlWteM zI=+cV{PXvke|GuPma=t$mYzM{aF}oQ_ob_NmV7EtE`Hkjv`*Q0Ma>gs`JE5EeSiKo zem~pp?@Ryuu+n?4QygY|Pk&!|zWUJ-VgEaiE;avNR&(?D&-3+lH7~v6Z@rn3yZ76I zNqfpqO;!JNbWN*Qobh|Z*#}=ov3GTUHEuoyx@BU&&%c69wyylj)6u`4HBaCC>0ZXh z_xFqT^Y^K5aARURYT6X{DwUFl`Avn*b(*}Todrg`NY$?3nf_`KLk zeoiab+c@jDqTbV8KN~)NU0?q+Na(UaC-M7Uw-fcc__xHf|$WIS* z*d|Lp;G13(Fpd92Tl0fCCnp-a&Pv_qU?{)uS@Y>YxeeR?@oxsL5KCZWYwWT3!L*X{+{`hy`JapoHn{| z^!R5`z`uQ`rgwdsv*vgC{@=0x%m2UI{r+BjrQ&zKbJNp``ky>|zh@~+mtUAO1_shf2_oo=$*|2X*L z%5uA_^CM>_Pv5M2+5SBTaAOoB5j^-fQ|-dV%g)DExLa{kZh~AIIvx z7oC6etoGr9sGpJR8YVveu{OVUb$-p|+4r{8^Ut^z`RVse#`4WP=gx0D|M%$Te>I6( z?-$p7nmqr@|3HqOg`lQxvl?ns*TI@qFP`n|gwxX<_vTqg?<`t-ie3KahSj;M)8C$9 zcWT~r_i^Y(`d@5A^13avLw+?+CDdTwNv>`wv1`+V^$ zo;aTXwU}nt-if)U^Lrxyf?01*u+OvXPTIHh>MYTv*K52Ro*CwHi?loM_tx9FWdEN# zPH)fKrTmZk_i6g~^fbOx)A#>*sypr5Ed79*W5)9YCBJEb7EQV|R^R`9FWvSbr~i!_ zt^X3vDF@8&pILi!*Aurn&9(PiR{l`CrgoX5-And&i$ln!VE|``nQ;;#-Xl+)bSx z`|EVwvq{T$ayhPJyBMRIpt^2u@>bAF-0;E^8~?ul|L?u-t@+o_S8tt@`g2wI-c|KE z(*l`~9&8o6?Q9skDc7?5v$OoKfdBLIE4H?noY{D$`s$xLyVsHDZ>`Zk`@Znr;paV9 zrbav~+h}iZr@rA76VuU9wz#~nOh=z~S*47INTDt0e4?k0H7O%Wn z*(#Sur1)9Owft{=h}msxBn7fWis9Wbn3ZTsT=NO{nL>*c`E&xyLR8&_}yl#T|Wg( zze{Dk%bs!i#xtHXN4-8LX3EXxzg1-BV)sRM%0mak;~!7@*L_L~{S6vEp9`-4x13c_ zi@);onfd;mM`xJF|2cU-)RMb!#}h9$Nt?9D74z2C&Sd*8Z+Y9`@KVP}uLU!{tJHrD zuivW4ksY_Z^537f zoqwux)<*5{kvXihHnSFYo_lze!)R+Ij&6hlC(j zP(}Agq2uJun0C3U7klE{W5kTpZ={}_9{6eLl!udyPkbsbpZ1e!>%otbYo2~uVri|P ze)IXI=a$azZe8D3_J97IhvpGXCg(v>@Gv&6c;O`dgtr?G^L+$G-2RxCMPC2%&fUMb zQzTQ*bJx~MjY-?g-|K_+X6f>smFziR_igiU#`tf=8+bDReTX^}aAxEAk0;-sPg{Z&^MZ(Dln z?77U~1vB_1i!0ZqW$OLjwe{RNakpnZ=Rg}XFJ~TZeDq=VU*%-*ip^v-g@8Rd2c`*0 znZ{?GlKik!<$H~4eg1Qw-!+CWFVFw?C9VC*`&EDL&fWjsC?I;plbplN{B{+Wk9%{9 z6wf>Z>Z5UAo4HypC1U4=H>cLtmKwL5u~jJM{Bba70k`b@#7w>Vs%JCPj};!5HGiL2 zSwHoAWb>x20=ZYRpVv((-+lY;uWGZr)83ihKl4)T-DWPFVQd2G1KS-Bp8xC0Kc5S0 zr{3`|{NNN)%z4`}`tzSP?ur)#!HG5qHPJFM<$`7d_9=wl%5jcn%Z*s+V0iz@xrV%j zlf28GPKr5e`*YUuXUv;w0=5N2u5>U=XMB>=paTkt!^d)Emv3Y5I=q=j{8jqrhqoR` zG(Or^r#;8L0X!YrKWFyr>dNq6*03?B+w+&UxGiSulC*ewI?9q;_xtwK8w|gH`hC*B zP6#yA@Vol^Q$xvyr)M@b^I2sCn@6kL2c8-(Jf$C;IjEpncMn|3H_mgvsg@ z?w)d0y(sK?g8chuaTl7mJ=?VMr_(g21^JmAipi3Mp`z7B@HvVC7c>hy9 z{b|pieM*i0c`E$ZRi9UrmhZfB^SRykov!Vr>-T=U_2}Wv4{zLzMZ{A}7#Nv^A*pG) zkW{abM%>?5;r-Y5JTv_(KD9_K@!YQ2`Tcu#-}}1m?}d%IjgLT$ffIHn@g=ve*=vDQ zM&y*BkZ1&7jX&ZatsAIkj@urV@RuXxw(yxqBBf3NTR zw)MpJd(W#Ce(X4Is|UJ%(f9as!<{8Jmrjq{b=G&!QQbU2P)E<)+W!8Z^O6jUr-1HW z*q?uSfzG!L2J$R&#b*p>#~Mx7lx3NJIQL}o?1yizO>@|NNOYY*e)a9!zxXBDpDN4$ z^w{&Rbba)eZqPkwcV5SRzjelD`jYH-Q&j?Drd(uX;qaN!;P7!1WBgkO(0TIRJawO1 zm(7{E{lZ@dP)BH{cKj=;(v=_f^jDwsj=OZ<+%P@;nXS{i-#b2^vpxg5rcR$)lQHRZ(zPh} zwB1j2^Ccy%p5AkBo|J$3xzF}Hi`BkA$eh-C=*+WC;mwafGu}$K|9vwzFS#w}=d^#J z>x!oOmOeSWy=HO!#sx;-bADahzR&di*7%QI;YWD$XGGV=g%yRR*xiXcBUE9lN8{R%(38E5KDnlFAn zZ@=Gu+F7N5JI~h77v>aru>usrjj?Y*^E9jWm;C(y@BM$bb2Fk>$duhk{42_N%gp&B zsLy|2BRr$x(&Tv}`~N)E-~Q6y<=*ByAN$JRTsMw*sn~o9)M72R`?j$^H|kVzL1WoPbzK;K2wR!qP(1xIjYn$g?%|Cbl&)NH+fjo=+`|QSw z#u{fo6zs^+4LFnBZ}aKIzfCJao0)%oFZ;ZA!?feM`~QCX``2=FeEQtdu=;1O7est2 znZ0kCU)Fw5^Eg=rwRz0YXvXm^Oq?CI7GR5MfXtrEdnQ&dmrQ>ML9dq$F&uF zW-YLOv%$Ik{t=$#Y^w4vY=7T8-?zU0@9Vz``p&CsZe00eldf6!b#?sRH@9{!iQJc1 z_f$K+^78rO$}4kDe&0O*Z_etsX99G-d!1tUI;@(0$M%II|E)L2Eu9V5o8E6;UQ;oR z-+;Hd0QQOfr{lTg2FF&vS<#;l|?DrgFgLdz4+Py{nCml~N&_8^w^;AK} z_oe!^nV0PxKhNEK-tPAo`5UI*+fEakF84*#gjBuFdjCp04?+Zu5}yZ)DD4)o`VXcg6E>^Y`2m(};T=b^TR) zQDy%m@3YUngMJ=5U!&T8vR-f3i$!Pc>K-u5OMtGS{%wDH*Ezf2Z+z#Q+e_c`FH~JO z_hG7D?>xsez3sEUmmYq)DtvF&ef#&#pPnu|XzR{4SJLI#r1d-Br|!F9_DAS;Z+^|= zf0O=L9xhmDtHs{-^)uu7 zlK1~SGdB-a4VbfG{{i*o;Ow>)OLn{F@G&cVcT`Hm6vv>=bE@C%{L5}2X~!#<^ptt6 z^qmK66OH-5->ZKAZ`0DBZ58`$?PC6Y%l}{2E!hDY6#UKK#-@6I)%oqmW;|-^0c2?GG*4g$X=N;=*?;X5e-z?>4)c!mj|L(loi4P{}IXq40 zTEA^Lyzl$o_4W5BdN-y`nD#w-_xpX-Pr4o7gN8e<1pD9p5_c)?LJNXTP3zj{md>SpR*54-m?~~cI+wJSMjY` z{!hZ?rEI@VpYa!I{xmrws&+{|zN`8}v;3X-Dznp|vA}<)lg=syM7KFD%>~%l*}m`6^Gv^Jt~e1vfVCUsBS}&LiKKierYuLR8w{GI=_XKzfIO?*LC@&bz;|4&u7?a{vxmHF=Y zoW!_)e&_4bDyDts{d_j>jCgC2XiLq>SJn4_$NgvB@T{=%{nk6#=C@}gCh9Gm_dSq% zYVkSC<@VJ^?^WBp_4dwM`2K_K?FT#dysx;}9cS{MOSsZ!pT#uC)X#r@ezQ7r!GQVH z+Plx6p3Yl7zwVbi0jLQ^;D#Zp|(E z%xH$>8eXrqaLerVk~4C1Z=UDbS+sK1!^o!Na@9HYZt&h!UKU!O(4`DymMM%DRj%rouuBE79C9WzeveRg=m+Nm`T6?Y1cpOyCD zJ15O2^7;AAX)$tZb{=_?lzaN7+)k0mb2DbF=MWJW{$k0xKKASfCq!>!hl7@%saJ(k;`!q`$av&B5^Zgzxq{mzU@%&iWp(QzT=b!c=|9r|<8B zcYg16ytg>DVTQKN6T_V@&#%3czNGrz-N@&3)u(?8Bc~O={rOKPeXk{V(f!)*z0!AI zxc&WL6rOq_^;t3JPygCi!O?doTc>m0dv5prWqec?c6*!mm)@K$ zTXOiY+_w!o{(W74f4?8wjL*9DvwkZ-EBa#j*tqxe4IZ81oi}58mBVvE{!)LN|Dp5c zdWVgv&bpZO-WJ;cpT#v7if$yff0aL@v-C;xI$?jC!prw8ef zL-twT*ZFBy&6WT<7t`!_CEL`W%PRHKw_RAX>lvrym8V~eXRdDjrhe*iapp!)8U9yw z!?v)ZXYqRRv-o#y3o{Bi!yYJkMNjNf-DBzd9XtV_A3hB7vMV_g$ynL@^Hfb^lHByf zB(~L-=enofnQizz_SP-AsZ2+Yiq!fw99Y81A)=mpME`aEz6n#jPv|dwvWoL;D9f~> zzGTpRIiAcd(e7Y)()iD*z4`A-mRo=QKJ{kh^SQq_Km7FeA811Guj1Sl(UX@f-6t^Ypdw5SrJf^_$|Jh$PlWTT7iT-N&`N6jBdvo7x zI<5DYy%IczwDZFJe_xjWHUCyDe`3zU4OTJDrCC{bpFXBH|<6PG+Th;-(qUQdwJGTw*A3FcH_1Wvk*VaumW|aG~fc@+1 zvkMGri|;SxQIG#T>-v}V8v{=6o&K)w`|jhqd4=8o=5_y6FzNlo`}dB$(e%#Kd)vgc znT{T9sX-31_etKz%|nacAMdYuqCA;%>2(&4EshQgZnOnvY;nZwBT7tn*vK1p3$ozv zSfIlKi@wEXRkK;U7E7;cWN0)M7SLEH`Sp^XND+XHjU~2mHPtd8&yKJ+n+#47g zPYP-*opbs1*LMb%wt4mbtX*&Am30woo;N%L4XUe`JA$i&4a-1lKho3E=7nX~-khbh z3%s0p%`6Aar#*4&%io9YZs?j0+J0>|QyA=Ka4)GO-eJLwzlPVGL1xUzY2;(;V!s-7 z-}2_-Ro_`S1YYzxto_=ubLYiq+1mb{-LYfRMDV(dkK!6j-`s!w)z0Rh?Mpv?mae;!-nQ`RScMHqpt^B!h#+`Q zA!Dh7JM+v;g)+?TQXfOUF$y8Oj!$9%JPvvf_i`t%E; z92A-f+DuHY@sMTb7jjV-Z%WBn>PmjJ%E(PztkO+is=kD*=A+?-aB-}Z4 zF4zZPGmgbLEUD3Aurx@hKOF$aL*z6`=O^_@}N;ze6Qqumva&FSgs{6CJpBYCUWzvKc*8LVy91Sda(`*c9dwlLl=#L94x`u_~;1FR`8bN11(%vqUM{Kn}6@}eDM!?tM-f* zyVuH$Of0wb2yE4EU})Sb23nq~uJkSD_3K@7;M51&7qIo(;pfku#cekW&psZ?XZd7; ztp3g?lm2F}l{C1w>{`|3pi}1r3$J$ESSP#o3KtU#M~N4%9ow%${M8p2GacoWNQp(t z^)aB;y2eUL`=8Ex4v8X%1REAFP5$>^tJM45v$sxy=FNP@#z$5C4X>fAvROC;G8Te1 zrgI8@oFQiBI~f#LpmRb5Zfr1q>>c7SzbQNTk8;hEQ!vYJ&<~7}*0ET4GY1#(1 zwzVZLGMn#wmVs`x>S=e-`+D>B*E$=ywO75sDaIjTFDL=nGOdHE5m0bg(8_fA^5x&| zN7$opnD|0h2D^#>cz^Te&Ds5Nz0e~!8a)LCH2h9^yjF*VCunI#D#sbbnrX@9mQx_! zlVEO)di}AuxcGlQ&#YCQ5LY^Qv#@mON^+cqdV)hh;er`x)qDB0Aik{V-CLkzLlTh> z8gAaa`Mp1;n+qBYogkaf1iXf?lxApr${F#kq!PBoGg;}w(wjGL-aWo#?JQ47BHiNR zu;9j|#tS?Ue}e>bMP|&JWwm!ok%RQ@Wxfzs9ry%Y#r}WWALF^uZFoML8XO8IxyRo} z6ahL)2FAw4x1(O~6x=AQ12Hq<4QpC@dV2b?ed1P-Sm0Q*v%#V8?8#V2p$pE{6`Vf4 zzOpf1hLaQbwE1R(jXbd7z|N&pYOD9&)4u>x1PZok+6nt;L#D*y>__$&aOWRdKp zsqT|-y>up6tY@!-=2Kz8!hrSHf6w3ezK0L0{k4)pK#X{qID8j_f`NK}@K}#V|B-3eK20vogiSX5Y14?^{YD z&B5+Eu!9q6ONL2a@AWuHSS+YzVmkV*w^IzXCJ^jW0SRvKxe_i%_L}A0*%7Y!G)!<~ zwo8n)QdtMb|M+Abw?Oc57YS!r^?EzHv)0<{EUi&iI=33JlcZka^S(^zGT#eIjQ`d5 zyEZR`ZVrLi0ZmKOG$x$?`bwz36kM@_DlZkr#z!7CexSM@;zei#L4ykv`4@5-8@ zLU-8C%F=aLE*`XS6~%jCS*VD?qy~pV6+3lA$pH-mBt2D}a%Qx+m}z=MhD=)@m$5S0U_SqpA6F&)k8Uw;pgIxt-= z;pwozLTLVEa6*In781Y+ua!hQEV%LZ<5g>Xu2$yc5OIH0Y=kd5@<1C^Z1;iMayXSG zj3%PdM1(VSG%z%dCZf@jXtX4PR0Eg=_GooBTAhuyiE!2*qiv$mhV^K}8nfDfiVVx% znPgUeH*75cxL_FVsg3s3M!Wmyy?pra$iQ^>Km5PFn>j49%b54TT@FFj#@4B^LYv+= zDR?s-YWn|QorT4*Q-Omm)zz^>qwCUX>uA#}U%s6AGRLm=T*>|A4~5?OgqL0owiSPJ zHXJeVjxi>(0W<`>@A~yOh`|lGx8V#OAps4&N0(uT1!D`R1N&H6x_-ySZ-yNi2iJ_m z*Z?}gy2C#i->AuNF#(Nt)wO3q(;>K;^aqp}8y_jz_2L_%c@yohV8{Efrg%F>-7G9! z-9Jq54bP1R&}abRN{0-LOrt4iG>c&?55djH(K2Z87t(8#4R{Xkj=lbO-|JoXOJl#s zUN4RRQv3h4mH%bPc{R`(0zGaHk-f60CBXwm*0#io{jYc3H*x;G?fvib@mVWWFZ@~h z{`dU&S2O3qk8^NnIFQW6AtGNK+XFq?2y|S`YEGWRAKKTwZqsJ=*zsYm*UIqdMZ4u~ zcn;6^o_agH_I`b3Is5r53nY)*|K9ii%e=hP)ibm9?!JBR^Bnh`2hq3Wd^4j)`NJ6EJi4t}*3_Egm|JYx9zuxrm zJ@bFlf?j?NyZ`-v4R6m{mafAFOQhc4-S>X;-sdO3vaxk-^k_}0{(Iy5-{bmU`AnaE zow#1gWB-*sw$Q0kPzW^^Gd3>T-ULqeV7nR%3xb)ChN$#zx2jqFdRKksw#1r?x)R%W zgd8^bvvS7XD2dMwjgR)1#eOflUTXifcK-L*6{ez=|~m zxuUQCiscZwKItmrT<^xWoE#$Cx2L%wrh5`X8<#nKyzDJnx_H6k&xICSUNM+o`>EVg z*>BJmeOfJGj+oZfwfl8%^Xz{S*~WAXNo1-{%d{Ccz?cn zj@P~Sr*?e4vq$>v)UT&(kAGhg!gT%T(|f;Wdu9AMzO3+^k#GG?CDYGR=U;YnzxR2o zo^7yipPu}d=ym&UZQC3C_`4*y7Yj;8@M_UBzH7Ebt z5awih(BG+#C7LthRp}kKts%jx5i_}ri|U$BlSZE zR==)D?s;pKwy#}b_l;uc{QPH4_V3>oHoI3o z%emdle5#Cx)63uK=%?z8`q{<*YG-}@cjn~@TSQ9Y$nkbquw(64qbNjkdo?I^slD)c zV7M;ekMs4?l|J4AGphNf7A^hsW(RAs@D9~Io(@xf3puDw@RTpLK2aDjU2B_Ei^>+C z&?GI9;D=$WW&N8@1Qmv_zW8a5js8RbTPZz~`Ri6bc-s6a_Eu^RXWFMTTYOVvoleEf zysci^=#>z>`km0XC4awfT`x7I&uMkUlK&yLX?~K?I+yGo_a61U`pkf*K6lN&46R?6 zwy~|q6h42mF<|r3w^KZsHvZQvwro53_OvCBbJLC<>ukaGW;Qv_J9~`li}Ke7tP!1Q z?;E0Vt+YFxckO1qnc2M6`t$xH_oWkG1lO-#d-je);knIy z2Mahiy#6nl;CH*~{G0F3=lR`*T?nAy&~V@}H;0IPeQyuas)=r{JAxW&3%nc3*Cla=~k+|FvrhYZE*IBx%S6ljaLFucTj}PRODO@>|{i)}STEU!E zYf;yi%*_9p>||Xgr(&_L*J0!ve$OrG3UGM*K|s|F^TEo zp4)G~ZGF3#Kl^k-zge+e77krET}veUw^Y=1-L~6x>shDnT+h2lc#3~0-}$=w zxG(a${79X&gi^-FN3V{^AsR6W(M;Q!js^uz571xey3SkCn`!FfZxh!#1&5X!F4(c< z?XxRkv#z(yes!e#vFC5K8`hk%(*4s#WvpTjeZyAJfCNA z_ZO?#!;HPAp%W_OZw0*7S+Yk!W83Xt+Z;>Z{(fRR=hfnQo>l1!EVjvoo6GInA27G# zoAn+8^TtPOr=J9^NQ4vvEM-~>0e4;=nk5D=8yc5$ZDEYM*|g=Y7Nh5?^RFZet~Ygf zPfdGUHZ$EZ^!Ele1M9GZz1#H9TGrfL^eIL9LZJNDJx9A66F3(KX-->p`s1~wrKc}v zH{L(0bws-H(UWZ(A8(ar?PuQ{{amJxC(EEBT%klWMK^9M*GHW_+1K8xB-Ac*_;`HZ zd;57+`)_9*mO6Fzr)-Rc_QqT*NjLwNy!7O3oveEG=o5X{Hr^|~;r8vE!$*~rxe~V@ zez!Gwb_EsbH&Tq6eI2201PMLzp>Tx~^TaG?o`aJFQu@=|%%^z>A z=F~ZN?%wIz#ky+39C5qvY2SA0+x}~fV&W~eHzzHE8cj{-NqtN8KK?jtnK1v_=dyk0 zTh=r_I$HcQucht!p<_{j;^*`8B2rGDk7+NwBRnrw`u^5*v-2~!cyH@^+%6Wnec(sX z2~$y*^J@d<@T@xi{+exLQs(WjqQ1y;TK?X%uPMmS?R^`hVe#wimGU)pG1n`;Np?N{ z8OJH|dfLISk(2jdHroGjFVj)+-_vh2BYb8vt-;~r#%qT&5JG!)9a!OH=ons|s(oV4 z%IW{-?pYVQaOuHTbf=5i9X>RoSs*?_dG9GtjZo4d~ z*qF4w?AsNGjccy|R=r}o@`6n0dfCjl5w%j`ORJRxwta3%eU1Rm@{L zYPsL)JGk`%PBRsUSy{UBe{d)KLIm|OrtRkf_NbPnFZl6t?Y~ps?Z4LA^O^IWBUf%b1mtK8d+)9Qu=4U@f1T2hL{iyEDjC{T~|MO2wTXU0dbJ&-= zHu?pL3nH91C$G5H(#DyWTrcBXEcY-n;HyvcJiF@FM`tGdUgPvNNHk%0AAiI`3qD8x zZ9imt|E^zrAg=Y%8TpmB5drna*I~hq?V_ggh_arW^%+aXb-6B*V`K>?zke zJG1_{*5O2nSKE3)0$&=yDt7=Q!XE$eOc`D!5Qy<A6s8P^Y-+<_xrQ%&+pt| z5`X{izdOIB%k05{2+588nhF6mXG#sV5bj&AW}xn{+ib_o#z*tLIKHl7JZix6_Ghi> zAFHrWrLohE|LU>j2Y;UR_~R7^!}HQ@3)u?Pqr1*kGS13%S3mzl`|ozfOOyAsC$5;9 zHNQM|de*7^S3WbfC$4zzP#1e{#hF`vVZL@r3vPr&Z+kL->i!fx_4)nVF5it=5OJ?l z%3yo3_twm{XMC?_-kbgL>yq#3=XSr5LpY5^F4|$ij~lO(OArws7n9)EP?j5D%zTtZ z%yi98i6v7T46e2v7uXP@xNOm@9PTS+uTSlJ|3B>bO_n51-=brWQdZ2!+Eci{*d~vq z>+6|aCpW*E#dWGjGqi5<^WZp%Xx=Sqx5f6iG;Y20&a*Ky2#(<}l=DE{JgImD=)jJ?APRx23;y!eKXVZ^bx^Y8aOmtUj4*Z#ZBfxU>B;jmfO;PBD#`q34L{IF-%fjJCs zZ#{SxqH*n;__@sN-S(U(X9=h)#x|{9slD*Y{o|n>sxn?3GZWrkDiv~>_PjPA&yM@^ ztja51+cqC~>LhySWbM;=J-UrAmufR6H|?6tb?nl&{rYFL#cnP$Ihf>hQk~uD?awKX z>t^Np^C#!8cD{Y`PL=trth2Y?NXXfhw?2B2bK=*Ym0z=)-`|;D{%J$$yD)8$+276H zN1v0(J>O+>t3395zgoxn=l*r|ac|{K_|*{g?}JDdmag-MRtO^!#?W#qIo%mTmA-UA6zCiJ=fy%9H};y7oG|gE97>UOw~HR zW%a?Nrd@w#&Ur7>Z!~SY{@I-^bu*LS?aYYQx%YaPp6KMSzxG8e$$EclPf%f)EuYv? z<%8k-c3bR=&Ytz&TOpeHyzuI@J3GHF_s%wWemCQtXM80)-{(BL{nzU6wf<1O^5;`q z_`9{Q3wM;hs@2V&{_5nnQ#&46y;Gh#`|$zu+}j_lAKTwtxBX1+s{bdq1t&}O*Pgra zirGCoT04XPv&Q|aALsYhp4zRMiR2N8D-bzAAOch+NUiNd zbaF9O=fpcK*zwoYgc*_4Fx|D9lS3qY@l`{3;A1ix-*R(^Y~SCOhT#gN&`dC8Y -#include -#include -#include - - -namespace env_manager -{ -namespace component_manager -{ - -using namespace std::chrono_literals; - -const std::string DEFAULT_CLIENT_NODE_NAME = "env_manager_client_node"; -const std::string DEFAULT_CLIENT_NAME = "client"; - -template -class ClientComponent: public rclcpp::Node -{ -public: - explicit ClientComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_CLIENT_NODE_NAME, options) - { - _client = create_client(DEFAULT_CLIENT_NAME, 10); - } - - virtual void callback( - typename rclcpp::Client::SharedFuture future) = 0; - - void populate_request( - const std::shared_ptr& request) - { - while (!_client->wait_for_service(1s)) - { - if (rclcpp::ok()) - { - RCLCPP_ERROR(this->get_logger(), - "Client interrupted while waiting for service. Terminating..."); - } - } - - auto result_future = _client->async_send_request( - request, std::bind(&ClientComponent::callback, - this, std::placeholders::_1)); - } - -private: - typename rclcpp::Client::SharedPtr _client; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/component_manager.hpp b/env_manager/include/component_manager/component_manager.hpp deleted file mode 100644 index 815044d..0000000 --- a/env_manager/include/component_manager/component_manager.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ -#define COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ - -#include "component_manager/visibility_control.h" -#include "config/options.hpp" - -#include -#include -#include -#include - -#include - - -namespace env_manager -{ -namespace component_manager -{ - -/** - * This class implements the system for managing and configuring loaded components. - * - * It is assumed that the loaded components are inherited from the classes - * provided in the library for each node type. - */ -class ComponentManager -{ -public: - ComponentManager(std::weak_ptr executor); - - void register_components( - const std::map &comps, - const std::map &nodes, - const std::string& ns); - - void remove_components_from_executor(); - - void remap_components_namespace(const std::string& ns); - -private: - std::weak_ptr _executor; - std::vector _loaders; - std::map _node_wrappers; - std::map _nodes; -}; - -} // namespace env_manager -} // namespace component_manager - -#endif // COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_ diff --git a/env_manager/include/component_manager/component_template.hpp b/env_manager/include/component_manager/component_template.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/env_manager/include/component_manager/node_component.hpp b/env_manager/include/component_manager/node_component.hpp deleted file mode 100644 index 482b698..0000000 --- a/env_manager/include/component_manager/node_component.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ -#define ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ - -#include "component_manager/visibility_control.h" - -#include "rclcpp/node.hpp" - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_NODE_NODE_NAME = "env_manager_node"; - -//TODO: interhit all another components from this node_component -class NodeComponent: public rclcpp::Node -{ -public: - explicit NodeComponent(const rclcpp::NodeOptions& options) - :Node(DEFAULT_NODE_NODE_NAME, options) - { - auto ret = rcutils_logging_set_logger_level( - get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); - if (ret != RCUTILS_RET_OK) - { - RCLCPP_ERROR(get_logger(), - "Error setting severity: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } -}; - -} // namesapce component_manager -} // namesapce env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ diff --git a/env_manager/include/component_manager/publisher_component.hpp b/env_manager/include/component_manager/publisher_component.hpp deleted file mode 100644 index 5cec622..0000000 --- a/env_manager/include/component_manager/publisher_component.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ -#define ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ - -#include "component_manager/visibility_control.h" - -#include "rclcpp/rclcpp.hpp" - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_PUB_NODE_NAME = "env_manager_pub_node"; -const std::string DEFAULT_PUB_TOPIC_NAME = "pub_topic"; - -template -class PublisherComponent: public rclcpp::Node -{ -public: - explicit PublisherComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_PUB_NODE_NAME, options) - { - _pub = create_publisher(DEFAULT_PUB_TOPIC_NAME, 10); - auto ret = rcutils_logging_set_logger_level( - get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); - if (ret != RCUTILS_RET_OK) - { - RCLCPP_ERROR(get_logger(), - "Error setting severity: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } - - void populate_publication(const MessageT& msg) - { - _pub->publish(std::move(msg)); - } - -private: - typename rclcpp::Publisher::SharedPtr _pub; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/service_component.hpp b/env_manager/include/component_manager/service_component.hpp deleted file mode 100644 index 86bac7c..0000000 --- a/env_manager/include/component_manager/service_component.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ -#define ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ - -#include "component_manager/visibility_control.h" - -#include - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_SERVICE_NDOE_NAME = "env_manager_service_node"; -const std::string DEFAULT_SERVICE_NAME = "service"; - -template -class ServiceComponent: public rclcpp::Node -{ -public: - explicit ServiceComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_SERVICE_NDOE_NAME, options) - { - _service = create_service( - DEFAULT_SERVICE_NAME, std::bind( - &ServiceComponent::callback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - virtual void callback(std::shared_ptr request, - std::shared_ptr response) = 0; - -private: - typename rclcpp::Service::SharedPtr _service; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/subscriber_component.hpp b/env_manager/include/component_manager/subscriber_component.hpp deleted file mode 100644 index 80fb873..0000000 --- a/env_manager/include/component_manager/subscriber_component.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ -#define ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace env_manager -{ -namespace component_manager -{ - -const std::string DEFAULT_SUB_NODE_NAME = "env_manager_sub_node"; -const std::string DEFAULT_SUB_TOPIC_NAME = "sub_topic"; - -template -class SubscriberComponent : public rclcpp::Node -{ -public: - explicit SubscriberComponent(const rclcpp::NodeOptions& options) - : Node(DEFAULT_SUB_NODE_NAME, options) - { - _sub = create_subscription( - DEFAULT_SUB_TOPIC_NAME, 10, this->callback); - auto ret = rcutils_logging_set_logger_level( - get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); - if (ret != RCUTILS_RET_OK) - { - RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } - - virtual void callback(const MessageT& msg) = 0; - -private: - typename rclcpp::Subscription::SharedPtr _sub; -}; - -} // namespace component_manager -} // namespace env_manager - -#endif // ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_ diff --git a/env_manager/include/component_manager/visibility_control.h b/env_manager/include/component_manager/visibility_control.h deleted file mode 100644 index e26be49..0000000 --- a/env_manager/include/component_manager/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef COMPONENT_MANAGER__VISIBILITY_CONTROL_H_ -#define COMPONENT_MANAGER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define COMPONENT_MANAGER_EXPORT __attribute__ ((dllexport)) - #define COMPONENT_MANAGER_IMPORT __attribute__ ((dllimport)) - #else - #define COMPONENT_MANAGER_EXPORT __declspec(dllexport) - #define COMPONENT_MANAGER_IMPORT __declspec(dllimport) - #endif - #ifdef COMPONENT_MANAGER_BUILDING_LIBRARY - #define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_EXPORT - #else - #define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_IMPORT - #endif - #define COMPONENT_MANAGER_PUBLIC_TYPE COMPONENT_MANAGER_PUBLIC - #define COMPONENT_MANAGER_LOCAL -#else - #define COMPONENT_MANAGER_EXPORT __attribute__ ((visibility("default"))) - #define COMPONENT_MANAGER_IMPORT - #if __GNUC__ >= 4 - #define COMPONENT_MANAGER_PUBLIC __attribute__ ((visibility("default"))) - #define COMPONENT_MANAGER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define COMPONENT_MANAGER_PUBLIC - #define COMPONENT_MANAGER_LOCAL - #endif - #define COMPONENT_MANAGER_PUBLIC_TYPE -#endif - -#endif // COMPONENT_MANAGER__VISIBILITY_CONTROL_H_ diff --git a/env_manager/include/config/config.hpp.cmake b/env_manager/include/config/config.hpp.cmake deleted file mode 100644 index 708944f..0000000 --- a/env_manager/include/config/config.hpp.cmake +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_CONFIG_H_ -#define ENV_MANAGER_CONFIG_CONFIG_H_ - -namespace env_manager -{ -namespace config -{ - -constexpr char kConfigurationFilesDirectory[] = "@ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY@"; -constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@"; - -} // namespace config -} // namespace env_manager - -#endif // ENV_MANAGER_CONFIG_CONFIG_H_ diff --git a/env_manager/include/config/config_file_resolver.hpp b/env_manager/include/config/config_file_resolver.hpp deleted file mode 100644 index 28848d0..0000000 --- a/env_manager/include/config/config_file_resolver.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ -#define ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ - -#include "lua_file_resolver.hpp" - -namespace env_manager -{ -namespace config -{ - -class ConfigurationFileResolver : public FileResolver -{ -public: - explicit ConfigurationFileResolver( - const std::vector& cfg_files_dirs); - - std::string GetPath(const std::string& basename) const; - std::string GetContent(const std::string& basename) const; - -private: - std::vector _config_files_dirs; - -}; - -} // namespace config -} // namespace env_manager - -#endif // ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_ diff --git a/env_manager/include/config/lua_file_resolver.hpp b/env_manager/include/config/lua_file_resolver.hpp deleted file mode 100644 index 2c9d900..0000000 --- a/env_manager/include/config/lua_file_resolver.hpp +++ /dev/null @@ -1,129 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ -#define ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ - -#include "lua.hpp" -#include -#include -#include -#include - -namespace env_manager -{ -namespace config -{ - -class FileResolver -{ -public: - virtual ~FileResolver() {} - virtual std::string GetPath(const std::string& basename) const = 0; - virtual std::string GetContent(const std::string& basename) const = 0; -}; - -class LuaParameterDictionary { - public: - // Constructs the dictionary from a Lua Table specification. - LuaParameterDictionary(const std::string& code, - std::unique_ptr file_resolver); - - LuaParameterDictionary(const LuaParameterDictionary&) = delete; - LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete; - - // Constructs a LuaParameterDictionary without reference counting. - static std::unique_ptr NonReferenceCounted( - const std::string& code, std::unique_ptr file_resolver); - - ~LuaParameterDictionary(); - - // Returns all available keys. - std::vector GetKeys() const; - - // Returns true if the key is in this dictionary. - bool HasKey(const std::string& key) const; - - // These methods CHECK() that the 'key' exists. - std::string GetString(const std::string& key); - double GetDouble(const std::string& key); - int GetInt(const std::string& key); - bool GetBool(const std::string& key); - std::unique_ptr GetDictionary(const std::string& key); - - // Gets an int from the dictionary and CHECK()s that it is non-negative. - int GetNonNegativeInt(const std::string& key); - - // Returns a string representation for this LuaParameterDictionary. - std::string ToString() const; - - // Returns the values of the keys '1', '2', '3' as the given types. - std::vector GetArrayValuesAsDoubles(); - std::vector GetArrayValuesAsStrings(); - std::vector> - GetArrayValuesAsDictionaries(); - - private: - enum class ReferenceCount { YES, NO }; - LuaParameterDictionary(const std::string& code, - ReferenceCount reference_count, - std::unique_ptr file_resolver); - - // For GetDictionary(). - LuaParameterDictionary(lua_State* L, ReferenceCount reference_count, - std::shared_ptr file_resolver); - - // Function that recurses to keep track of indent for ToString(). - std::string DoToString(const std::string& indent) const; - - // Pop the top of the stack and CHECKs that the type is correct. - double PopDouble() const; - int PopInt() const; - bool PopBool() const; - - // Pop the top of the stack and CHECKs that it is a string. The returned value - // is either quoted to be suitable to be read back by a Lua interpretor or - // not. - enum class Quoted { YES, NO }; - std::string PopString(Quoted quoted) const; - - // Creates a LuaParameterDictionary from the Lua table at the top of the - // stack, either with or without reference counting. - std::unique_ptr PopDictionary( - ReferenceCount reference_count) const; - - // CHECK() that 'key' is in the dictionary. - void CheckHasKey(const std::string& key) const; - - // CHECK() that 'key' is in this dictionary and reference it as being used. - void CheckHasKeyAndReference(const std::string& key); - - // If desired, this can be called in the destructor of a derived class. It - // will CHECK() that all keys defined in the configuration have been used - // exactly once and resets the reference counter. - void CheckAllKeysWereUsedExactlyOnceAndReset(); - - // Reads a file into a Lua string. - static int LuaRead(lua_State* L); - - // Handles inclusion of other Lua files and prevents double inclusion. - static int LuaInclude(lua_State* L); - - lua_State* L_; // The name is by convention in the Lua World. - int index_into_reference_table_; - - // This is shared with all the sub dictionaries. - const std::shared_ptr file_resolver_; - - // If true will check that all keys were used on destruction. - const ReferenceCount reference_count_; - - // This is modified with every call to Get* in order to verify that all - // parameters are read exactly once. - std::map reference_counts_; - - // List of all included files in order of inclusion. Used to prevent double - // inclusion. - std::vector included_files_; -}; -} // namespce config -} // namespace env_manager - -#endif // ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_ diff --git a/env_manager/include/config/options.hpp b/env_manager/include/config/options.hpp deleted file mode 100644 index 190c23f..0000000 --- a/env_manager/include/config/options.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef ENV_MANAGER_CONFIG_OPTIONS_HPP_ -#define ENV_MANAGER_CONFIG_OPTIONS_HPP_ - -#include "config/lua_file_resolver.hpp" - -#include - -namespace env_manager -{ -namespace config -{ - -struct ComponentOption -{ - static ComponentOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::string library; - std::string class_name; -}; - -struct EnvironmentOption -{ - static EnvironmentOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::string ns; - std::map components; -}; - -struct NodeOption -{ - enum NodeType - { - Publisher, - Subscriber, - Service, - Client, - Node - }; - - static NodeOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::string name; - std::string msg_type; - NodeType type; - static const std::unordered_map node_type_remap; -}; - -struct EnvManagerOption -{ - static EnvManagerOption create_option( - LuaParameterDictionary* lua_parameter_dictionary); - std::map environments; - std::map nodes; -}; - -} -} -#endif // ENV_MANAGER_CONFIG_OPTIONS_HPP_ diff --git a/env_manager/include/env_manager/environment_base.hpp b/env_manager/include/env_manager/environment_base.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/env_manager/include/env_manager/environment_manager.hpp b/env_manager/include/env_manager/environment_manager.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/env_manager/include/env_manager/environment_node_manager.hpp b/env_manager/include/env_manager/environment_node_manager.hpp deleted file mode 100644 index cb5bca6..0000000 --- a/env_manager/include/env_manager/environment_node_manager.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ -#define ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ - -#include "config/options.hpp" - -#include - -#include -#include - -extern "C" -{ -#include -#include -} - -namespace env_manager -{ - -class EnvironmentNodeManager -{ -public: - EnvironmentNodeManager(); - - ~EnvironmentNodeManager(); -}; - -} // namespace env_manager - -#endif // ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_ diff --git a/env_manager/package.xml b/env_manager/package.xml deleted file mode 100644 index 4a1128c..0000000 --- a/env_manager/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - env_manager - 0.0.0 - env_manager - splinter1984 - Apache-2.0 - - ament_cmake_ros - - boost - rcl - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/env_manager/src/component_manager/component_manager.cpp b/env_manager/src/component_manager/component_manager.cpp deleted file mode 100644 index 96bf858..0000000 --- a/env_manager/src/component_manager/component_manager.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include "component_manager/component_manager.hpp" -#include "component_manager/node_component.hpp" -#include "component_manager/publisher_component.hpp" -#include "component_manager/subscriber_component.hpp" -#include "component_manager/service_component.hpp" -#include "component_manager/client_component.hpp" -#include "glog/logging.h" - -#include "rcl/remap.h" -#include - -namespace env_manager -{ -namespace component_manager -{ - -ComponentManager::ComponentManager(std::weak_ptr executor) - : _executor(executor) -{ -} - -static rclcpp::NodeOptions create_options( - const config::NodeOption &node_opts, - const std::string &node_name, - const std::string &ns) -{ - rclcpp::NodeOptions opts; - std::vector args = {"--ros-args", - "--disable-rosout-logs", - "--disable-stdout-logs", - "--enable-external-lib-logs", - "--log-level", "WARN", - }; - args.push_back("-r"); - args.push_back("__ns:=/" + ns); - args.push_back("-r"); - args.push_back("__node:=" + node_name); - args.push_back("-r"); - - switch (node_opts.type) - { - case config::NodeOption::NodeType::Publisher: - args.push_back(DEFAULT_PUB_TOPIC_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Subscriber: - args.push_back(DEFAULT_SUB_TOPIC_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Service: - args.push_back(DEFAULT_SERVICE_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Client: - args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name); - break; - case config::NodeOption::NodeType::Node: - args.erase(args.end()); - break; - } - - opts.arguments(args); - - return opts; -} - -static rclcpp::NodeOptions create_default_options(const std::string &ns) -{ - rclcpp::NodeOptions opts; - opts.arguments({ - "--ros-args", "-r", std::string("__ns:=/" + ns), - "--disable-rosout-logs", - "--disable-stdout-logs", - "--enable-external-lib-logs", - "--log-level", "FATAL", - }); - - return opts; -} - -void ComponentManager::register_components( - const std::map &comps, - const std::map &nodes, - const std::string& ns) -{ - if (comps.empty()) - return; - - for (const auto &[name, comp]: comps) - { - auto class_name = "rclcpp_components::NodeFactoryTemplate<" + comp.class_name + ">"; - - LOG(INFO) << "Provide lib: " << comp.library << " namespace: " + ns; - auto loader = new class_loader::ClassLoader(comp.library); - auto classes = - loader->getAvailableClasses(); - - for (auto clazz: classes) - { - if (clazz != class_name) - continue; - rclcpp::NodeOptions opts = create_default_options(ns); - if (clazz == class_name) - { - auto node_opts = nodes.at(name); - opts = create_options(node_opts, name, ns); - } - LOG(INFO) << "Create instance of class: " << clazz; - auto node_factory = - loader->createInstance(clazz); - auto wrapper = node_factory->create_node_instance(opts); - auto node = wrapper.get_node_base_interface(); - - _node_wrappers.insert({name, wrapper}); - _nodes.insert({name, node}); - if (auto exec = _executor.lock()) - exec->add_node(node); - } - - _loaders.push_back(loader); - } -} - -void ComponentManager::remap_components_namespace(const std::string &ns) -{ - char* nms = const_cast(ns.c_str()); - for (auto& [name, wrapper]: _node_wrappers) - { - auto node = (rclcpp::Node*)wrapper.get_node_instance().get(); - auto opts = node->get_node_options(); - auto ret = rcl_remap_node_namespace( - &opts.get_rcl_node_options()->arguments, - NULL, node->get_name(), rcl_get_default_allocator(), - &nms); - - if (ret == RCL_RET_OK) - LOG(INFO) << "Succesfully remap node with ns: " + ns; - } - - //std::logic_error("Not implemented." + ns); -} - -void ComponentManager::remove_components_from_executor() -{ - if (_nodes.empty()) - { - LOG(WARNING) << "Unable to remove nodes from executor."; - return; - } - - if (auto exec = _executor.lock()) - { - for (const auto &[node_name, node]: _nodes) - { - LOG(INFO) << "Remove node '" << node_name << "' from executor."; - exec->remove_node(node); - } - } -} - -} // namespace env_manager -} // namespace component_manager diff --git a/env_manager/src/config/config_file_resolver.cpp b/env_manager/src/config/config_file_resolver.cpp deleted file mode 100644 index f945deb..0000000 --- a/env_manager/src/config/config_file_resolver.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "config/config_file_resolver.hpp" - -#include -#include -#include - -#include "config/config.hpp" - -#include - -namespace env_manager -{ -namespace config -{ - -ConfigurationFileResolver::ConfigurationFileResolver( - const std::vector& cfg_files_dirs) - : _config_files_dirs(cfg_files_dirs) -{ - _config_files_dirs.push_back(kConfigurationFilesDirectory); -} - -std::string ConfigurationFileResolver::GetPath( - const std::string &basename) const -{ - for (const auto& path: _config_files_dirs) - { - const std::string filename = path + "/" + basename; - std::ifstream stream(filename.c_str()); - if (stream.good()) - { - LOG(INFO) << "found filename: " << filename; - return filename; - } - } - - LOG(FATAL) << "File '" << basename << "' was not found."; -} - -std::string ConfigurationFileResolver::GetContent( - const std::string &basename) const -{ - CHECK(!basename.empty()) << "File basename cannot be empty." << basename; - const std::string filename = GetPath(basename); - std::ifstream stream(filename.c_str()); - return std::string(std::istreambuf_iterator(stream), - std::istreambuf_iterator()); -} - -} // namespace config -} // namespace env_manager diff --git a/env_manager/src/config/lua_file_resolver.cpp b/env_manager/src/config/lua_file_resolver.cpp deleted file mode 100644 index 71ec231..0000000 --- a/env_manager/src/config/lua_file_resolver.cpp +++ /dev/null @@ -1,446 +0,0 @@ -#include "config/lua_file_resolver.hpp" -#include "glog/logging.h" - -#include -#include -#include -#include - -namespace env_manager -{ -namespace config -{ - -namespace { -// Replace the string at the top of the stack through a quoted version that Lua -// can read back. -void QuoteStringOnStack(lua_State* L) { - CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value."; - int current_index = lua_gettop(L); - - // S: ... string - lua_pushglobaltable(L); // S: ... string globals - lua_getfield(L, -1, "string"); // S: ... string globals - lua_getfield(L, -1, - "format"); // S: ... string globals format - lua_pushstring(L, "%q"); // S: ... string globals format "%q" - lua_pushvalue(L, current_index); // S: ... string globals - // format "%q" string - - lua_call(L, 2, 1); // S: ... string globals quoted - lua_replace(L, current_index); // S: ... quoted globals - - lua_pop(L, 2); // S: ... quoted -} - -// Sets the given 'dictionary' as the value of the "this" key in Lua's registry -// table. -void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) { - lua_pushstring(L, "this"); - lua_pushlightuserdata(L, dictionary); - lua_settable(L, LUA_REGISTRYINDEX); -} - -// Gets the 'dictionary' from the "this" key in Lua's registry table. -LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) { - lua_pushstring(L, "this"); - lua_gettable(L, LUA_REGISTRYINDEX); - void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1); - lua_pop(L, 1); - CHECK(return_value != nullptr); - return reinterpret_cast(return_value); -} - -// CHECK()s if a Lua method returned an error. -void CheckForLuaErrors(lua_State* L, int status) { - CHECK_EQ(status, 0) << lua_tostring(L, -1); -} - -// Returns 'a' if 'condition' is true, else 'b'. -int LuaChoose(lua_State* L) { - CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b)."; - CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value."; - - const bool condition = lua_toboolean(L, 1); - if (condition) { - lua_pushvalue(L, 2); - } else { - lua_pushvalue(L, 3); - } - return 1; -} - -// Pushes a value to the Lua stack. -void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); } -void PushValue(lua_State* L, const std::string& key) { - lua_pushstring(L, key.c_str()); -} - -// Reads the value with the given 'key' from the Lua dictionary and pushes it to -// the top of the stack. -template -void GetValueFromLuaTable(lua_State* L, const T& key) { - PushValue(L, key); - lua_rawget(L, -2); -} - -// CHECK() that the topmost parameter on the Lua stack is a table. -void CheckTableIsAtTopOfStack(lua_State* L) { - CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; -} - -// Returns true if 'key' is in the table at the top of the Lua stack. -template -bool HasKeyOfType(lua_State* L, const T& key) { - CheckTableIsAtTopOfStack(L); - PushValue(L, key); - lua_rawget(L, -2); - const bool key_not_found = lua_isnil(L, -1); - lua_pop(L, 1); // Pop the item again. - return !key_not_found; -} - -// Iterates over the integer keys of the table at the top of the stack of 'L• -// and pushes the values one by one. 'pop_value' is expected to pop a value and -// put them into a C++ container. -void GetArrayValues(lua_State* L, const std::function& pop_value) { - int idx = 1; - while (true) { - GetValueFromLuaTable(L, idx); - if (lua_isnil(L, -1)) { - lua_pop(L, 1); - break; - } - pop_value(); - ++idx; - } -} - -} // namespace - -std::unique_ptr -LuaParameterDictionary::NonReferenceCounted( - const std::string& code, std::unique_ptr file_resolver) { - return std::unique_ptr(new LuaParameterDictionary( - code, ReferenceCount::NO, std::move(file_resolver))); -} - -LuaParameterDictionary::LuaParameterDictionary( - const std::string& code, std::unique_ptr file_resolver) - : LuaParameterDictionary(code, ReferenceCount::YES, - std::move(file_resolver)) {} - -LuaParameterDictionary::LuaParameterDictionary( - const std::string& code, ReferenceCount reference_count, - std::unique_ptr file_resolver) - : L_(luaL_newstate()), - index_into_reference_table_(-1), - file_resolver_(std::move(file_resolver)), - reference_count_(reference_count) { - CHECK_NOTNULL(L_); - SetDictionaryInRegistry(L_, this); - - luaL_openlibs(L_); - - lua_register(L_, "choose", LuaChoose); - lua_register(L_, "include", LuaInclude); - lua_register(L_, "read", LuaRead); - - CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str())); - CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0)); - CheckTableIsAtTopOfStack(L_); -} - -LuaParameterDictionary::LuaParameterDictionary( - lua_State* const L, ReferenceCount reference_count, - std::shared_ptr file_resolver) - : L_(lua_newthread(L)), - file_resolver_(std::move(file_resolver)), - reference_count_(reference_count) { - CHECK_NOTNULL(L_); - - // Make sure this is never garbage collected. - CHECK(lua_isthread(L, -1)); - index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX); - - CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; - lua_xmove(L, L_, 1); // Moves the table and the coroutine over. - CheckTableIsAtTopOfStack(L_); -} - -LuaParameterDictionary::~LuaParameterDictionary() { - if (reference_count_ == ReferenceCount::YES) { - //CheckAllKeysWereUsedExactlyOnceAndReset(); - } - if (index_into_reference_table_ > 0) { - luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_); - } else { - lua_close(L_); - } -} - -std::vector LuaParameterDictionary::GetKeys() const { - CheckTableIsAtTopOfStack(L_); - std::vector keys; - - lua_pushnil(L_); // Push the first key - while (lua_next(L_, -2) != 0) { - lua_pop(L_, 1); // Pop value, keep key. - if (!lua_isnumber(L_, -1)) { - keys.emplace_back(lua_tostring(L_, -1)); - } - } - return keys; -} - -bool LuaParameterDictionary::HasKey(const std::string& key) const { - return HasKeyOfType(L_, key); -} - -std::string LuaParameterDictionary::GetString(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopString(Quoted::NO); -} - -std::string LuaParameterDictionary::PopString(Quoted quoted) const { - CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value."; - if (quoted == Quoted::YES) { - QuoteStringOnStack(L_); - } - - const std::string value = lua_tostring(L_, -1); - lua_pop(L_, 1); - return value; -} - -double LuaParameterDictionary::GetDouble(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopDouble(); -} - -double LuaParameterDictionary::PopDouble() const { - CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value."; - const double value = lua_tonumber(L_, -1); - lua_pop(L_, 1); - return value; -} - -int LuaParameterDictionary::GetInt(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopInt(); -} - -int LuaParameterDictionary::PopInt() const { - CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value."; - const int value = lua_tointeger(L_, -1); - lua_pop(L_, 1); - return value; -} - -bool LuaParameterDictionary::GetBool(const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopBool(); -} - -bool LuaParameterDictionary::PopBool() const { - CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value."; - const bool value = lua_toboolean(L_, -1); - lua_pop(L_, 1); - return value; -} - -std::unique_ptr LuaParameterDictionary::GetDictionary( - const std::string& key) { - CheckHasKeyAndReference(key); - GetValueFromLuaTable(L_, key); - return PopDictionary(reference_count_); -} - -std::unique_ptr LuaParameterDictionary::PopDictionary( - ReferenceCount reference_count) const { - CheckTableIsAtTopOfStack(L_); - std::unique_ptr value( - new LuaParameterDictionary(L_, reference_count, file_resolver_)); - // The constructor lua_xmove()s the value, no need to pop it. - CheckTableIsAtTopOfStack(L_); - return value; -} - -std::string LuaParameterDictionary::DoToString( - const std::string& indent) const { - std::string result = "{"; - bool dictionary_is_empty = true; - - const auto top_of_stack_to_string = [this, indent, - &dictionary_is_empty]() -> std::string { - dictionary_is_empty = false; - - const int value_type = lua_type(L_, -1); - switch (value_type) { - case LUA_TBOOLEAN: - return PopBool() ? "true" : "false"; - break; - case LUA_TSTRING: - return PopString(Quoted::YES); - break; - case LUA_TNUMBER: { - const double value = PopDouble(); - if (std::isinf(value)) { - return value < 0 ? "-math.huge" : "math.huge"; - } else { - return std::to_string(value); - } - } break; - case LUA_TTABLE: { - std::unique_ptr subdict( - PopDictionary(ReferenceCount::NO)); - return subdict->DoToString(indent + " "); - } break; - default: - LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type); - } - }; - - // Integer (array) keys. - for (int i = 1; i; ++i) { - GetValueFromLuaTable(L_, i); - if (lua_isnil(L_, -1)) { - lua_pop(L_, 1); - break; - } - result.append("\n"); - result.append(indent); - result.append(" "); - result.append(top_of_stack_to_string()); - result.append(","); - } - - // String keys. - std::vector keys = GetKeys(); - if (!keys.empty()) { - std::sort(keys.begin(), keys.end()); - for (const std::string& key : keys) { - GetValueFromLuaTable(L_, key); - result.append("\n"); - result.append(indent); - result.append(" "); - result.append(key); - result.append(" = "); - result.append(top_of_stack_to_string()); - result.append(","); - } - } - result.append("\n"); - result.append(indent); - result.append("}"); - - if (dictionary_is_empty) { - return "{}"; - } - return result; -} - -std::string LuaParameterDictionary::ToString() const { return DoToString(""); } - -std::vector LuaParameterDictionary::GetArrayValuesAsDoubles() { - std::vector values; - GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); }); - return values; -} - -std::vector> -LuaParameterDictionary::GetArrayValuesAsDictionaries() { - std::vector> values; - GetArrayValues(L_, [&values, this] { - values.push_back(PopDictionary(reference_count_)); - }); - return values; -} - -std::vector LuaParameterDictionary::GetArrayValuesAsStrings() { - std::vector values; - GetArrayValues(L_, - [&values, this] { values.push_back(PopString(Quoted::NO)); }); - return values; -} - -void LuaParameterDictionary::CheckHasKey(const std::string& key) const { - CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n" - << ToString(); -} - -void LuaParameterDictionary::CheckHasKeyAndReference(const std::string& key) { - CheckHasKey(key); - reference_counts_[key]++; -} - -void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() { - for (const auto& key : GetKeys()) { - CHECK_EQ(1, reference_counts_.count(key)) - << "Key '" << key << "' was used the wrong number of times."; - CHECK_EQ(1, reference_counts_.at(key)) - << "Key '" << key << "' was used the wrong number of times."; - } - reference_counts_.clear(); -} - -int LuaParameterDictionary::GetNonNegativeInt(const std::string& key) { - const int temp = GetInt(key); // Will increase reference count. - CHECK_GE(temp, 0) << temp << " is negative."; - return temp; -} - -// Lua function to run a script in the current Lua context. Takes the filename -// as its only argument. -int LuaParameterDictionary::LuaInclude(lua_State* L) { - CHECK_EQ(lua_gettop(L), 1); - CHECK(lua_isstring(L, -1)) << "include takes a filename."; - - LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L); - const std::string basename = lua_tostring(L, -1); - const std::string filename = - parameter_dictionary->file_resolver_->GetPath(basename); - if (std::find(parameter_dictionary->included_files_.begin(), - parameter_dictionary->included_files_.end(), - filename) != parameter_dictionary->included_files_.end()) { - std::string error_msg = - "Tried to include " + filename + - " twice. Already included files in order of inclusion: "; - for (const std::string& filename : parameter_dictionary->included_files_) { - error_msg.append(filename); - error_msg.append("\n"); - } - LOG(FATAL) << error_msg; - } - parameter_dictionary->included_files_.push_back(filename); - lua_pop(L, 1); - CHECK_EQ(lua_gettop(L), 0); - - const std::string content = - parameter_dictionary->file_resolver_->GetContent(basename); - CheckForLuaErrors( - L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str())); - CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0)); - - return lua_gettop(L); -} - -// Lua function to read a file into a string. -int LuaParameterDictionary::LuaRead(lua_State* L) { - CHECK_EQ(lua_gettop(L), 1); - CHECK(lua_isstring(L, -1)) << "read takes a filename."; - - LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L); - const std::string file_content = - parameter_dictionary->file_resolver_->GetContent( - lua_tostring(L, -1)); - lua_pushstring(L, file_content.c_str()); - return 1; -} - -} // namespace config -} // namespace env_manager diff --git a/env_manager/src/config/options.cpp b/env_manager/src/config/options.cpp deleted file mode 100644 index 2fbe53d..0000000 --- a/env_manager/src/config/options.cpp +++ /dev/null @@ -1,83 +0,0 @@ -#include "config/options.hpp" - -#include "glog/logging.h" - -namespace env_manager -{ -namespace config -{ - -ComponentOption ComponentOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - return ComponentOption{ - lua_parameter_dictionary->GetString("lib"), - lua_parameter_dictionary->GetString("class"), - }; -} - -EnvironmentOption EnvironmentOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - auto comps = - lua_parameter_dictionary->GetDictionary("components"); - std::map components; - for (const auto &comp: comps->GetKeys()) - components.insert({ - comp, ComponentOption::create_option(comps->GetDictionary(comp).get()) - }); - - return EnvironmentOption{ - lua_parameter_dictionary->GetString("namespace"), - components, - }; -} - -NodeOption NodeOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - return NodeOption{ - lua_parameter_dictionary->GetString("name"), - lua_parameter_dictionary->GetString("msg_type"), - node_type_remap.at(lua_parameter_dictionary->GetString("type")), - }; -} - -const std::unordered_map -NodeOption::node_type_remap = - { - {"Publisher", NodeOption::NodeType::Publisher}, - {"Subscriber", NodeOption::NodeType::Subscriber}, - {"Service", NodeOption::NodeType::Service}, - {"Client", NodeOption::NodeType::Client}, - {"Node", NodeOption::NodeType::Node}, - }; - -EnvManagerOption EnvManagerOption::create_option( - LuaParameterDictionary *lua_parameter_dictionary) -{ - - auto lua_env_dictionary = lua_parameter_dictionary->GetDictionary("environments"); - auto envs_dict = lua_env_dictionary->GetArrayValuesAsDictionaries(); - std::map environments; - for (const auto& env: envs_dict) - environments.insert({ - env->GetString("namespace"), EnvironmentOption::create_option(env.get()) - }); - - auto lua_node_dictionary = lua_parameter_dictionary->GetDictionary("nodes"); - auto nodes_keys = lua_node_dictionary->GetKeys(); - std::map nodes; - for (const auto& node: nodes_keys) - nodes.insert({ - node, NodeOption::create_option(lua_node_dictionary->GetDictionary(node).get()) - }); - - return EnvManagerOption{ - environments, - nodes, - }; -} - -} // namespace config -} // namespace env_manager diff --git a/env_manager/src/env_manager/environment_node_manager.cpp b/env_manager/src/env_manager/environment_node_manager.cpp deleted file mode 100644 index 7b95a3e..0000000 --- a/env_manager/src/env_manager/environment_node_manager.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "env_manager/environment_node_manager.hpp" - -namespace env_manager -{ - -} // namespace env_manager diff --git a/env_manager/src/main.cpp b/env_manager/src/main.cpp deleted file mode 100644 index 88f2456..0000000 --- a/env_manager/src/main.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "component_manager/component_manager.hpp" -#include "config/config_file_resolver.hpp" -#include "config/lua_file_resolver.hpp" -#include "config/options.hpp" - -#include "config/config.hpp" - -#include "glog/logging.h" - -int main(int argc, const char* argv[]) -{ - google::InitGoogleLogging(argv[0]); - FLAGS_logtostderr = 1; - - rclcpp::init(argc, argv); - auto exec = std::make_shared(); - - std::vector dirs = {}; - auto cfg_resolver = - ::std::make_unique<::env_manager::config::ConfigurationFileResolver>( - std::vector{ - std::string(env_manager::config::kSourceDirectory) + - "/config" - } - ); - - auto code = cfg_resolver->GetContent("config.lua"); - //LOG(INFO) << code; - ::env_manager::config::LuaParameterDictionary lua_dict( - code, std::move(cfg_resolver) - ); - - auto env_manager_opts = - ::env_manager::config::EnvManagerOption::create_option( - &lua_dict - ); - - std::vector comp_mngrs; - - for (const auto& [env, opts]: env_manager_opts.environments) - { - LOG(INFO) << "Start to initialize environment: " << env; - env_manager::component_manager::ComponentManager cmg(exec); - cmg.register_components(opts.components, env_manager_opts.nodes, opts.ns); - //cmg.upload_components_to_executor(&exec); - comp_mngrs.push_back(std::move(cmg)); - } - - //std::string ns = "/"; - //comp_mngrs.begin()->remap_components_namespace(ns); - - exec->spin(); - - for (auto &cmg: comp_mngrs) - cmg.remove_components_from_executor(); - - rclcpp::shutdown(); - - return 0; -} From 78d890b62e677883d031c734072fdc3cf5ccc03a Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 16 Sep 2023 20:45:37 +0300 Subject: [PATCH 2/7] =?UTF-8?q?=F0=9F=9A=A7=20add=20logic=20to=20env=5Fman?= =?UTF-8?q?ager=20lib?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- env_manager/env_manager/CMakeLists.txt | 76 +++++++ env_manager/env_manager/LICENSE | 202 ++++++++++++++++++ .../include/env_manager/env_manager.hpp | 87 ++++++++ env_manager/env_manager/package.xml | 22 ++ env_manager/env_manager/src/env_manager.cpp | 167 +++++++++++++++ .../env_manager/src/run_env_manager.cpp | 32 +++ .../env_manager_interfaces/CMakeLists.txt | 36 ++++ env_manager/env_manager_interfaces/LICENSE | 202 ++++++++++++++++++ .../env_manager_interfaces/msg/EnvState.msg | 4 + .../env_manager_interfaces/package.xml | 27 +++ .../srv/ConfigureEnv.srv | 3 + .../env_manager_interfaces/srv/LoadEnv.srv | 4 + .../env_manager_interfaces/srv/UnloadEnv.srv | 3 + env_manager/gz_enviroment/CMakeLists.txt | 26 +++ env_manager/gz_enviroment/LICENSE | 202 ++++++++++++++++++ .../include/gz_enviroment/gz_enviroment.hpp | 16 ++ env_manager/gz_enviroment/package.xml | 18 ++ .../gz_enviroment/src/gz_enviroment.cpp | 0 .../planning_scene_manager/CMakeLists.txt | 26 +++ env_manager/planning_scene_manager/LICENSE | 202 ++++++++++++++++++ .../planning_scene_manager.hpp | 29 +++ .../planning_scene_manager/package.xml | 18 ++ .../src/planning_scene_manager.cpp | 31 +++ rbs_bringup/launch/bringup.launch.py | 2 +- rbs_bt_executor/CMakeLists.txt | 8 +- rbs_simulation/launch/get_urdf.launch.py | 2 +- 26 files changed, 1439 insertions(+), 6 deletions(-) create mode 100644 env_manager/env_manager/CMakeLists.txt create mode 100644 env_manager/env_manager/LICENSE create mode 100644 env_manager/env_manager/include/env_manager/env_manager.hpp create mode 100644 env_manager/env_manager/package.xml create mode 100644 env_manager/env_manager/src/env_manager.cpp create mode 100644 env_manager/env_manager/src/run_env_manager.cpp create mode 100644 env_manager/env_manager_interfaces/CMakeLists.txt create mode 100644 env_manager/env_manager_interfaces/LICENSE create mode 100644 env_manager/env_manager_interfaces/msg/EnvState.msg create mode 100644 env_manager/env_manager_interfaces/package.xml create mode 100644 env_manager/env_manager_interfaces/srv/ConfigureEnv.srv create mode 100644 env_manager/env_manager_interfaces/srv/LoadEnv.srv create mode 100644 env_manager/env_manager_interfaces/srv/UnloadEnv.srv create mode 100644 env_manager/gz_enviroment/CMakeLists.txt create mode 100644 env_manager/gz_enviroment/LICENSE create mode 100644 env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp create mode 100644 env_manager/gz_enviroment/package.xml create mode 100644 env_manager/gz_enviroment/src/gz_enviroment.cpp create mode 100644 env_manager/planning_scene_manager/CMakeLists.txt create mode 100644 env_manager/planning_scene_manager/LICENSE create mode 100644 env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp create mode 100644 env_manager/planning_scene_manager/package.xml create mode 100644 env_manager/planning_scene_manager/src/planning_scene_manager.cpp diff --git a/env_manager/env_manager/CMakeLists.txt b/env_manager/env_manager/CMakeLists.txt new file mode 100644 index 0000000..36c88ed --- /dev/null +++ b/env_manager/env_manager/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(env_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(INCLUDE_DEPENDS + rclcpp + rclcpp_lifecycle + env_manager_interfaces + pluginlib +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dep IN ITEMS ${INCLUDE_DEPENDS}) + find_package(${Dep} REQUIRED) +endforeach() +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + + + +add_library(${PROJECT_NAME} SHARED + src/env_manager.cpp +) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${INCLUDE_DEPENDS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIRONMENT_MANAGER_BUILDING_DLL") + +add_executable(run_env_manager src/run_env_manager.cpp) +target_include_directories(run_env_manager PRIVATE include) +target_link_libraries(run_env_manager ${PROJECT_NAME}) +ament_target_dependencies(run_env_manager ${INCLUDE_DEPENDS}) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +install(TARGETS run_env_manager + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_include_directories( + include +) +ament_export_dependencies( + ${INCLUDE_DEPENDS} +) + +ament_package() diff --git a/env_manager/env_manager/LICENSE b/env_manager/env_manager/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/env_manager/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp new file mode 100644 index 0000000..cf8db26 --- /dev/null +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -0,0 +1,87 @@ +#ifndef __ENV_MANAGER_HPP__ +#define __ENV_MANAGER_HPP__ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "env_manager_interfaces/srv/load_env.hpp" +#include "env_manager_interfaces/srv/configure_env.hpp" +#include "env_manager_interfaces/srv/unload_env.hpp" + +#include "pluginlib/class_loader.hpp" + + +namespace env_manager +{ +using EnvInterfaceSharedPtr = std::shared_ptr; +using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +struct EnvSpec +{ + std::string name; + std::string type; + EnvInterfaceSharedPtr env_ptr; +}; + +class EnvManager : public rclcpp::Node +{ +public: + // EnvManager(rclcpp::Executor::SharedPtr executor, + // const std::string & json_config_path, + // const std::string & node_name = "env_manager", + // const std::string & node_namespace = "", + // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() + // .allow_undeclared_parameters(true) + // .automatically_declare_parameters_from_overrides(true)); + EnvManager(rclcpp::Executor::SharedPtr executor, + const std::string & node_name = "env_manager", + const std::string & node_namespace = "", + rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + virtual ~EnvManager() = default; + + + // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); + EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); + + EnvStateReturnType configureEnv(const std::string& env_name); + + EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); + + +protected: + void initServices(); + rclcpp::Node::SharedPtr getNode(); + void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); + + void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); + + void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); + + + +private: + rclcpp::Node::SharedPtr m_node; + std::mutex m_env_mutex; + std::vector m_active_envs; + rclcpp::Service::SharedPtr + m_load_env_srv; + rclcpp::Service::SharedPtr + m_configure_env_srv; + rclcpp::Service::SharedPtr + m_unload_env_srv; + + + rclcpp::CallbackGroup::SharedPtr m_callback_group; + rclcpp::Executor::SharedPtr m_executor; + + std::shared_ptr> m_loader; + + + +}; +} // namespace env_manager +#endif // __ENV_MANAGER_HPP__ \ No newline at end of file diff --git a/env_manager/env_manager/package.xml b/env_manager/env_manager/package.xml new file mode 100644 index 0000000..ca6a9ff --- /dev/null +++ b/env_manager/env_manager/package.xml @@ -0,0 +1,22 @@ + + + + env_manager + 0.0.0 + TODO: Package description + banana + Apache-2.0 + + ament_cmake + + env_manager_interfaces + rclcpp + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp new file mode 100644 index 0000000..dc0f3e8 --- /dev/null +++ b/env_manager/env_manager/src/env_manager.cpp @@ -0,0 +1,167 @@ +#include "env_manager/env_manager.hpp" +#include "nlohmann/json.hpp" + +static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "rclcpp_lifecycle"; +static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "rclcpp_lifecycle::LifecycleNode"; + +namespace env_manager +{ +EnvManager::EnvManager( + rclcpp::Executor::SharedPtr executor, + const std::string & node_name, + const std::string & node_namespace, + rclcpp::NodeOptions & options) + : rclcpp::Node(node_name, node_namespace, options), + m_executor(executor), + m_loader(std::make_shared>( + ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) +{ + initServices(); +} + +// EnvManager::EnvManager( +// rclcpp::Executor::SharedPtr executor, +// const std::string & json_config_path, +// const std::string & node_name, +// const std::string & node_namespace, +// rclcpp::NodeOptions & options) +// : rclcpp::Node(node_name, node_namespace, options), +// m_executor(executor), +// m_loader(std::make_shared>( +// ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) +// { +// initServices(); +// } + + + +void +EnvManager::initServices() +{ + using namespace std::placeholders; + + m_load_env_srv = create_service + ("~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); + m_configure_env_srv = create_service + ("~/configure_env", std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); + m_unload_env_srv = create_service + ("~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); +} + +rclcpp::Node::SharedPtr +EnvManager::getNode() +{ + return m_node; +} + +void +EnvManager::loadEnv_cb( + const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) +{ + std::lock_guard lock(m_env_mutex); // Добавьте эту строку + response->ok = loadEnv(request->name, request->type).get() != nullptr; +} + +void +EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) +{ + std::lock_guard guard(m_env_mutex); + response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; +} + +EnvInterfaceSharedPtr +EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_type) +{ + EnvInterfaceSharedPtr loaded_env = nullptr; + RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); + try + { + if (m_loader->isClassAvailable(env_class_type)) + { + loaded_env = m_loader->createSharedInstance(env_class_type); + } else + { + RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s'", env_name.c_str()); + return nullptr; + } + } + catch (const pluginlib::PluginlibException& e) + { + RCLCPP_ERROR( + get_logger(), "Error happened during creation of enviroment '%s' with type '%s':\n%s", + env_name.c_str(), env_class_type.c_str(), e.what()); + return nullptr; + } + EnvSpec enviroment; + enviroment.name = env_name; + enviroment.type = env_class_type; + enviroment.env_ptr = loaded_env; + return addEnv(enviroment); +} + +// EnvInterfaceSharedPtr +// EnvManager::loadEnv(const std::string env_name) +// { +// std::string env_class_type = "some_default_namespace::" + env_name; + +// return loadEnv(env_name, env_class_type); +// } + +EnvStateReturnType +EnvManager::configureEnv(const std::string& env_name) +{ + std::lock_guard guard(m_env_mutex); + + auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { + return env.name == env_name; + }); + + if (it == m_active_envs.end()) { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::FAILURE; + } + + return EnvStateReturnType::SUCCESS; +} + +void +EnvManager::unloadEnv_cb( + const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) +{ + std::lock_guard lock(m_env_mutex); + + auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&request](const EnvSpec& env) { + return env.name == request->name; + }); + + if (it != m_active_envs.end()) { + m_executor->remove_node(it->env_ptr->get_node_base_interface()); + m_active_envs.erase(it); + response->ok = true; + } else { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + request->name.c_str()); + response->ok = false; + } +} + + +EnvInterfaceSharedPtr +EnvManager::addEnv(const EnvSpec& enviroment) +{ + std::lock_guard lock(m_env_mutex); + // generate list of active enviroments + // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size()); + // m_active_envs[unique_env_name] = enviroment.env_ptr; + m_active_envs.push_back(enviroment); + m_executor->add_node(enviroment.env_ptr->get_node_base_interface()); + return enviroment.env_ptr; +} + +} \ No newline at end of file diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp new file mode 100644 index 0000000..488807f --- /dev/null +++ b/env_manager/env_manager/src/run_env_manager.cpp @@ -0,0 +1,32 @@ +#include "rclcpp/rclcpp.hpp" +#include "env_manager/env_manager.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + std::shared_ptr executor = + std::make_shared(); + std::string manager_node_name = "env_manager"; + + auto em = std::make_shared(executor, manager_node_name); + + RCLCPP_INFO(em->get_logger(), "Env manager is starting"); + + std::thread em_thread( + [em]() + { + em->loadEnv("gz_enviroment", "env_manager::GzEnviroment"); + // while (rclcpp::ok()) + // { + // RCLCPP_INFO(em->get_logger(), "Env manager is running"); + // } + + } + ); + + executor->add_node(em); + executor->spin(); + em_thread.join(); + rclcpp::shutdown(); + return 0; +} diff --git a/env_manager/env_manager_interfaces/CMakeLists.txt b/env_manager/env_manager_interfaces/CMakeLists.txt new file mode 100644 index 0000000..3bf6ce3 --- /dev/null +++ b/env_manager/env_manager_interfaces/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(env_manager_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + srv/ConfigureEnv.srv + srv/LoadEnv.srv + srv/UnloadEnv.srv + msg/EnvState.msg + DEPENDENCIES lifecycle_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/env_manager/env_manager_interfaces/LICENSE b/env_manager/env_manager_interfaces/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/env_manager_interfaces/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/env_manager/env_manager_interfaces/msg/EnvState.msg b/env_manager/env_manager_interfaces/msg/EnvState.msg new file mode 100644 index 0000000..fa4c82e --- /dev/null +++ b/env_manager/env_manager_interfaces/msg/EnvState.msg @@ -0,0 +1,4 @@ +string name +string type +string plugin_name +lifecycle_msgs/State state diff --git a/env_manager/env_manager_interfaces/package.xml b/env_manager/env_manager_interfaces/package.xml new file mode 100644 index 0000000..4fb3920 --- /dev/null +++ b/env_manager/env_manager_interfaces/package.xml @@ -0,0 +1,27 @@ + + + + env_manager_interfaces + 0.0.0 + TODO: Package description + solid-sinusoid + Apache-2.0 + + ament_cmake + rosidl_default_generators + + builtin_interfaces + lifecycle_msgs + + builtin_interfaces + lifecycle_msgs + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + ament_cmake + + diff --git a/env_manager/env_manager_interfaces/srv/ConfigureEnv.srv b/env_manager/env_manager_interfaces/srv/ConfigureEnv.srv new file mode 100644 index 0000000..4292382 --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/ConfigureEnv.srv @@ -0,0 +1,3 @@ +string name +--- +bool ok \ No newline at end of file diff --git a/env_manager/env_manager_interfaces/srv/LoadEnv.srv b/env_manager/env_manager_interfaces/srv/LoadEnv.srv new file mode 100644 index 0000000..d6d46cc --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/LoadEnv.srv @@ -0,0 +1,4 @@ +string name +string type +--- +bool ok \ No newline at end of file diff --git a/env_manager/env_manager_interfaces/srv/UnloadEnv.srv b/env_manager/env_manager_interfaces/srv/UnloadEnv.srv new file mode 100644 index 0000000..4292382 --- /dev/null +++ b/env_manager/env_manager_interfaces/srv/UnloadEnv.srv @@ -0,0 +1,3 @@ +string name +--- +bool ok \ No newline at end of file diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt new file mode 100644 index 0000000..2d0c543 --- /dev/null +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(gz_enviroment) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/env_manager/gz_enviroment/LICENSE b/env_manager/gz_enviroment/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/gz_enviroment/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp new file mode 100644 index 0000000..eaf5c1b --- /dev/null +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -0,0 +1,16 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "ros_gz_bridge/convert.hpp" + +namespace env_manager +{ +class GzEnviroment : public rclcpp_lifecycle::LifecycleNode +{ +public: + +private: + + +}; +} + diff --git a/env_manager/gz_enviroment/package.xml b/env_manager/gz_enviroment/package.xml new file mode 100644 index 0000000..b906232 --- /dev/null +++ b/env_manager/gz_enviroment/package.xml @@ -0,0 +1,18 @@ + + + + gz_enviroment + 0.0.0 + TODO: Package description + bill-finger + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp new file mode 100644 index 0000000..e69de29 diff --git a/env_manager/planning_scene_manager/CMakeLists.txt b/env_manager/planning_scene_manager/CMakeLists.txt new file mode 100644 index 0000000..4a46988 --- /dev/null +++ b/env_manager/planning_scene_manager/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(planning_scene_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/env_manager/planning_scene_manager/LICENSE b/env_manager/planning_scene_manager/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/planning_scene_manager/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp b/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp new file mode 100644 index 0000000..a3023e8 --- /dev/null +++ b/env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp @@ -0,0 +1,29 @@ +#ifndef __PLANNING_SCENE_MANAGER_H__ +#define __PLANNING_SCENE_MANAGER_H__ + +#include "rclcpp/rclcpp.hpp" +#include "moveit/planning_scene_monitor/planning_scene_monitor.h" + +namespace planning_scene_manager +{ +class PlanningSceneManager : public planning_scene_monitor::PlanningSceneMonitor +{ +public: + PlanningSceneManager(); + + bool GetPlanningObject(); + bool SetPlanningObject(); + + bool SetPlanningScene(); + bool GetPlanningScene(); + + bool UpdatePlanningScene(); + +private: + +}; + + +} // namespace planning_scene_manager + +#endif // __PLANNING_SCENE_MANAGER_H__ \ No newline at end of file diff --git a/env_manager/planning_scene_manager/package.xml b/env_manager/planning_scene_manager/package.xml new file mode 100644 index 0000000..ea47535 --- /dev/null +++ b/env_manager/planning_scene_manager/package.xml @@ -0,0 +1,18 @@ + + + + planning_scene_manager + 0.0.0 + TODO: Package description + Bill Finger + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/planning_scene_manager/src/planning_scene_manager.cpp b/env_manager/planning_scene_manager/src/planning_scene_manager.cpp new file mode 100644 index 0000000..8c9ab4a --- /dev/null +++ b/env_manager/planning_scene_manager/src/planning_scene_manager.cpp @@ -0,0 +1,31 @@ +#include "planning_scene_manager/planning_scene_manager.hpp" +#include "moveit_msgs/msg/collision_object.hpp" +#include "moveit/planning_scene_interface/planning_scene_interface.h" + +namespace planning_scene_manager +{ + +bool PlanningSceneManager::SetPlanningObject() +{ + moveit_msgs::msg::CollisionObject object; + std::shared_ptr pcm_; + planning_scene_monitor::LockedPlanningSceneRW scene(pcm_); + scene->removeAllCollisionObjects(); + RCLCPP_INFO(pnode_->get_logger(), "All object has been delete"); + return true; +} + +} // namespace planning_scene_manager + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // Добавляем объект в сцену коллизии + node->SetPlanningObject(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py index eeb1c1b..14d78ff 100644 --- a/rbs_bringup/launch/bringup.launch.py +++ b/rbs_bringup/launch/bringup.launch.py @@ -141,7 +141,7 @@ def generate_launch_description(): DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers") ) declared_arguments.append( - DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?") + DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?") ) declared_arguments.append( DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg") diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt index 165ac0c..0193f1b 100644 --- a/rbs_bt_executor/CMakeLists.txt +++ b/rbs_bt_executor/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(ament_index_cpp REQUIRED) find_package(rbs_skill_interfaces REQUIRED) find_package(behavior_tree REQUIRED) find_package(control_msgs REQUIRED) -find_package(component_interfaces REQUIRED) +# find_package(component_interfaces REQUIRED) if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -38,7 +38,7 @@ set(dependencies behavior_tree std_msgs control_msgs - component_interfaces + # component_interfaces ) include_directories(include) @@ -61,8 +61,8 @@ list(APPEND plugin_libs rbs_add_planning_scene_object) add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp) list(APPEND plugin_libs rbs_assemble_process_state) -add_library(rbs_detect_object SHARED src/DetectObject.cpp) -list(APPEND plugin_libs rbs_detect_object) +# add_library(rbs_detect_object SHARED src/DetectObject.cpp) +# list(APPEND plugin_libs rbs_detect_object) foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) diff --git a/rbs_simulation/launch/get_urdf.launch.py b/rbs_simulation/launch/get_urdf.launch.py index 52f61d6..d034a8d 100644 --- a/rbs_simulation/launch/get_urdf.launch.py +++ b/rbs_simulation/launch/get_urdf.launch.py @@ -20,7 +20,7 @@ def generate_launch_description(): "prefix": "", "sim_mujoco": "true", "simulation_controllers": str(initial_joint_controllers_file_path), - "with_gripper": "false" + "with_gripper": "true" }) robot_desc = doc.toprettyxml(indent=' ') From 49660eb9266d38c52342134da62e68b46b60fd23 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sun, 17 Sep 2023 15:19:05 +0300 Subject: [PATCH 3/7] add gz_enviroment --- .../env_manager/env_base_interface.hpp | 23 ++++++++ env_manager/env_manager/src/env_manager.cpp | 11 +++- .../env_manager/src/run_env_manager.cpp | 2 +- env_manager/gz_enviroment/CMakeLists.txt | 42 ++++++++++++++ env_manager/gz_enviroment/gz_enviroment.xml | 7 +++ .../include/gz_enviroment/gz_enviroment.hpp | 26 ++++++++- env_manager/gz_enviroment/package.xml | 6 ++ .../gz_enviroment/src/gz_enviroment.cpp | 56 +++++++++++++++++++ 8 files changed, 168 insertions(+), 5 deletions(-) create mode 100644 env_manager/env_manager/include/env_manager/env_base_interface.hpp create mode 100644 env_manager/gz_enviroment/gz_enviroment.xml diff --git a/env_manager/env_manager/include/env_manager/env_base_interface.hpp b/env_manager/env_manager/include/env_manager/env_base_interface.hpp new file mode 100644 index 0000000..d10e92b --- /dev/null +++ b/env_manager/env_manager/include/env_manager/env_base_interface.hpp @@ -0,0 +1,23 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + + +namespace env_interface +{ + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class EnvBaseIntrerface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +{ +public: + EnvBaseIntrerface() = default; + + virtual ~EnvBaseIntrerface() = default; + + virtual CallbackReturn on_init() = 0; + + + + +}; +} // namespace env_interface diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index dc0f3e8..de222aa 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/env_manager/env_manager/src/env_manager.cpp @@ -81,9 +81,16 @@ EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_ty if (m_loader->isClassAvailable(env_class_type)) { loaded_env = m_loader->createSharedInstance(env_class_type); - } else + } + else { - RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s'", env_name.c_str()); + RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s' with class type '%s'", + env_name.c_str(), env_class_type.c_str()); + RCLCPP_INFO(this->get_logger(), "Available classes"); + for (const auto& available_class : m_loader->getDeclaredClasses()) + { + RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str()); + } return nullptr; } } diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp index 488807f..f28ed98 100644 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ b/env_manager/env_manager/src/run_env_manager.cpp @@ -15,7 +15,7 @@ int main(int argc, char ** argv) std::thread em_thread( [em]() { - em->loadEnv("gz_enviroment", "env_manager::GzEnviroment"); + em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); // while (rclcpp::ok()) // { // RCLCPP_INFO(em->get_logger(), "Env manager is running"); diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index 2d0c543..0584003 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -5,11 +5,39 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclcpp_lifecycle + tf2_ros + tf2_msgs + ros_gz + pluginlib + ignition-transport11 + ignition-msgs8 +) + # find dependencies find_package(ament_cmake REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# TODO: generate_parameter_library + + + +add_library(${PROJECT_NAME} SHARED src/gz_enviroment.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "GZENVIROMENT_BUILDING_DLL") +#TODO: replace rclcpp_lifecycle with custom interface class +pluginlib_export_plugin_description_file(rclcpp_lifecycle gz_enviroment.xml) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -23,4 +51,18 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/env_manager/gz_enviroment/gz_enviroment.xml b/env_manager/gz_enviroment/gz_enviroment.xml new file mode 100644 index 0000000..3729aa0 --- /dev/null +++ b/env_manager/gz_enviroment/gz_enviroment.xml @@ -0,0 +1,7 @@ + + + Gazebo enviroment for env_manager + + \ No newline at end of file diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index eaf5c1b..0f2b3ec 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -1,14 +1,36 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "ros_gz_bridge/convert.hpp" +// #include "ros_gz_bridge/convert.hpp" +#include +#include +#include +#include +#include "gz/msgs/pose_v.pb.h" -namespace env_manager +namespace gz_enviroment { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + class GzEnviroment : public rclcpp_lifecycle::LifecycleNode { public: + GzEnviroment(); + CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; + + +protected: + std::unique_ptr m_tf2_broadcaster; + std::shared_ptr m_gz_node; + + std::vector m_follow_frames; + std::string m_topic_name = "/world/mir/dynamic_pose/info"; private: + void onGzPoseSub(const gz::msgs::Pose_V& pose_v); + }; diff --git a/env_manager/gz_enviroment/package.xml b/env_manager/gz_enviroment/package.xml index b906232..e08ac80 100644 --- a/env_manager/gz_enviroment/package.xml +++ b/env_manager/gz_enviroment/package.xml @@ -9,6 +9,12 @@ ament_cmake + rclcpp + rclcpp_lifecycle + tf2_ros + tf2_msgs + ros_gz + ament_lint_auto ament_lint_common diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index e69de29..cde3079 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -0,0 +1,56 @@ +#include "gz_enviroment/gz_enviroment.hpp" + +namespace gz_enviroment +{ + +GzEnviroment::GzEnviroment() +: rclcpp_lifecycle::LifecycleNode("gz_enviroment") {} + +CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) +{ + RCLCPP_INFO(this->get_logger(), "GzEnvirometn is on_configure"); + m_tf2_broadcaster = std::make_unique(this); + m_gz_node = std::make_shared(); + m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_cleanup"); + + m_tf2_broadcaster.reset(); + m_gz_node.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_activate"); + + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_deactivate"); + + + return CallbackReturn::SUCCESS; +} + +void onGzPoseSub(const gz::msgs::Pose_V& pose_v) +{ + +} + + +} + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + gz_enviroment::GzEnviroment, rclcpp_lifecycle::LifecycleNode +); \ No newline at end of file From 48e17c2481a9032e7113ee1e6b1cb13c9fea06ca Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 26 Sep 2023 23:18:28 +0300 Subject: [PATCH 4/7] add gz env --- .../env_manager/env_base_interface.hpp | 2 + .../include/env_manager/env_manager.hpp | 13 ++- env_manager/env_manager/src/env_manager.cpp | 78 +++++++-------- .../env_manager/src/run_env_manager.cpp | 14 +-- env_manager/gz_enviroment/CMakeLists.txt | 1 + .../include/gz_enviroment/gz_enviroment.hpp | 11 ++- .../gz_enviroment/src/gz_enviroment.cpp | 94 ++++++++++++++++--- env_manager/gz_enviroment/test.json | 0 8 files changed, 145 insertions(+), 68 deletions(-) create mode 100644 env_manager/gz_enviroment/test.json diff --git a/env_manager/env_manager/include/env_manager/env_base_interface.hpp b/env_manager/env_manager/include/env_manager/env_base_interface.hpp index d10e92b..355f0e5 100644 --- a/env_manager/env_manager/include/env_manager/env_base_interface.hpp +++ b/env_manager/env_manager/include/env_manager/env_base_interface.hpp @@ -1,6 +1,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +// TODO: BaseInterface class for next enviroments based on LifecycleNodeInterface +// It's should be like ControllerInterfaceBase class from ros2_control namespace env_interface { diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp index cf8db26..d821699 100644 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -43,11 +43,20 @@ public: // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); - + EnvInterfaceSharedPtr unloadEnv(); EnvStateReturnType configureEnv(const std::string& env_name); - EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); + //TODO: Define the input data format + // Set Target for RL enviroment + EnvInterfaceSharedPtr setTarget(); + EnvInterfaceSharedPtr switchTarget(); + EnvInterfaceSharedPtr unsetTarget(); + // Load Constraints for RL enviroment + EnvInterfaceSharedPtr loadConstraints(); + EnvInterfaceSharedPtr switchConstraints(); + EnvInterfaceSharedPtr unloadConstraints(); + protected: void initServices(); diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index de222aa..50159bf 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/env_manager/env_manager/src/env_manager.cpp @@ -59,7 +59,7 @@ EnvManager::loadEnv_cb( const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) { - std::lock_guard lock(m_env_mutex); // Добавьте эту строку + std::lock_guard lock(m_env_mutex); response->ok = loadEnv(request->name, request->type).get() != nullptr; } @@ -71,6 +71,43 @@ EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Req response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; } +void +EnvManager::unloadEnv_cb( + const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) +{ + std::lock_guard lock(m_env_mutex); + + auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&request](const EnvSpec& env) { + return env.name == request->name; + }); + + if (it != m_active_envs.end()) { + m_executor->remove_node(it->env_ptr->get_node_base_interface()); + m_active_envs.erase(it); + response->ok = true; + } else { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + request->name.c_str()); + response->ok = false; + } +} + + +EnvInterfaceSharedPtr +EnvManager::addEnv(const EnvSpec& enviroment) +{ + std::lock_guard lock(m_env_mutex); + // generate list of active enviroments + // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size()); + // m_active_envs[unique_env_name] = enviroment.env_ptr; + m_active_envs.push_back(enviroment); + m_executor->add_node(enviroment.env_ptr->get_node_base_interface()); + return enviroment.env_ptr; +} + + EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_type) { @@ -131,44 +168,9 @@ EnvManager::configureEnv(const std::string& env_name) env_name.c_str()); return EnvStateReturnType::FAILURE; } - + it->env_ptr->configure(); + // it->env_ptr->activate(); return EnvStateReturnType::SUCCESS; } -void -EnvManager::unloadEnv_cb( - const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) -{ - std::lock_guard lock(m_env_mutex); - - auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&request](const EnvSpec& env) { - return env.name == request->name; - }); - - if (it != m_active_envs.end()) { - m_executor->remove_node(it->env_ptr->get_node_base_interface()); - m_active_envs.erase(it); - response->ok = true; - } else { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - request->name.c_str()); - response->ok = false; - } -} - - -EnvInterfaceSharedPtr -EnvManager::addEnv(const EnvSpec& enviroment) -{ - std::lock_guard lock(m_env_mutex); - // generate list of active enviroments - // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size()); - // m_active_envs[unique_env_name] = enviroment.env_ptr; - m_active_envs.push_back(enviroment); - m_executor->add_node(enviroment.env_ptr->get_node_base_interface()); - return enviroment.env_ptr; -} - } \ No newline at end of file diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp index f28ed98..419e409 100644 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ b/env_manager/env_manager/src/run_env_manager.cpp @@ -12,21 +12,11 @@ int main(int argc, char ** argv) RCLCPP_INFO(em->get_logger(), "Env manager is starting"); - std::thread em_thread( - [em]() - { - em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); - // while (rclcpp::ok()) - // { - // RCLCPP_INFO(em->get_logger(), "Env manager is running"); - // } - - } - ); + em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); + em->configureEnv("gz_enviroment"); executor->add_node(em); executor->spin(); - em_thread.join(); rclcpp::shutdown(); return 0; } diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index 0584003..1aa81a5 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib ignition-transport11 ignition-msgs8 + ros_gz_bridge ) # find dependencies diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index 0f2b3ec..4730cb4 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -23,15 +23,18 @@ public: protected: + + +private: std::unique_ptr m_tf2_broadcaster; std::shared_ptr m_gz_node; - std::vector m_follow_frames; - std::string m_topic_name = "/world/mir/dynamic_pose/info"; -private: + void onGzPoseSub(const gz::msgs::Pose_V& pose_v); - + void doGzSpawn(); + + std::string getGzWorldName(); }; } diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index cde3079..35b50a1 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -1,22 +1,23 @@ #include "gz_enviroment/gz_enviroment.hpp" - +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "ros_gz_bridge/convert.hpp" namespace gz_enviroment { GzEnviroment::GzEnviroment() : rclcpp_lifecycle::LifecycleNode("gz_enviroment") {} -CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) +CallbackReturn +GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) { - RCLCPP_INFO(this->get_logger(), "GzEnvirometn is on_configure"); - m_tf2_broadcaster = std::make_unique(this); - m_gz_node = std::make_shared(); - m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); + RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_configure"); + return CallbackReturn::SUCCESS; } -CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) +CallbackReturn +GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_cleanup"); @@ -26,15 +27,23 @@ CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) return CallbackReturn::SUCCESS; } -CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) +CallbackReturn +GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_activate"); - + m_tf2_broadcaster = std::make_unique(this); + m_gz_node = std::make_shared(); + + std::string m_topic_name = "/world/" + getGzWorldName() +"/dynamic_pose/info"; + std::string m_service_spawn = "/world/" + getGzWorldName() + "/create"; + + m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); return CallbackReturn::SUCCESS; } -CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) +CallbackReturn +GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_deactivate"); @@ -42,11 +51,72 @@ CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) return CallbackReturn::SUCCESS; } -void onGzPoseSub(const gz::msgs::Pose_V& pose_v) +void +GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) { - + m_follow_frames = {"box1", "box2", "box3", "box4"}; + for (const auto &it: pose_v.pose()) + { + if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) + continue; + + geometry_msgs::msg::PoseStamped rpose; + ros_gz_bridge::convert_gz_to_ros(it, rpose); + + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = it.name(); + + t.transform.translation.x = rpose.pose.position.x; + t.transform.translation.y = rpose.pose.position.y; + t.transform.translation.z = rpose.pose.position.z; + + t.transform.rotation.x = rpose.pose.orientation.x; + t.transform.rotation.y = rpose.pose.orientation.y; + t.transform.rotation.z = rpose.pose.orientation.z; + t.transform.rotation.w = rpose.pose.orientation.w; + + m_tf2_broadcaster->sendTransform(t); + } } +std::string +GzEnviroment::getGzWorldName() +{ + bool executed{false}; + bool result{false}; + unsigned int timeout{5000}; + std::string service{"/gazebo/worlds"}; + gz::msgs::StringMsg_V worlds_msg; + + while (rclcpp::ok() && !executed) { + RCLCPP_INFO(this->get_logger(), "Requesting list of world names."); + executed = m_gz_node->Request(service, timeout, worlds_msg, result); + } + + if (!executed) { + RCLCPP_INFO(this->get_logger(), "Timed out when getting world names."); + return ""; + } + + if (!result || worlds_msg.data().empty()) { + RCLCPP_INFO(this->get_logger(), "Failed to get world names."); + return ""; + } + RCLCPP_INFO(this->get_logger(), "%s", worlds_msg.data(0).c_str()); + return worlds_msg.data(0); +} + +void +GzEnviroment::doGzSpawn() +{ + gz::msgs::EntityFactory entity; + gz::msgs::Model model; + gz::msgs::Link link; + +} } diff --git a/env_manager/gz_enviroment/test.json b/env_manager/gz_enviroment/test.json new file mode 100644 index 0000000..e69de29 From 71063b19aa297da3a5d1a858254a26cafdb832ed Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sun, 15 Oct 2023 16:24:30 +0300 Subject: [PATCH 5/7] enviroment base interface class --- .../env_manager/env_base_interface.hpp | 26 ++++++-- .../env_manager/src/env_base_interface.cpp | 63 +++++++++++++++++++ 2 files changed, 84 insertions(+), 5 deletions(-) create mode 100644 env_manager/env_manager/src/env_base_interface.cpp diff --git a/env_manager/env_manager/include/env_manager/env_base_interface.hpp b/env_manager/env_manager/include/env_manager/env_base_interface.hpp index 355f0e5..e08e4a3 100644 --- a/env_manager/env_manager/include/env_manager/env_base_interface.hpp +++ b/env_manager/env_manager/include/env_manager/env_base_interface.hpp @@ -1,5 +1,5 @@ #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" // TODO: BaseInterface class for next enviroments based on LifecycleNodeInterface // It's should be like ControllerInterfaceBase class from ros2_control @@ -9,16 +9,32 @@ namespace env_interface using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class EnvBaseIntrerface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - EnvBaseIntrerface() = default; + EnvInterfaceBase() = default; - virtual ~EnvBaseIntrerface() = default; + virtual ~EnvInterfaceBase() = default; + + virtual CallbackReturn init( + const std::string& t_env_name, + const std::string & t_namespace = "", + const rclcpp::NodeOptions & t_node_options = rclcpp::NodeOptions() + ); + + const rclcpp_lifecycle::State& configure(); virtual CallbackReturn on_init() = 0; - + std::shared_ptr getNode(); + std::shared_ptr getNode() const; + + const rclcpp_lifecycle::State & getState() const; + + + +private: + std::shared_ptr m_node; }; diff --git a/env_manager/env_manager/src/env_base_interface.cpp b/env_manager/env_manager/src/env_base_interface.cpp new file mode 100644 index 0000000..8b2dd68 --- /dev/null +++ b/env_manager/env_manager/src/env_base_interface.cpp @@ -0,0 +1,63 @@ +#include "env_manager/env_base_interface.hpp" + +namespace env_interface +{ + +CallbackReturn +EnvInterfaceBase::init( + const std::string& t_env_name, + const std::string & t_namespace, + const rclcpp::NodeOptions & t_node_options) +{ + m_node = std::make_shared( + t_env_name, t_namespace, t_node_options, false); + + if (on_init() == CallbackReturn::FAILURE) + { + return CallbackReturn::FAILURE; + } + + m_node->register_on_configure( + std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1) + ); + m_node->register_on_activate( + std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1) + ); + m_node->register_on_cleanup( + std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1) + ); + m_node->register_on_deactivate( + std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1) + ); + m_node->register_on_error( + std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1) + ); + m_node->register_on_shutdown( + std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1) + ); + + return CallbackReturn::SUCCESS; +} + +const rclcpp_lifecycle::State& +EnvInterfaceBase::configure() +{ + return getNode()->configure(); +} + +std::shared_ptr +EnvInterfaceBase::getNode() +{ + if (!m_node.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet"); + } + return m_node; +} + +const rclcpp_lifecycle::State & EnvInterfaceBase::getState() const +{ + return m_node->get_current_state(); +} + +} // namespace env_interface From 18d2772cefa8ae68df40676a19be3acfed96355f Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 17 Oct 2023 15:41:04 +0300 Subject: [PATCH 6/7] adapted the environment for EnvInterfaceBase --- env_manager/env_interface/CMakeLists.txt | 56 +++++ env_manager/env_interface/LICENSE | 202 ++++++++++++++++++ .../include/env_interface/env_interface.hpp | 14 ++ .../env_interface/env_interface_base.hpp} | 3 - env_manager/env_interface/package.xml | 18 ++ .../src/env_interface_base.cpp} | 2 +- env_manager/env_manager/CMakeLists.txt | 1 + .../env_manager/config/test_params.yaml | 9 + .../include/env_manager/env_manager.hpp | 9 +- env_manager/env_manager/package.xml | 1 + env_manager/env_manager/src/env_manager.cpp | 55 +++-- env_manager/gz_enviroment/CMakeLists.txt | 3 +- env_manager/gz_enviroment/gz_enviroment.xml | 4 +- .../include/gz_enviroment/gz_enviroment.hpp | 15 +- env_manager/gz_enviroment/package.xml | 1 + .../gz_enviroment/src/gz_enviroment.cpp | 126 +++++++---- rbs_simulation/launch/simulation.launch.py | 2 +- 17 files changed, 447 insertions(+), 74 deletions(-) create mode 100644 env_manager/env_interface/CMakeLists.txt create mode 100644 env_manager/env_interface/LICENSE create mode 100644 env_manager/env_interface/include/env_interface/env_interface.hpp rename env_manager/{env_manager/include/env_manager/env_base_interface.hpp => env_interface/include/env_interface/env_interface_base.hpp} (86%) create mode 100644 env_manager/env_interface/package.xml rename env_manager/{env_manager/src/env_base_interface.cpp => env_interface/src/env_interface_base.cpp} (97%) create mode 100644 env_manager/env_manager/config/test_params.yaml diff --git a/env_manager/env_interface/CMakeLists.txt b/env_manager/env_interface/CMakeLists.txt new file mode 100644 index 0000000..867b8e1 --- /dev/null +++ b/env_manager/env_interface/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(env_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(INCLUDE_DEPENDS + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN LISTS INCLUDE_DEPENDS) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} SHARED + src/env_interface_base.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${INCLUDE_DEPENDS}) + +target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIROMENT_INTERFACE_BUILDING_DLL") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY include/ + DESTINATION include/env_interface +) +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${INCLUDE_DEPENDS}) + +ament_package() diff --git a/env_manager/env_interface/LICENSE b/env_manager/env_interface/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/env_manager/env_interface/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/env_manager/env_interface/include/env_interface/env_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface.hpp new file mode 100644 index 0000000..3a69802 --- /dev/null +++ b/env_manager/env_interface/include/env_interface/env_interface.hpp @@ -0,0 +1,14 @@ +#include "env_interface/env_interface_base.hpp" + +namespace env_interface +{ + class EnvInterface : public EnvInterfaceBase + { + public: + EnvInterface() = default; + virtual ~EnvInterface() = default; + + }; +} // namespace env_interface + + diff --git a/env_manager/env_manager/include/env_manager/env_base_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface_base.hpp similarity index 86% rename from env_manager/env_manager/include/env_manager/env_base_interface.hpp rename to env_manager/env_interface/include/env_interface/env_interface_base.hpp index e08e4a3..b4c0c5f 100644 --- a/env_manager/env_manager/include/env_manager/env_base_interface.hpp +++ b/env_manager/env_interface/include/env_interface/env_interface_base.hpp @@ -1,9 +1,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -// TODO: BaseInterface class for next enviroments based on LifecycleNodeInterface -// It's should be like ControllerInterfaceBase class from ros2_control - namespace env_interface { diff --git a/env_manager/env_interface/package.xml b/env_manager/env_interface/package.xml new file mode 100644 index 0000000..d545986 --- /dev/null +++ b/env_manager/env_interface/package.xml @@ -0,0 +1,18 @@ + + + + env_interface + 0.0.0 + TODO: Package description + bill-finger + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/env_manager/env_manager/src/env_base_interface.cpp b/env_manager/env_interface/src/env_interface_base.cpp similarity index 97% rename from env_manager/env_manager/src/env_base_interface.cpp rename to env_manager/env_interface/src/env_interface_base.cpp index 8b2dd68..5f5f841 100644 --- a/env_manager/env_manager/src/env_base_interface.cpp +++ b/env_manager/env_interface/src/env_interface_base.cpp @@ -1,4 +1,4 @@ -#include "env_manager/env_base_interface.hpp" +#include "env_interface/env_interface_base.hpp" namespace env_interface { diff --git a/env_manager/env_manager/CMakeLists.txt b/env_manager/env_manager/CMakeLists.txt index 36c88ed..5f2b07c 100644 --- a/env_manager/env_manager/CMakeLists.txt +++ b/env_manager/env_manager/CMakeLists.txt @@ -10,6 +10,7 @@ set(INCLUDE_DEPENDS rclcpp_lifecycle env_manager_interfaces pluginlib + env_interface ) # find dependencies diff --git a/env_manager/env_manager/config/test_params.yaml b/env_manager/env_manager/config/test_params.yaml new file mode 100644 index 0000000..8ba46b3 --- /dev/null +++ b/env_manager/env_manager/config/test_params.yaml @@ -0,0 +1,9 @@ +env_manager: + ros__parameters: + gz_enviroment: + type: gz_enviroment/GzEnviroment + + + gz_enviroment: + ros__parameters: + mesh_name: "monkey" \ No newline at end of file diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp index d821699..b6a563d 100644 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -7,12 +7,15 @@ #include "env_manager_interfaces/srv/configure_env.hpp" #include "env_manager_interfaces/srv/unload_env.hpp" +#include "env_interface/env_interface.hpp" + #include "pluginlib/class_loader.hpp" namespace env_manager { -using EnvInterfaceSharedPtr = std::shared_ptr; +using EnvInterface = env_interface::EnvInterface; +using EnvInterfaceSharedPtr = std::shared_ptr; using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; struct EnvSpec @@ -43,7 +46,7 @@ public: // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); - EnvInterfaceSharedPtr unloadEnv(); + EnvStateReturnType unloadEnv(const std::string &env_name); EnvStateReturnType configureEnv(const std::string& env_name); EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); @@ -87,7 +90,7 @@ private: rclcpp::CallbackGroup::SharedPtr m_callback_group; rclcpp::Executor::SharedPtr m_executor; - std::shared_ptr> m_loader; + std::shared_ptr> m_loader; diff --git a/env_manager/env_manager/package.xml b/env_manager/env_manager/package.xml index ca6a9ff..5abbdf0 100644 --- a/env_manager/env_manager/package.xml +++ b/env_manager/env_manager/package.xml @@ -12,6 +12,7 @@ env_manager_interfaces rclcpp rclcpp_lifecycle + env_inteface ament_lint_auto ament_lint_common diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index 50159bf..c69e5d2 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/env_manager/env_manager/src/env_manager.cpp @@ -1,8 +1,8 @@ #include "env_manager/env_manager.hpp" #include "nlohmann/json.hpp" -static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "rclcpp_lifecycle"; -static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "rclcpp_lifecycle::LifecycleNode"; +static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "env_interface"; +static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "env_interface::EnvInterface"; namespace env_manager { @@ -13,7 +13,7 @@ EnvManager::EnvManager( rclcpp::NodeOptions & options) : rclcpp::Node(node_name, node_namespace, options), m_executor(executor), - m_loader(std::make_shared>( + m_loader(std::make_shared>( ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) { initServices(); @@ -77,21 +77,7 @@ EnvManager::unloadEnv_cb( env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) { std::lock_guard lock(m_env_mutex); - - auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&request](const EnvSpec& env) { - return env.name == request->name; - }); - - if (it != m_active_envs.end()) { - m_executor->remove_node(it->env_ptr->get_node_base_interface()); - m_active_envs.erase(it); - response->ok = true; - } else { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - request->name.c_str()); - response->ok = false; - } + response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; } @@ -102,8 +88,16 @@ EnvManager::addEnv(const EnvSpec& enviroment) // generate list of active enviroments // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size()); // m_active_envs[unique_env_name] = enviroment.env_ptr; + if (enviroment.env_ptr->init( + enviroment.name, get_namespace()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) + { + RCLCPP_ERROR(get_logger(), "Enviroment with name %s cannot been initialized", enviroment.name.c_str()); + return nullptr; + } + m_active_envs.push_back(enviroment); - m_executor->add_node(enviroment.env_ptr->get_node_base_interface()); + m_executor->add_node(enviroment.env_ptr->getNode()->get_node_base_interface()); return enviroment.env_ptr; } @@ -145,6 +139,26 @@ EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_ty return addEnv(enviroment); } +EnvStateReturnType +EnvManager::unloadEnv(const std::string& env_name) +{ + auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { + return env.name == env_name; + }); + + if (it != m_active_envs.end()) { + m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); + m_active_envs.erase(it); + return EnvStateReturnType::SUCCESS; + } else { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::ERROR; + } + return EnvStateReturnType::FAILURE; +} + // EnvInterfaceSharedPtr // EnvManager::loadEnv(const std::string env_name) // { @@ -169,7 +183,10 @@ EnvManager::configureEnv(const std::string& env_name) return EnvStateReturnType::FAILURE; } it->env_ptr->configure(); + // it->env_ptr->getNode()->configure(); // it->env_ptr->activate(); + it->env_ptr->getNode()->activate(); + return EnvStateReturnType::SUCCESS; } diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index 1aa81a5..a49bff5 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ignition-transport11 ignition-msgs8 ros_gz_bridge + env_interface ) # find dependencies @@ -38,7 +39,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "GZENVIROMENT_BUILDING_DLL") #TODO: replace rclcpp_lifecycle with custom interface class -pluginlib_export_plugin_description_file(rclcpp_lifecycle gz_enviroment.xml) +pluginlib_export_plugin_description_file(env_interface gz_enviroment.xml) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/env_manager/gz_enviroment/gz_enviroment.xml b/env_manager/gz_enviroment/gz_enviroment.xml index 3729aa0..59d0901 100644 --- a/env_manager/gz_enviroment/gz_enviroment.xml +++ b/env_manager/gz_enviroment/gz_enviroment.xml @@ -1,7 +1,7 @@ - + base_class_type="env_interface::EnvInterface"> Gazebo enviroment for env_manager \ No newline at end of file diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index 4730cb4..3797527 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -6,16 +6,18 @@ #include #include #include "gz/msgs/pose_v.pb.h" +#include "env_interface/env_interface.hpp" namespace gz_enviroment { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class GzEnviroment : public rclcpp_lifecycle::LifecycleNode +class GzEnviroment : public env_interface::EnvInterface { public: GzEnviroment(); + CallbackReturn on_init() override; CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; @@ -24,17 +26,20 @@ public: protected: + void onGzPoseSub(const gz::msgs::Pose_V& pose_v); + + bool doGzSpawn(); private: std::unique_ptr m_tf2_broadcaster; std::shared_ptr m_gz_node; std::vector m_follow_frames; - - void onGzPoseSub(const gz::msgs::Pose_V& pose_v); - - void doGzSpawn(); + std::string m_topic_name; + std::string m_service_spawn; std::string getGzWorldName(); + std::string createGzModelString(const std::vector& pose, const std::string& mesh_filepath, const std::string& name); + }; } diff --git a/env_manager/gz_enviroment/package.xml b/env_manager/gz_enviroment/package.xml index e08ac80..65f99ad 100644 --- a/env_manager/gz_enviroment/package.xml +++ b/env_manager/gz_enviroment/package.xml @@ -14,6 +14,7 @@ tf2_ros tf2_msgs ros_gz + env_interface ament_lint_auto ament_lint_common diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index 35b50a1..3a80041 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -1,25 +1,46 @@ #include "gz_enviroment/gz_enviroment.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "ros_gz_bridge/convert.hpp" -namespace gz_enviroment +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace gz_enviroment { + +GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {} + +CallbackReturn GzEnviroment::on_init() { - -GzEnviroment::GzEnviroment() -: rclcpp_lifecycle::LifecycleNode("gz_enviroment") {} - -CallbackReturn -GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) -{ - RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_configure"); - - + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); return CallbackReturn::SUCCESS; } +CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) { + // Configuration of parameters + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure"); + m_gz_node = std::make_shared(); + m_topic_name = std::string("/world/") + getGzWorldName() +"/dynamic_pose/info"; + m_service_spawn = std::string("/world/") + getGzWorldName() + "/create"; + + if (!doGzSpawn()) { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { + // Setting up the subscribers and publishers + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate"); + + m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); + m_tf2_broadcaster = std::make_unique(getNode()); + + return CallbackReturn::SUCCESS; +} + CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_cleanup"); + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); m_tf2_broadcaster.reset(); m_gz_node.reset(); @@ -27,34 +48,21 @@ GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) return CallbackReturn::SUCCESS; } -CallbackReturn -GzEnviroment::on_activate(const rclcpp_lifecycle::State&) -{ - RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_activate"); - m_tf2_broadcaster = std::make_unique(this); - m_gz_node = std::make_shared(); - - std::string m_topic_name = "/world/" + getGzWorldName() +"/dynamic_pose/info"; - std::string m_service_spawn = "/world/" + getGzWorldName() + "/create"; - - m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); - - return CallbackReturn::SUCCESS; -} - CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(this->get_logger(), "GzEnviroment is on_deactivate"); + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); return CallbackReturn::SUCCESS; } +// TODO: Check do this via EntityComponentManager void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) { - m_follow_frames = {"box1", "box2", "box3", "box4"}; + // TODO: Read from config + m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6", "monkey"}; for (const auto &it: pose_v.pose()) { if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) @@ -65,7 +73,7 @@ GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) geometry_msgs::msg::TransformStamped t; - t.header.stamp = this->get_clock()->now(); + t.header.stamp = getNode()->get_clock()->now(); t.header.frame_id = "world"; t.child_frame_id = it.name(); @@ -92,35 +100,75 @@ GzEnviroment::getGzWorldName() gz::msgs::StringMsg_V worlds_msg; while (rclcpp::ok() && !executed) { - RCLCPP_INFO(this->get_logger(), "Requesting list of world names."); + RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); executed = m_gz_node->Request(service, timeout, worlds_msg, result); } if (!executed) { - RCLCPP_INFO(this->get_logger(), "Timed out when getting world names."); + RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); return ""; } if (!result || worlds_msg.data().empty()) { - RCLCPP_INFO(this->get_logger(), "Failed to get world names."); + RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); return ""; } - RCLCPP_INFO(this->get_logger(), "%s", worlds_msg.data(0).c_str()); + RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str()); return worlds_msg.data(0); } -void +bool GzEnviroment::doGzSpawn() { - gz::msgs::EntityFactory entity; - gz::msgs::Model model; - gz::msgs::Link link; + gz::msgs::EntityFactory req; + gz::msgs::Boolean rep; + bool result; + unsigned int timeout = 5000; + // TODO: From Config + std::vector pps{1.0, 0.0, 1.0, 0.0, 0.0}; + std::string mesh_file = "monkey.stl"; + std::string mesh_name = "monkey"; + // ENDTODO + std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name); + req.set_sdf(sdf_string); + bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); + return executed; +} + +std::string +GzEnviroment::createGzModelString( + const std::vector& pose, + const std::string& mesh_filepath, + const std::string& name + ) +{ + std::string model_template = + std::string("") + + "" + + "" + + "" + +std::to_string(pose[0])+" " + +std::to_string(pose[1])+" " + +std::to_string(pose[2])+" " + +std::to_string(pose[3])+" " + +std::to_string(pose[4])+" " + +std::to_string(pose[5])+"" + + "" + + "file://"+mesh_filepath+"" + + "" + + "" + + "file://"+mesh_filepath+"" + + "" + + "" + + "" + + ""; + return model_template; } } #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - gz_enviroment::GzEnviroment, rclcpp_lifecycle::LifecycleNode + gz_enviroment::GzEnviroment, env_interface::EnvInterface ); \ No newline at end of file diff --git a/rbs_simulation/launch/simulation.launch.py b/rbs_simulation/launch/simulation.launch.py index 46b5665..25dc634 100644 --- a/rbs_simulation/launch/simulation.launch.py +++ b/rbs_simulation/launch/simulation.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?") ) declared_arguments.append( - DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?") + DeclareLaunchArgument("gazebo_gui", default_value="true", description="Launch env_manager?") ) sim_gazebo = LaunchConfiguration("sim_gazebo") From 6eee02baccbbe7ecde744b806e2a7ed419f5d5d9 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 9 Nov 2023 15:39:19 +0300 Subject: [PATCH 7/7] clang-format --- .../include/env_interface/env_interface.hpp | 17 +- .../env_interface/env_interface_base.hpp | 36 +- .../env_interface/src/env_interface_base.cpp | 68 ++-- .../include/env_manager/env_manager.hpp | 113 +++--- env_manager/env_manager/src/env_manager.cpp | 269 +++++++-------- .../env_manager/src/run_env_manager.cpp | 25 +- env_manager/gz_enviroment/CMakeLists.txt | 1 + .../include/gz_enviroment/gz_enviroment.hpp | 45 +-- .../gz_enviroment/src/gz_enviroment.cpp | 272 ++++++++------- .../add_planning_scene_objects_service.cpp | 59 ++-- .../src/assemble_state_server.cpp | 324 ++++++++--------- .../src/gripper_control_action_server.cpp | 155 +++++---- .../src/move_cartesian_path_action_server.cpp | 266 +++++++------- .../move_to_joint_states_action_server.cpp | 233 ++++++------- .../src/move_topose_action_server.cpp | 249 +++++++------- .../src/moveit_update_planning_scene.cpp | 325 +++++++++--------- .../src/pick_place_pose_loader.cpp | 188 +++++----- 17 files changed, 1299 insertions(+), 1346 deletions(-) diff --git a/env_manager/env_interface/include/env_interface/env_interface.hpp b/env_manager/env_interface/include/env_interface/env_interface.hpp index 3a69802..09fb681 100644 --- a/env_manager/env_interface/include/env_interface/env_interface.hpp +++ b/env_manager/env_interface/include/env_interface/env_interface.hpp @@ -2,13 +2,10 @@ namespace env_interface { - class EnvInterface : public EnvInterfaceBase - { - public: - EnvInterface() = default; - virtual ~EnvInterface() = default; - - }; -} // namespace env_interface - - +class EnvInterface : public EnvInterfaceBase +{ +public: + EnvInterface() = default; + virtual ~EnvInterface() = default; +}; +} // namespace env_interface diff --git a/env_manager/env_interface/include/env_interface/env_interface_base.hpp b/env_manager/env_interface/include/env_interface/env_interface_base.hpp index b4c0c5f..5977b71 100644 --- a/env_manager/env_interface/include/env_interface/env_interface_base.hpp +++ b/env_manager/env_interface/include/env_interface/env_interface_base.hpp @@ -1,38 +1,40 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include +#include namespace env_interface { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +// template class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: - EnvInterfaceBase() = default; + EnvInterfaceBase() = default; - virtual ~EnvInterfaceBase() = default; + virtual ~EnvInterfaceBase() = default; - virtual CallbackReturn init( - const std::string& t_env_name, - const std::string & t_namespace = "", - const rclcpp::NodeOptions & t_node_options = rclcpp::NodeOptions() - ); + virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "", + const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions()); - const rclcpp_lifecycle::State& configure(); + const rclcpp_lifecycle::State& configure(); - virtual CallbackReturn on_init() = 0; + virtual CallbackReturn on_init() = 0; - std::shared_ptr getNode(); - std::shared_ptr getNode() const; + std::shared_ptr getNode(); + std::shared_ptr getNode() const; - const rclcpp_lifecycle::State & getState() const; + const rclcpp_lifecycle::State& getState() const; + // virtual void setActionSpace(const A_space& action, const std::size_t& size); + +protected: + // A_space action_space; + // S_space observation_space; - private: - std::shared_ptr m_node; - - + std::shared_ptr m_node; }; -} // namespace env_interface +} // namespace env_interface diff --git a/env_manager/env_interface/src/env_interface_base.cpp b/env_manager/env_interface/src/env_interface_base.cpp index 5f5f841..c4e2d26 100644 --- a/env_manager/env_interface/src/env_interface_base.cpp +++ b/env_manager/env_interface/src/env_interface_base.cpp @@ -3,61 +3,43 @@ namespace env_interface { -CallbackReturn -EnvInterfaceBase::init( - const std::string& t_env_name, - const std::string & t_namespace, - const rclcpp::NodeOptions & t_node_options) +CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace, + const rclcpp::NodeOptions& t_node_options) { - m_node = std::make_shared( - t_env_name, t_namespace, t_node_options, false); + m_node = std::make_shared(t_env_name, t_namespace, t_node_options, false); - if (on_init() == CallbackReturn::FAILURE) - { - return CallbackReturn::FAILURE; - } + if (on_init() == CallbackReturn::FAILURE) + { + return CallbackReturn::FAILURE; + } - m_node->register_on_configure( - std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1) - ); - m_node->register_on_activate( - std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1) - ); - m_node->register_on_cleanup( - std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1) - ); - m_node->register_on_deactivate( - std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1) - ); - m_node->register_on_error( - std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1) - ); - m_node->register_on_shutdown( - std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1) - ); + m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1)); + m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1)); + m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1)); + m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1)); + m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1)); + m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1)); - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -const rclcpp_lifecycle::State& -EnvInterfaceBase::configure() +const rclcpp_lifecycle::State& EnvInterfaceBase::configure() { - return getNode()->configure(); + return getNode()->configure(); } -std::shared_ptr -EnvInterfaceBase::getNode() +std::shared_ptr EnvInterfaceBase::getNode() { - if (!m_node.get()) - { - throw std::runtime_error("Lifecycle node hasn't been initialized yet"); - } - return m_node; + if (!m_node.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet"); + } + return m_node; } -const rclcpp_lifecycle::State & EnvInterfaceBase::getState() const +const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const { - return m_node->get_current_state(); + return m_node->get_current_state(); } -} // namespace env_interface +} // namespace env_interface diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp index b6a563d..692478f 100644 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -11,7 +11,6 @@ #include "pluginlib/class_loader.hpp" - namespace env_manager { using EnvInterface = env_interface::EnvInterface; @@ -20,80 +19,68 @@ using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInter struct EnvSpec { - std::string name; - std::string type; - EnvInterfaceSharedPtr env_ptr; + std::string name; + std::string type; + EnvInterfaceSharedPtr env_ptr; }; class EnvManager : public rclcpp::Node { -public: - // EnvManager(rclcpp::Executor::SharedPtr executor, - // const std::string & json_config_path, - // const std::string & node_name = "env_manager", - // const std::string & node_namespace = "", - // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() - // .allow_undeclared_parameters(true) - // .automatically_declare_parameters_from_overrides(true)); - EnvManager(rclcpp::Executor::SharedPtr executor, - const std::string & node_name = "env_manager", - const std::string & node_namespace = "", - rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)); - virtual ~EnvManager() = default; +public: + // EnvManager(rclcpp::Executor::SharedPtr executor, + // const std::string & json_config_path, + // const std::string & node_name = "env_manager", + // const std::string & node_namespace = "", + // rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() + // .allow_undeclared_parameters(true) + // .automatically_declare_parameters_from_overrides(true)); + EnvManager(rclcpp::Executor::SharedPtr executor, const std::string& node_name = "env_manager", + const std::string& node_namespace = "", + rclcpp::NodeOptions& node_options = rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + virtual ~EnvManager() = default; + // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); + EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); + EnvStateReturnType unloadEnv(const std::string& env_name); + EnvStateReturnType configureEnv(const std::string& env_name); + EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); - // EnvInterfaceSharedPtr loadEnv(const std::string& env_name); - EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type); - EnvStateReturnType unloadEnv(const std::string &env_name); - EnvStateReturnType configureEnv(const std::string& env_name); - EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment); - - //TODO: Define the input data format - // Set Target for RL enviroment - EnvInterfaceSharedPtr setTarget(); - EnvInterfaceSharedPtr switchTarget(); - EnvInterfaceSharedPtr unsetTarget(); - // Load Constraints for RL enviroment - EnvInterfaceSharedPtr loadConstraints(); - EnvInterfaceSharedPtr switchConstraints(); - EnvInterfaceSharedPtr unloadConstraints(); - + // TODO: Define the input data format + // Set Target for RL enviroment + EnvInterfaceSharedPtr setTarget(); + EnvInterfaceSharedPtr switchTarget(); + EnvInterfaceSharedPtr unsetTarget(); + // Load Constraints for RL enviroment + EnvInterfaceSharedPtr loadConstraints(); + EnvInterfaceSharedPtr switchConstraints(); + EnvInterfaceSharedPtr unloadConstraints(); protected: - void initServices(); - rclcpp::Node::SharedPtr getNode(); - void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); + void initServices(); + rclcpp::Node::SharedPtr getNode(); + void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response); - void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, - env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); + void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response); - void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); - - + void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response); private: - rclcpp::Node::SharedPtr m_node; - std::mutex m_env_mutex; - std::vector m_active_envs; - rclcpp::Service::SharedPtr - m_load_env_srv; - rclcpp::Service::SharedPtr - m_configure_env_srv; - rclcpp::Service::SharedPtr - m_unload_env_srv; - - - rclcpp::CallbackGroup::SharedPtr m_callback_group; - rclcpp::Executor::SharedPtr m_executor; - - std::shared_ptr> m_loader; - + rclcpp::Node::SharedPtr m_node; + std::mutex m_env_mutex; + std::vector m_active_envs; + rclcpp::Service::SharedPtr m_load_env_srv; + rclcpp::Service::SharedPtr m_configure_env_srv; + rclcpp::Service::SharedPtr m_unload_env_srv; + rclcpp::CallbackGroup::SharedPtr m_callback_group; + rclcpp::Executor::SharedPtr m_executor; + std::shared_ptr> m_loader; }; -} // namespace env_manager -#endif // __ENV_MANAGER_HPP__ \ No newline at end of file +} // namespace env_manager +#endif // __ENV_MANAGER_HPP__ \ No newline at end of file diff --git a/env_manager/env_manager/src/env_manager.cpp b/env_manager/env_manager/src/env_manager.cpp index c69e5d2..00231da 100644 --- a/env_manager/env_manager/src/env_manager.cpp +++ b/env_manager/env_manager/src/env_manager.cpp @@ -1,22 +1,22 @@ #include "env_manager/env_manager.hpp" #include "nlohmann/json.hpp" -static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAMESPACE = "env_interface"; -static constexpr const char * ENV_INTERFACE_BASE_CLASS_NAME = "env_interface::EnvInterface"; +static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE = + "env_interface"; +static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME = + "env_interface::EnvInterface"; -namespace env_manager -{ -EnvManager::EnvManager( - rclcpp::Executor::SharedPtr executor, - const std::string & node_name, - const std::string & node_namespace, - rclcpp::NodeOptions & options) - : rclcpp::Node(node_name, node_namespace, options), - m_executor(executor), - m_loader(std::make_shared>( - ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME)) -{ - initServices(); +namespace env_manager { +EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor, + const std::string &node_name, + const std::string &node_namespace, + rclcpp::NodeOptions &options) + : rclcpp::Node(node_name, node_namespace, options), m_executor(executor), + m_loader( + std::make_shared>( + ENV_INTERFACE_BASE_CLASS_NAMESPACE, + ENV_INTERFACE_BASE_CLASS_NAME)) { + initServices(); } // EnvManager::EnvManager( @@ -24,7 +24,7 @@ EnvManager::EnvManager( // const std::string & json_config_path, // const std::string & node_name, // const std::string & node_namespace, -// rclcpp::NodeOptions & options) +// rclcpp::NodeOptions & options) // : rclcpp::Node(node_name, node_namespace, options), // m_executor(executor), // m_loader(std::make_shared>( @@ -33,130 +33,111 @@ EnvManager::EnvManager( // initServices(); // } +void EnvManager::initServices() { + using namespace std::placeholders; - -void -EnvManager::initServices() -{ - using namespace std::placeholders; - - m_load_env_srv = create_service - ("~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); - m_configure_env_srv = create_service - ("~/configure_env", std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); - m_unload_env_srv = create_service - ("~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); + m_load_env_srv = create_service( + "~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2)); + m_configure_env_srv = + create_service( + "~/configure_env", + std::bind(&EnvManager::configureEnv_cb, this, _1, _2)); + m_unload_env_srv = create_service( + "~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2)); } -rclcpp::Node::SharedPtr -EnvManager::getNode() -{ - return m_node; -} +rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; } -void -EnvManager::loadEnv_cb( +void EnvManager::loadEnv_cb( const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) -{ - std::lock_guard lock(m_env_mutex); - response->ok = loadEnv(request->name, request->type).get() != nullptr; + env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) { + std::lock_guard lock(m_env_mutex); + response->ok = loadEnv(request->name, request->type).get() != nullptr; } -void -EnvManager::configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, - env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) -{ - std::lock_guard guard(m_env_mutex); - response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; +void EnvManager::configureEnv_cb( + const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request, + env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) { + std::lock_guard guard(m_env_mutex); + response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS; } -void -EnvManager::unloadEnv_cb( +void EnvManager::unloadEnv_cb( const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request, - env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) -{ - std::lock_guard lock(m_env_mutex); - response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; + env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) { + std::lock_guard lock(m_env_mutex); + response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS; } +EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) { + std::lock_guard lock(m_env_mutex); + // generate list of active enviroments + // std::string unique_env_name = enviroment.name;// + "_" + + // std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] = + // enviroment.env_ptr; + if (enviroment.env_ptr->init(enviroment.name, get_namespace()) == + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::ERROR) { + RCLCPP_ERROR(get_logger(), + "Enviroment with name %s cannot been initialized", + enviroment.name.c_str()); + return nullptr; + } -EnvInterfaceSharedPtr -EnvManager::addEnv(const EnvSpec& enviroment) -{ - std::lock_guard lock(m_env_mutex); - // generate list of active enviroments - // std::string unique_env_name = enviroment.name;// + "_" + std::to_string(m_active_envs.size()); - // m_active_envs[unique_env_name] = enviroment.env_ptr; - if (enviroment.env_ptr->init( - enviroment.name, get_namespace()) == - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) - { - RCLCPP_ERROR(get_logger(), "Enviroment with name %s cannot been initialized", enviroment.name.c_str()); - return nullptr; - } - - m_active_envs.push_back(enviroment); - m_executor->add_node(enviroment.env_ptr->getNode()->get_node_base_interface()); - return enviroment.env_ptr; + m_active_envs.push_back(enviroment); + m_executor->add_node( + enviroment.env_ptr->getNode()->get_node_base_interface()); + return enviroment.env_ptr; } - -EnvInterfaceSharedPtr -EnvManager::loadEnv(const std::string& env_name, const std::string& env_class_type) -{ - EnvInterfaceSharedPtr loaded_env = nullptr; - RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); - try - { - if (m_loader->isClassAvailable(env_class_type)) - { - loaded_env = m_loader->createSharedInstance(env_class_type); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Loading enviroment is not available '%s' with class type '%s'", - env_name.c_str(), env_class_type.c_str()); - RCLCPP_INFO(this->get_logger(), "Available classes"); - for (const auto& available_class : m_loader->getDeclaredClasses()) - { - RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str()); - } - return nullptr; - } - } - catch (const pluginlib::PluginlibException& e) - { - RCLCPP_ERROR( - get_logger(), "Error happened during creation of enviroment '%s' with type '%s':\n%s", - env_name.c_str(), env_class_type.c_str(), e.what()); - return nullptr; - } - EnvSpec enviroment; - enviroment.name = env_name; - enviroment.type = env_class_type; - enviroment.env_ptr = loaded_env; - return addEnv(enviroment); -} - -EnvStateReturnType -EnvManager::unloadEnv(const std::string& env_name) -{ - auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { - return env.name == env_name; - }); - - if (it != m_active_envs.end()) { - m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); - m_active_envs.erase(it); - return EnvStateReturnType::SUCCESS; +EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name, + const std::string &env_class_type) { + EnvInterfaceSharedPtr loaded_env = nullptr; + RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str()); + try { + if (m_loader->isClassAvailable(env_class_type)) { + loaded_env = m_loader->createSharedInstance(env_class_type); } else { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - env_name.c_str()); - return EnvStateReturnType::ERROR; + RCLCPP_ERROR( + this->get_logger(), + "Loading enviroment is not available '%s' with class type '%s'", + env_name.c_str(), env_class_type.c_str()); + RCLCPP_INFO(this->get_logger(), "Available classes"); + for (const auto &available_class : m_loader->getDeclaredClasses()) { + RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str()); + } + return nullptr; } - return EnvStateReturnType::FAILURE; + } catch (const pluginlib::PluginlibException &e) { + RCLCPP_ERROR( + get_logger(), + "Error happened during creation of enviroment '%s' with type '%s':\n%s", + env_name.c_str(), env_class_type.c_str(), e.what()); + return nullptr; + } + EnvSpec enviroment; + enviroment.name = env_name; + enviroment.type = env_class_type; + enviroment.env_ptr = loaded_env; + return addEnv(enviroment); +} + +EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) { + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&env_name](const EnvSpec &env) { return env.name == env_name; }); + + if (it != m_active_envs.end()) { + m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface()); + m_active_envs.erase(it); + return EnvStateReturnType::SUCCESS; + } else { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::ERROR; + } + return EnvStateReturnType::FAILURE; } // EnvInterfaceSharedPtr @@ -167,27 +148,25 @@ EnvManager::unloadEnv(const std::string& env_name) // return loadEnv(env_name, env_class_type); // } -EnvStateReturnType -EnvManager::configureEnv(const std::string& env_name) -{ - std::lock_guard guard(m_env_mutex); - - auto it = std::find_if(m_active_envs.begin(), m_active_envs.end(), [&env_name](const EnvSpec& env) { - return env.name == env_name; - }); - - if (it == m_active_envs.end()) { - RCLCPP_ERROR(this->get_logger(), - "Environment '%s' not found in the active environments list.", - env_name.c_str()); - return EnvStateReturnType::FAILURE; - } - it->env_ptr->configure(); - // it->env_ptr->getNode()->configure(); - // it->env_ptr->activate(); - it->env_ptr->getNode()->activate(); - - return EnvStateReturnType::SUCCESS; +EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) { + std::lock_guard guard(m_env_mutex); + + auto it = std::find_if( + m_active_envs.begin(), m_active_envs.end(), + [&env_name](const EnvSpec &env) { return env.name == env_name; }); + + if (it == m_active_envs.end()) { + RCLCPP_ERROR(this->get_logger(), + "Environment '%s' not found in the active environments list.", + env_name.c_str()); + return EnvStateReturnType::FAILURE; + } + it->env_ptr->configure(); + // it->env_ptr->getNode()->configure(); + // it->env_ptr->activate(); + it->env_ptr->getNode()->activate(); + + return EnvStateReturnType::SUCCESS; } -} \ No newline at end of file +} // namespace env_manager diff --git a/env_manager/env_manager/src/run_env_manager.cpp b/env_manager/env_manager/src/run_env_manager.cpp index 419e409..1506122 100644 --- a/env_manager/env_manager/src/run_env_manager.cpp +++ b/env_manager/env_manager/src/run_env_manager.cpp @@ -1,22 +1,21 @@ #include "rclcpp/rclcpp.hpp" #include "env_manager/env_manager.hpp" -int main(int argc, char ** argv) +int main(int argc, char** argv) { - rclcpp::init(argc, argv); - std::shared_ptr executor = - std::make_shared(); - std::string manager_node_name = "env_manager"; + rclcpp::init(argc, argv); + std::shared_ptr executor = std::make_shared(); + std::string manager_node_name = "env_manager"; - auto em = std::make_shared(executor, manager_node_name); + auto em = std::make_shared(executor, manager_node_name); - RCLCPP_INFO(em->get_logger(), "Env manager is starting"); + RCLCPP_INFO(em->get_logger(), "Env manager is starting"); - em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); - em->configureEnv("gz_enviroment"); + em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment"); + em->configureEnv("gz_enviroment"); - executor->add_node(em); - executor->spin(); - rclcpp::shutdown(); - return 0; + executor->add_node(em); + executor->spin(); + rclcpp::shutdown(); + return 0; } diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index a49bff5..90ecb8d 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ignition-msgs8 ros_gz_bridge env_interface + ignition-gazebo6 ) # find dependencies diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index 3797527..852e875 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -1,6 +1,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" // #include "ros_gz_bridge/convert.hpp" +#include #include #include #include @@ -8,6 +9,10 @@ #include "gz/msgs/pose_v.pb.h" #include "env_interface/env_interface.hpp" +#include +#include +#include + namespace gz_enviroment { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -15,32 +20,28 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class GzEnviroment : public env_interface::EnvInterface { public: - - GzEnviroment(); - CallbackReturn on_init() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; - + GzEnviroment(); + CallbackReturn on_init() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; protected: + void onGzPoseSub(const gz::msgs::Pose_V& pose_v); - void onGzPoseSub(const gz::msgs::Pose_V& pose_v); - - bool doGzSpawn(); + bool doGzSpawn(); private: - std::unique_ptr m_tf2_broadcaster; - std::shared_ptr m_gz_node; - std::vector m_follow_frames; - std::string m_topic_name; - std::string m_service_spawn; - - std::string getGzWorldName(); - std::string createGzModelString(const std::vector& pose, const std::string& mesh_filepath, const std::string& name); - + std::unique_ptr m_tf2_broadcaster; + std::shared_ptr m_gz_node; + std::vector m_follow_frames; + std::string m_topic_name; + std::string m_service_spawn; + std::string m_world_name; + std::string getGzWorldName(); + std::string createGzModelString(const std::vector& pose, const std::string& mesh_filepath, + const std::string& name); }; -} - +} // namespace gz_enviroment diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index 3a80041..25d1d03 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -2,173 +2,179 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "ros_gz_bridge/convert.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include +#include +#include -namespace gz_enviroment { +#include +#include -GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {} +namespace gz_enviroment +{ + +GzEnviroment::GzEnviroment() : env_interface::EnvInterface() +{ +} CallbackReturn GzEnviroment::on_init() { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); - return CallbackReturn::SUCCESS; -} + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()"); + gz::sim::EntityComponentManager ecm; -CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State& ) { - // Configuration of parameters - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure"); - m_gz_node = std::make_shared(); - m_topic_name = std::string("/world/") + getGzWorldName() +"/dynamic_pose/info"; - m_service_spawn = std::string("/world/") + getGzWorldName() + "/create"; - - if (!doGzSpawn()) { - return CallbackReturn::ERROR; + // Получение всех сущностей, которые представляют модели + const auto modelEntities = ecm.EntitiesByComponents(gz::sim::components::Model()); + + // Вывод списка моделей + for (const auto entity : modelEntities) + { + const auto modelName = ecm.Component(entity); + if (modelName) + { + std::cout << "Model Name: " << modelName->Data() << std::endl; } - - return CallbackReturn::SUCCESS; + } + return CallbackReturn::SUCCESS; } -CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { - // Setting up the subscribers and publishers - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate"); - - m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); - m_tf2_broadcaster = std::make_unique(getNode()); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn -GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) +CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); - - m_tf2_broadcaster.reset(); - m_gz_node.reset(); - - return CallbackReturn::SUCCESS; + // Configuration of parameters + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure"); + m_gz_node = std::make_shared(); + m_world_name = getGzWorldName(); + m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info"; + m_service_spawn = std::string("/world/") + m_world_name + "/create"; + + if (!doGzSpawn()) + { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; } -CallbackReturn -GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) +CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); - - - return CallbackReturn::SUCCESS; + // Setting up the subscribers and publishers + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate"); + + m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this); + m_tf2_broadcaster = std::make_unique(getNode()); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); + + m_tf2_broadcaster.reset(); + m_gz_node.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&) +{ + RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); + + return CallbackReturn::SUCCESS; } // TODO: Check do this via EntityComponentManager -void -GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) +void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v) { - // TODO: Read from config - m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6", "monkey"}; - for (const auto &it: pose_v.pose()) - { - if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) - continue; - - geometry_msgs::msg::PoseStamped rpose; - ros_gz_bridge::convert_gz_to_ros(it, rpose); - - geometry_msgs::msg::TransformStamped t; + // TODO: Read from config + m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6", "monkey" }; + for (const auto& it : pose_v.pose()) + { + if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end()) + continue; - t.header.stamp = getNode()->get_clock()->now(); - t.header.frame_id = "world"; - t.child_frame_id = it.name(); + geometry_msgs::msg::PoseStamped rpose; + ros_gz_bridge::convert_gz_to_ros(it, rpose); - t.transform.translation.x = rpose.pose.position.x; - t.transform.translation.y = rpose.pose.position.y; - t.transform.translation.z = rpose.pose.position.z; + geometry_msgs::msg::TransformStamped t; - t.transform.rotation.x = rpose.pose.orientation.x; - t.transform.rotation.y = rpose.pose.orientation.y; - t.transform.rotation.z = rpose.pose.orientation.z; - t.transform.rotation.w = rpose.pose.orientation.w; - - m_tf2_broadcaster->sendTransform(t); - } + t.header.stamp = getNode()->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = it.name(); + + t.transform.translation.x = rpose.pose.position.x; + t.transform.translation.y = rpose.pose.position.y; + t.transform.translation.z = rpose.pose.position.z; + + t.transform.rotation.x = rpose.pose.orientation.x; + t.transform.rotation.y = rpose.pose.orientation.y; + t.transform.rotation.z = rpose.pose.orientation.z; + t.transform.rotation.w = rpose.pose.orientation.w; + + m_tf2_broadcaster->sendTransform(t); + } } -std::string -GzEnviroment::getGzWorldName() +std::string GzEnviroment::getGzWorldName() { - bool executed{false}; - bool result{false}; - unsigned int timeout{5000}; - std::string service{"/gazebo/worlds"}; - gz::msgs::StringMsg_V worlds_msg; + bool executed{ false }; + bool result{ false }; + unsigned int timeout{ 5000 }; + std::string service{ "/gazebo/worlds" }; + gz::msgs::StringMsg_V worlds_msg; - while (rclcpp::ok() && !executed) { - RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); - executed = m_gz_node->Request(service, timeout, worlds_msg, result); - } + while (rclcpp::ok() && !executed) + { + RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names."); + executed = m_gz_node->Request(service, timeout, worlds_msg, result); + } - if (!executed) { - RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); - return ""; - } + if (!executed) + { + RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names."); + return ""; + } - if (!result || worlds_msg.data().empty()) { - RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); - return ""; - } - RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str()); - return worlds_msg.data(0); + if (!result || worlds_msg.data().empty()) + { + RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names."); + return ""; + } + RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str()); + return worlds_msg.data(0); } -bool -GzEnviroment::doGzSpawn() +bool GzEnviroment::doGzSpawn() { - gz::msgs::EntityFactory req; - gz::msgs::Boolean rep; - bool result; - unsigned int timeout = 5000; - // TODO: From Config - std::vector pps{1.0, 0.0, 1.0, 0.0, 0.0}; - std::string mesh_file = "monkey.stl"; - std::string mesh_name = "monkey"; - // ENDTODO - std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name); - req.set_sdf(sdf_string); - bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); + gz::msgs::EntityFactory req; + gz::msgs::Boolean rep; + bool result; + unsigned int timeout = 5000; + // TODO: From Config + std::vector pps{ 1.0, 0.0, 1.0, 0.0, 0.0 }; + std::string mesh_file = "monkey.stl"; + std::string mesh_name = "monkey"; + // ENDTODO + std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name); + req.set_sdf(sdf_string); + bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result); - return executed; + return executed; } -std::string -GzEnviroment::createGzModelString( - const std::vector& pose, - const std::string& mesh_filepath, - const std::string& name - ) +std::string GzEnviroment::createGzModelString(const std::vector& pose, const std::string& mesh_filepath, + const std::string& name) { - std::string model_template = - std::string("") + - "" + - "" + - "" - +std::to_string(pose[0])+" " - +std::to_string(pose[1])+" " - +std::to_string(pose[2])+" " - +std::to_string(pose[3])+" " - +std::to_string(pose[4])+" " - +std::to_string(pose[5])+"" + - "" + - "file://"+mesh_filepath+"" + - "" + - "" + - "file://"+mesh_filepath+"" + - "" + - "" + - "" + - ""; - return model_template; + std::string model_template = std::string("") + "" + + "" + "" + std::to_string(pose[0]) + " " + + std::to_string(pose[1]) + " " + std::to_string(pose[2]) + " " + std::to_string(pose[3]) + + " " + std::to_string(pose[4]) + " " + std::to_string(pose[5]) + "" + + "" + "file://" + mesh_filepath + + "" + "" + "" + + "file://" + mesh_filepath + "" + + "" + "" + "" + ""; + return model_template; } -} +} // namespace gz_enviroment #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - gz_enviroment::GzEnviroment, env_interface::EnvInterface -); \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, env_interface::EnvInterface); \ No newline at end of file diff --git a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp index 3ecb596..008107d 100644 --- a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp +++ b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp @@ -1,24 +1,25 @@ +#include "moveit/move_group_interface/move_group_interface.h" +#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" +#include #include #include -#include #include -#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" -#include "moveit/move_group_interface/move_group_interface.h" #include "rclcpp/rclcpp.hpp" -using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject; +using AddPlanningSceneObject = + rbs_skill_interfaces::srv::AddPlanningSceneObject; rclcpp::Node::SharedPtr g_node = nullptr; void handle_service( const std::shared_ptr request_header, const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - // Create collision object for the robot to avoid - auto const collision_object = [frame_id = - "world", object_name=request->object_id, object_pose=request->object_pose] { + const std::shared_ptr response) { + (void)request_header; + // Create collision object for the robot to avoid + auto const collision_object = [frame_id = "world", + object_name = request->object_id, + object_pose = request->object_pose] { moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id = frame_id; collision_object.id = object_name; @@ -40,32 +41,32 @@ void handle_service( box_pose.orientation.y = object_pose[4]; box_pose.orientation.z = object_pose[5]; box_pose.orientation.w = object_pose[6]; - - collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; return collision_object; - }(); + }(); - // Add the collision object to the scene - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - planning_scene_interface.applyCollisionObject(collision_object); + // Add the collision object to the scene + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + planning_scene_interface.applyCollisionObject(collision_object); - response->success = true; + response->success = true; } -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); - g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); - auto server = g_node->create_service("add_planing_scene_object_service", handle_service); - rclcpp::spin(g_node); - rclcpp::shutdown(); - g_node = nullptr; - return 0; -} \ No newline at end of file +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + g_node = + rclcpp::Node::make_shared("add_planing_scene_object", "", node_options); + auto server = g_node->create_service( + "add_planing_scene_object_service", handle_service); + rclcpp::spin(g_node); + rclcpp::shutdown(); + g_node = nullptr; + return 0; +} diff --git a/rbs_skill_servers/src/assemble_state_server.cpp b/rbs_skill_servers/src/assemble_state_server.cpp index d359528..25ec2c5 100644 --- a/rbs_skill_servers/src/assemble_state_server.cpp +++ b/rbs_skill_servers/src/assemble_state_server.cpp @@ -1,18 +1,18 @@ +#include #include #include #include #include -#include -#include #include +#include -#include -#include #include +#include +#include -#include #include +#include #include #include @@ -24,12 +24,11 @@ #define ASSEMBLE_POSITION_OFFSET 0.5 #define ASSEMBLE_ORIENTATION_OFFSET 0.5 -#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ +#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ (package_dir) + "/models/" + (assemble_name) + "/model.sdf" -static inline geometry_msgs::msg::Pose get_pose( - const std::vector & tokens) -{ +static inline geometry_msgs::msg::Pose +get_pose(const std::vector &tokens) { geometry_msgs::msg::Pose p; p.position.set__x(std::stod(tokens.at(0))); p.position.set__y(std::stod(tokens.at(1))); @@ -41,23 +40,22 @@ static inline geometry_msgs::msg::Pose get_pose( return p; } -static inline geometry_msgs::msg::PoseStamped get_pose_stamped( - const std::string & pose_string) -{ +static inline geometry_msgs::msg::PoseStamped +get_pose_stamped(const std::string &pose_string) { std::stringstream ss(pose_string); std::istream_iterator begin(ss); std::istream_iterator end; std::vector tokens(begin, end); - + geometry_msgs::msg::PoseStamped ps; ps.set__pose(get_pose(tokens)); return ps; } -static std::vector get_assemble_poses( - const std::string & assemble_name, const std::string & part_name, - const std::string& package_dir, const std::string & assemble_prefix) -{ +static std::vector +get_assemble_poses(const std::string &assemble_name, + const std::string &part_name, const std::string &package_dir, + const std::string &assemble_prefix) { std::vector result; std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf"; @@ -66,12 +64,11 @@ static std::vector get_assemble_poses( auto model = doc.RootElement()->FirstChildElement("model"); auto joint = model->FirstChildElement("joint"); - while (joint) - { + while (joint) { auto frame_id = joint->FirstChildElement("child")->GetText(); if (frame_id != part_name) continue; - auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); + auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); ps.header.set__frame_id(assemble_prefix + part_name); result.push_back(ps); joint = joint->NextSiblingElement("joint"); @@ -80,164 +77,167 @@ static std::vector get_assemble_poses( return result; } -class AssembleStateServer: public rclcpp::Node -{ +class AssembleStateServer : public rclcpp::Node { public: - AssembleStateServer(rclcpp::NodeOptions options) - : Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) - { - assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); - assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); - tf_buffer_ = std::make_unique(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - service_ = create_service( - SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); + AssembleStateServer(rclcpp::NodeOptions options) + : Node(SERVICE_NAME, + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); + assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + service_ = create_service( + SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + void + callback(std::shared_ptr + request, + std::shared_ptr + response) { + auto state = static_cast(request->req_kind); + bool call_status = false; + std::string assemble_name = request->assemble_name; + std::string part_name = request->part_name; + auto assemble = assembles_.find(request->assemble_name); + if (assemble == assembles_.end()) + assembles_.insert(std::make_pair( + request->assemble_name, + Assemble{.part = request->part_name, + .ws = request->workspace, + .state = NONE, + .poses = + get_assemble_poses(assemble_name, part_name, + assemble_dir_, assemble_prefix_)})); + RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", + assemble_name.c_str()); + switch (state) { + case NONE: + call_status = false; + break; + case INITIALIZE: + call_status = (assembles_.at(assemble_name).state == NONE) && + on_initialize(&assembles_.at(assemble_name)); + break; + case VALIDATE: + response->validate_status = + (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && + on_validate(&assembles_.at(assemble_name)); + break; + case COMPLETE: + call_status = (assembles_.at(assemble_name).state == VALIDATE) && + on_complete(&assembles_.at(assemble_name)); + if (call_status) + assembles_.erase(assemble_name); + break; } - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - bool call_status = false; - std::string assemble_name = request->assemble_name; - std::string part_name = request->part_name; - auto assemble = assembles_.find(request->assemble_name); - if (assemble == assembles_.end()) - assembles_.insert(std::make_pair(request->assemble_name, Assemble { - .part=request->part_name, - .ws=request->workspace, - .state=NONE, - .poses=get_assemble_poses(assemble_name, part_name, assemble_dir_, assemble_prefix_) - })); - RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", assemble_name.c_str()); - switch(state) - { - case NONE: - call_status = false; - break; - case INITIALIZE: - call_status = (assembles_.at(assemble_name).state == NONE) && - on_initialize(&assembles_.at(assemble_name)); - break; - case VALIDATE: - response->validate_status = (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && - on_validate(&assembles_.at(assemble_name)); - break; - case COMPLETE: - call_status = (assembles_.at(assemble_name).state == VALIDATE) && - on_complete(&assembles_.at(assemble_name)); - if (call_status) - assembles_.erase(assemble_name); - break; - } + response->call_status = call_status; + response->assemble_name = assemble_name; + } - response->call_status = call_status; - response->assemble_name = assemble_name; - } - private: - enum AssembleReqState - { - NONE=-1, - INITIALIZE=0, - VALIDATE=1, - COMPLETE=2 - }; + enum AssembleReqState { + NONE = -1, + INITIALIZE = 0, + VALIDATE = 1, + COMPLETE = 2 + }; - struct Assemble - { - std::string part; - std::string ws; - AssembleReqState state; - std::vector poses; - std::unique_ptr tf2_broadcaster; - }; + struct Assemble { + std::string part; + std::string ws; + AssembleReqState state; + std::vector poses; + std::unique_ptr tf2_broadcaster; + }; - bool on_initialize(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", assemble->part.c_str()); - try - { - assemble->tf2_broadcaster = std::make_unique(this); - } catch (const tf2::TransformException &ex){ - RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what()); - return false; - } - assemble->state = INITIALIZE; - for (const auto& it: assemble->poses) - { - geometry_msgs::msg::TransformStamped t; - t.header.frame_id = assemble->ws; - t.child_frame_id = it.header.frame_id; - auto pose = it.pose; - t.transform.translation.x = pose.position.x; - t.transform.translation.y = pose.position.y; - t.transform.translation.z = pose.position.z; - t.transform.rotation.x = pose.orientation.x; - t.transform.rotation.y = pose.orientation.y; - t.transform.rotation.z = pose.orientation.z; - t.transform.rotation.w = pose.orientation.w; - assemble->tf2_broadcaster->sendTransform(t); - } - - return true; + bool on_initialize(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", + assemble->part.c_str()); + try { + assemble->tf2_broadcaster = + std::make_unique(this); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", + ex.what()); + return false; + } + assemble->state = INITIALIZE; + for (const auto &it : assemble->poses) { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = assemble->ws; + t.child_frame_id = it.header.frame_id; + auto pose = it.pose; + t.transform.translation.x = pose.position.x; + t.transform.translation.y = pose.position.y; + t.transform.translation.z = pose.position.z; + t.transform.rotation.x = pose.orientation.x; + t.transform.rotation.y = pose.orientation.y; + t.transform.rotation.z = pose.orientation.z; + t.transform.rotation.w = pose.orientation.w; + assemble->tf2_broadcaster->sendTransform(t); } - bool on_validate(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", assemble->part.c_str()); - std::string frame_from = assemble_prefix_ + assemble->part; - std::string frame_to = assemble->part; - geometry_msgs::msg::TransformStamped ts; - try - { - ts = tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); - } catch (const tf2::TransformException &ex) { - return false; - } - auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && - ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && - ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; - auto orient_validate = ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && - ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && - ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; + return true; + } - RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate); - assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE; + bool on_validate(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", + assemble->part.c_str()); + std::string frame_from = assemble_prefix_ + assemble->part; + std::string frame_to = assemble->part; + geometry_msgs::msg::TransformStamped ts; + try { + ts = + tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + return false; + } + auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; + auto orient_validate = + ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; - return assemble->state == VALIDATE; + RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", + pos_validate, orient_validate); + assemble->state = (pos_validate && orient_validate) ? VALIDATE : INITIALIZE; + + return assemble->state == VALIDATE; + } + + bool on_complete(Assemble *assemble) { + RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", + assemble->part.c_str()); + try { + assemble->tf2_broadcaster.reset(); + assemble->tf2_broadcaster = NULL; + assemble->poses.clear(); + } catch (const std::exception &ex) { + RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", + ex.what()); + return false; } - bool on_complete(Assemble* assemble) - { - RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", assemble->part.c_str()); - try - { - assemble->tf2_broadcaster.reset(); - assemble->tf2_broadcaster = NULL; - assemble->poses.clear(); - } catch (const std::exception &ex) { - RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", ex.what()); - return false; - } + assemble->state = COMPLETE; - assemble->state = COMPLETE; + return true; + } - return true; - } - - private: - std::map assembles_; - std::shared_ptr tf_listener_; - std::unique_ptr tf_buffer_; - std::unique_ptr tf2_broadcaster_; - std::mutex mt; - rclcpp::Service::SharedPtr service_; - std::string assemble_dir_; - std::string assemble_prefix_; + std::map assembles_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf2_broadcaster_; + std::mutex mt; + rclcpp::Service::SharedPtr service_; + std::string assemble_dir_; + std::string assemble_prefix_; }; #include "rclcpp_components/register_node_macro.hpp" diff --git a/rbs_skill_servers/src/gripper_control_action_server.cpp b/rbs_skill_servers/src/gripper_control_action_server.cpp index 8c6f682..74addd0 100644 --- a/rbs_skill_servers/src/gripper_control_action_server.cpp +++ b/rbs_skill_servers/src/gripper_control_action_server.cpp @@ -7,108 +7,113 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" #include "rbs_skill_interfaces/action/gripper_command.hpp" +#include "rclcpp_action/rclcpp_action.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" #include "sensor_msgs/msg/joint_state.hpp" - - +#include "std_msgs/msg/float64_multi_array.hpp" namespace rbs_skill_actions { - class GripperControlActionServer: public rclcpp::Node { +class GripperControlActionServer : public rclcpp::Node { - public: - using GripperCommand = rbs_skill_interfaces::action::GripperCommand; - explicit GripperControlActionServer(rclcpp::NodeOptions options): Node("gripper_control_action_server", options.allow_undeclared_parameters(true)) - { - this->actionServer_ = rclcpp_action::create_server( - this, - "gripper_control", - std::bind(&GripperControlActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GripperControlActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&GripperControlActionServer::accepted_callback, this, std::placeholders::_1) - ); - publisher = this->create_publisher("/gripper_controller/commands", 10); - join_state_subscriber = this->create_subscription( - "/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1)); - } +public: + using GripperCommand = rbs_skill_interfaces::action::GripperCommand; + explicit GripperControlActionServer(rclcpp::NodeOptions options) + : Node("gripper_control_action_server", + options.allow_undeclared_parameters(true)) { + this->actionServer_ = rclcpp_action::create_server( + this, "gripper_control", + std::bind(&GripperControlActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperControlActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&GripperControlActionServer::accepted_callback, this, + std::placeholders::_1)); + publisher = this->create_publisher( + "/gripper_controller/commands", 10); + join_state_subscriber = + this->create_subscription( + "/joint_states", 10, + std::bind(&GripperControlActionServer::joint_state_callback, this, + std::placeholders::_1)); + } +private: + rclcpp_action::Server::SharedPtr actionServer_; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr + join_state_subscriber; + double gripper_joint_state{1.0}; - private: - - rclcpp_action::Server::SharedPtr actionServer_; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr join_state_subscriber; - double gripper_joint_state{1.0}; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + void joint_state_callback(const sensor_msgs::msg::JointState &msg) { + gripper_joint_state = msg.position.back(); + } - void joint_state_callback(const sensor_msgs::msg::JointState & msg) - { - gripper_joint_state = msg.position.back(); - } + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { - rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { - - RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position); - (void)uuid; - if(goal->position > 0.9 or goal->position < 0) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } + RCLCPP_INFO(this->get_logger(), + "goal request recieved for gripper [%.2f m]", goal->position); + (void)uuid; + if (goal->position > 0.9 or goal->position < 0) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) { + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - void accepted_callback(const std::shared_ptr goal_handle) { - std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach(); - } - - void execute(const std::shared_ptr goal_handle){ + void accepted_callback(const std::shared_ptr goal_handle) { + std::thread{std::bind(&GripperControlActionServer::execute, this, + std::placeholders::_1), + goal_handle} + .detach(); + } - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - auto feedback = std::make_shared(); - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Goal Canceled"); - return; - } - RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state); + void execute(const std::shared_ptr goal_handle) { - std_msgs::msg::Float64MultiArray command; + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + auto feedback = std::make_shared(); - using namespace std::chrono_literals; + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Goal Canceled"); + return; + } + RCLCPP_INFO(this->get_logger(), "Current gripper state %f", + gripper_joint_state); - command.data.push_back(goal->position); - RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper"); - publisher->publish(command); - std::this_thread::sleep_for(3s); + std_msgs::msg::Float64MultiArray command; + using namespace std::chrono_literals; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); - - } - }; -} - + command.data.push_back(goal->position); + RCLCPP_INFO(this->get_logger(), "Publishing goal to gripper"); + publisher->publish(command); + std::this_thread::sleep_for(3s); + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal successfully completed"); + } +}; +} // namespace rbs_skill_actions RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer); - // int main(int argc, char ** argv) { // rclcpp::init(argc, argv); // rbs_skill_actions::GripperControlActionServer server; // rclcpp::spin(server); // return 0; -// } \ No newline at end of file +// } diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp index c3deb63..f223160 100644 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp @@ -7,9 +7,9 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" -#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp" +#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,156 +26,156 @@ #include */ // For Visualization -//#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveCartesianActionServer : public rclcpp::Node -{ +class MoveCartesianActionServer : public rclcpp::Node { public: - using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; - //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), - // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); - } + // explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_cartesian_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveCartesianActionServer::cancel_callback, this, _1), + // std::bind(&MoveCartesianActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_cartesian", - std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_cartesian", + std::bind(&MoveCartesianActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveCartesianActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveCartesianActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", - goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, - goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, - goal->target_pose.orientation.w); - (void)uuid; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO( + this->get_logger(), + "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, " + "%f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, + goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, + goal->target_pose.orientation.z, goal->target_pose.orientation.w); + (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), + goal_handle) + .detach(); + // std::thread( + // [this, goal_handle]() { + // execute(goal_handle); + // }).detach(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + moveit::core::RobotState start_state( + *move_group_interface.getCurrentState()); + + std::vector waypoints; + auto current_pose = move_group_interface.getCurrentPose(); + // waypoints.push_back(current_pose.pose); + geometry_msgs::msg::Pose target_pose = goal->target_pose; + // target_pose.position = goal->target_pose.position; + waypoints.push_back(target_pose); + + RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", + target_pose.position.x, target_pose.position.y, + target_pose.position.z); + // waypoints.push_back(start_pose.pose); + moveit_msgs::msg::RobotTrajectory trajectory; + const double jump_threshold = 0.0; + const double eef_step = 0.001; + double fraction = move_group_interface.computeCartesianPath( + waypoints, eef_step, jump_threshold, trajectory); + if (fraction > 0) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + moveit::core::MoveItErrorCode execute_err_code = + move_group_interface.execute(trajectory); + if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "Failure in move:"); + } + + // move_group_interface.move(); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); + goal_handle->abort(result); } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - void accepted_callback(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach(); - // std::thread( - // [this, goal_handle]() { - // execute(goal_handle); - // }).detach(); - } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); - - moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); - - std::vector waypoints; - auto current_pose = move_group_interface.getCurrentPose(); - //waypoints.push_back(current_pose.pose); - geometry_msgs::msg::Pose target_pose = goal->target_pose; - //target_pose.position = goal->target_pose.position; - waypoints.push_back(target_pose); - - RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", - target_pose.position.x, target_pose.position.y, target_pose.position.z); - //waypoints.push_back(start_pose.pose); - moveit_msgs::msg::RobotTrajectory trajectory; - const double jump_threshold = 0.0; - const double eef_step = 0.001; - double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - if(fraction>0) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory); - if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); - }else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) - { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - - //move_group_interface.move(); - }else{ - RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); - goal_handle->abort(result); - } - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; } + } }; // class MoveCartesianActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options); - rbs_skill_actions::MoveCartesianActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveCartesianActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp index 9f79472..b7adb84 100644 --- a/rbs_skill_servers/src/move_to_joint_states_action_server.cpp +++ b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp @@ -1,16 +1,15 @@ +#include #include #include #include -#include - #include "rclcpp/rclcpp.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" #include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,143 +25,139 @@ #include */ // For Visualization -//#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveToJointStateActionServer : public rclcpp::Node -{ +class MoveToJointStateActionServer : public rclcpp::Node { public: - using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates; + using MoveitSendJointStates = + rbs_skill_interfaces::action::MoveitSendJointStates; - //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), - // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); - } + // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_to_joint_states_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), + // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_to_joint_states", - std::bind(&MoveToJointStateActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveToJointStateActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveToJointStateActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_to_joint_states", + std::bind(&MoveToJointStateActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveToJointStateActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveToJointStateActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = + rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str()); - (void)uuid; - if (false) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", + goal->robot_name.c_str()); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), + goal_handle) + .detach(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + std::vector joint_states = goal->joint_values; + for (auto &joint : joint_states) { + RCLCPP_INFO(this->get_logger(), "Joint value %f", joint); } - - void accepted_callback(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach(); + + move_group_interface.setJointValueTarget(goal->joint_values); + move_group_interface.setMaxVelocityScalingFactor( + goal->joints_velocity_scaling_factor); + move_group_interface.setMaxAccelerationScalingFactor( + goal->joints_acceleration_scaling_factor); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = (move_group_interface.plan(plan) == + moveit::core::MoveItErrorCode::SUCCESS); + if (success) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + move_group_interface.execute(plan); + // move_group_interface.move(); + } else { + RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); - - std::vector joint_states = goal->joint_values; - for (auto &joint : joint_states) - { - RCLCPP_INFO(this->get_logger(), "Joint value %f", joint); - } - - - move_group_interface.setJointValueTarget(goal->joint_values); - move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor); - move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor); - - - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); - if(success) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - move_group_interface.execute(plan); - //move_group_interface.move(); - }else{ - RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); - } - - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } - - - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; } + + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } }; // class MoveToJointStateActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_topose", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_topose", "", node_options); - rbs_skill_actions::MoveToJointStateActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveToJointStateActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp index 4bf83ad..aba0780 100644 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ b/rbs_skill_servers/src/move_topose_action_server.cpp @@ -7,9 +7,9 @@ #include "rclcpp_components/register_node_macro.hpp" // action libs -#include "rclcpp_action/rclcpp_action.hpp" -#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp" +#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -18,8 +18,8 @@ // moveit libs #include "moveit/move_group_interface/move_group_interface.h" #include "moveit/planning_interface/planning_interface.h" -#include "moveit/robot_model_loader/robot_model_loader.h" #include "moveit/planning_scene_interface/planning_scene_interface.h" +#include "moveit/robot_model_loader/robot_model_loader.h" /* #include @@ -27,148 +27,149 @@ #include */ // For Visualization -//#include -#include +// #include +#include "moveit_msgs/action/move_group.hpp" #include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp" -#include "moveit_msgs/action/move_group.hpp" +#include -namespace rbs_skill_actions -{ +namespace rbs_skill_actions { -class MoveToPoseActionServer : public rclcpp::Node -{ +class MoveToPoseActionServer : public rclcpp::Node { public: - using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; + using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose; - //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) - explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node) - { - // using namespace std::placeholders; - // this->action_server_ = rclcpp_action::create_server( - // this->get_node_base_interface(), - // this->get_node_clock_interface(), - // this->get_node_logging_interface(), - // this->get_node_waitables_interface(), - // "move_topose", - // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), - // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), - // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); - } + // explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) + explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) + : Node("move_topose_action_server"), node_(node) { + // using namespace std::placeholders; + // this->action_server_ = rclcpp_action::create_server( + // this->get_node_base_interface(), + // this->get_node_clock_interface(), + // this->get_node_logging_interface(), + // this->get_node_waitables_interface(), + // "move_topose", + // std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2), + // std::bind(&MoveToPoseActionServer::cancel_callback, this, _1), + // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1)); + } - void init() - { - - action_server_ = rclcpp_action::create_server( - node_->get_node_base_interface(), - node_->get_node_clock_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "move_topose", - std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1), - std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1) - ); + void init() { - } + action_server_ = rclcpp_action::create_server( + node_->get_node_base_interface(), node_->get_node_clock_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "move_topose", + std::bind(&MoveToPoseActionServer::goal_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&MoveToPoseActionServer::cancel_callback, this, + std::placeholders::_1), + std::bind(&MoveToPoseActionServer::accepted_callback, this, + std::placeholders::_1)); + } private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Server::SharedPtr action_server_; - using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", - goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z, - goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, - goal->target_pose.orientation.w); - (void)uuid; - if (false) { - return rclcpp_action::GoalResponse::REJECT; - } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + rclcpp_action::GoalResponse + goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO( + this->get_logger(), + "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, " + "%f, %f]", + goal->robot_name.c_str(), goal->target_pose.position.x, + goal->target_pose.position.y, goal->target_pose.position.z, + goal->target_pose.orientation.x, goal->target_pose.orientation.y, + goal->target_pose.orientation.z, goal->target_pose.orientation.w); + (void)uuid; + if (false) { + return rclcpp_action::GoalResponse::REJECT; } - - rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received cancel request"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - void accepted_callback(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach(); - } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing action goal"); - const auto goal = goal_handle->get_goal(); - auto result = std::make_shared(); - - moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); - move_group_interface.startStateMonitor(); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - move_group_interface.setPoseTarget(goal->target_pose); - move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity); - move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration); - moveit::planning_interface::MoveGroupInterface::Plan plan; - moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan); - if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN){ - move_group_interface.plan(plan); - } - if(plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Planning success"); - //move_group_interface.execute(plan); - moveit::core::MoveItErrorCode move_err_code = move_group_interface.execute(plan); - if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) - { - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); - } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) - { - RCLCPP_ERROR(this->get_logger(), "Failure in move:"); - } - + rclcpp_action::CancelResponse + cancel_callback(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received cancel request"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } - }else{ - RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code)); - goal_handle->abort(result); - } + void accepted_callback(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), + goal_handle) + .detach(); + } - if (goal_handle->is_canceling()) { - goal_handle->canceled(result); - RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); - return; - } + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing action goal"); + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + moveit::planning_interface::MoveGroupInterface move_group_interface( + node_, goal->robot_name); + move_group_interface.startStateMonitor(); + + move_group_interface.setPoseTarget(goal->target_pose); + move_group_interface.setMaxVelocityScalingFactor( + goal->end_effector_velocity); + move_group_interface.setMaxAccelerationScalingFactor( + goal->end_effector_acceleration); + moveit::planning_interface::MoveGroupInterface::Plan plan; + moveit::core::MoveItErrorCode plan_err_code = + move_group_interface.plan(plan); + if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) { + move_group_interface.plan(plan); } + if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_INFO(this->get_logger(), "Planning success"); + // move_group_interface.execute(plan); + moveit::core::MoveItErrorCode move_err_code = + move_group_interface.execute(plan); + if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) { + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Successfully executed action goal"); + } else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) { + RCLCPP_ERROR(this->get_logger(), "Failure in move:"); + } + + } else { + RCLCPP_WARN_STREAM(this->get_logger(), + "Failed to generate plan because " + << error_code_to_string(plan_err_code)); + goal_handle->abort(result); + } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); + return; + } + } }; // class MoveToPoseActionServer -}// namespace rbs_skill_actions +} // namespace rbs_skill_actions -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - auto node = rclcpp::Node::make_shared("move_topose", "", node_options); +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("move_topose", "", node_options); - rbs_skill_actions::MoveToPoseActionServer server(node); - std::thread run_server([&server]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - server.init(); - }); + rbs_skill_actions::MoveToPoseActionServer server(node); + std::thread run_server([&server]() { + rclcpp::sleep_for(std::chrono::seconds(3)); + server.init(); + }); - rclcpp::spin(node); - run_server.join(); + rclcpp::spin(node); + run_server.join(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/rbs_skill_servers/src/moveit_update_planning_scene.cpp b/rbs_skill_servers/src/moveit_update_planning_scene.cpp index 366bf35..8840992 100644 --- a/rbs_skill_servers/src/moveit_update_planning_scene.cpp +++ b/rbs_skill_servers/src/moveit_update_planning_scene.cpp @@ -1,20 +1,20 @@ +#include #include #include -#include -#include #include #include -#include #include +#include +#include #include -#include -#include #include +#include +#include -#include #include +#include #include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp" @@ -22,8 +22,8 @@ #define INIT_SCENE_PARAM_NAME "init_scene" #define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths" -static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &pose) -{ +static geometry_msgs::msg::Pose +pose_from_pose3d(const ignition::math::Pose3d &pose) { geometry_msgs::msg::Pose result; auto position = pose.Pos(); result.position.set__x(position.X()); @@ -37,129 +37,125 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p return result; } -static std::string get_correct_mesh_path( - const std::string& uri, const std::vector &resources) -{ +static std::string +get_correct_mesh_path(const std::string &uri, + const std::vector &resources) { std::string result = ""; std::regex reg(R"((?:model|package)(?:\:\/)(.*))"); std::smatch m; - if (std::regex_match(uri, m, reg)) - { + if (std::regex_match(uri, m, reg)) { std::string rel_path = m[1]; - std::for_each(resources.begin(), resources.end(), - [&result, &rel_path](const std::string& res){ - if (result.empty()) - result = std::filesystem::exists(res + rel_path)? std::string(res + rel_path): result; - }); + std::for_each(resources.begin(), resources.end(), + [&result, &rel_path](const std::string &res) { + if (result.empty()) + result = std::filesystem::exists(res + rel_path) + ? std::string(res + rel_path) + : result; + }); } - return "file://" + result; + return "file://" + result; } -static moveit_msgs::msg::CollisionObject -get_object(const sdf::Model *model, const std::vector &resources) -{ +static moveit_msgs::msg::CollisionObject +get_object(const sdf::Model *model, const std::vector &resources) { moveit_msgs::msg::CollisionObject obj; obj.header.frame_id = "world"; obj.id = model->Name(); obj.pose = pose_from_pose3d(model->RawPose()); size_t link_count = model->LinkCount(); - for (size_t i = 0; i < link_count; ++i) - { + for (size_t i = 0; i < link_count; ++i) { auto link = model->LinkByIndex(i); auto collision = link->CollisionByIndex(0); auto link_pose = pose_from_pose3d(link->RawPose()); auto geometry = collision->Geom(); - switch(geometry->Type()) - { - case sdf::GeometryType::MESH: - { - auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); - shapes::Mesh *m = shapes::createMeshFromResource( - !path.empty()? path: geometry->MeshShape()->Uri()); - auto scale = geometry->MeshShape()->Scale().X(); - m->scale(scale); + switch (geometry->Type()) { + case sdf::GeometryType::MESH: { + auto path = + get_correct_mesh_path(geometry->MeshShape()->Uri(), resources); + shapes::Mesh *m = shapes::createMeshFromResource( + !path.empty() ? path : geometry->MeshShape()->Uri()); + auto scale = geometry->MeshShape()->Scale().X(); + m->scale(scale); - Eigen::Vector3d centroid = {0, 0, 0}; - for (size_t i = 0; i < 3 * m->vertex_count; i += 3) - { - const auto x = m->vertices[i]; - const auto y = m->vertices[i+1]; - const auto z = m->vertices[i+2]; + Eigen::Vector3d centroid = {0, 0, 0}; + for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { + const auto x = m->vertices[i]; + const auto y = m->vertices[i + 1]; + const auto z = m->vertices[i + 2]; - centroid.x() += x*(1-scale); - centroid.y() += y*(1-scale); - centroid.z() += z*(1-scale); - } - - centroid = centroid / m->vertex_count; - for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { - m->vertices[i] -= centroid.x(); - m->vertices[i + 1] -= centroid.y(); - m->vertices[i + 2] -= centroid.z(); - } - - shape_msgs::msg::Mesh mesh; - shapes::ShapeMsg m_msg; - shapes::constructMsgFromShape(m, m_msg); - mesh = boost::get(m_msg); - obj.meshes.push_back(mesh); - obj.mesh_poses.push_back(link_pose); - break; - } case sdf::GeometryType::BOX: { - auto sdf_box = geometry->BoxShape(); - shape_msgs::msg::SolidPrimitive box; - box.type = shape_msgs::msg::SolidPrimitive::BOX; - auto sdf_box_size = sdf_box->Size(); - box.dimensions.push_back(sdf_box_size.X()); - box.dimensions.push_back(sdf_box_size.Y()); - box.dimensions.push_back(sdf_box_size.Z()); - obj.primitives.push_back(box); - obj.primitive_poses.push_back(link_pose); - break; + centroid.x() += x * (1 - scale); + centroid.y() += y * (1 - scale); + centroid.z() += z * (1 - scale); } - case sdf::GeometryType::PLANE: - { - auto sdf_plane = geometry->PlaneShape(); - shape_msgs::msg::Plane plane; - auto normal = sdf_plane->Normal(); - plane.coef[0] = normal.X(); - plane.coef[1] = normal.Y(); - plane.coef[2] = normal.Z(); - obj.planes.push_back(plane); - obj.plane_poses.push_back(link_pose); - break; + + centroid = centroid / m->vertex_count; + for (size_t i = 0; i < 3 * m->vertex_count; i += 3) { + m->vertices[i] -= centroid.x(); + m->vertices[i + 1] -= centroid.y(); + m->vertices[i + 2] -= centroid.z(); } - case sdf::GeometryType::EMPTY: - case sdf::GeometryType::CYLINDER: - case sdf::GeometryType::SPHERE: - case sdf::GeometryType::HEIGHTMAP: - case sdf::GeometryType::CAPSULE: - case sdf::GeometryType::ELLIPSOID: - case sdf::GeometryType::POLYLINE: - break; + + shape_msgs::msg::Mesh mesh; + shapes::ShapeMsg m_msg; + shapes::constructMsgFromShape(m, m_msg); + mesh = boost::get(m_msg); + obj.meshes.push_back(mesh); + obj.mesh_poses.push_back(link_pose); + break; + } + case sdf::GeometryType::BOX: { + auto sdf_box = geometry->BoxShape(); + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; + auto sdf_box_size = sdf_box->Size(); + box.dimensions.push_back(sdf_box_size.X()); + box.dimensions.push_back(sdf_box_size.Y()); + box.dimensions.push_back(sdf_box_size.Z()); + obj.primitives.push_back(box); + obj.primitive_poses.push_back(link_pose); + break; + } + case sdf::GeometryType::PLANE: { + auto sdf_plane = geometry->PlaneShape(); + shape_msgs::msg::Plane plane; + auto normal = sdf_plane->Normal(); + plane.coef[0] = normal.X(); + plane.coef[1] = normal.Y(); + plane.coef[2] = normal.Z(); + obj.planes.push_back(plane); + obj.plane_poses.push_back(link_pose); + break; + } + case sdf::GeometryType::EMPTY: + case sdf::GeometryType::CYLINDER: + case sdf::GeometryType::SPHERE: + case sdf::GeometryType::HEIGHTMAP: + case sdf::GeometryType::CAPSULE: + case sdf::GeometryType::ELLIPSOID: + case sdf::GeometryType::POLYLINE: + break; } } return obj; } -static std::vector -get_objects(const sdf::World *world, const std::string &model_resources) -{ +static std::vector +get_objects(const sdf::World *world, const std::string &model_resources) { std::vector resources; std::regex reg("\\:+"); - std::sregex_token_iterator begin( - model_resources.begin(), model_resources.end(), reg, -1), end; + std::sregex_token_iterator begin(model_resources.begin(), + model_resources.end(), reg, -1), + end; std::copy(++begin, end, std::back_inserter(resources)); std::vector result; auto models_count = world->ModelCount(); - for (size_t i = 0; i < models_count; ++i) - { - try{ + for (size_t i = 0; i < models_count; ++i) { + try { auto model = world->ModelByIndex(i); result.push_back(get_object(model, resources)); - } catch (std::exception &ex){ + } catch (std::exception &ex) { std::cerr << ex.what() << std::endl; } } @@ -167,81 +163,80 @@ get_objects(const sdf::World *world, const std::string &model_resources) return result; } -class UpdatePlanningSceneServer: public rclcpp::Node -{ +class UpdatePlanningSceneServer : public rclcpp::Node { public: - UpdatePlanningSceneServer(rclcpp::NodeOptions options) - : Node("update_planning_scene_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) - { - std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); - std::string model_resources = get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); + UpdatePlanningSceneServer(rclcpp::NodeOptions options) + : Node("update_planning_scene_node", + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string(); + std::string model_resources = + get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string(); - if (!scene.empty()) - { - RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); - init_scene(scene, model_resources); - } - service_ = create_service( - SERVICE_NAME, std::bind(&UpdatePlanningSceneServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); + if (!scene.empty()) { + RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str()); + init_scene(scene, model_resources); + } + service_ = + create_service( + SERVICE_NAME, + std::bind(&UpdatePlanningSceneServer::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + bool init_scene(const std::string &init_scene, + const std::string &model_resources) { + sdf::Root root; + sdf::ParserConfig config; + config.AddURIPath("package://", model_resources); + config.AddURIPath("model://", model_resources); + sdf::Errors errors = root.Load(init_scene, config); + if (!errors.empty()) { + for (auto const &e : errors) + RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str()); + return false; + } + auto world = root.WorldByIndex(0); + auto objects = get_objects(world, model_resources); + planning_scene_.applyCollisionObjects(objects); + return true; + } + + void + callback(std::shared_ptr< + rbs_skill_interfaces::srv::PlanningSceneObjectState::Request> + request, + std::shared_ptr< + rbs_skill_interfaces::srv::PlanningSceneObjectState::Response> + response) { + auto state = + static_cast(request->req_kind); + moveit_msgs::msg::CollisionObject obj; + obj.id = request->frame_name; + switch (state) { + case ADD: + case UPDATE: + obj.meshes.resize(1); + obj.mesh_poses.resize(1); + obj.operation = state == ADD ? moveit_msgs::msg::CollisionObject::ADD + : moveit_msgs::msg::CollisionObject::MOVE; + obj.meshes.at(0) = std::move(request->mesh); + obj.mesh_poses.at(0) = std::move(request->pose.pose); + break; + case REMOVE: + obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; + break; } - bool init_scene(const std::string &init_scene, const std::string &model_resources) - { - sdf::Root root; - sdf::ParserConfig config; - config.AddURIPath("package://", model_resources); - config.AddURIPath("model://", model_resources); - sdf::Errors errors = root.Load(init_scene, config); - if (!errors.empty()) - { - for (auto const &e: errors) - RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str()); - return false; - } - auto world = root.WorldByIndex(0); - auto objects = get_objects(world, model_resources); - planning_scene_.applyCollisionObjects(objects); - return true; - } - - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - moveit_msgs::msg::CollisionObject obj; - obj.id = request->frame_name; - switch(state) - { - case ADD: - case UPDATE: - obj.meshes.resize(1); - obj.mesh_poses.resize(1); - obj.operation = state == ADD? - moveit_msgs::msg::CollisionObject::ADD: - moveit_msgs::msg::CollisionObject::MOVE; - obj.meshes.at(0) = std::move(request->mesh); - obj.mesh_poses.at(0) = std::move(request->pose.pose); - break; - case REMOVE: - obj.operation = moveit_msgs::msg::CollisionObject::REMOVE; - break; - } - - response->call_status = planning_scene_.applyCollisionObject(obj); - } + response->call_status = planning_scene_.applyCollisionObject(obj); + } private: - enum UpdatePlanningSceneRequestState - { - ADD=0, - REMOVE=1, - UPDATE=2 - }; + enum UpdatePlanningSceneRequestState { ADD = 0, REMOVE = 1, UPDATE = 2 }; - rclcpp::Service::SharedPtr service_; - moveit::planning_interface::PlanningSceneInterface planning_scene_; + rclcpp::Service< + rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_; + moveit::planning_interface::PlanningSceneInterface planning_scene_; }; #include "rclcpp_components/register_node_macro.hpp" diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp index 423f983..1818727 100644 --- a/rbs_skill_servers/src/pick_place_pose_loader.cpp +++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp @@ -1,4 +1,4 @@ -//#include "rbs_skill_servers" +// #include "rbs_skill_servers" #include "rbs_skill_servers/pick_place_pose_loader.hpp" using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer; @@ -6,105 +6,107 @@ using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses; using namespace std::chrono_literals; // rclcpp::Node::SharedPtr g_node = nullptr; - - GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options) -: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) -{ - tf_buffer_ = - std::make_unique(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + : Node("grasp_place_pose_loader", + options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); - srv_ = create_service("/get_pick_place_pose_service", - std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2)); + srv_ = create_service( + "/get_pick_place_pose_service", + std::bind(&GetGraspPickPoseServer::handle_server, this, + std::placeholders::_1, std::placeholders::_2)); } -void -GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, - rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response) -{ - std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name - try { - tf_data = tf_buffer_->lookupTransform( - "world", rrr.c_str(), - tf2::TimePointZero); - } catch (const tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not transform %s to %s: %s", - rrr.c_str(), "world", ex.what()); - return; - } - // TODO: Here need to check the parameter - // We can use another parameter library from PickNik - grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array(); - RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", - request->grasp_direction.x, - request->grasp_direction.y, - request->grasp_direction.z); - RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", - request->place_direction.x, - request->place_direction.y, - request->place_direction.z); - Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose); - Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data); - //std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl; - //std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl; - Eigen::Vector3d vec_grasp(0.15,0.15,0.02); - Eigen::Vector3d vec_place(0,0,0.15); - response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp); - response->place = collect_pose(place_pose, request->place_direction, vec_place); +void GetGraspPlacePoseServer::handle_server( + const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr + request, + rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr + response) { + std::string rrr = + "ASSEMBLE_" + request->object_name; // TODO: replace with better name + try { + tf_data = + tf_buffer_->lookupTransform("world", rrr.c_str(), tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s", + rrr.c_str(), "world", ex.what()); + return; + } + // TODO: Here need to check the parameter + // We can use another parameter library from PickNik + grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose") + .as_double_array(); + RCLCPP_INFO(this->get_logger(), + "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f", + request->grasp_direction.x, request->grasp_direction.y, + request->grasp_direction.z); + RCLCPP_INFO(this->get_logger(), + "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f", + request->place_direction.x, request->place_direction.y, + request->place_direction.z); + Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose); + Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data); + // std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << + // std::endl << grasp_pose.linear() << std::endl; std::cout << "place_pose = " + // << std::endl << place_pose.translation() << std::endl << + // place_pose.linear() << std::endl; + Eigen::Vector3d vec_grasp(0.15, 0.15, 0.02); + Eigen::Vector3d vec_place(0, 0, 0.15); + response->grasp = + collect_pose(grasp_pose, request->grasp_direction, vec_grasp); + response->place = + collect_pose(place_pose, request->place_direction, vec_place); } -std::vector -GetGraspPlacePoseServer::collect_pose( - const Eigen::Affine3d pose, - const geometry_msgs::msg::Vector3 direction, - const Eigen::Vector3d scale_vec) -{ - std::vector pose_v_; - pose_v_.push_back(tf2::toMsg(pose)); - Eigen::Vector3d posedir; - Eigen::Affine3d pose_ = pose; - tf2::fromMsg(direction, posedir); - Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose(); - Ixy.diagonal() = posedir; - Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero(); - Iz.diagonal() = Eigen::Vector3d::UnitZ(); +std::vector GetGraspPlacePoseServer::collect_pose( + const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, + const Eigen::Vector3d scale_vec) { + std::vector pose_v_; + pose_v_.push_back(tf2::toMsg(pose)); + Eigen::Vector3d posedir; + Eigen::Affine3d pose_ = pose; + tf2::fromMsg(direction, posedir); + Eigen::Matrix3d Ixy = + Eigen::Matrix3d::Zero(); // posedir * posedir.transpose(); + Ixy.diagonal() = posedir; + Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero(); + Iz.diagonal() = Eigen::Vector3d::UnitZ(); - if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0] - { - std::cout << "\n TBD" << std::endl; - } - else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0] - { - // Gp -- grasp point frame - pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y - pose_v_.push_back(tf2::toMsg(pose_)); - pose_.pretranslate(Iz * scale_vec); // Gp-y + z - pose_v_.push_back(tf2::toMsg(pose_)); - pose_.pretranslate(Ixy * scale_vec); // Gp+z - pose_v_.push_back(tf2::toMsg(pose_)); - } - else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1] - { - pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp - pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp - } - else - { - std::cout << "\n TBD" << std::endl; - } - return pose_v_; + if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()) + .all()) // IF Direction == [1,0,0] + { + std::cout << "\n TBD" << std::endl; + } else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()) + .all()) // IF Direction == [0,1,0] + { + // Gp -- grasp point frame + pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y + pose_v_.push_back(tf2::toMsg(pose_)); + pose_.pretranslate(Iz * scale_vec); // Gp-y + z + pose_v_.push_back(tf2::toMsg(pose_)); + pose_.pretranslate(Ixy * scale_vec); // Gp+z + pose_v_.push_back(tf2::toMsg(pose_)); + } else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()) + .all()) // IF Direction == [0,0,1] + { + pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp + pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp + } else { + std::cout << "\n TBD" << std::endl; + } + return pose_v_; } -Eigen::Affine3d -GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector pose) -{ - Eigen::Vector3d point(std::vector(pose.begin(), pose.begin()+3).data()); - Eigen::Quaterniond quat(std::vector(pose.begin()+3, pose.end()).data()); - Eigen::Affine3d aff; - aff.translation() = point; - aff.linear() = quat.toRotationMatrix(); - return aff; +Eigen::Affine3d +GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector pose) { + Eigen::Vector3d point( + std::vector(pose.begin(), pose.begin() + 3).data()); + Eigen::Quaterniond quat( + std::vector(pose.begin() + 3, pose.end()).data()); + Eigen::Affine3d aff; + aff.translation() = point; + aff.linear() = quat.toRotationMatrix(); + return aff; }