From 1860708ba13fd9e94eea3897a8c037a1d1804b55 Mon Sep 17 00:00:00 2001 From: Justin Date: Mon, 29 Jan 2024 15:55:12 -0500 Subject: [PATCH] removed test_competitor package --- ariac_gazebo/config/sensors.yaml | 15 + ariac_gazebo/config/trials/assembly.yaml | 2 +- ariac_gazebo/config/trials/combined.yaml | 6 + ariac_gazebo/config/trials/kitting.yaml | 10 +- ariac_gazebo/config/trials/tutorial.yaml | 8 +- ariac_gazebo/launch/ariac.launch.py | 2 +- test_competitor/CMakeLists.txt | 90 - test_competitor/config/sample.yaml | 58 - test_competitor/config/sensors.yaml | 45 - .../test_competitor/test_competitor.hpp | 379 ---- test_competitor/launch/competitor.launch.py | 81 - test_competitor/launch/moveit_test.launch.py | 81 - test_competitor/meshes/assembly_insert.stl | Bin 261884 -> 0 bytes test_competitor/meshes/assembly_station.stl | Bin 10284 -> 0 bytes test_competitor/meshes/battery.stl | Bin 5484 -> 0 bytes test_competitor/meshes/bin.stl | Bin 10484 -> 0 bytes test_competitor/meshes/conveyor.stl | Bin 6684 -> 0 bytes test_competitor/meshes/kit_tray.stl | Bin 1484 -> 0 bytes test_competitor/meshes/kit_tray_table.stl | Bin 35084 -> 0 bytes test_competitor/meshes/pump.stl | Bin 684 -> 0 bytes test_competitor/meshes/regulator.stl | Bin 26484 -> 0 bytes test_competitor/meshes/sensor.stl | Bin 17184 -> 0 bytes test_competitor/package.xml | 26 - test_competitor/rviz/test_competitor.rviz | 654 ------ test_competitor/src/competitor.cpp | 32 - test_competitor/src/moveit_test.cpp | 25 - test_competitor/src/test_competitor.cpp | 1984 ----------------- 27 files changed, 38 insertions(+), 3460 deletions(-) create mode 100644 ariac_gazebo/config/sensors.yaml delete mode 100644 test_competitor/CMakeLists.txt delete mode 100644 test_competitor/config/sample.yaml delete mode 100644 test_competitor/config/sensors.yaml delete mode 100644 test_competitor/include/test_competitor/test_competitor.hpp delete mode 100644 test_competitor/launch/competitor.launch.py delete mode 100644 test_competitor/launch/moveit_test.launch.py delete mode 100644 test_competitor/meshes/assembly_insert.stl delete mode 100644 test_competitor/meshes/assembly_station.stl delete mode 100644 test_competitor/meshes/battery.stl delete mode 100644 test_competitor/meshes/bin.stl delete mode 100644 test_competitor/meshes/conveyor.stl delete mode 100644 test_competitor/meshes/kit_tray.stl delete mode 100644 test_competitor/meshes/kit_tray_table.stl delete mode 100644 test_competitor/meshes/pump.stl delete mode 100644 test_competitor/meshes/regulator.stl delete mode 100644 test_competitor/meshes/sensor.stl delete mode 100644 test_competitor/package.xml delete mode 100644 test_competitor/rviz/test_competitor.rviz delete mode 100644 test_competitor/src/competitor.cpp delete mode 100644 test_competitor/src/moveit_test.cpp delete mode 100644 test_competitor/src/test_competitor.cpp diff --git a/ariac_gazebo/config/sensors.yaml b/ariac_gazebo/config/sensors.yaml new file mode 100644 index 000000000..471438794 --- /dev/null +++ b/ariac_gazebo/config/sensors.yaml @@ -0,0 +1,15 @@ +robot_cameras: + floor_robot_camera: + active: false + type: rgb + + ceiling_robot_camera: + active: false + type: rgbd + +static_sensors: + conveyor_breakbeam: + type: break_beam + pose: + xyz: [-0.352, 3.91, 0.88] + rpy: [0, 0, pi] diff --git a/ariac_gazebo/config/trials/assembly.yaml b/ariac_gazebo/config/trials/assembly.yaml index effad9a55..488e63245 100644 --- a/ariac_gazebo/config/trials/assembly.yaml +++ b/ariac_gazebo/config/trials/assembly.yaml @@ -1,5 +1,5 @@ # Trial Name: assembly.yaml -# ARIAC2023 +# ARIAC2024 # ENVIRONMENT SETUP diff --git a/ariac_gazebo/config/trials/combined.yaml b/ariac_gazebo/config/trials/combined.yaml index f2055bc24..758a5baaf 100644 --- a/ariac_gazebo/config/trials/combined.yaml +++ b/ariac_gazebo/config/trials/combined.yaml @@ -9,6 +9,12 @@ kitting_trays: # Which kitting trays will be spawn tray_ids: [1] slots: [4] +assembly_inserts: + as1: '0.0' + as2: '0.0' + as3: '0.0' + as4: '0.0' + parts: bins: # bin params - 8 total bins each bin has nine total slots (1-9) bin1: diff --git a/ariac_gazebo/config/trials/kitting.yaml b/ariac_gazebo/config/trials/kitting.yaml index aa1efd973..6b065fd1f 100644 --- a/ariac_gazebo/config/trials/kitting.yaml +++ b/ariac_gazebo/config/trials/kitting.yaml @@ -1,5 +1,5 @@ -# Trial Name: qc.yaml -# ARIAC2023 +# Trial Name: kitting.yaml +# ARIAC2024 # ENVIRONMENT SETUP @@ -9,6 +9,12 @@ kitting_trays: # Which kitting trays will be spawn tray_ids: [3, 8] slots: [1, 4] +assembly_inserts: + as1: '0.0' + as2: '0.0' + as3: '0.0' + as4: '0.0' + parts: bins: # bin params - 8 total bins each bin has nine total slots (1-9) bin2: diff --git a/ariac_gazebo/config/trials/tutorial.yaml b/ariac_gazebo/config/trials/tutorial.yaml index 679e69052..3fc9fe8fb 100644 --- a/ariac_gazebo/config/trials/tutorial.yaml +++ b/ariac_gazebo/config/trials/tutorial.yaml @@ -1,5 +1,5 @@ # Trial Name: tutorial.yaml -# ARIAC2023 +# ARIAC2024 # ENVIRONMENT SETUP @@ -9,6 +9,12 @@ kitting_trays: # Which kitting trays will be spawn tray_ids: [3, 5, 8, 0] slots: [1, 3, 5, 6] +assembly_inserts: + as1: '0.0' + as2: '0.0' + as3: '0.0' + as4: '0.0' + parts: agvs: agv1: diff --git a/ariac_gazebo/launch/ariac.launch.py b/ariac_gazebo/launch/ariac.launch.py index ff09af3dc..3d35622c3 100644 --- a/ariac_gazebo/launch/ariac.launch.py +++ b/ariac_gazebo/launch/ariac.launch.py @@ -196,7 +196,7 @@ def generate_launch_description(): ) declared_arguments.append( - DeclareLaunchArgument("competitor_pkg", default_value="test_competitor", description="name of competitor package")) + DeclareLaunchArgument("competitor_pkg", default_value="ariac_gazebo", description="name of competitor package")) declared_arguments.append( DeclareLaunchArgument("sensor_config", default_value="sensors", description="name of user configuration file") diff --git a/test_competitor/CMakeLists.txt b/test_competitor/CMakeLists.txt deleted file mode 100644 index aff54842e..000000000 --- a/test_competitor/CMakeLists.txt +++ /dev/null @@ -1,90 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(test_competitor) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -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 - moveit_ros_planning_interface - ariac_msgs - shape_msgs - tf2_kdl -) - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(${PROJECT_NAME} SHARED - src/test_competitor.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - include -) - -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Add main competitor executable -add_executable(competitor src/competitor.cpp) -target_include_directories(competitor PRIVATE include) -target_link_libraries(competitor - ${PROJECT_NAME} -) -ament_target_dependencies(competitor ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install(TARGETS competitor - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -# Add moveit test executable -add_executable(moveit_test src/moveit_test.cpp) -target_include_directories(moveit_test PRIVATE include) -target_link_libraries(moveit_test - ${PROJECT_NAME} -) -ament_target_dependencies(moveit_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install(TARGETS moveit_test - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY - config - launch - meshes - rviz - DESTINATION share/${PROJECT_NAME} -) - -ament_export_libraries( - ${PROJECT_NAME} -) - -ament_export_include_directories( - include -) - -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -ament_package() \ No newline at end of file diff --git a/test_competitor/config/sample.yaml b/test_competitor/config/sample.yaml deleted file mode 100644 index e19d6d467..000000000 --- a/test_competitor/config/sample.yaml +++ /dev/null @@ -1,58 +0,0 @@ -# Example sensor configuration with one of each sensor type -sensors: - breakbeam_0: - type: break_beam - visualize_fov: true - pose: - xyz: [-0.35, 3, 0.95] - rpy: [0, 0, pi] - - proximity_sensor_0: - type: proximity - visualize_fov: true - pose: - xyz: [-0.573, 2.84, 1] - rpy: [pi/2, pi/6, pi/2] - - laser_profiler_0: - type: laser_profiler - visualize_fov: true - pose: - xyz: [-0.573, 1.486, 1.526] - rpy: [pi/2, pi/2, 0] - - lidar_0: - type: lidar - visualize_fov: false - pose: - xyz: [-2.286, -2.96, 1.8] - rpy: [pi, pi/2, 0] - - rgb_camera_0: - type: rgb_camera - visualize_fov: false - pose: - xyz: [-2.286, 2.96, 1.8] - rpy: [pi, pi/2, 0] - - rgbd_camera_0: - type: rgbd_camera - visualize_fov: false - pose: - xyz: [-2.286, 4.96, 1.8] - rpy: [pi, pi/2, 0] - - basic_logical_camera_0: - visualize_fov: false - type: basic_logical_camera - pose: - xyz: [-2.286, 2.96, 1.8] - rpy: [pi, pi/2, 0] - - advanced_logical_camera_0: - visualize_fov: false - type: advanced_logical_camera - pose: - xyz: [-2.286, -2.96, 1.8] - rpy: [pi, pi/2, 0] - \ No newline at end of file diff --git a/test_competitor/config/sensors.yaml b/test_competitor/config/sensors.yaml deleted file mode 100644 index 885fdc262..000000000 --- a/test_competitor/config/sensors.yaml +++ /dev/null @@ -1,45 +0,0 @@ -robot_cameras: - floor_robot_camera: - active: true - type: rgb - - ceiling_robot_camera: - active: true - type: rgbd - -static_sensors: - right_bins_camera: - type: advanced_logical_camera - pose: - xyz: [-2.286, 2.96, 1.8] - rpy: [pi, pi/2, 0] - - left_bins_camera: - type: advanced_logical_camera - pose: - xyz: [-2.286, -2.96, 1.8] - rpy: [pi, pi/2, 0] - - kts1_camera: - type: advanced_logical_camera - pose: - xyz: [-1.3, -5.8, 1.8] - rpy: [pi, pi/2, pi/2] - - kts2_camera: - type: advanced_logical_camera - pose: - xyz: [-1.3, 5.8, 1.8] - rpy: [pi, pi/2, -pi/2] - - conveyor_breakbeam: - type: break_beam - pose: - xyz: [-0.352, 3.91, 0.88] - rpy: [0, 0, pi] - - conveyor_camera: - type: advanced_logical_camera - pose: - xyz: [-0.573, 3.915, 1.8] - rpy: [pi/2, pi/2, 0] diff --git a/test_competitor/include/test_competitor/test_competitor.hpp b/test_competitor/include/test_competitor/test_competitor.hpp deleted file mode 100644 index 7f87b7a66..000000000 --- a/test_competitor/include/test_competitor/test_competitor.hpp +++ /dev/null @@ -1,379 +0,0 @@ -#include -#include -#include - -#include - -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include "tf2/exceptions.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -class TestCompetitor : public rclcpp::Node -{ -public: - /// Constructor - TestCompetitor(); - - ~TestCompetitor(); - - void AddModelsToPlanningScene(); - - // Floor Robot Public Functions - void FloorRobotSendHome(); - bool FloorRobotSetGripperState(bool enable); - bool FloorRobotChangeGripper(std::string station, std::string gripper_type); - bool FloorRobotPickandPlaceTray(int tray_id, int agv_num); - bool FloorRobotPickBinPart(ariac_msgs::msg::Part part_to_pick); - bool FloorRobotPickConveyorPart(ariac_msgs::msg::Part part_to_pick); - bool FloorRobotPlacePartOnKitTray(int agv_num, int quadrant); - - // Ceiling Robot Public Functions - void CeilingRobotSendHome(); - bool CeilingRobotSetGripperState(bool enable); - - // ARIAC Functions - bool StartCompetition(); - bool EndCompetition(); - bool LockAGVTray(int agv_num); - bool UnlockAGVTray(int agv_num); - bool MoveAGV(int agv_num, int destination); - bool SubmitOrder(std::string order_id); - - bool CompleteOrders(); - bool CompleteKittingTask(ariac_msgs::msg::KittingTask task); - bool CompleteAssemblyTask(ariac_msgs::msg::AssemblyTask task); - bool CompleteCombinedTask(ariac_msgs::msg::CombinedTask task); - - // Mutex Lock for conveyor parts - std::mutex conveyor_parts_mutex; - -private: - // Robot Move Functions - bool FloorRobotMovetoTarget(); - bool FloorRobotMoveCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - std::pair FloorRobotPlanCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - void FloorRobotWaitForAttach(double timeout); - - bool CeilingRobotMovetoTarget(); - bool CeilingRobotMoveCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - std::pair CeilingRobotPlanCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - void CeilingRobotWaitForAttach(double timeout); - bool CeilingRobotWaitForAssemble(int station, ariac_msgs::msg::AssemblyPart part); - bool CeilingRobotMoveToAssemblyStation(int station); - bool CeilingRobotPickAGVPart(ariac_msgs::msg::PartPose part); - bool CeilingRobotAssemblePart(int station, ariac_msgs::msg::AssemblyPart part); - - geometry_msgs::msg::Quaternion SetRobotOrientation(double rotation); - - // Helper Functions - void LogPose(geometry_msgs::msg::Pose p); - geometry_msgs::msg::Pose MultiplyPose(geometry_msgs::msg::Pose p1, geometry_msgs::msg::Pose p2); - geometry_msgs::msg::Pose BuildPose(double x, double y, double z, geometry_msgs::msg::Quaternion orientation); - geometry_msgs::msg::Pose FrameWorldPose(std::string frame_id); - double GetYaw(geometry_msgs::msg::Pose pose); - geometry_msgs::msg::Quaternion QuaternionFromRPY(double r, double p, double y); - - moveit_msgs::msg::CollisionObject CreateCollisionObject(std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose); - void AddModelToPlanningScene(std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose); - - // AGV location - std::map agv_locations_ = {{1, -1}, {2, -1}, {3, -1}, {4, -1}}; - - - // Callback Groups - rclcpp::CallbackGroup::SharedPtr client_cb_group_; - rclcpp::CallbackGroup::SharedPtr topic_cb_group_; - - // MoveIt Interfaces - moveit::planning_interface::MoveGroupInterface floor_robot_; - moveit::planning_interface::MoveGroupInterface ceiling_robot_; - - moveit::planning_interface::PlanningSceneInterface planning_scene_; - - trajectory_processing::TimeOptimalTrajectoryGeneration totg_; - - // TF - std::unique_ptr tf_buffer = std::make_unique(get_clock()); - std::shared_ptr tf_listener = std::make_shared(*tf_buffer); - - // Subscriptions - rclcpp::Subscription::SharedPtr orders_sub_; - rclcpp::Subscription::SharedPtr agv1_status_sub_; - rclcpp::Subscription::SharedPtr agv2_status_sub_; - rclcpp::Subscription::SharedPtr agv3_status_sub_; - rclcpp::Subscription::SharedPtr agv4_status_sub_; - rclcpp::Subscription::SharedPtr competition_state_sub_; - rclcpp::Subscription::SharedPtr kts1_camera_sub_; - rclcpp::Subscription::SharedPtr kts2_camera_sub_; - rclcpp::Subscription::SharedPtr left_bins_camera_sub_; - rclcpp::Subscription::SharedPtr right_bins_camera_sub_; - rclcpp::Subscription::SharedPtr conveyor_camera_sub_; - rclcpp::Subscription::SharedPtr breakbeam_sub_; - rclcpp::Subscription::SharedPtr conveyor_parts_sub_; - - rclcpp::Subscription::SharedPtr floor_gripper_state_sub_; - rclcpp::Subscription::SharedPtr ceiling_gripper_state_sub_; - - rclcpp::Subscription::SharedPtr as1_state_sub_; - rclcpp::Subscription::SharedPtr as2_state_sub_; - rclcpp::Subscription::SharedPtr as3_state_sub_; - rclcpp::Subscription::SharedPtr as4_state_sub_; - - // Assembly States - std::map assembly_station_states_; - - // Orders List - ariac_msgs::msg::Order current_order_; - std::vector orders_; - - unsigned int competition_state_; - - // Gripper State - ariac_msgs::msg::VacuumGripperState floor_gripper_state_; - ariac_msgs::msg::Part floor_robot_attached_part_; - ariac_msgs::msg::VacuumGripperState ceiling_gripper_state_; - ariac_msgs::msg::Part ceiling_robot_attached_part_; - - // Sensor poses - geometry_msgs::msg::Pose kts1_camera_pose_; - geometry_msgs::msg::Pose kts2_camera_pose_; - geometry_msgs::msg::Pose left_bins_camera_pose_; - geometry_msgs::msg::Pose right_bins_camera_pose_; - geometry_msgs::msg::Pose conveyor_camera_pose_; - geometry_msgs::msg::Pose breakbeam_pose_; - - // Trays - std::vector kts1_trays_; - std::vector kts2_trays_; - - // Bins - std::vector left_bins_parts_; - std::vector right_bins_parts_; - std::vector> conveyor_parts_; - std::vector conveyor_part_detected_; - std::vector conveyor_parts_expected_; - - // Sensor Callbacks - bool kts1_camera_recieved_data = false; - bool kts2_camera_recieved_data = false; - bool left_bins_camera_recieved_data = false; - bool right_bins_camera_recieved_data = false; - bool conveyor_camera_recieved_data = false; - bool breakbeam_received_data = false; - bool conveyor_parts_recieved_data = false; - - void kts1_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void kts2_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void left_bins_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void right_bins_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void conveyor_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void breakbeam_cb(const ariac_msgs::msg::BreakBeamStatus::ConstSharedPtr msg); - void conveyor_parts_cb(const ariac_msgs::msg::ConveyorParts::ConstSharedPtr msg); - - // Gripper State Callback - void floor_gripper_state_cb(const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg); - void ceiling_gripper_state_cb(const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg); - - void as1_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - void as2_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - void as3_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - void as4_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - - // AGV Status Callback - void agv1_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - void agv2_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - void agv3_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - void agv4_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - - // Orders Callback - void orders_cb(const ariac_msgs::msg::Order::ConstSharedPtr msg); - - // Competition state callback - void competition_state_cb(const ariac_msgs::msg::CompetitionState::ConstSharedPtr msg); - - // ARIAC Services - rclcpp::Client::SharedPtr quality_checker_; - rclcpp::Client::SharedPtr pre_assembly_poses_getter_; - rclcpp::Client::SharedPtr floor_robot_tool_changer_; - rclcpp::Client::SharedPtr floor_robot_gripper_enable_; - rclcpp::Client::SharedPtr ceiling_robot_gripper_enable_; - - // Breakbeam parameters - bool breakbeam_status = false; - float breakbeam_time_sec; - - // Constants - double conveyor_speed_ = 0.2; - double kit_tray_thickness_ = 0.01; - double drop_height_ = 0.005; - double pick_offset_ = 0.003; - double assembly_offset_ = 0.02; - double battery_grip_offset_ = -0.05; - - std::map part_types_ = { - {ariac_msgs::msg::Part::BATTERY, "battery"}, - {ariac_msgs::msg::Part::PUMP, "pump"}, - {ariac_msgs::msg::Part::REGULATOR, "regulator"}, - {ariac_msgs::msg::Part::SENSOR, "sensor"} - }; - - std::map part_colors_ = { - {ariac_msgs::msg::Part::RED, "red"}, - {ariac_msgs::msg::Part::BLUE, "blue"}, - {ariac_msgs::msg::Part::GREEN, "green"}, - {ariac_msgs::msg::Part::ORANGE, "orange"}, - {ariac_msgs::msg::Part::PURPLE, "purple"}, - }; - - // Part heights - std::map part_heights_ = { - {ariac_msgs::msg::Part::BATTERY, 0.04}, - {ariac_msgs::msg::Part::PUMP, 0.12}, - {ariac_msgs::msg::Part::REGULATOR, 0.07}, - {ariac_msgs::msg::Part::SENSOR, 0.07} - }; - - // Quadrant Offsets - std::map> quad_offsets_ = { - {ariac_msgs::msg::KittingPart::QUADRANT1, std::pair(-0.08, 0.12)}, - {ariac_msgs::msg::KittingPart::QUADRANT2, std::pair(0.08, 0.12)}, - {ariac_msgs::msg::KittingPart::QUADRANT3, std::pair(-0.08, -0.12)}, - {ariac_msgs::msg::KittingPart::QUADRANT4, std::pair(0.08, -0.12)}, - }; - - std::map rail_positions_ = { - {"agv1", -4.5}, - {"agv2", -1.2}, - {"agv3", 1.2}, - {"agv4", 4.5}, - {"left_bins", 3}, - {"right_bins", -3} - }; - - // Joint value targets for kitting stations - std::map floor_kts1_js_ = { - {"linear_actuator_joint", 4.0}, - {"floor_shoulder_pan_joint", 1.57}, - {"floor_shoulder_lift_joint", -1.57}, - {"floor_elbow_joint", 1.57}, - {"floor_wrist_1_joint", -1.57}, - {"floor_wrist_2_joint", -1.57}, - {"floor_wrist_3_joint", 0.0} - }; - - std::map floor_kts2_js_ = { - {"linear_actuator_joint", -4.0}, - {"floor_shoulder_pan_joint", -1.57}, - {"floor_shoulder_lift_joint", -1.57}, - {"floor_elbow_joint", 1.57}, - {"floor_wrist_1_joint", -1.57}, - {"floor_wrist_2_joint", -1.57}, - {"floor_wrist_3_joint", 0.0} - }; - - std::map ceiling_as1_js_ = { - {"gantry_x_axis_joint", 1}, - {"gantry_y_axis_joint", -3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map ceiling_as2_js_ = { - {"gantry_x_axis_joint", -4}, - {"gantry_y_axis_joint", -3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map ceiling_as3_js_ = { - {"gantry_x_axis_joint", 1}, - {"gantry_y_axis_joint", 3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map ceiling_as4_js_ = { - {"gantry_x_axis_joint", -4}, - {"gantry_y_axis_joint", 3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map floor_conveyor_js_ = { - {"linear_actuator_joint", 0.0}, - {"floor_shoulder_pan_joint", 3.14}, - {"floor_shoulder_lift_joint", -0.9162979}, - {"floor_elbow_joint", 2.04204}, - {"floor_wrist_1_joint", -2.67035}, - {"floor_wrist_2_joint", -1.57}, - {"floor_wrist_3_joint", 0.0} - }; - - std::map agv_destination_ = { - {ariac_msgs::msg::AGVStatus::KITTING, "kitting"}, - {ariac_msgs::msg::AGVStatus::ASSEMBLY_FRONT, "assembly station front"}, - {ariac_msgs::msg::AGVStatus::ASSEMBLY_BACK, "assembly station back"}, - {ariac_msgs::msg::AGVStatus::WAREHOUSE, "warehouse"} - }; - - std::vector order_planning_scene_objects_; -}; diff --git a/test_competitor/launch/competitor.launch.py b/test_competitor/launch/competitor.launch.py deleted file mode 100644 index 4d013de2b..000000000 --- a/test_competitor/launch/competitor.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - OpaqueFunction, -) - -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration -from launch.conditions import IfCondition -from launch_ros.actions import Node - -from launch_ros.substitutions import FindPackageShare - -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder - - -def launch_setup(context, *args, **kwargs): - urdf = os.path.join(get_package_share_directory("ariac_description"), "urdf/ariac_robots/ariac_robots.urdf.xacro") - - moveit_config = ( - MoveItConfigsBuilder("ariac_robots", package_name="ariac_moveit_config") - .robot_description(urdf) - .robot_description_semantic(file_path="config/ariac_robots.srdf") - .trajectory_execution(file_path="config/moveit_controllers.yaml") - .planning_pipelines(pipelines=["ompl"]) - .to_moveit_configs() - ) - - test_competitor = Node( - package="test_competitor", - executable="competitor", - output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True}, - ], - arguments=['--ros-args', '--log-level', 'move_group_interface:=warn', '--log-level', 'moveit_trajectory_processing.time_optimal_trajectory_generation:=error'] - ) - - start_rviz = LaunchConfiguration("rviz") - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("test_competitor"), "rviz", "test_competitor.rviz"] - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2_moveit", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True} - ], - condition=IfCondition(start_rviz) - ) - - nodes_to_start = [ - test_competitor, - rviz_node - ] - - return nodes_to_start - -def generate_launch_description(): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument("rviz", default_value="false", description="start rviz node?") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/test_competitor/launch/moveit_test.launch.py b/test_competitor/launch/moveit_test.launch.py deleted file mode 100644 index 1f05bae7d..000000000 --- a/test_competitor/launch/moveit_test.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - OpaqueFunction, -) - -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration -from launch.conditions import IfCondition -from launch_ros.actions import Node - -from launch_ros.substitutions import FindPackageShare - -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder - - -def launch_setup(context, *args, **kwargs): - urdf = os.path.join(get_package_share_directory("ariac_description"), "urdf/ariac_robots/ariac_robots.urdf.xacro") - - moveit_config = ( - MoveItConfigsBuilder("ariac_robots", package_name="ariac_moveit_config") - .robot_description(urdf) - .robot_description_semantic(file_path="config/ariac_robots.srdf") - .trajectory_execution(file_path="config/moveit_controllers.yaml") - .planning_pipelines(pipelines=["ompl"]) - .to_moveit_configs() - ) - - moveit_test = Node( - package="test_competitor", - executable="moveit_test", - output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True}, - ], - arguments=['--ros-args', '--log-level', 'move_group_interface:=warn', '--log-level', 'moveit_trajectory_processing.time_optimal_trajectory_generation:=error'] - ) - - start_rviz = LaunchConfiguration("rviz") - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("test_competitor"), "rviz", "test_competitor.rviz"] - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2_moveit", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True} - ], - condition=IfCondition(start_rviz) - ) - - nodes_to_start = [ - moveit_test, - rviz_node - ] - - return nodes_to_start - -def generate_launch_description(): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument("rviz", default_value="false", description="start rviz node?") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/test_competitor/meshes/assembly_insert.stl b/test_competitor/meshes/assembly_insert.stl deleted file mode 100644 index df6427de1336ef2eb93f6eb47a33a875a7efa63c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 261884 zcmbrndEEciod183EkuSk(Q1lmY)Of(>$0RGnRX$|l!#JU!dMI0sc29cv=}KN6shZd zRm$Gt%hU*M8cUXD#@G^mulqUg$LoDR@AvEaeE#@-ZZ~ev&UxO?`aI9`Jm>X#zplLw zIp~!K9=y*Ho9wms(>B>|yB(jn$^XB9ue{&f+?Rf~*YY1<_>S)mG`>i!hLqR zap{YX`ojDVxBSEWBQF2W(vdIw%>3=g%$4-pzIE1oOIXYFfs21G^r3rxxg&n@G=8Wzj(JI-};UBOcB;HecL_PC?);l z_V4b9_y6J1r61q__&t`t_@+CRvQF9Pee-F_r}jT~{-zt|3h|d4-Zn*8%XIp5!0S%v zh;?rL>cU+2@40U|bn&DUH(0p+#1ofZyZhJYH~Q55O1XDD_}lZO(?8N%Vp_{IGW6Ws zx31Z6kvg{7VAsVXu6baoWBn^GYRV;@I!JE`*FH_BzPY(>9+sI8=H=$g*6-)w*uyuf zUYIL7SI;oHBpn{3+{iTFPZU zkZBciOIXV^GCilHxjAeW+wE+)%tyxk#(nNvT5|JSE}oyi`+7xQc=>3Gu$ITo^HfH6 zc|0G>*bnW+e!jfXol8qje(q6IcGb;bPfTl>Muwi7TkqCG%KCWw8~(Vo$$Eb&E9be_ zetQ0?_pIyj-PfEp|I(k;6@}aq)-wI#qbmKu3(o9_eXe{%nU6Ehxbx`l*VOr-ERU#1 zx+Sb-8kteAt?Jg`BJ;r-^bCIVG1W3ykFni;4fce!Ouyw*4=6KO*C6YH`Cy&c!b9M3 zkQv?TeN0%(bXrwclzhozyup#jo!|Qs%8h5hS2o^fF}?&X)A2sF4y~x=tfINI&YREb z`pe?E^I1_Z+47vO4inZgomKmbBQNZTzh3+7Vpq@J?7WdJj0ql_r!wpL=oTBg6c z%h-!{Eo?p9^0jwgc+dXFF73DNo9FGRNykb^Z;5Fw)5z=+b8}lAed|1S^~V<;xaigS zkp1sn`tjpqFPZmhZV78m)3j>0%fHjrq4lxATBCNXIz9i=_fIM^>Buc%Ez|9)?fS8l ze$cAa{;~t^QTl;}dr|dVcd;Y8`jqUkl)aYzBsSRa=9A}-z2tYrcQI~>463I#VJ*|$qAsIQeF;8< zv0^P|<=`{!_nEsEt!s98XTQ4eD|2%utYzB%xS7GSKA2nlu4m1&*%H=b{zJzWD5>3@ zv39huYDX_#1uZeHWg5Be&fPv<_Vdm|EXD(nv%#I#En`JY6)!=oRU)#Uu$Jiq)?U9@ z=*${ij$Ix1)g$|L_sFf@*4x$g8&9p;<*d$VVOP*uk6XO&HC?OZsmuO>4<|N=cV_9-%5|H!4A zl$NlHF8RkQ&l1)$eYa27lIj|iXIPF0poj5Mj88lU z^l7H&Wmlth*7ACEOjLHxLa?|P-p z&6%*4>DX1>IXk;5ktOF4M;Z89)(U5}_+8{;S3O}Z)5w!(iC7xH+r`!;ZfBEq?lyW^P)-TBhSg_x@oPOKUWsDdr&d}ajvQy*jyc3lZOHWwKboTS=6E(UE8ByjoBTf97(e2OYnZX5X zna&K>2)VPwg|Y1}dlbF4J?h1Fd%{|#z2?h&lvWWl_|%ZLV;N~-A2alXwM_5%v#0))QEz>?5HlBJEAC51Hk7C!4*RBzOc7rAgH*LSt?9R*a z23U3WaQsz#32UV#tYtcTd-YeG%Q;8UIVoNM_M9nEjmdfOn!M_9>?c_H(qRX?6+D z3FRbQ=7V{$m&r)8?(CIX!dj-&zj}7&4g=fm&TJ*hz<#Equ*9gO&&`=g+Y`x{My@ej zw;PmxaQ2K}!tdJS*b_A#uF*0bKUO_;XYFNo=ElgYlhZvd!Ipf|FLu=v)-sI@UF}NV zXMqvTPVZJBlitS+i;ON&hP6z4Zp!GEd(q4XWlcx-nGe!i!e;=qOe5EQu$#fY4tAEz zRLW(Z+a1Tmw3cb)Y9*eJL`cLqzW4KKvYiiWnNECFqb1!AF0$*>L+*-GE*73>pxz;y znAS3l+^>&}Rk<%AhueXS0yiER^?GZdu`27F^&BnJ$o0O&#Q9gRTjt{~Xa2C%f!5}y z{&=2rWYU{CFLhXpIS7pm-9$@^iHc}%Vk+)*6Gd@%(h}A(-HxvBeY_65wvlOVjWSGF z%e2=`xrZxfH6niB>E(PzoRCvky{+v|5j|lo)2(Ol{^4E55!uQ+&a@VDfd3#JxyIDG zZ(ol6usXfJ`lR37&@Wrdbi1qJL-0P>mrtpT0xjVzn-Qt!S`*eXjSO995YI*(oTuDF z`wx#ub7$F{gr{lhsQZU*qslsGMX{cJBWw$8?ys6#XWOgSe`(by{fI9iW{9;jH=*m3 z2E}m8(^^<~Xyod5HM%38igKDHR?n%Z#_cAoWg2<1qKHd!pMj5h++X$_oxAm279Q5! z$^7P><VqmOqq;W1WB9tEAVsB!sn0BVV~rrJIxR$nCXvw%2Y6YngWc>Rm?eOt^vg z%t0?GZQr=N9bErs2bZ=V+&z`~;x#WS=`CR`(}#Aq(Uio9)RS9Zy|PsolO&d}8N? zN|b>|CYm_$#=G{h2%dq_tx<-FX)V*pHIm_ukA7fZIrp;{qDTnf6^~ z+5eh;&|V_1c59z@n^C0KiJcr~g8-MvfJK1wKQQZ)2Xj|hWJCC0NH-9D{ee_o}GIJp6N92WfZ%L)-)Y^ zYi^1bJtn!WV@<@*_jloCtT@?Q%e2R^Ss&$oHy$AC1HTe~RXu~&M~Rl=W#X^UvR`TT zsHRo%i9}&-KeR`cCk@mQEm}x#vtQL1j(d_kk4k&-yME@8d+Pe!*@U%Br-jw;j(6v> z>yVRk6uWcIQS@VSDl%a$)9mMylNTNiioW=Sg;yIbJ*VuiK9SYu&LOO28vl6YInN$P z8Q3g$)Wkk`hU~)ad0?*J=j)Tv5Y{s7b3%RdtZP+?eHfF(IIrs7`26Tvhm?Les%Z&p znSNil*ZypSmyY|v3?@o;RKyJKyzP-ii;+&ORiB@Su$F1=1`c~dJx$hoxL67vAh9Sd z%zWS_Cb1}yObBb4rXQ2}z)umuGUu+HRkZ09wI6P6f0t1RYfaPdJvaRrpM=Zv3F6wl z*RQu6c%PU5+wNto@K;A4yU#d=Ghr>$#J<~ps@}ZxGgxfbJ+$R}L|VdHrrQ-o1kQ6J zY}cnzBJ%Cu`OMOGpI+;GmLaTVn!WZ#x71sa`m`^R{)yeY*|{C?iM2hYQho?)ndUAt zw-ohB98th!_qurVE3bIq!mkf`!{||udep)--`ad(|L?zM^t$^!td~h|32T{-3|;q+ z-0^LtzxWa&{oFG+(&ub2xo0q8 zEz>h+gT-z|mA40c*$(D4l`vsa-Bh zU+$i(9P#>H%KS&3C6Yt!rX$x@5dowQT1B~(Wv$449X*j8$}%0f*1=t3d?NQd@yO8e ziO3TVXTn;hIs0U1s81TYQ%mtBtj_pE?%v`PS-}$z$BkMDYnkS0#>9)F4TZnrmMgbs zUK8Arn}qOjyfwyFOMTtkrr^6V5S#PmI@YTV=vp^yAnYs-JHO=a^s*=El3upj+h}6ZrPM z|MTUmA*_|Vtl2u8V}hNN8?RZyIVRY5xhb9{oMYnZ|N80t!VQP4hOic~C^u8HbvVbw z`Mf7j-`wz6rgtZb=&(`4_6NzeO3Fnw#q=^hxO;{_@(k$T| z6If*;j@H^uSSyj`Y#q)qkr;KBaE=L9+U2*@IH+xvb4;-2<6UQotmmQVY_?U_@|tH~ z8D8fmtab1K4_+8v=gu*axOBEvCajfsch!Wo@=RydgtcDy8r#vK47&){gaa-hBB01DY9_2@I#E=8_k%kIZc{&h_AgFlCnHLwy^gPvURz~iNXvBX`S|2j?tO^#^Uegl z&D%7{c}J)I<|lXV)-ug$V)B*+eaL-D+Dq+@!XwvKnMe*jG#$D2yxwru-zm~6_H#y& z6J6dZ$u3$)*Mzl9BVTiI-bfkWOq9E7-^%)Vwr_FkFLHCAXf4xz&RuTRxI-={FW+)- zZ=7e)z7=T+YnkRgIk&jwEhOIFDZh)GetRc(r0u;Z^Bux>p{CpSeRvO^RJ= z*LKqD44N3yGF`1?d>3ARG3yo8RoUNvw`>v2u5%Uo*nh{Ohmh?Iw+!BRgKBl!y z*ZJsHR5urO4dz_SD0&rnb>+F*WYuP^nQq?-`_*?}yw+t~yz}3?dxph$YHH6egG?Rl zSG83ptYtbU{Z$jzidUK?oMR#5!#O766K4tMn8=x7)r7U;QD+I~m|)!y zEzJ^MQHjpb@@j6!%37v#Cp%k*32S91S~X#<>`|*Gti>sVU1FASj)}x5t0t_Kr$DPF ztd&#IEa4mzdE0Q+gtZdGt(ve_PGqx$b4)M_Ja=9-VJ&Q7cnvzo1are15VLiduof1> zyA-P?tcA7b$wq7K&M}db#cUlWthMqQG+`~T;FVitt=tW)x>eR9GT;VsmT-;L~Q7&roP)1A~|T8j$9*4o?hi#i+9fbC!?D) zZsb`aIkeYw=WaOC^zY~4iiIKl$-Rv4$e5G4)(r8$ZkpGzK)(q z4rQ5++}A-r$~nrOA?uNHlit_S6GK{*oAg@8_@L8@aEeCz3;1rX$xn#!sr3xm)s`h2KKS{eJDa32T}59Y_D3OqoI6 zddd4Yxviz#-1XKv#_!1#!dj;NEWJP76~gZ);c<9&L%GPuteN+g#wW-^Sj%*t2-Q}N zZ#m1k)~hpnJ9CiqIv*y6w3vgW*ZJt)4&t{mGh>zec3RTD2VlZl9;wJuNgF*~xF$YPn^D(|1ENd|PIc0gJMy_?3NDgJ0j$G?tjX!m(ixwXJ(+|&c zb9eJ@2aR^U-`C4;et2(b%|BjW{rAhaELe{R9P*PQ|KfYk7(Hv@|4k9GCA3Uu4ZilLKj{c= zaDMQc*DUX__D-YRq#XV1>&jcc_kQ~kqjkSgzkkKe%LAWq-4uacMawku5AJrv==4YM z#VlrUn;)#d_{7KlwrOwH#2>zO(>(H;-+R{RKQFm*im(=aN^jl28fER_)8VJ#wy(1|jt?efeZc7^xhJu7Sp9~CPhy|JsF znAS3#)m4Af(5>@Px6YU1;SRd=`tl8_FI|4n=tXCKt$h1)tzBO{+V^W;tuweBg4<$$6q1gl*I`GQYHW_GZK-MB&+MyQ5TjUdPJ8qh{YFJ@eEO(^}b`N#||#y4Q|( zoDwszv&60(y&&^^m{_8W6?;@6tYsQG5l8PuOMHbFO&rCDCbB@D)G_wEg|L=s92C~!b&dw#i^rjp8L!Z??z;=y@pWSp*0N>T7TTUWdSy-AamVM54!Hm4O6+?syx`e-0;IW$4O2jd{qPE}h zx}~?g<%A{PLdibC+++?&-|2B%uJ)$1zmarCf92z;X zcE4&%%;34r?<VTmWg7YV#~wI3;~)RMT05g# z)(1Vzn(%o5d1gKeVJ*|hll3uPQDx4jceTtQ=}jH1!9uW}t(7%M`ple{(PakXYuysJ zu$@6`nMR)YUH1de5NojiAU*ch`;yX+X)V*~Q|}r2IFWZN|J23mc!}&$$df&)+Xwrb zJ8R)TLL*Oh&T&j#q6}J6}IT*k^$|GKkz}x?IvT3N10MWjgis zQO0r?mz2LGkT{xMChG%vvOdOfNg=G|_!{}-^wXPaJVF`Q1nM9=+k< zuNGet%do}tKCuwCAJabX^ggkyDEmHp1AB>Rba3CC*KQfTB!q~BRd@2TxGSY4TCZ0)8uD1Q#3r2rB z?bPD0qGh@zTo$$z8aZ)LZ53<0>&votHBOB;KIR#tbB}vZxkH|;+HReXLRiZ*^2s0D zcXZh+`72|-LuPb&hnkqmv*uB6iD@miN5_Hn#>uH^w?XiBgC{;Y>D)oCX1DD zqA$M0<8E(&952xl)-vsRDy`~IEbPwNC)knWsgWDs-V@d`jhwxt-J=|(dVJcb7Awgf zHLaCZ>rrp_509=b+?L-G)-v7B$M{5F&L56gu<)bT-D`BvkzXs{=$zbsn6Q><132T`~&V5jS5-xXS>}t99vE?IAtew5KyH6~HwM-*VdOkiI zlo*b=ot)w(=Ni&;POR~6PF_C!Ow*GnoY76I7{#=gkxomH)4$U5p0Ji_&rMxXc@Kb< zlXJ%O=;mb664o+}Jek3^x^ggkiSlH=vXW?>PQcGCNG;(f2`rV+^ftN{p@oK4$^z^AErnO8@>KOODM95fm zd^nb$9T_?Md|e-j!mVW*IXgoi0hAcdD+&vbziO@BXHILGMvh0VRzj>x1Z!)+Vxnd1 zX$fnYMotUss(0IZ#|>P`=9;f?r-RxHojF0 zVJ*|hSKateqkq4{=Q;wP$TI_O2yN}`26MOXIr`IGPAPZccROtF(bMk9-x)PAt(6tk z?l`Ou?(?yDEbisM*=O|7@4UafC*ifyu8$C26Vvo&8+Kn1_y&L9nYf*IeLBCmh`0RB z>AQ}8`RYr{J>~D8v*T#=vQd$@S$Nv$+7Iv!iwWYAXqiUdX}cXqFDQS_1EO0Si<}S0 z$@v31<+ibp32T{leWg_ksRM7BaM|#dBEitWSIwMk76wkX@hpe0jzw{ZSr=0Na zawn5rrd?4y^9nJ&22JNwTyKha_Cch<4a+6#?moK19Y0(ChIH;*xnYSNw}kIuOe5!} zxO!3E$sjgiOfI>7r_nRIzp2H}@XfRK8Qt?^tQ`8jW#KN#kC{w zBgJ-Eov~NELF^d0vE81qmTBaP?T**^g*<^})n+}jQnLn;PrT1gqmzHaUo13XEz@5A zWz{b7T!LtyQC#cmdyYPOms9II6XQE8P7}*_-FvjvZPd}N^PcdT!!&YkziRBmJs)Q& z>?h}NTevO1C9Gu{`IATNI(o({dD=IL;A7^^nnRnQXCTBav;jOTpBGZ1@yX6@`t zb{jqD@n0$TW!X#W{=w4{2y2-}e$K5=8@+soQ5{|UZkZ3fW$s1r!Cyc8>7%`FKd-!N zGUKTWF|B2KauO!8%zUs5=RSc^_|V7q9o_keV@oXBoNIf+TBgh1{)&A^-+bzEbv{Pj zdz|G-05cd50R4$a?A(3-{+LonyhNSz5Y{q{{PN9r9-Z?KeBBsJ~|orjhUa4?B+b@BVsv8Qn#m2Xm8SzvO5DdE!NRCLF?ArtQb-=<;qmZ}50`W<=k7 z@{XhbJNEqYF7I8o-feWkZTtm$JVOX;nMO`5QKO992s;wCH?Wsz32T`~e)~(FHoAI= zr{Rt=ATrNhsmRToj}m2A%QW(2&e=KR;n-_quRfoV-q>zWOlz5*tSIj|UY)GqcsQ@- zc8{`_Y2=Ad~zk zqtN%f#O?GvaRR-K$3c$oY6)wZMxNXhjpw7(!OB_t>Zgy6yw|zqz2HP)L;#8H+B(RI zmTBb4sc7u4$}<+cPj<-Z6Kl?&y}v5Nw3g|~`mkrf60@Fd8OY6U(EAc=nMTeHboFqF zknwPiqqFzn;c~mv668e7G;(&-8uju13U=k#*-;pBV!M{GmTAY@?XKqYtD_pyV+$=Y zt(AEueG)BocGaIbxo?Y=xLnd3yXuK)Ez_y5?|C_YI1;vn+wxn&TBeb+9?J}JD(d6i z#8lp2nb{`3>WOJB)7f|XI?7oMB34bgxoP$)Xo+bp(~p^d+UTSYa}wq>5xb)2@jmXY zN1-LGWg24#h8oea6TDIyjvgtbf~H~ql3m-T`DjaO=kX{|*1q^E_|Q>RtVas1$tT4Gu&<&r+>$M|fp)SV4RSX|D?cUi|unR%J{mH&Gb!^Rw3 zu>8b3-icabTFZ3epx%p?yKqJ~9)NkynVj^==wcB$|y_?ELg{~$`uy{Kbc(8t{q4J{$F1y@89KwRIH?r8LVa6{cBrgOT?eQ^UhBm{bGxA%5O~Guvn$= zttFZthkxTeUC+I9;xq1Y(T<~|&OEQ&x2LywrHork*pr#ISDIZ> zclgUrqo-~6&icEe@y#8Un3F|IxQA$&PM`X?q&#DBOYGI!R^dhSjAdFYqddJTX5G1@mYCK`xujhl_ z#wESEZ(q(0{j`?pNhC8~=f$_PYI7#@+9tj6?RbVlOlz6WnlJs}xk2%}UTf?O@dn88 zD=lFy(_WkX=$5mA&wb3n!}r-~v{mWnwyOo3Nft}|6$M9dxqi@8O8V|Tc546C9Gu{Ilil3gT>leIq{aPyPW7rj}LB% zX)V(^Q}q$Rh<7c$Lo&~~N%M}{64P4QUrC?DyN=-)#YvPpnFG=j`}Q+fY?m2KbT>_B zZfdJK&GM^RYUeMv{?`V3&!69Yk&?eUyz$-lE6<&gNpFd1Ez`)*l+M4S%3r}RwAqwdGUXF8g9Zlktvrim{G@1 zzsQ$cRwAs$-Uv{8Xv%bVRCiqS& z#H=qFTD8ypzax9^GwVyTQqeMwoaTDZ)7>vYJ1&Kfj6dSedX^ zR`XwOnD&V#td-S#@^g<`MIF}4dYs)2oMR%Zd5ibGW~vSo*2+q&dz5?boFU@4uO2y7 zhl$8&Wo^#3Dq7TTt*pn{o;$}x*5>T!ny^;Z^vFrnhfL3j6OkL=-VkV+PS2Y;pCPOjxta4qI45$M&GD$u_9I%w zGNk2^W|a7MRH?%`ku$pVyjkZngy-fXmu|AcSUJZ8e;0&0zVv;5p-}(TRyoJSv4?L| zi1iPD{}kaI6a1AA>iEa)-#tY*#{@nfqVb}o=O(Ph-vxnK{@gKBbvVZaf8_&Wzir<< zML5RSLNAfoAN`$py z`OO)p^uswOveNebEAK|R4inbOx>4KNw6bny zTNN#8w^r85>{vO+MApshSedX^EWbJ3m7Y7tL{{4DoSU##*3GP4nXp#Y%B;0J$3#}z zthJl4*2-(pgtd}B>lugE%o`#fV}tl*^t$ zI&z&46VqC>Jv4INaTJR-@h<+6a^rWAt0kJ4)}mar>|N`vGyUL+wPQHnKqktp^TGRY zA*^K@nO0RRp==09*N%Tg3;*HP*4BiW)}kNu+}^c5&4A$dUT&}NTickLCnh1RW!n9# za}B{LXKuYd=q)xq={eRB!q#p&)>CItW3`+f5)Wp4u#AjIjT22wYcW=#Cw9d%FIolZ z_Qvv==d6!@&U?aIrrp1K&rTi8hs*LR^6F{{YngW4&3r7UC3u{SC@U%>UDu%Iqgl14 z(~s&CyD=(%i-j@EZonL{Uop>&u2*w?9!1X~rnNE$+3o5KYCcBkEq=)@aSLnN)Isa4 zWjbTkd$?Hp$m<{$j#eyxmavv-ubVO-r4Bq@yg}Ik#!nZRgT#sbeDnl4j3_y#<4b1d zV=<$M|HvHR88YfMF3}7wPHQnAp^?W7Fk-A z+GDSH1Gly%rnO8XS6{;ZM;+{L87nM~c#e_I$t!kTcgzseT9g|axt<5N?mXOyZ~e`- zS9EjkHwRwby^(OAU2a@@$(HAoZ<9^lTQXrS(>Hg|wxQ?FI zOuxy2)|-C&k@6l{p0TzB&*GzH8kzJOGo0Ry)zANJhcB@!_$e}@fYx`q_oR^X1bc4I zgtbgRynD6{efLe?)%D{!okw`d?f<&%13J6noq~)>TKK{49XRC4`z0oXB{9kwh64P3yk)QHv-sNOI)>&)4FLm>==r=YpqPKQ$|Fv%?n6Q@V%+oq|;yp5~neo%V2foErOP?mTBb4d~~)u`pBM-F1G8kGSXPMEx*`qPgu*e>n?M?_|{AA zU)CURAyGS?%xi-5ynojc(^{sH>l*B4aPi!?d|=yd&POlq-U=ZH|G}Fh@hiM7()f~| zu+}t9NqFtr^Ib1{_?NnMzGxdwZ}DpOb}eBo(-}i7zgWrQ0SEr7>>so$wR?Qr+LoBs zGL2mK4{Y~~-~2_XWBScAv`*>1+|j+ zmEYuP32RN$)KT{+{O(0hengp%_$%fBi^+Rrq)*J#qY+K;PqJ(ejjr3CLnc$nAW0>(8%lzb=7`;n@1PB;@5!r zbp>x2qqXqz`YojVvZE!eWf~d!=JG8hR@BeG@~hI1={L{N`eOHej`m$O6Ip9$nayB_XD@GCr~0x({|y-@@77tZ`y?7GguMJJOptu_var zs3SCTjT5^FVDYr;H!Lwj?1~=xw>hNe9lyGNgqYT%+|bC(W|_f-P0p`#4v`%HHpeBk z#I%-a67P_8T8lzJHZ+NzboZTY|B&7HtoWObd&L zTfXkSR}{b7oxpnE#~Y1k;c?n`AUPFMEoPKLPM?u8bwpx?AYpX(7%XI3l zewX#}{ChvCoTKPDwPOv8PkyOo>#1WEVp_{I@{h~6URWQuT()kBuVTB*6c!U-OZwzj zzb2-&C^s}R9=yiXH-6#+#h2JGxwjsLmavv-`>}d9c-2~UonvwFul8#8b}eBo)3M#U z&N-$2`dQU>vGyTLoUz@WAcrx*UQK)5)Sh3x;R$80rRRB1H@#)wMQ--m@)y&WI}bX?n|Zf9^fe>?jq(w`T-nXj!J^JO(zvB#^HSfIbk)>RoZQt)-UQxy^a^Lfw zNDiZDI&vLd?%%k#{QDD5+G6AGy@efbeqJf}(C&={{-O%yCcV~SVn~Z}lV0oKewVR2 z=*ll_N?Ggw?BG%d&uf3S!Atu(Nbkq0Cx)~rH|ezwYzo`@@9vk-F0hiaK6rm~i_KnDY?t>%Su2zqxv|7TBnK_ik(*W(g1=zD z@o9&ZI{2&g`MdE+pCyL0Xi3s*tKv&e*y6%dvTChGxhoUNLCbXH+A8iUd8URh@l2iE zy#tr^k@Q(&NQ*g0dOPRTL91SLOZU~>uD6U&d`Z%49kCLKAuZE&tj5+}=7aZXnFDh0 zILr;@CcUqN6;+5KEz`A*c+vZ>@zGP_sp%naq^$F=dzCsO_m)^ZHAHf#-E`#Is_}e` z9{IMDw(ux22ly_ul0HieY0;9T_w!MD{^dR1T-N!K=RCWVi#N!A6}i@t(S=A3W#J7% zN3L~@ed1`1w;r%1>p5ESwP;1Ib(lyFTBak{I=*EuYMeLykRX9K1E&%d zz5L&HFSCZ{-1bv@7QYgCmPigh!*t~8OXxLyIpJRK-IN|?-NnKwH|ezw6GK|`HtBuO z%jh1r^{X~Xt0*hJmX<`Wb#yK2iR7SVI&$p?c1wJPr{-5X>>1>|p!;>+X`g>_8Mma@ zI!p{{nXYw=*Lj%_c46j#ve>Da8?++VI!q)7Ez^-}9phFlocP4+H*s{Ac#d+D-t?mo zLt4y1((8PTJzR;Yv#U`%qdAMBUHidY(3dq{aNwrtIc4D`F2CiU%2-9N zb(lyFWtom#>*y?z{bP|icSO(r%HB@7Nw0O77}BELq}MvSK9soRKG*E>aeh}4yYiVM z>9r0MLt2!Z^jb$Zxzt2AclndQI%X3__i=yO zv-orGGLdJA5Exsh_vjlTHt!29QJl4)$ORLylDT|S2-=*!5 zYaJ$%lb(l8Nww$W(Otyv#;PgH9w&0G!$fjuuj$BrKT6Edt@Bf;owZF_-en^9{pgA0 zP?qV)wT|(MD!$#Ln{|g)oAcVY}9n*YP66uq}O?u`a>07SNe3%&0GF|J4huiPwmz=`e8yPF!@!_wAq7|9H zKU&{UWZof?gO=&YwT}3bZ{GXMn__YNtu}IarzC%)Eb=UooDHt}NqM>yIwkSfsQWX{ z@-t8W#->=w-M&!g8SC5piHDDF+x+@smB_V@uII$mg|L?C$TywA7u&OpM71iO60yDWz|Ahi?)Z3{Jlk4v$NcW-O?c9f^?}wgYwlCZ zjlB5un`^6jB01D4 zYa%&lnT}lR==xFO-JkvWi<|PM9CeVxn}=ERN&oK?$LqW&hO|uAI>wf`xXqo{Je4Ou z`D?xQIFa#Vy|ou2Ih18Oa;+n45KF`wVxzXc$g@Opuu;>IvHXdZ9RJ2=oWk3G{Pjc1 z!b{|DLneK%4~_SaVpkADTBd8Oy7?}5$U7f=x^xCTIYTvkEgHu>J`RkXwJI4Az zD{`~WM-a(D%XH*g$9Pv8?Xbzi%NnE(a%@Yi!PMRFgM}EoZ?Z$ zOT>;N_qN*;$)POMk=r`9>HIFyQhaT8AG9K~U-j|TGDLFFG99_r5x=|MI$z&3qszG4 zt3~epRq?wJ$)POMk^4IEa3%I3{z$yjg{HkvOnM*t^u&->Eq8MJS%1qw9iKk&r>De= zqUCu`dT&>y4u~Nw)3v_%-R(}l^3+6@#GH&Yu`gPYiG%x9y9kjSv`j~?b&O+%#eY5h z=u;9|5_95F-+OM2Q6eAcD5DU`p)AvpYaNODwmW8SGp|nXOG)qd!SZ|%Vn~Z}liv5E zL>crWt)jhn&cxJ_YfL@faY{duLs_OH*E+^g`0}ae?_bUa*`pkVMV=*+L#&oq3$4g? zj~Yh+qvt$p^AeX}yV$5#QDowvK4vIHawwNrBy{B3s?^c>tIgsW@EP`Mkq`adjj978 zIh18Oa#P2`bq61F8m-ECz!6g9{%la9C5Yrumg&f~jXF77-YsWD|IYIJMsGQ{ zp%uBti6)YRmg&f~4n~$4-2T4*v^nj~xu0`F?gNtE-vRW*kQU`8z1EQ#Buz}&Hll8kg_-fa_+;caWB#lIB!sn0 zBSTYCjTu6`|G?AxoX=hOiBccsBB#EVu$F1$w6;c;)X`bXg3EgQ=RQ@m_P_UW^T^bJ z+!EF@?Ye8+P93a6m$m6_pDlHK?#FMRN2U(smavv-*IoTCb>QPoT)x$pOS$WuaP(9i z%z21uEyjnIkd6#py(k2p)HxqM@8VL&8z29=LeP&{Vp=QZl8y}B{E~IiIcxskw@V#Q z=)Sn;l3HR~E9H`o3|(!Oeqh7SdF>;w?)%}AT4Gu&<&vKIsy$__ob%)hukYu>CAGw~ zR>~zExsEROL>U#HfzbQGa1Jf%L)-sLU ztf*24YY@*s?W}EUnXVQ{KOn4S+CFA(?th-}*v;R!_CbHocYcrB(v_S_{*o`&=zy4|nYccA(ZofLhT8#R;u3B$3by$m0 zzv|PgBdkRW|KI1=T$r_8=a~4nU*2!DxaFM&TIC!Q-?((0(M^{R5zaC3nTyvQown_r z2kLN+iNE{qdZXt)Ylv_^+``*-yx-`4PhVr84inbe{zvy6J$#Qlu7A&E{Dti>p7yKtw~5Y}R(e{l2a2x~E$C+&9U)zo1v>}sEvt&Xr3 zd+o68dgrvaV~6|8yYD{GDihXX7rp5n_gD>KEq3aiK77yB5Y~b|^OI|>hOibbJnr;+ zt%k4`qj13+?!6kqTKJgfK4|UL5Z1!SZ2aEU5!PbeT>H7z5!SlwlwH2^l-t%BXq9tJ z{N;|n`pU6a4-uYo(|5S{9^2pjz;y@eFk!8ozx1}pp1tOJt0AoQr@LMI*#Ep~b%eFf z+2*c~8(qKtYU;4o^EO}mahu(`!Di`o41PgX}*3oAMIt{bhU4r^WX?4w5AX=yctwcc~>_7~jvcZUuT&N1=p z`~J%Xr@n58aE^)p+T&g$&WW?Tvu)S)(WfnMJ!F7z9WHnG&wl8FCvW!B0m3;ZmfrH| z3y%2n5aApX*PML*1^fQ-uz@<9W8z`=9bNF1yB;w>IL8DfZM5FY1_6hU#mo*{q6urQY>6hUWjk)~md1W?gzp>EMIIAHZV79d9$V!A z;T#jMx?%klPV~+(@$4UOu)?i@b4eZ|-2hwu8FW8yBay>x!~KG8WQKKJ!Y=7;Yr zonzv~uf2GF_+HdGCVqa#Mf16bZEe>%CSLx}7tIgfCpyOjB@N#vI>!X9-C*!O(K#j< zk>UG9=a^vBhwl@eV}h9-zE5iE)6O8)sZIp9NFq^}-QO+^JoDbhdImZOC_VCSw32PD64Bt$cuohlv_{PgQ zCh&a2H^R;_fp;Cgb#{&kJoxZ47U!5?ml(c>bB+miqTzcu=a^u(8@`8gjtNQ{zK3&; z30gaR59b^cjL7gkoO4Vt>cjVN&N0Ev4&TE$#{||hd=KXw6TV?x0wl@U5D2 zOmIRTzEyLM2~Od|w`$HY!AXDkR?RskxJ?+oRdbFBN*caZbB+mGJAA9=921Pl@U5D2 zOfc%hw`$HY!ORZdsyW957BhUScECB`>irV76t8AK*4mY|Odqx9_A5N|a*m0Ip7os- zJe&z@oxIOuR(KNU920MT&J`>8L=)CJ;`Q6D@TA%~Ccb&*l`D8r6V|%?Kek=r`G<2% z?E3nvR`9zftaZ^UI+bzeg`c7&X}@cJgX z32T|oo$RU!YklnUmn{5hkH?hy=H^_7b4-lo@%!zoA&SD!(=XPV%-+U)51{r~N+zVnFpg3)S!3*fQx+>VJ6#5f`^a@#6vnYJyogv&Dh zu^kQ_4bQm=Yvr6U+jHlb7|R1=H9!2a%md#6V)^1r8@e&cDeCCakxMy_?3NDg(Fj$G%1_VZ3S|8LvP zTA7=B$Acoz63Ic!w0{di$^5Hzn6Q?A*-`a}2y10-=H@=Pc}qCQMEe_)%G6ewu$F%p zvT_~PYJZ!tQXST6e`&E2VXgLe6DtwcYJUZ>5@9X>a-r&HK0H>|^6wBtmTU0y(zC?yFOee~|a9$hCA6$)POMk(>2V>KNBvzHDW!$kbiy z=|;C{6=j)@-1nn+2H1Gcxw(rrvsUE3A3c#A$}$}}{g`;Tc;vacs~%*n$ka_eGeq%l z19a=*iuZ|@dcMHg9=CRYNDf-2Bh%X2`?M^+9dBtr7&)GL(wF4WD%0(+&uT5T_a>~> z{w8iE!diILNz7ov<+@e=6m}q~sRhc@2&N0#c2FrwVO!)U$ZL6GPVmiuzs;zR4iO6-;+FvDF!m6zmxsMro zpO_rVG99_r(Y2_oSlTwuDeF91l$-Rds2QR}OVM(DO+S{2ea5YF9VMnpdee_Wq+D{M zr8-JUw4Q&pW!4(6;8I8A^n5}jXS$A*)VB)1TUr%@vJxkv6}j(uPb3E|(~)Z(=>u)^ z9yOLbFS>re!=b~!$$SiH(UPS1t-?x{@Bg_E-ebIb6^|S()lt&t=Ejy-2y2;+O#C?c zb^`6oZw%(_-LLoBgEs-3?#B}63R^#sh6#LC@C8q00`Yho(qGh_air=H=x2bum zPmbRSNP1sKPYh|9u66LsPrlK>SmkLrIeyL^d6q~HwVRGyTSXrjFY3rMddjj^&Th)yf-`HBDju4cEW$;c3T9IoV zCXzGVl9W{I7|;34Sov*;w&x~>v?w>}wT|)VmVQuH-aKKX(~rotA10DRS*9b`I`Xvz z#tJLR8#Er#$o+gQAMBUF%?N(0l5z#kt(1*E&oLX;E&{YaPrtwnZKJ11L^%XF;+ zf7tb-)PZ;AJu7ljA8k*1-;bUc(lTA^7>{nLBcn?Wy-nMb-pofKhO|ugb(AlEF;?k0 zIsP?Z()&7kVo1w$tz$e^qs#|yf02`2jdER5{q9GsWJt?&tz+y<%8JU4LylK!qH|fK5@ay+z7QN-S^Jqn` z{V?c1QreLtG`D(~Z4D{|kDo=DDgZ{5H8)uFEU-I!E= z#g_sg@E>`<8GkkTg233f7h*`ubnVCVUYqYQ#9q;gT>D`nIcR0v(288^U@wQFA6Ojk zO_PI{$XnM*uXUIh(lTA^7|&p-gVE)^X>w>u-nx!FOC*QdO-HV+n%<)_ANaxi=7T+| znGd3j1$sWDMN5)iTQ%)>DVH2ZAzmUxZIy{3Ez`A*@#q$x7=oO51IkT$t;58Smg!nY z*Q!1Oz+2LGaBe06}i@7A~}?0I&!VU5m|apSzg;opCyL0OxIS8ZMS#^><9lrj_oz+eI3|tA%?U} z*E+^KP8qALXL8)z$g@OpsNHnr+N!blDRpGkQWl;lzsHE&*HOL$$r?-!Wtom#>*%bV zn4#2xjpC!G>qz=6;X0yay0&V(KFa7ax0#O+l$-QghlwFA%1wH$gW1Ej=y|N29DB9M zvqW;J-E`!>Rg3su>cD4YtZYk3pCyL0Xi3s*tH!>4kvjP8V}2#TFDH2;n)JxE4im|l zrc)B>?OvN-^pnnSM)U4*(rX>#Ra<&Kq-7epzf~*!NUJD|-;DB}Had}O9b;=RL~_tF z9hvm@G?7-3&M#T>Ms(6^9k#0>Ez`*L>u4QSTIF}o^X57_k!u}epIC_Gpk+ET>Gey6 z_!DIJoEGK{^`zH2#;djvLt2!SbmYE{W#sf6nR4?kc+#6X3NfT*8o7Q&5Z{aJ_y28u zk!u~}Ra=PUOw%ce^nT8Z{kVtBbKcEQdaYwT=Y<&3qST}#_v@n}NcWluQR^75k3tM- znMST(L(EvkN+`>{jogpbLWty0lIh5#*KZoeuJ|ngzhx(#xBT-<_@vi59QzDunMQ6; z!u&#kU%!*?a+6-`U^THuR?>1ha{b0+c4x}s*YBkJTMbFCbudmLhO|s0&(24_M!-n( z#S3&I56?%mn1j%fq5FO`Z71F1Amfakg-?3 zk$`MlN_wqhY`cXR(lU+Qk8bHlT18p5jL5Z)>HPy+q9oIiN$=PB%zU`*?dXozc_D_h zC^hNG{eE5uc42-EM7no|$fUPKa_FJy$hGI*4ZPOa88QlyXNlxcx9P~F_j^?72RZ!W zh;-=uUMVu^Es-2*HyydQ$`M&wMGn1e2id&r0kF<((k7#7lTOv8s zZaQ*pmAxp|PHad?{_R@QXNe&#)5yKwE&X5}FbAZ2L?e^l63L-<(~)bd#&*@uIbXA( zBzha47@73hIgb|gg^pb7@QPxs5mQkTt0=K((r1YwEz`*DZjjL>ojFMClk{50cm_*L zhO|s0_dc;9NT-E~MO{)$3~8B0?stPlWX2@F+r+Eoj1#%`W4z-OB01A^N+P{))d)GG zi)?#MdaYyHx921jEz`)o-z}|5&q+^9C^zY~jrOij6wXNlyXWjZqH{aLM8i3yKS z(rX>WR*Ce7v?w*{$iWYYWnqqLV_xbZy+()lG?zDAMs zTE}?*SW(L~a__H7Khi47%2#O7iCpWj_el;~rX!QyM>3_3j4oxlw~=cdj>3~eNv0!{ z-tRc24t_+oJXq-7ep-_@E{ zQI@SQa;;;$a~2{wl$1W96`AxlhRd(XNw+N}z1HCvZb-{Cay#ez0x;i8;P->9M|2|B zIy~peLCbVx()&7=k+BB82!f2o`i`Jyo;`CG+FpCyL0Oe6Pu z)Cf7>Sw?obNw0Ny9~{y$joj`ZX%*@If^yPp9pnAHjP8(@Y2?0+Vu|()lpBAQ^rntN z3~5nn(vkbID)UUw;}bnTNpHrg5JOs&nsnseCzhV$$@o$L=^lm1q_;$JsNHnr+N$wx zQ0j;!Qj)bIliu$*O{*x&bmUsc_{35OyboVhpd`FOzJ3sy^xlgWA~}?0I&!VUyEER0 zbw^2FMUhD_Yp{<1l0#XhBiA|{x7!*#&q<#phO}r$(vf@LJ~LMM^L+85?fKZZ7h*_@ zQj?C{@8=_A=ipKE<&LEH{pg?e4QWwQ(vf@LUg~g;{dv-B9b?~Kh#@UXO*(SFM>SSL zx@Rpi=`E2QYLAVf6S-+sv88z*aC|kSWg5A+ zt3~7x$o|q;(rX=#8HTh>BloMe)L|muKSL*Stz+yN3XvSNOh+cYpTUNptbCOXoyfHg z$0f-@EA^ojne^T>lsZD7hl%7+mg&f~4o3j_ZV+Yhy^Q#hq|XvVTBiF}_4f>XTOwcC<69QQ z*JwrVd)^btLCbVx(%b!eU=!~q9sYMt@24Ykv?VFG9uSHOeBZ0Oh>MDjQdeMgDsJ^Cl*b5(~m+7X;E&{ z`#SpfEVDYvu`MONucIf1v`p7JI?uq0D*lR|$Kzx~DL3*gksP#4N3N}MEE-FsENexs zb(lyFWtom#>u?O`QS{nQ`YbV|MN5)iTQ#nutXgtd+vHGgVz{I?brfPq%XF>7(Na7E zIgUk=Uh6P1q-DC+;a!c@nXhlLg86nAT9IoVCX$1e>Byv)U9Eouo0&=-%qCyrLML*q zV|oon%XDPu-nW;s@W^~oige~6Uw%q@KUV$y?vR#gT8FJ< zNXs;Gf37WSFa+tWu6+3^>9r1f(IG9<$o&bjcpr~DG+!-4CvvT0>=_D?9JEYFCcWRI z3K1)zEYDixT1RJBBkUJCImo6XlittA0y1O5*U*regM76t>9r1fh9ND}$h}>aIzo_c zTS|JZV;n6NVo1w0azE!YqwBV3)N38yafY-gHR;IxoG&9YQ+(kInQ~)Skx6ffBywFXB^J2nRCv|`Ql#E`>|plETcQ5MX5o75-WxCeU^}M&l5abZI#dee4k5x|$X_>BdVCjr7|Aob|wvp*= zzUmj5^xoPFksN9_9l0Of{#_hAHDB_hBzuF%r1ud(<9#U0bmUsccy#+YV1K0~<{NXvj z^mZTg%Hh^A`(V;*9o`3rv?w*{$n8Gp9B#Ao_0FW%I=l}KX;Es@k$c}><^yYw?RuV* zUh5dIs6q^BQEJkWTWe2Qq;nIJ^%0r$mPih@n~q#tHTKk{Rk0FE!fU6F$fURHBRQ01 zI&!UJJi5hRJ&Mdh)<@E3i6Jf8k#ywVu1X!w!IrY_l3wc=+f^Zkv?w*{$bIZn#>zSD z8Hr1hUh5d|AB7mwqST}#_qS?`$jo5k5}#5c*E+o8BxjmVNu>96lvc%ylFsfEUlN)0 zmPih@n~vO%Zhu2hj1!;89ed_H>9fR;7VStna#`oRNxp#02{Pv>?|n(Hbr7?M7}BDo zq$Bq$s>}x~CwreqG;*zDysI@cNJ*w6liqr2P7U$x-en@s63L-<(~)bd#?exlbF3u$ zptU0Rbu@Qm`97`b$hD5KC6+oElf>=Tid^e3ksQi09l6#qp24zevvX3G=Q-)K#E_Qh z+N$x{p!6dij-0Gi%8firBnK_ik(*W(+vO=zY!?g9KA7}bVn~aYB)zuEJ5Hh^a=h-6 zUh6P1q-DC+;ra0Dbla2O^rOt+kQU`8z1CsdjrSqP(M#l6B01D;vqmY<7 z>9fR;7A;A7ZPn|4wD}kB^x`!i-Fe!@_djKug=;=}pQR`N;Itj?zVmnI5B~25%x^W8 zNq<}?&&`>zmTBY{cl1LGLD?s)@xU`XavSF$U)z=YgvT#!jr_unc;BnO_IX;?64pw& z5NYk)+z0nQ=2IQDOkbQs3!%5!`OrtYzLuEQO1Y#@dVbNyA3ukdb#G|^CZ}Ra+GRown?bb3qOQfu6deV=t?03ZG_u2FG@-6T2SJH~* z=Np}r*?uk5gtbhMzku15#CJJOILAaAg`04W3CHto9nLX_oGKb4+B9nkAfLB9Zv&Y%fv@%!_9 zDW)Z?Z(Ov5b4=um4&84Ux-Twztej&aUwW7& zoMR$i5SS&LW1{_nfLrAp6Zzi3Y#q)qk*_Vx63#J^ZxPHA&M}d%6wDING2y+WoeyhG zuaC(-=&^!GjtSO(-c*>aBZReB|9M|{Wx`sl|Gd$jbp zn5TTxtF6O1Ca{HkeX%8+V*-22_W@hNIVP~_e9f*UoMVD@!}sWBd+r<)tWExE=q%wJ z6O@$i#ccbe3?A31*h>PRtCelf zy(YY(*n@e0Vs=+^jtTbpyw%gz;T#jh68t61**ctKf|ByIf3}X~n4q*yQ;vvoMf1S67P1GjZJ#{{FE-`BTy=IL8E5lHb0! z1a}tMQ?aq^miApOVJ*|#8m6R{aE=M?cZVYY6V~Fn+A!f96Wm4($0a7L#nb*_!Z{|m zlNt8ZCalHV3B!bQOmI^)?AuLPi}x~y3FnyLj%;{0Fkvm;@E9hXV}km!OSD#E!dlcl zOgP5`<2JnGn6MUOI7~Rl1oJ<-^kEzyLv7{g)0IVPC@yiw5BVZvJ2!Z6_+6WDQ{(6x1# zuogBwOgP5`>oHFn+d52Gi?umSIL8E@C{Ik=I!ss#A2Uq&_MC4&=Bu~uU!1b-4rq1O zchP3~Ztd)u(>bm$-?N=1oMVD7l@8x0I>!XxfgQf(bdCv5!o#lsW~t9?fAIX7V~-rgDZyC$sFzH6{j9oEVd>{a)}T6r42 zYQkFWTSzOl%36%Vu=jD#t(B+xt8SIG@_xXo32Uu(op&e2s~+SPobLKndEb&iRZ@4`)3 zixv*w?>ff>GB@5lU2eV4G!ZS|s;^8~i$3Kxx+QES?h~^)e1~kpT3F2R9kK~)VU@#o z$R@0Xo3Ix2@Ex)VYth2tJ7g2qVibn&kWE;NksiK7HeoGnVfYT>eIS;X7m# z*5YeD!}p2KF|qP}qH|2(6LV|STB37IusaVEuER7wc=&F~gthSV!#71Hti@h3eD7nz zTI^TDw>T!O#hy5PCu71|?7PD^EGDc4J$zqc!dkR&`1ZquwHSrry9^W7Vx)&}CQMk1 z*&M!SFkvk$X869{>%*4+Ykh;TwOtd|%Gds8V+Iq}^4I=WuESdS;^M07uvWeUxoW~% z`7+|F32V{9;k$76+*6 zKl*VCr(JO7{0TQ7zx0`nA5-L(u$JkMJ@K5SUw!?tN zOZVAx=Y@S=J(@q{Gw)k^-m#?5&6%*4>HmA!=}Wg9_tZkq z&3*5<$Bh2?hZpgC{e?GO{gI{bAGX)Rdw%8evgM^)-?(cjsSu;Cj`DlUhhKf}($cGTFFic& z>{FID|Ils=kNW9X=G&fkt?CJDnO^JB=PW(_>w6Zut>c$BT(m>TUB!|M=5O_g?q$CA}rAW%|f>UbJ-o2ku(vW(F5CgN#*X@CVzyv!8S1 zmavxT^qi9B<{tgi4=+7+tBcB5B?nu2>pkDM^xV^SExip*dP`W#w8yX^7DF)4A!teJ zm}%9bwM?fDN}8K{;$we1zs(QUFTdkGZ@bSdZSl&5g|*H)Z~lt2j$3-;UJIokS8sd# z(ie_@TuE;UYnhHEVlmyXL0-7V^3y)KV_6@MyZ!Q|FCDn!!o`pJ!u+FN@%g1E?YI5H zUB7+S{3joO&eGGq_1Kc$64o;P$*r$iI(nBU6?$%N|2wX=yvCY)m9K1E{KL;Lt#$2V ziU)|+gKs{$uMfE;tY!KKzrAYd=RbSm*q5k|g%Gsegj?Ixv0yFJZ5^H89d&+pncpyF zOz^c2fAzPP=C|0Xc+NGR`Prpg*PS1GQ4`iOopGb2&ig#%?9t{Y+@XAb$E)*OZ~NHN zpC7(^ndewZok0`UGX1Q_oxQZ>rF)O9r1PRXbbhz|0xceA+SmSU&Eu91e&-X$Uets= zvgxc%Ja`%1#cs~a{;}JxCof&M>plz2?WSj(zI5qY&n&aHeA7uw_xsj9lNsy@Ynk5b z#h+N(>g@eWNoB_wb$e8?tN3vCr5j)Ok)>^av{xzjx(na4wA-$Gj_c@lwVtq+>G*j{ zD(if{^CkH9Wv?8Mk5^htSj)7>t*ofUZVi_24!q-~-(Fh#tS6V9CA%p5)jrpKc4_Wa zJB)XnZq@dLwM-wj-{njHcJ7YjUCl&Bk@b-{fi<6zCcSNywM-|LfG$x+zdmez0TU9ZkgJ?6mPQO^nhK)-wH<%fGU8^=-Q?K(|(M%-@~6^#1?cy+ka3 zyW!(Y_kG{fOT4r8Wv4A&xOO5LTcWj0-}{2kF75W#y~fd!<97GZqw5iA<92JA&YG_) z%KP90KJv+>uYKSd#rMTt@g??r?LKHN)3Isjc8~I|=Gd^^2fasG%d}(0VppTiuGniA zvxU8sp)rC7Q67>FfS>`O;7SdB=e$ zV~D_WrjF`GRmUjhl4H89!+zHuz^83{jMnd3%XE7_*E%0%)zT`j+7;LN$Xcee&TG%L zt1TplarcS}UFXAuwM=IpWHmRZiBaNn;>1`6QE{xNM#!pTWG&N);p=HaQTU>3&q~D` zxV5eATFZ3elIlykRwW`EC0e4riI!+d;;Y&!6V@`F_-f{Cu;^AL(&y9|PgGBk^1F-H zGTrt=9&Rxnjvl()_{9E{8lSjmEz_>AvG&DSJ3b@Uj*rUvsQyYTYSCJzvp%Z#(W+e- zI!zGcu6X`f$V{PSI`dQ`g#<$m2Z)|~O{B&fdNO*r zG!I9X6;Wh!ep&2OIg*i#(yQfukUB^d+4RBr>Ut!~h53}Ce3DNxky~nJ?kJ-oifqp# zvTD>QNBYudN14)j$D(23$RLVrpAEt?;Iv)R7x}a;pE31(UJ^xi&*QXqmwua3F{9<| zTstC)Z2I|ZKWeY8PKsV#Wjwqeae~BtWSMQUy&sV;L7sXz3(YO%&IMIqPN~O&+|la?0;!UhU3v zt{?9Gy1i1y+>FoF8tPT z`3?3>`MqN=`0j9z$Mznd?#LjD><5mvvMHJb&>?+3~`PEtNS6{w$xXMAzPkfs@4@;sl1@;npUKC^yMfP#4-8|g% zFK-&>8I5K>A^eBR#m}!aIih z?X^de@Z3_9K@{2ZzVegd(x>k^zK`O}Jwxfa^-g;36$uMR22o^NH^YkR&OqlJeWQve zmW6(M+3*X0-8ZcpPZmW%22o@Wmb`5E;47>jjg-_nA4ql?-GA$VD~E@lxlfX%JVGXe zD6$WF`{l#>J^PNcwaVu!3eXq8*cL4gEm2XBK@{2SJ4s2clTjmABc6Vi+2H=+N3Y*4 z-2?NqlrxATd-Y`>9B#GtZe!~MjjFBBI+<8gbq8$?RyotbnkcfZ|8d6Aoq=L^?K!Os zqi|ZKwC62@D6;doof_qlDCQ)J>|6(lBHQzb=oNl2yX~`1C<=4@5D^&}MA;`wgRM8= z)gUFcr9^ouONmYpmaE8LkwFyMma8~_K%++1HN_v)B5GHrgCnBIwr)lw(^!Tw$L45-v|>$q%na22o^NZ(`4N zr>2`S`H%m-<(A=f-#Q>!eUC&w!=sI?GZ{pYz27fx9R7WU1IPF0*ujdYhFiYy%i-=< z>|V<6@|2O=5~9fd%opz*F8BBw%Q$VLkJ=3Bvl)6PzMq)1-qauSSvU7U{tsSu~S{z!U+z%2(Hv7(^zKT0&-IhJKy$aio*ry_jY{x!f z+cA$U_v(@8wB-@T$%_o4JX^(vo?pa?9F@l_`$`F?EHx$3K2`ZodrNtJ zd(nsh$RLVr`{2l=B4!vdHXw@ZEM^!HMK)uJ?95r)GRQ7@ePr8hOTu2kmSne_U^Lhsw@EjszAt*^Ud`0-s1NS?*JCcl)A ziVUL2roSo*-D@}5iBe~rl{UV7IOll>rE{P6K>n4d$(%tH*_M*-E4xe7) zG(m>tRVNd&J-;}yV0A}acdw|V&nw+ZqE9R%qR8ex6@~83btA6xhqD9n;hZg3A!iUp zw*6q_!jYNC&gafd$oJWch94w@D7MS~ibC(mH1FLFbmnARj$Y&~9pQxMB!eij*>{p% z$5$2Gvh>N~cn8D!Aik=IBAb1u6H9v@bc~}_WTeLkFlP`&w&S4K5A1pEy_N0r>MEx4 z>PCD;22o^N7Q%``2S?U5$tY1|XF50{ifrp;LqD*(mDgRdnx#)snB_PkGBSuF z+j|pHA7^eoP(*J@)V+!&G45E%Ac}13N}LTKi6hIX&H$7?N@X7V5Jk3SI!+UeWE`c+ zr}~tl=-!cR_*ycEBAYpf+%izAElY~2s0BHLD6%b;ZA514lieblA;084!bicvWgf@< z^*txNT5ZoF?c_+@U#qqDoY_g3Z2Q5Ohu(|QO|oJJOS?4Zn72PP-21@&Q><+HTn|s(=fIRh_6~+5l0g*NZ+iN> z!{yI8aGarU#$W&C9mmd64nV#Cb?q;wn-N+ck1F(>j4q07m1C`%+l*TGkjf`IB2)ai z+9!TL{J@6$r^Vn0GQ4Q8|M>lOL>cv6-~8F|{(TQf?@ikpbfe5HlBtOz z+wn@oqO|ST|L&&YSxX$4`tTk|2dQ1TZ6}Is?^B%5G4m1E#Iu#x_bNmLKn77{a}BfG zrP0pE{&sl$;rpleX=$TA^R9=6D>rg{+Wd2yOi|NEdrL+X*|c3LlBsn&s`Cd=yc>-seKDBa{i#&h0s^m%$k|a)Y#k(|>^*rW522o@? z)`|?IV(GVDyusN%N2x98Gf`y6-##;gC`Ec^JL(I|z(}8-W!tmw8y@og{gbBB6D7;w zo6(9WvM*WX?%|_L9x%R3m7Usf+vCsH{fFaP*(+H!jY?M37A|52GKeBuWw9e>fFzd8 zI&;^!n@O%(%Rm&_FIw;N;X%#mHFi+@@Uh=lXKP z=8VhE^!Z%v%1_uDC^pO^vPJ?&W^_?xs~nM~;=?PCTVs|+M{ghv7E01*ZEhv8We`QS z%3_CHY5f1W?+;(D(a{2!E&JgZfl^Z*j6GUWHN}d#Sz;!rrkl?#PmLc`p`}hMRxp^v9T4+PN}q< zw3V4xtGp(PKkU^MtJ@Do+|KG&V!ze9xgwh6Ge7@5{7v)dsmd|^^wK#2qSuxwwlpJY3ydJ9QF!Uw6uMd4X5R#MUB7c zIkwa#QDoa5hc{5XzH{-(Sz2X#0ND($v|@GdN5t)95Jk5AM_3XhPD53fcBQ5kz2i^ds8lDPZ+N1vq#lcTcqDSEL~jw56+h$35M?PgdX#Z~7W z_3Ja`LAkf`3_{7v&`ZSM05yXsvMCGkw=+M#?>#y@S8Pi&S0A!bUWKq%nz0!~kE5r- zTWM`+mLyz#C@vw2?1)RuAc}12N`8W5?uAy=XXmh@O$5-MQi&ovmqf%RwRcTY?I=UJ zH)=r^mt?nOWLw4}bBA0;YjV+{BLd)l5M?dWnZtS$=MVVv%zxM;Yu9Ws+8uNRP!na_ zr9H?!PBW_ZZkv|DY}rTm5fK0xM3GG&oMk3Plu>3Qn5D=WM3GII4om%&$JfxFr<{!T zT;9vOCg11XZ2e$m22qv_u`LTTesG*GNx5+Kp(vH|>XSu8ukr$BbWvoh9MXv^09b+$Qv;Rlpk8c{nn5}~Bq z`KZVsifom|4n3EhdhKHurhKBUrD7bN;)POtm7nOzAc}02#qLE*$(E=_)_|UqRw9eY zGVkmN$!xV0v8dMQ-*;P7`NUe%pc+Y>$1aqmBqWiHE{g1#K6D=Z^+U^?t=NZ_LDoPh z#agxuaW)`>D6&-+JMwUfsb?Qnuc}q{ifXpR>_%Ej?k!Pdr{WTL$=CdhyT!7ZPTA`onkmz9LFM> zt;%A=%*h~%?A*f1hMfNY7FMn=-E*of>4e8FjiMGrzTJ#2ifolbYh~eHb?-x~HhW*$ z!a2VylxE9S2(M-a*Mw}9#g6$=RtEI448vpoQ zJL0-i!YTRLxi-5QC7b({eeJD!{}v|>ADF*OisAT6@Bh2&#No#-+-aQUOuuN1FUn=V zCMTP}$WCA1^S8pPZ=U+u;hZOTNS@xmT~SueUj&OBGQJg-{R*3G{!TpqhLA_1bB2Ep zu9#uJ;R(Z=cZlCHm|+l`{5@H&$xZ}6ix6O0H^J7!tjf4dgIuu zkwFyM{!T!|C3^o+XLRj7*PXNFDrDaZ%YJ!JHg_}o8e;9=vMYA>9_WVet81J*ymFy! z#u;I~?W=fHT1yn!-p#ll)VsSnwMZAa?wl=Gq4`xj7(|iHeagSq!{5t4`OGQ9A~O@(g4UMYh*1W)4ZL*_I_y7RRz1)`twD$Y$U9x2edWyyku2%Plog zWK)ha+unYqKo-C@s%uwr22qw*v2E?*L{D#E30EZJ?3*-Z(xfWSpzbNB0KjVU{wCzqoN6ZFTvlri@2Q(qR8gAGcqfheSyYSR91~zkTZy) zPT1~-O>895-|MS5Dt~=XCy0C=WDrF*zYUR(iZ%XHq0X6XDQ6Hxw!hC2r&M_7k~RF= zG+WBPPhAp4HoxhWf8&=n+*+heh&3(pCGa2F*I~$}XUHOenwmp-rKaT!qR6HkXN-!v zM(Lwe<_x09rfy~tSw)Gnb}1uB3C$Tqk!>A}d}5NL_hy7*I^3vDbKhUY+&lfzu zY)(mo1O9pa;F+bKm&$*A|FzBegAAg`p7-s=Q%iATIqvAEhU#O5KfSYw`j$>LI$@o9 z@3N`gAARtqCT3hZm5=`V_nSV*=%O@2u{Ze1GO4A=m#A&^(J%*JzHwG+OBtbu=dO}a z($pK)jpsMsaeC7S8AOr2_$9%P+|obq-Y503&+1o}Z0iFLye#!0dv(&`tERr!Jp6M_ zA7l_kHv3MDv>#0$qkHanUTRloXwM3Mcy9f~xn zC0>0+jLMACD2?50t&bV)JX!`(WP4O$6SYR#53K;Dp}u>GG!pGOrKc#!Ad2i)JhEh( zdF;7lR(&)QS$o|z!v}9#F`*>KUiHW`kUwa^vV7VD#U<9@gEs%zVE-4dm{#|eZ|Y08l5#VMBHMZ%5m~c?Y38y*wm6D$D6h6x z^X7e~>4OZS$fg`ec2IvENoSmc)_6r4@5kPA>0pWZuP9Id(w7EWQI(~SvAW8pnL!lU zDvPa_BEuqv@}lRgI6Jj<)NWS|h$1^@5Jk4i>O(DMjFD$9{g5m*5p1$0mMh70&LE0x zmBkKQqTI3Oq0I2m#2{|asMX=dahCFdE8By(M4%R%~o0LSR*l% z%WxmZz2OIgRWDpFwY&7eKW_Wb^PJH|sa>;G7Q49(|LC6vPrdDd@~O8S(E2EyAjB4G zo6ml*^-&jn@BJM{7bPDh_WgS-o3tR#AIkN4=D+^Gu|;lXc-5*+E_1v-daiQJoQy8Y z>!ar?$LdOE)rTa~KK1hFe{>*=s$G)~>bc5#9#U>b7o~R1R$1)iod?od*+yDq_a`4t z3!XEG>ay+IGYdDgRg<-(waYCWtmu#^vU6#F%?Xz^`~5)qoriX>xl@)gttfZ4D99j+ z?7u<@PhXp7SO7`QV8=h?KeDih!7|p)4D6;?liFh6{9Avlh zNR;34O7H!@x3u%gtXUP6xk|^okmr8Kla>s)wH@=McVV-u8QYj+naf`45G-+Ej6U1vNY&S zW?8bm(x}2-kwFyM+?%41%xWHDdfg>gx4tO6emb9LQbGn%WFJsP#1Z~Ow=%K}vO@MU zS2lKoR$FDwBkUC!M3HSDrIxY?pth%1bmurh=MP5)iWzeTQDi%Qi1SCYMkDL_a+faF zYUid)M=UDI$gLlngDA4SR+0T^Vuj>u6<{++Bu_(lBSxiazx>Z-K^)Ht+bQ! zn#Uz%5Jk4vD)byuQd=72AuUUe6E&)cWXK?jY|B_=wi||Zu%a}0gpyZEPo{$vQDj>m zBa5RLRBNT2zOBCe9jzLzT^>^tMYiMl@SHl&D!$Um#ivxAacu3hYd)pM8HWs_$hJ+4 zduQplY>C#4tH`l))r*1*qR9UEg^@3b+YH%4_d#j!ifY%q>ft}gAc}1JuIQtQ07lvm z-6+~-bmIg)m!3xiK!&WPqlqqCk{D4&<2ln>$t!S%+(E8L^3E-TD6$f$7qm*A5$L+2E(5(gSiv35N4an%CiYQ6A*_QV3oY1Ju*IJg;t|cb&yJQe$9h8l-)wiKn77{GhddXaOZa`S^&07i;**kBAfkXRuSa$+Cml{RcB%&JMOYWM?32qP#xZ1FvD^kQ*!N`;W?}6j@74jBKsXIBkZd zWf0Y6+XiRn!KCNXB55bRCAB$c5JlbQxjlUqxSqO{Lut1Nad?Qh$Dog@w4x#jY~`?py)wX5hwWicX> zp+*r!w#pinT54~qOYJ1zNTC16x+{?H z(@6XtfXeY602y7BY>C+_$DV61isG1bM)zz#`0LLPT=u%ntkoojP-cjI@cwJ3)!peY zx{sR1Aj;(juT_qW4A!`;cIW>nV$lyiq1&Zt45ISrl?>L9!8kbYgEeH_`jfCe8~^tK zol%iNl5OYam3 z%WqrH<@K$P;m^tFqR3V`Vg{U2YoC#o+4AXE(Y;T_5$Tgb6xlvgM~+PI7)Y)fX3hMk z@)z1Q=S5YHyeJu6l*TTUvmen1M`g)y^fURyk&ln}$;MO6x;=CEb=K((_i&lay%9Y@<{|HZ5OfOGdUCKGCUN?HYG8XK-)H zwvACs`F#{Ur#zhc(2DB0%3%|sgDGo3l-5Tm`o}D5P}}23M%{-bL(kPx$S(Dxi&7tE zs~mDQ&L@_-P4=(*mCfcUo~QeiJy$+4F^D4ju=T&4tZ3+GkY|t$ku8zbE3>IJQC`&6Kv_pyjL1NeK@{1{k!E{dd#ALD-e*ZTS3PHRQQiYf zPv{3`UV665a(We%q4mro-%bWmWP5e9_w-Y)&)G9&=$w1gy^*tRal?zuUHaOavw&nf zCl|L%<2SIAlz6=I6a00*N1NTTBnDAruk+5|K(4gk&DkLByR}o6!DVSGGhu=6GK4Zi zZ0mm(!)1NYmI!4=c!TIAM#UPJleHWBl`P&(eHcX_C6sM@=sEjfjmx&}wlVq4jZ$0m z4Ejxj7!muh57v;O-vx*fZ<3S28ZxY15i^j%8ZxZ4VN1wh4H@=CVRxr7h*I>Sc|?>! z25ZP*W-{=WE7p)<&zYSd)1H$-l*^G9<)~QWvf8zc>CfCKwMCTQBg-g@ zvKD07+9ZjrnZ$^b7yBT?WudG${ZSdE-iflV^w)|tWLR(dYefc8mi&nNI1kp4VNL7r zIT=J*Z~AgY22s|PzE@)n8P>GEw39*9bR8svC_nF82G?l3E(^u8NUW{Bfl*=*<#In} zV2#T@A<0L@sfashREimJcJetYPAp`w#$~muyw6Mo(46l4o@FJS&QDyp0>QT~(pMCVH)FG@xi zrLmi>a%4H1d`~(RDgVPe(}$kDu+1m&YhCScMpHJ5D6)CSnq`X8H|UfRl0SDoN%!Z? zh*F*@zGj7NRQ82Gq0DxkDIx%!pCltiiVf@; z6ur2Qh#AP}qSUV0Do5;t^GD?qgzS<}7J5&{r-`s|WDrHRPb~4CjLx$<*ET1xnybjK za&d3k-?1bHQDpo5iHNU|HK?tF+9~@%q3o|>2g&H7B(G+x-0PzfLpAa&ZdbbPmcrwp zk7OBa0lI9pG}$s}r@9fVbv`0wScb|LKzl0`*)x0INW0#3RwlLf$yBp#M$zho&$ZH( z_BV=&K~$G*UCFezeK$g}51tc>~*gl|x&lgVJ8r z7)7?qdLCYlZ@tj!JHsZQVb2i$gAAg`&hHKIM#_lqs`#|6c2(A>+CGx?(cTk9c7CE~ zKCv6)O8TVpjAT=;FGYN^DXKAwY?bvqc91qv@s4VA8g;ZJEpi+br&Kb!D6&-!9h40j z=g3Ou+hk=ZOU|}Md_@LPWK+gcHY(lYXw5V#_wLA2y28=sj4q07mBac#c5BP1;u1?A zBd72NWDrHRWjdlIN}_j7XEL3NG!o@6ctXnF%p4I#w)Z?vcg+qaDQQMm>6D^&qG>{Yr2(99jNs$K0lEm5}TCGC>uA-3lc zG4=S3_)#|F}8=SB6-z9WO|t^33vifrdaL(g?DC_R_2;$0&%w9MSLzlu)`qR8gm zWabTO%c$mIYp2|sZDMF*CJ7cLap-v5X!$8Y0~^qc)X zwmvUK-g*mxe`Rcm`RBIhg!UKfi9r8jP#p0ekt83y-$1X_J2-{coT5^`}4#gifs26zlj_F+G(WUDE?~q zI6J8=f8}4lAFS@YEcMa;eo()=OAMmO_Foc+-wz^AOkc7e9 zb+qS3neCV{;$8WTu}w_#*kqG$m6`{~uCkuTJjftQ^Dvup%5LH+-@Zu_87KHAu07+V zC_^$#6xoa)+FyE)v@af&PQ3i9kzV!qwe0veyoo^+*?-ves({{c0_a1K7r1_;w zLg^g_J=aRdTERQlU6fZ;&sB~$AR5b%_CryhS5)r~QKM8ABi;}qgDA4SAMq9v8M5Ig z-Ec)I+s6~6%6c9#92rEBoqGdBmT5mUUhM~0lq;Q`&#NwqY?Wg_bQdfiB|pg1sHH@{ zkEhr6_iBkj6xo)<@E@4@$m=fq;T4sxc%{RZkUEwWt*7&brrI6Dbe_4uQZ zHhlb@33;{TqwI@lKlE2{6d7b+rbrB;C>dVE*g@<^vRCR|HQobh)O1D{MYhVJwa~$& z=hkQIo$R1BExvL?22s`(>$zHrd0@{|o?5-@7Ybx}&*N7RWOPwv&&&fmSbA?|U%6}C z&HOw_6xo)Au++3Al0-`!BYjF}ZcDl-vQ=(#07;{o`K0$njq>_PSDYR0XHvT;vQ>^d z7Rl^5LeBmgt*wuIi6ipXf3ysuEa8guEXQ&GG5!^HiuCD)?UxjJ+xvutBZDZiea4Bi z0jy}n-fcNG4~{lF30Fjst+M1Q<^j1HIjg3#rSiL4QCS1#dNa8i5kdmc*CX$mpV^cVb)r!%`zcu4FSbLs?|b+_oevH5pwL*)wYe zxvDK0vVGnI`$XHy^gS8NRZSGxmc%%#!K8qAN_#F; z^5J8@B(1e4iZ5o7;XM$Fl0jL>vIaw*+-YlVQN!0FGtub4jCUqG&rBc+PjQLnp>^lj zsRiw~!cv?_6m^1g$}^BWlTl(+o{)3*I! ztzA=Fl4@(0bj79MOBt0#_H1U5=FM>(Thcsz0z7~pB-gt z|7~5{J+I_zwdX`x#>8gd`Q3!2L^A3qQ?ZYwB;s9Uibh0{t+Hkw(vCb_MO}05P4^Mb zwkhwM<>As_uai|1ifn3dJ`ZPvW%U`cGLp?1M3Jqs^t^YwRIPncoT_!Jx8pDVnNYeP zR9Q3czt2Dv*($4#_I|LH#ND3xe5%o{-ewD*n%b4ERauOl-7SeCTV;(Z?gwGvYWJ=? z?3FH$6NKKa^a$f_n~W|>BQaa$crz1>N(|NL?IyRY*}A28H;Rldifol*t)Nlqo>Mi- z%F^pI8pyP^Cdv_#*p7)Jzsp>P&adi2ZwOfWbQ|h(ZM-=^Mi)i4%5j&9vs%eBnWa)T z%Cg(qtJ;waQDpNBm-*WI57*z5=Ari!?K$;^p`J@h;{9Ft+BW-Pl*Vqh%JF_8XYM_h zG_YOoc}NKvU6k52+x=zpNHK$I^q!U3db7%N>c4;0MUky?ymN>1d2M~x8ynOWOKAVy z52DDn{>Qr?&CE;fpmLTUedw}QKfW=4nD#&;(R1xb|J}hZN@EvF&u8XAOZ~N779OcB zy`RL9aD=%{B#La6)knPPL>=7uy3>wGe zaoLiuQH5og#-L8P9A1qK*0`*8Z85@%PGb<2djm39Lq@*mWUz(|N>5Rc!5T7nrpg(t zA;T6fd=&d2gDA>a-Un;QkQ`5zMAncY>mx?^L5_+HqFj#5HW{pOx$AfHc^IX(m194C72Icr>2?9+=A9q*315t1WsMm1TyJ5r5N zWUK5`Mhf5*!=>obHIB%PV#Z=J!%?3YV!Q8H-QeVXIMNeCC^I7JBZDfe8dd1|GzL+529gZckil$ZJ`dKA;kSC?_LY6Gh75TL z%_D5$GzL-1CyNm|eKJ@>hU~Ez;SHuSh;lh@8pvRc%W7Blc=9}qQd>m%MyNjz){wzF z?--LciZx`&9&1#6DItTX{LKN*oHb;~@~Mx$evmez1lNz73PF z6>G?lb=5rjwq%ml${W~9^lgbXRVcEp&5;Y|%*i0ix)P^5GFU@~wK+2M(-=fqo8t|- zX$+#Q&GBy3GzL-D$2hBz!5T8G&2ck2jX{((Elyr!ux1h??oy{Q%=Ue)q@+J8qtv@K zSSagbf99+q!@AjDT{4KWKK7-AHDp*f`+H6XQP!1+mbg}|A;a3-mqap%vNreigAAgm z%`qn1L^6o7p7*tuHIo>9%Rq+9LMhgglte_vwKB?)o>68fUdiXd8kf~B^M3IjgLt}s zmPYC1E}v?tl#jBeg|BV$qABYvMi)i4%HcVgxsxOsWr>lOv4ln+WOPwvs~kQGIpj(< zQ8n@`wzXPamGwOKgAAg`wsj33r5S6jlo{sznd4GNnd&3BLu7HlsNI%MqekhRX?E}SwI+&e_Z{b2t`*O+Itgp+I$`U% z%6&!`rS%bt=jcoa({J^KQg%SvrFNY)P&u*&(7|-CMikj9M`ogt#Izq`Qhs=%%Pj*L zM3GH-%jco7t9N-d+Yfm%Ta56voJSW$w#wlLu^*Y9TV7=iETN&bWOPw#S14;i*ph~k z)=Kild=jNZ&$a52VIiZ7QXfL)a;2LR>4)^4(x5Dk_nFd@y-%He(V@#$e<3C8!?sio6u%)6P zgDA4+Ua(xb$uh4V|C-~>&u&%ib>|tSr2h1)mM^b9Vw+NLD3d*%K@{2g8)Y}GwMlBJ zC^mb?-qqtrT~R6$IB?Mw%P;P}ZK*fki6T2^5Jk5B=Fh4}ZjxFmiaif_NA-#NhEo3g zw@0p6o^jT;=?0W2vU3JeWb1DU-F5ONYa#7X;?5@RIVqmkrVzd-_X{hupDID(4KM^!MD%o^$B3 z<$YC^TFNwPwA4vOx#3qfF3-K@qQUEyTDZL9uJw|pUb1dc?!3>&eXSi4MfM@bEL{HM zPwSzh=HXoin2Reovpol{%XyIjx7Ad2k2p0iZ>$8WyA3_V}a^UBjs*);8%{+i??XRSZpK{ALUd&AonEf-yV^RbjrYxNg4&%SMg z^pygl$j&VTQDh(esm06PpO~9k%B<+<$jcTg_k7DHrM}-x$)LpK45G-^U)%i6ChMn` z5~FVX;7GCD!*f?DS9!@MrM{B&_}rJ4v)=dWQr|_=Uo710e>YC$oIw=XZ+Y|bV}0r-E*Yth#V=W<+_H!<8uF`O-@zGYyEksMpD;+y1T5Jk4_ZhR34 zjP#dDpW0*nWS49|6j|tRneOq6^~yHN@Te-H^mk3o&Mh3gS{t|PdykY?d${-xBpF26 z_X$Ng&i%RD(%&wjHK3Kq8AOro{?to+ zs@e03v6>}Zv6i(UA~G_FBKybRTeAGd0XvO%@QSw`Uq1ZJ<8{W#BC z&G=WQ&CKg&t!k|mCC(l$$?n(>hU}6-6xsB_nRl*TBO7J!EE{FEp63jr$hJQZ3kUC9 za#XZY)U+%DD2XDQwkwOsYFd3-26~B{K@{1!EopXe(CoR+=k10dh071EUnY-C zO)CmAi1MC`?L7}2M3hlG%8(^pw@ET*Zj|--0b~Zr6|lZk8g#M zK@{2Cr_7&MZ0oU~|6}mpRp+JH&?_Ck`GG&Lh$5SP7X>XG+uG|r&kl;+=cbs-(OG=y zmJFiEX5Y!SJ7ySh2dzc2&fdYW)QA~IM3Fr;X3%@pt^BZEOHB09WF{<$S)@-k`^zHB z5n}^N1|=qE5Jfg)iDZ4M=KN79-sS2tHq7^-Wcw6O==?)sF80kUS1DZ2M=MjvDlMYhjTaW=r7kGRj?K}CAr^N9M$Ad2jK z&!JIS?$zE|xdr>E$TN^Z6i33o^C-hMTrm#yjutg%5Jk3aend;KaGBQH!f8cq6T`NX zK@@e~HZk@CaY=Umu&3^?6&XZ1deJGx@k*RnxVpA*vRAfnnyqbO%$y9O$hJ-F`Ka0+ zM>C{OxL!D5C8gg4ayq}zK-jO3UWHD?e-_Im&G)Zn~Jb{gN&V|7PfUFo)0 zSGLys5q6ghqR94sgcW6!@vW0zP%g2;YfD)GGAyBC8OR`t?7R=2QaRo&Zd$Uu{h*za z*Jm~%KM4~>Hs@CqjkFIN{Ybe!OSod31DAVZu;n7#q+FjZLqsxU5Jk52qm7pAohy2J zGo(fK+M!YK=WTqY8nQ3l@cHG!i)}Z)D?0V@L(99DcyXnBPRi&fAAM}_$5XdTxr{t( zKn77{Q>L@Esy#zR!XAm@XwNAkWUN(96xr)9^0&c$@7`g2^V4W;+30z?=lsyge;#~y z_7=&O(Cgbfhbc z5JmR8U;EVHz31a^v-$g z!yEunWLru?Yat~yt&BaHVi8)UG!IK+mYE>?eUCpq*yx^J#&=%7J?Y!!rKj($oXm)x ze&bI*J6P@KyQf=SMg~Pe22o^>9(iUkccw%rXqCxa-mJ*S8O zptWtDL3&`Vm3~;yLqEtMidr;P5-ZxJ6~FSA!Nt1_(k;8Aniv(Ny&{Ti+ObR$E4Fp} z-j5At&E7KIvH1iUkqo50B8qJGolA+Wk0j34M>;rN5{V+)_9}AW@Hmw{j-n!KmtrdW zsEDt~Ac}1JsEDs1iRET1JT@qgoR=aN+9+#RpFtGav|UNsM~!}@cP8Asyzmc)&kRnx zd)M@aNbc2|QMC-B$X2AFxt8qZvN;1Tv2M3L>aiXG%hn3Bj$0W%2MZAO-jBHJ-z zWTSAJsHq2Rmlixbv1Hi~ve{o2EtOn%M@w3tsqs}w6xsRe!aI*>!?(ER*}>}jyfI=p zyw8-R#1X)VD6(nuGe$Xyq4NN5uycmn6^d;45vPg$UUB_wXMF+HTF2lC)qPZW`Yc&$hMD)Y!pwa^xKXs6?xl>Mifp4QDoE4 z=XTd;OXV!QYsyA=XCpHK>oX#XY@a_P`vEDbxr(+pvh%h|aq=RAD6;c75sb~LGqavGgO7n33sy%ZmgKe#qHSjzlvkg7Z zGTWY$P}cLXB~29WcRy-+VaDq8?b(e<_Es3#9#us8^n;U5smjAR8jPF_8IA{4Lw0_y zg|uhqL3;yPXZw$^)MOB4Um}ZR{}I*)@$QJ@C5r6)tVR^soL`>tVnol?%_EscKb*PC zc93l6;Ud1Gl(2@o#{P0iB#LaySnN5ZJ-BUg@)Yoa_7NuT=*NkjyYF&G=>eUL#E z`(w;l6g&R$xGi^VesibaVEIHRE4u1-_YYpbW1MSwn#>tQkv(YG>(7sqcTw!}=$xx> zY~JHk?$uUQa&_cu?;rfv?s0Fh$?NVPeCw3R!{!X4$hPeYNxXcIqqp3%$ye&VCHJoL z86z@}FwV7P5JmQB%{;FC*c--oEGPZy_IWRAa&UV0gP#7k+uuKU%egzI`;Qy`?f$_L zhwd=WI+H;Z*?C-owW`10^!~v7$O2mObsjjo>F;9~`Zj|MqR6&>ge1=X%wAir*W_LG zZtWMFb$@!*$XQwXq!X5y&?qv9BD>u2{^r+6KC5Bob-ly=gDV%>Cf(yu`i9N^@BRA0 z_*RAtqR6K0N|JbWBfH9lkLc-VH6yunwJlQKC-wTc{1+wih>NH z$o9I$Nw}H$cbl~;^*dp<<=M)Uc}2pjkwFyM>^qIB)pJXt_Lh5K*^NDi>{djP&Amw$ zZp+3d>i#3k2(w*kSI!`cn#%r)qOovSH#RZd8&D5!Y;=Cs5%W@JlM!dmAc}12O|FBE zUUUXL<^SD3_~M%)f5og(&LE2HD;oA0x5v#!?s0fhyr9w4ZPwg8-GA6SN7jH|jVQ8z z+_1Ml%Xiz*+S(pR5u`ni&I9&QaW)`>D6;LNA{K>|)b<9du{TikYX1@OE*V6TZT}H{ zP};2rinbYrpWfKxgFn5^_(qWoqR6)1MBGlTWm}vrS0UGPqR3|78KW{op56?dm)%E1 z02oz86xr@0PTS4QZFluI4wJ3zM#$FYI0=(seO3+GUaPP^@HpipY42uLGyTX;6J%RI zA{K?7m#pCl!rIkm5Jk3PyPJ2|ZJc|BH>e!}C`z@BQeb?GAQ?oFZT}IL8atR}ajbWe4DV*-eaIln`z(}ovoBX>NT1D+7P*hmDD=^0 zaa2RL`v}Vb3s+8x!qeRZ?xr#;n%(@6&Dggnno-%;R@5AiT{EYq76St@P4{9f)NosSU7_MayMK)v9oMC^bvzp@s<-hIiBD2jth$7pu zL}brtOE^PEmdd0#7LBYL8AOrIbxV=@r&Nzh=QEE=_T2M`Tm~6Lk?nbeH=rasvQ)H9drk|U#iChef^5g4 zy|}%#yE>WBp4(!?iG|jODBBOAXy>!&wWP#Rw<)1HgDA48^ZA~;Ey<{*U7BkB=xJ?D z6xr@Cq#Zjrq8`{fD?j60M&!cDAc}12O*ZopWz-RX;%LVV5mPsPw5JK8$fiu^9)}Xg zD4Y_SGl(KPkJ}Hv{=d)D@AP#ZeC56;4Rv0Y=d@pvw|wXYCl6=hT{4Iw+maaG`6Y8c zJ4fxSACBbe{XaMK2_hdA8AOr2=MJA6>huyiIRD4@uBrFCbap;=sm~6TPm-UwVVqCW zJl=WfXNSryMFcfb@3vf_T`P{)vsLusRftH245G;9{EA|^$Co-sbI@Eq{LvGK%B09^T`kxj(Tzs#x8(?|8Cxa5Vba2}5U>WpSLRrsXRNGKeDkSJ$2}G`lFEwWCQ|D?2z< z&qvm6p~$wLhZV)Bvbf!&QvB(8L@Y`MQJhmgkGpsMpR;90WKY<;BLKxB9BomMK@{0f zK6A=Y`Q3!Wg^*4oI6etz*;tI1Z$?$Qe%@$4x> zXK3>%oD8DK{``Zd43!;@)t&pJ1I||tPC9t(Ij0Pr7o(@ny)zj^k$vaprwnzQ5fQ+b zKe3qCs`kvaik`V_l-DX&mkgrF_F9F7d-G}gp0B$P?YlKf+UXpLe92EPK6$A7mN2;TkTH582tsmi?$smes?@dVi&t}%D@(gue^7?4(UXhpw8AOrob&HeN zwLffSx3VOrKE`sa)0Sm7PF`eKUR6W3WjFe`Ww}qy8pkDBX4{@qSrvQE$gq$>6eA19 z5=HTzT|YU;_KM!ic2F@cy(A}@g=jGK2k2+7C<##A9+h#i9UlUvb~1k zYY}BsToY@mteUksEF2j`k9U< z?vA*MWOLoJdDI@2Y=%cA%jtQ96~#QVa}?R0M_8YSe*a3_-OBPKX{Q!ZyNZGgqR6(4 zg+@WHvNNY=D=*^}36DbtQI;{aYq^R(pdZ=YHb=q{X8KVQWnIxotRFF||5@totsi{< z7AFpU@5~cjo|zznD6;n(o-kAnIlS6F`z|EAn{<#hJWaBt=G{Rsh$8#%ho3NLQDnQ1h<)DrhW+KEQtZRFxNF?aq9B7Pve|c0{PdJh zwXrB~Vz_JE&746L*{AJs!qB&*8DnIIP7P$Zk2pK49r}Vu2d-N@X-=iYi-)e}KWY^dK z@GWOcw)nMaecjsq@k`h7x9H3uifom|4lAm&$Mr`IlWg&K|N63#P>Q4VT;+&f$>^f| ztzv!`DeZ^*s9mEyaJyPvw-iwZ`l!1oeKpx^^%pxR-FWwbZ#-K}&sH8%U(MFoW!3xz z?XYlU5Jk4vDtxWvY1i+4Xm-;_`u3;Vm9F_a99mJ8^*mOW45IW^4YU1)jnG=j(=l&& z!8tmk^Gn(KzKa;rJ3Uu9YzY}%l)uvLufv4aYUa1xuvMCezp$aG$U32Yw%){^lR*^O zDr-~`Ur9gS`+*f_X>|H7pK7EB{+5~|B$f3%q9rnjB0K-WpVsK?+dg@g`k)@@3-j)e zUz?A=2xJCPWK(a#mRxw%t+Tb0{yMts5+z2@)nC{vNrxF-l>Z_^PZH~!zWxTuZu%yN z+R`dgAFT!LezXjt^i2-4`F)P~>+&z&>zP^75AUrs$GfJ}mUlBwUfOdrh$7pv(CT3Q zyPKc4n)>i~RpXJUU5~Ksqh%09w&xdpV0E+a>3a|4ExqcYQDk&c{u;k`Hhit@?)-<> zTUB$M z1tX69Z*37pHhovv#PeTylj5t2(qL;K8%62K8C{gWlyAEd7EUu?^vRWHNp>AGNaFMb zGs~-EhLA)ux+wpp1%Cf5Y~t+a-EfX9ge|gchP}RgxTAsApI2rOMRw2n)GKfQ(v&l? zr&s*p-ITmK8i=?=*3*nGO5Y1N+j5-dQQvm`3p9`PjYh4HA{O?S_aSd)22tK~vAyRJ z`)GBS+Vh37CAC*kn(94JMCMfwJtw1!@>lo0vk~<*jPkCZzcKY;xl#@7hvllz=%OT7 zX4|gB-}72;zXN2i+P^WP+c2|b2d(FEOC~)xql=QBn{E3Te@pL%XMTK^`tVwH+x1$7 z-EI09nZe$>Z1)#^JiF|H**p~8(SEq4i2Brr8C{gxHQRP2{&Lnu_gyL-tZB*YMJ2nm zd^w|w@?V^?uMJC$7;eN^%~6=nYWHYvlwR+87pVxl0wq2gZdti?vy$O$_ zwKAiN(ps5qISwm2=Z)v;td_nbthN;6&?D1R=M174Au%S(RyX@QKi&iR46k~418LNF z2h(XnqcX~WCCED)9!K`}_xmk1Tl3($+uF$**d~T$AcH8fxleIQ9miKA$5C1z*HwX#o@ zjM8$_qUH>u$hHq|rM@t^2C;|M6@IuGS-juH($h1By0&+9Bs*OW9pVcl(onwNdNViINgo+yyV_` z*TnE{wo$mHBt`lj71@-9$YqSZbM_r!`&Y#e_S&I?@aI|9fN`6BaGY2q?VB99?HtL~ z57$|!Jp753%Qw&a_TYq<{A;k)2cH?NyW1tHtmi6+Mwvkr*(!^zmLg`j<6Co-tFE7Y z>}9Fu(uJ=XeENi!xBaO+nL!lUDr;0~sVE*hc$M?@7l^Na;I#8!_UZ@I*pGPA1B2~P zTQ{}a{Dtebtmip{D6-#h;tGlVqq)~DfI$>TIK2;|@|klU92HR~Y_M7yRqV$!22mdv z1*7?!=$(0R?40@4Us^Wxaof?~>o8bDhICLnc>WnTb{MQ7*iXH%W|n8+^SpbJmdYkvYLQ>f!S{4AzkG$N3wlK4u?w zUWdUNGFBf3qrc~@AtTpwMH%NGzsnr?TuGvBIAu&_J&!)fAc}0tbey)A+54Ys>Xavq zvPGsoQqyuq7e%(pq36x0>ea8><@_`c*gnnHWADwpWe`Q1sIu5$Mb-A{AG+sj>Pz-Y zHL_&1cC=#=>6_6-k*#uA(Z)YjO=Oh*Y1dRU$w%pV=wO#2&9SC-+2PgX1Ko#hhH6w6 zL;LLKvFBYz7e#jTp;+OBtIj!3ah7H-hGs}#!WBuUiI&ktk*#vHPMpZ?HhG{xELwEf=ox; zZU$wPY?Z}swPrlU7%(*nF#a#&dfAtzFi% zq9B7PvQ<_ep|!H23tw}g-eVcbwyQ=~pH^FCJ)g0=W)MX-y+lOeJ708qk}G)}wIyFf z6xlh0D6;9hB4_!VcixlYcKJc|ArDCu**SwKvguL78(j0{=U<>bCyJhWI-`s72=!d$ zh%>~IhA|^s${Agh+tqWGV?R#Y_e&S- zd(v0ad5wOXIzcTc3NnZyn|^-APRqN|_c5xm1-BnG>zKNW(%6N{qow1%G(1ms zim2QsPGb;7d7C~eqH;-`)(27E*@!YGF;qhaBZJAK5`(BbW|-CoQSzvg{P3L97(~%) zPahRg*3JIRSwjXR&b$vYh+;fHok0{M&gl%I7!yrr5XD$)I)kV@hU0#4=0x$_HoXs` zrdv4nL6o1{iXKxn@zn)4Ub)8Fv~Vgbrji}>^RS7spk#DWvX5p*e5JO24+uZ5cR5kwWueoUjE>YRVMVo8Zr*~^lgLZW)OA%E}NGJ z+_dt9K3GG>eYf22%=f3*E7p*)=5aSpu|BLJ&Pg`w34BS}!x5(M6g4aPuw8 z$zF{$>~D0$)>FJQYsk3k16#H6Lp~4IkTG3rSwn`_&2!585X!5_Re1e=t>(98Zs>TeFkgDP+4o(XK=h6p|yFz45F;feMWaw z9*Om_&tMJ5Zhh=CSVM;OvCm))8P>->gEeGWANvf}kYRo7Ggw20^|8-j4H?$QK7)Ji z(~D4g?!B?3hn{p%mT;f1`hBp5{rS|_XRwA0pGEr&){rq>5;;T8$>*znA7l{a)6#+& zMEUHqU$t;*bl+Ps#W)S65#)26{`OL6j22l%^L`xh|l;d0yS8VQqEqy-GYb5jx zIfE#&eOB+!gEeIMA;TxuK7%!6IQHo?xPv~o38m-Wn_Nn| zC`-Oimi<0h!~T4F?K4z7j45EA%T`+?vpG6nU zAj)Ua1v7~9S#-e+qI?!zFoUQCOQI!?D9UlXBRqcps{D5Io>ef;9UR}hizP{Xc%L&; zS%jXAA0|)TBvz1Lv8JNEw#pinT8cNx$M1NRM>g+ud4`{9-i=Ya zS|645yj?55<5dzxw#s71-9&T$Uw!@RZS<~lsnvD690^Al?@X9M6xrOTxN*N~>AUqF zS@qrK-6-|3-G;{w)^6S@)7TGR=HmlBS6R<L0?-if|PecM~ny;D>@)7)pOMpC^?^L~+6LC;kVX(ywLB3tEHE4{mN z@j`2*Z?SMq*k9fU8AOrIHH^0`nzyO+&RwNZxp&GEN1HRcD6&1b z-cM4EG>00c=US`KC^EVzvRy8UZ+z;ba~`_-M@uOOK&vLp>2`AqM@ARrc4zFaY~lm! ztpD{n^<8T!3*z%f^Tm+CADTNz^`WxQJ1XalE=qlv?Q*Q{>{q1sWU3!Dca5s~?XhPL zR&L&=Q6D!w^p%00tE}faql+S2WwFCD9JJPKz3)?r;hK}1w}RB}dY6AC>n~??QDmzu zc4(CQ5W_W#Hg7k19&9ORbWvoxzclmGYbAzj-rT&U<@I4pIirgrTjf|?`N2;;QRqFg zN(|S0uz7Dyecbb3XARg=_(3zeD6&-+JN)1ep13LLxfrhbujc(b_3`B!&mOR)oY6&* zt+LqRah|yKgURkb+PpicnkTBS4#-w{GJ`0xRn`dAQbZZ!w+&0`IZvYCouFvH-do&(XUx7{x*C^&B+vSKwG)Y?s?U^gZ&_hjV0Vi71!#JZz#Fu2EZLyBvLt*Q);g4_|Sr zW~e&|)o2xbgW+ua za3n7G8Lm<9+8eW7j!{WBB=OB!EjuaqCKNMlVcX4cjZkE}tmiEw?YU}v4=A?YrqS44 z?lW8?6xlAv%w-j{Ug9(B=$s{=M;~NNLaALpk3OX1(p>F_XRaFO6V zZ#Amk=dzuam9*(L2>DceB8LW5>^h$r)OcE=nWu z^B9$Em1Il$!ECm7O|AO5%Hhw=n1oWheja^{_ak`&ONm;dHE>zaBlgk0YX@DUw#arl z`e-aq?X{vs_MMJe^>dZeTD6QxD7EY7Ss$aNK0p5y)~H?I>iT)sM`BDusa-#hJ~Ri- zMRH|HR1GD=YZ$tV^m{}N^(VM;6D3#pD_ugk@$IxO8RF#=L~H{)vlkneYA{8 zD7EY7(Z^VJW!q;<;^Z?t65pBodGtYs#@j`a9ev27NpqV$e~H#a`>Y!3ou5Y^WK2Sl z9erq>Y_FIRc0O6{y4+{DMkumf*7Mf3C;u@ihbRm)g*n4DLXqvVp6Ax5n?1E9^7B5! z8CumCrP=y<%zP}n$$v0=>fEc?ey(zyCM=1QQ0~J!+gi9JSDv9zY}e1D56aaf6xlJ4 zu}w@q%B!my=U&yW%YBAxgd*EzJ#Q^svOc_H@$HE)ey(!Pn1s?u{5(db*s7U*is3AY zs`2fKT6MY4aE(x8yR7H!ek32I)zurZYD;_I7{$-~j7cc6V^rh)NFGP4=(mTp(t2B2 zD3_xTGF&4R*)B&P^if(x>4a*y2Yw!XkTD5GcJwjcj}*gc6+I8FwAM-}m!l6dTq6|O zE=M2QZ?P0J@Lr9(>F4c!w2Vn8kIELjwLWPc+(E9W%k4ZAAt#1w)GpaB$2{n3tx>A+ zZHC%)xzBKoP-MHT=lR)L7TIsUb46Y5GhCzI$#ywLMSo6fNuBWX=!1+&D7EY7Z6B?7 zrUl?W`+3_(%b0{xyM7*hjQ1n$pfyTuX{GfW1EE}wKFDy5P-MH@_R*f5%}`rhQJ333 zT83-7BXLX72O|J`9M#yXsa-#hKFFAaB0Ks}Od{)`I8nX({Gk#0%{-U;4A-c4vR#f* zDJGGX(|>8FY_aX?@tPpGO~LOhTz$KaW1*4ZZj#KsV#X z44cZ4BO}AvGv(pP&a>xau!am~yfqiSHyEQLgQz^~JdHtAo)*^g zSVP9ljZnPBI>TTM8N6f5`(O3I45IQ|{b@5ND!;Xy#vp2DXLV~y2Bm!l z)qhKG<_&B~pKdT@sVRwmZXL`S(p1$LMYhUmRLy(m@!cAtc;Di3pWzyL7P9j@mS*N{ zAFLsR_n-NwSVIPHGjj%O$l%>%&R`80ylKoCtRaKZWaPJfTr1X)k>A~s!5T7n zkC%^%HDt)bN#6Xc-dkRe@(QH{MpD_3L? z<>!6bWepkmdl{Su8ARo8y-Z^eHGOs2E=R?;Nb=QX4Hn zn#uEEO_$*j=8{N;%0@l9>`~WD-VfGv8D8nU4>D9X>J68D@tVnU#hNa|5|j5qhRQ|_ zKYqzIll6l&U4|t;?}H4LjhcVPcdxO|=M2_#8P?$G46%(`f7#JBlWhrWx(wTuypIjH zee>3TS#_x-akO32?G;gE%dU*>TXBNyvW5&B{7pvxW>w z{^ZqV4H=T-$vemzG9;CgrGzzPNTw&tE^Ek8`H}fYPuLIbvum`QKRxw*2{oNT6xouu z4IX{(gg#h9hGcq?;VMX`PyKtWTdp5u z5G9#@{j4J<%!3S~B-78`2T_t~@8!V@Fi<9Hsbp@!$HhV`+}U=118$3BBKWLO{j4Azigee5$>Lx%OS&tMH1*2g}Bd#=4v94(aCmW5nL zh$34OstCE?2W!ZXj49sjGgw20WK7X(pTQb3rb{Ac$T>;gbYkiEK?YHhH=Uyv%pgkg z_S}6CC3*ASydPu`C3%~?A7l_Ed7Hc+WDqr763HM+&ut&`nQNbStJg~F zE{V|@M|+^>@+JNK=%UP)gz6O5XRwC-Nn&(1>@!$HhUVe^`t$I3IYP;_&Q$$AI76a* z_USV?Dvm^QtaEgq!5T6o$2#5j8LS~gay+>Y){r4Np1iuOAwzOJc?VfThUC~1lkW#> z$dDXQ-VfH0;n=6YR@_1DjZQ*BiEUZPM@1Cbl2DzB`hBp549S>IMSTWq$dHWbRMcm% zhK%Ww$Qg1@k~f_o`+bl>l;lmP)CDt$lDs{4A4Eysyf^uNa8yJ|-X`w{8AM6mChrFs zL`mKz?*|z~O_xM6h|=>Jd$ss&cb-3fOQ=-MtS@catoykG?fH^xZ&vDe$$IW`QIOF^ zk?nF(%>MAl)9?E97MNw%`5fWV3qwmXAniU%4RoziDuJF&R6@* z`-7>E7yWqiQoo(Tq zBHQJn_}@KmKHqQAiQ#ouyB>+2yPPw+D6(BHio>7%k2S~dd8M`b*A}lWb?+qhVb5+? z>X+~;`+3eFN+U5!WwVRojlW+r{mxQzQO&)>SGRq5pVg9|=M2>tMYhU*-bmuYH_!3A zWMX*Th5E@lySMxAa?a?Y$aXo+Tz_jqznw_)@C-S2mHj+t5T*W%Qh8?PjegAW+p?;8 z^(k*?b>a)_zP{BJKTrD6GSr7rWUK7wMe)mhE}7#u+{EyjkfG-@eUQ;bk?nHYL4E%r z{X&zyt6fV8N1HQ-P|e%-J-D@3)*`j!=Q%?)Mv<+upBKfh z&;Qzq`enTPuq6{aEN9q9KhGINsa>N~HoGXA-_W0@-_k1sq8gpabWbk!#{WFDE&F-S zAc}02%`S=^PFZIy?#BzCdRM8pMa2G(E#Fn@m+&h4dCnk8?HZ-B*+p^Yk!!C-TjCnA z?>y|CrG5#|mU0GBY*%Hoi(=ioR#G3W4!TC{9nXD7I}bn48AP#NmCY`SXTM!&t@Qi8 zBv-Bx`^?`Q(yp7I=M19QuF7UNzwNtvD6LIzle0$b(YA+_`XxMD${9qFt+Lrg@zG88 zmY%nM&^2P;c+K?{P-MFt zQTW(DrQa^;{GklCY82@^|Ly0|2N{!4WJe!kTh+WbQ50sVMrQ+O<^4SRAY&4W?C4|c z15+QKx!UsBg>pIiAj366k?nHyF^&;O;yX{JGFQ}PJ)ik~l*+DATV%VO^^xv2#PHoO zQ7)_1wvWVcjoKpH<>+H5StW443;1*Qjr@UDosV_c-Y{KRRtmmedy6 ze%@zHLXjP#8r!NQ?K(S2KUBk!xZG#BM!l2ma*WD0SHGjzsGOk_+jsviM;~OkMs1Pp za@&V~?UZgaB#C}+LDJ{^p38lPYt%d0F2|_s1I5?6J2$Dbv)}jd^XP+&Nhr1J=h26K zhpdlht{SbV<=Evu!!_!iY?otH<1_6@zhhz6k-O${^g)Je)E3z;M<0%R^k2$GI9Du7 zVO&6D=4>DY%w#arl z`WS0ek}J*7IabM%-|_MDK4TI}Bk}VX6}^FW&^cD^f&1|D=!1+&D7EY7(MMOVyjE;C zpE((mP-@rDqmQnSavoQ^=J$wP?lWAY8G3Joa=BgI*7|7Oefy%>`W;9=?=vQ$)Q6wP zsK!###&EtF(fauPM3?&v*LWUgYaW*Tq_z63e(R&W=khYz8?j}5^gQO#^-*1vMq+l1 ziXKOv#X2GH(FYl>QCnoY9DUH^NUD9e#Xa!z=!1+&D7EY7(MPjR=@;wbt9P%W zX6t({m!l6dT;q9|?UtgCv9#BU86?%dapP|KdGtZXB$V3q^K2f;mUyQ;C+mt=H=9Rd zxJGS}?Q-*+5bv>#Q2CsLOqZYt$~;F2|_4 z{@gcJ)CpVB=!1+&D7EY7Z6B?7rUl?W`+3_(%b0{xyM7*hD7KRK6yF-9vQCrQ520Mv z^ZuFBD6(D7`WT7Lwz#4$3)A+I7_RA##4SZ1nuBD^F@yI*n(DV>wO005GvA;SV-ia3 z`guE#^nD+nKRk0upJmMDKEpMdpXFF6m-W2;21JVCBn{Sc+9*HoGbW+bho8r&#+F*P zWHzl2+x7G4gN#Wiwd?26hjL@VEedA5oEKwWJaS}RhVyV;_DuG?%WzGX;rzA6laGoF zqr{%cIxmP}lo&HPcc28{(oWT1N(Vh*YWQiN3Cg@a)EJ(Be?@QgUt(w#_?6gbP@A}nUaBPPDRH?EgP#< z^qtLMiVLeHV9gZoKy>FCSl96P{l+9fc1&zE*YHnyTAH@STwC2G^?ANupU3C(eE<1; zxXt6``}KUhU+@3#_vg>|bK_H)IRcdk^;3IX;gO2Bv9a5PtDNSI*1G0+yd<*6C+=lE z@RY>(T&*PVl*IVdvLx`71npMW!%OAy#^-}&3@1=2-uMi1jzA@1eA+ihpc27z)%kj$ z5-~n~or{4=#Q5BKjzA@1eAi%(KqX>)sy|1d5-~p2pCeF-;CbYHTcHvWJsZC`%-2L8 z?cV?8n@&OLsfDki%qY^CgJa1Qy;QLqkW*5pJx~^TNhO=-E!goKr)#T>a25HA7@x6} z1fDV;J40P6JSD--P!o7cf}Noz@RS5QLrvf*33i5>z*7?J3^jqLB-j~h0#8Y>Gt>m0 zlE_xLtj`&{B6trKTlwoxK1y8WG$XVo@F-s=m@D%HDz8(UV6;2Isy$DjQVe#9c>DKjpy;STBbqqYp*9mrpn!r;M>XeIjI)kg+f={4ahwGXsr+7lQ5-SZHr zyoB>HD7y&iH&37vp_M55qZ=Vv&AlF|L}cq#MtIr1=2|hH`n}ILYK}+wI-wQg&_^C= z5O_*LD@Hp8o|4dt(e4jCC7~5V^ zI7YZmXXl^y$u!!PF1wCVs-PJ?^8_lDP-nXXkG5jqk;)03?f&gVi@;M7I@`5l;3)~6 z?bLVw~spwt8CI zeIzo*w7Ozv@3*g&FCM9!#_pbmJ~F)Dp5Ep38Rhel^qf!(REmL0HtE@$q^k$hR|M1jK6x}J*kza>q-#Fr zgkqE`r`grg*R0ZMUj(bO@@4%XJ^RL1B3dftOM1lk`k(IMUFXzR)KK~QUP5~JMQkHn zrF>DzhEA=^yXPr}?*_E4aw0t^cJ}*^Y7VkrDJSflP;@*}F$a~7o&7=~#b9`%?@?gJ?f3k)9K-Qog8UL#KCA_dI^f+2_)O?3_|DyOa}lPLwLA zv9q^v)4JJIH;V?3B_=g)7Xo}*Y0>P?^CD# z@O==KVqoWlV&IX=Y3#+~$6oLS-V3i{_)gs>6a$sVkWG3wO1?e3-=1RN5$)#((sRO9 zp08}^#p3!u*vWhPDFzr z=^LLO93%O+jKYV)>te)P}-|MQjI@=!zBtOnS_Su`VD<@C7vlriv>@y7m` zBT$JRN68$4O2jyJ=Ll3H#u+t7pb|08^Em>Q2-cXoKfJ9_iD2cMCs2tPN8((mP>Jx7 zn2!=K6)F)*=L}a9oWh(K&kU_nL7-BM@yswspb|0eoO1*!5$wC|)}>UaM2x4UG6o*c zS2kzHI))RdMEI#MZ(S!)i5Sl%bEQHhf>X_WsZfa+&sTFXP>JBAHXj3(i1Au&E(R(Q z(<_{}6)F)*kE@9zXZ~aQ?V$gzhs zoAh*sOaEf12v<2xyjZFC?fb>^)W#yde&9#fPk-&H{ZAgfYX5uQ_2TubA3ZwQq%Rgu zxXNkS(D}cj^wZbxT5r4Yxh+Nf_rKq@e&?$eYuz`-R(ek0k;-Z8xa-WDfTAz?$|>s$ zuHK&VJ$}W`_0HFyl-?Y|t_eIPO-@Q5|7AJZzw*_eTz})q9Vy1m|NPSRJr|vvVvsIP zPP9}i3F%QP-sKeihkt$g`qM}LH?@`57F(L(UpPwFwRn`5)7TLsIQ_laJp9C2L!ZC# z*zTd%WRHFBgsYsEJ@$E*ce?%;*;46g);yuSP*D87yG_t0B~6`DX;SaW`rP?@*7xoB@otrQ#ig=8`^(=-_eZel!Ld|$ zTC{Rfy8l@%X?K;Ee#UOMm8+a?^+%M97WK76F=#8LQ>q~FNaZxP(nX9l_fq+&#BLMx zM@dijN7n~i^hejCX`c7}l;(e4Dm+T<`(~h(lhV^0je2)5_0GZX>{>tZnr$gxt#+(k z*foJirE(fOMhWjgiuYCKC0{yi{nod{dPRD~aH6GBzNANt@9(&9+5K;G<$wOoImb@9 zIL*7xv;PynzJGo7?nm}(C&O0yV&McHshq}+eNg!m<71co_m+A}o|nJxyIYE(e6ec+ zk5o=$N2&U^qtlwD)pB`GtnI#@lOAp5L`&7>dnxI%gNnabn>T#$v|)Wcc5v6Qa?19R z2({X{%4yk*Y1I1fYKcelfPAsr^W0TV6OS1c`RniN{^;_wMm>4-=zd>`NSB5`u_Lrp zPJ3JBggoRc;)eJ9NQzw(?m7Irf7xOcC7lxKzY(LQQof`s9#l@0CGiptCz&+QnYGGC zJGIUwih&*ZcPE+jtNT*rGmJz@m6+Fbd<_nzw`kzGc62I=|!A)=+ycqKg>yD5fy z$X6=g8KO+ubri$b49*FuRJQWvgsYs!j`b?5x#i_F=ZCuHuJU|IuWRleCx}rK?s1~I zGNiV0j}zX~d0VmSccXgf18?2Bt>`INiKeu5TRGt>N;^-uih9fwu425^1T|L;y@c3x zf3ygxh&NBT%KIko586k0dCf_$>(Np<9i{3{8EgGEL(%*+arog=hwCWPqeM=$R9Xi~ zk2Uei6W=)ZfopE+u5DLKt$qLcPgiAYqE;N%#8`ctU`<2?t@R3fv6v!U#hO^haE}xD zn%Mt7YB(F}dRcAaCq3+FD<|-%O|)k~V@F&0uh0JCyF1f0r#!kU=2{)QCh#b_)7Vj} z_WT&z766yc3 zifSIn=1hgn{D?Y0xQde=ImP_o_i7?&UFty(s(xu6Xmdfd9-ToJDDaPZE{KWblH=al_G*gK|dc<&|rBc46M~uGD z)108@@_4Ud*90C#cN#lN#Tilm&sR9v^d@%Q=XiV_l!~09&-?p6X-)L=AlDo!J9d=H z2|Q9cjU6@jUmW^ssXoxXEagjj#BideQof`|jNsxL-D|ELM{PyTu|u!(m8_OZ?L&IB z6>130B3CKKJVA7+oMt|z(XKrzmC8q)&k53_4*edLh?Yw2Lwem-IvX&SwChts(ivBZ z6>Zg0sTD|%wtDd9OYZ;d`!4&GC_gjk%3C{`P8@y}zF0WnDyMZ#4V@G2aUwrucnl|8 z#dUj}`d;y&AdG-7D)a8=({ z6GV46B6*zXTcJU?$BF)A@H~X8`gUs&?s1}T-3H+vokeIjrDNx9RjOzOk2g)YN;!2` zlYOZ)W|uoQXFv9(H=W~sOAl%;V5|RQZaAU5P*L`tNa`k$JY~G`)K`{@t6veQh!Lkg zzf+`@`qrI4=qeg_hhFp8i4>1?m1z7%ifr9ODpgL$T2h`Fw*T%)=^nCnvNL{vdpc8L z*90EL(>VbeTj}+^j5q%3BIT=H=BtlBm+FLF6L=I|=TB(tC{=kD{h43dk?P?k!LA8B zimvk~G?U2-f2|hUev~-fqNjj}xrmbqpt5)!TiH{t7aUQC&eE`L%7s{LrlJ^37&& z{xrkqv(wn3>+=<3S0|*aZj0w4I$tS0TDCl2l`7>69WnZKH;r8%)mm${GALitBZd<# zmGUJ$V)Uo*6a$a)^3?!4VmN`v^OcI6B1Uj=70Kw+sP;Z5y(7|oA|{BI%ITON{j5zh zijmdz|2U zuqNE&MC*D`^;s?55}k0B()Ff7 zv{io>C+$(HtsIAyvNso4NqNQ>=V)z|;_1Z5SKE5B-=D&`oJ&E*);e=w8DyJhx ze|k-Olt!PfisaF_f+l^ahfc_eXsMiz80?zjXzjmlI`4yoCAHov$CPKFX2w$lb3X zTPl@`^sEQdxKa;tE$h98T@!c|-D&KhRQld)T1$KfV84=z^qOd?R1(soRDPA}dxqyr zdc<&|rBc46M~rUuN#BPP&&nW=uTi8&3@2JDrz1w+gK70qPjTi{KkK}sd`XWOPPA0Y zm-L9y-!Dlq{Op5D`C``u9;uwhR{HP_x)g(zT6wAL+?gW1CR!?|qg27^@6|@LlV^fD z)o_2NpS8pF4a{m&s-QKlxY9!%G2G+vG)lO4k`wN6LaP$-a>C2&C1j;`b(G2pSFz^L z6RzrW+L|PirzEs@)r8mFYpeOe`N65x#yw7G-f-QX6Yg=sPaiqq9w#*3+CAtVCo~J% zqr^QNP*yQxd#KniKtAlxB{I;#|_0QLb{DUCr~C zz1q0PiSCFtydmaPPPnSKC&n&gRHKromVGNsj9paytW7G-f7ali z&kYJHtumk^ReB7?kNfG&v~5H#@oc(N^_H6D@+rv(%j^I;c3EEnw!rb!&RD_ z^wV?Po40Io!aYuC2J5=r z)ln)ZTqRp~acaUnPH4`z>*0i}WVh?#9w)R)wEM#eSIKVohkKmRy3!t3PPj^TdtAB4 z39Ur!`Qe1CWHSrAPgWcEIH9$R`9Dp#N;Y#dC*0$NpI4>{SNZyw6a95~8l(Pnktgo1 zmc81z%4x3XYr;KFXy0W;b!xS7!d1PU&5yWi(68s|4xR7kr04nT|Fu-Qb|O8lqx$y% z(r!W@$m4yET@!c|&uMI>55G7}*MqIKR{8q+m=k!UavD2I)jt#7C?4lQ-C^-lCh5`) z&x8}vQaK%Wams6F-eK@Lh#jSJ0*_+TZc zM(y3EsUY;E(&J$-7EY8Zr=Q(-VS1acCh&MXy-`4VPPj*F7bQ_T zc21Nk@>Mz|tO-0G&r6tEcXQSaPapik!ymrSYwk~Vv13MQy>)_jF&cCiDxOqVMDmmb zb*f9%#TW=wqE$bBF)|$kRbP*ucY;kl}4-4U({S2?W_%DamBNOX@A8e_brnG^1DLSu|4LOJ0cC#FZD*U;<4S)#7F z6RzT1GEcZl<4vcUdBRm1Z}d%Gb1#*vG~U|%;e@L+-rD`)gsU{(+Wp~#tENYy6RuLa zu2^WR@q7iL_HY$8?UWPbRnp^V*VFcWZ@)Z!H}mnm+b;Y0cfWA0?_R(E#s9be_Ote` z^(|-FN{{b4JK-v)Ws6o$>1ty0^aFbi>1o;VXHQQaefRjb+g~=sliem<<+N;-D!!on z?7Pl5q}^ZN*?#G(znRLeZ#rFZ=POgb+R3n$-hFG*2s~0bec*$qB|5(AJVm(5OE?|F zRe8;)Vz}x97rZ=`D*9uJaMjK0AmUp{PI%e9=6`$FDJe#L1KSDrIC1WEFG@swXWI$) zIPu{#Uoa8FJx*-f`@@N-`@=m>yy?-C5>by5_c)P{U9X3GobXwY_qltVc<}o_NF%iF z5BE5MT@&AX>(8YcYV1CC?ZwH~x1vao??!n&S}LC(vh#X4;d5ozM=nb7;v3i=!#z&i z@x==h5#QN%!aYu$yd#J@hI^d2_O(Adv_f7F_c-xe_v}kVd}rHBFyq^eGT_9CR?j$#Bjn@PWzaS zU%tHLEzj-Nofgd~R%D-#KGSldR5>j>=6P4D%}*UUa7gtiS2fCB)qEi2LFKe;rN=6& zyzl;JVdE-TK4!a39wJA*W_vKQA&?Fs4Fffv_mpeOFDKn z?ST-i8S=;$p?)Sk`n)7cmD3SJZyVlzyB5~ zx@W6Dv>TxEKJZ;4e(wxXs+^Y1zXd@{t(M*&w2x}b?24MBSHF}h zd${QTfK#n^=?RphI5*>Nt>di7`juM$VVx|7}-e@cL_zBQ+%861% zzDk!J{h@Dcci-Ah*K59S`<(Duuvj?ZDyMy)AJ=K^Zoc>VsfHDteGqE3DOD=FRQYM? z*1NV2Y8}ycw!MbF8em7woxr0t!)ae*77HibTGNE9@-v+G zhu7Rye%&@5!&TEOoX2n#>8zrwWp}>1SKrz86;8I!R9Xi~k2O&%s1v2CJZmi$%3Fli Uc3%fQhSJF?CrTCjQeLY62ea}chyVZp diff --git a/test_competitor/meshes/assembly_station.stl b/test_competitor/meshes/assembly_station.stl deleted file mode 100644 index 48c05e674579fa4c78713049a6a31cc16eb2858b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10284 zcmbuEzl&Z)6o#)@h(9oD_78}nVkJo$QTTEN3BnRckyr#JsM+0!T8K#zOp(t*NMj)h z!lpF@v`7kz6uLX81T=2M#zHNES_Hw$Laf9&bI$wibGawi!a%}d&il+eGk5NtGhYt9 zv3l~@YcIUD>(Jo?yY}qay?@vL`?J;Y&%8gnyjHFsxw~}TykA>eD@&K}>Jhj0Awd=C zh#Ng5m?2UAS}jlhb5G(yf~trU4`xXG{pM=fasIx3d)63=~ctlT`cBk>?XRm6!0GbAoudbK?K*#n6O392GaJeVP|vhqsV_~F6Cg9KF( zCmzg@=(?B7&95FxJV*>xjYvE~6`AdR&!0J3PQ1G_@gPA}#EAzpBtHH9rSiw zsv=H2m?3d+|BL0vqnQT@sv=H2m?3dt{Yd%coy>y-RS_p1%#gUTbzqizkf18!#Df_U zkDNU|%h!qoRS_p1%#hf9Yd*`LISHyFPCS?)@zjU!&+@%Xf~trU4`xVQSYMyz?;r`P zB2GM*A#wHdi?jS&LV~J@6AxxceEr?!S^iBVK~=;Lh42g}U zGRw~>5>!Q;crZia$Q{>a`B_VXs)!R0W=MQ+@S9m)GmxMv;>3d)5@-MTcBYzWSjmu} zD&oY084@?Pwq~`~DpqR^JxEX$ad<56z4e&ba6Qxt?DXvU5u*6^IjF8^{aOGAd#s^yI<^*<`FY=55Etz_d`2zOekaS zq!s(9Y~PxXt(9huk+dD7_A#Md;+gIBQM-?7bLn*X^4jy8=8@gcjOLxH>qsbLD$1pdETeYU@p$7@7B2GM*Au-&E92E(wB2GM*Au)Uh=|O_3h!YQHNbqeQt`!NY^nQ$Y z2{R<(9qgxxp~~OKF)@6fo2vDzTi-N6)$l9I)n$f+Zhd195>)9nIVPAPp=w~7peo|z znKMH|RmRwZ1XU3y9?XzXwKVo1K~=l=HJph~yNF~JN8-TI~ps&tzi6U>m% zt#6v3O1H@|!3+uA`lbo0bekL#%#hHnZm%t#6v3O1H@|!3+uA zAg2kcbpLcB?gp*i)%jY>A>E_e8Ktw`+PjW~GS(Mq#qM$S+{Mj1Uw^xK=v=bCcz&pT zOk^t3KB}l*+nrN&n`KxhDjpNcm^*33KB}nO+dNb!cwgJ+x!T7B>jbGtdtC44rQWz_ zR?}!ymf@{Y@t9D?+(|3;QH95b)efq{t*=nElT7izqY=tjU!)bgM}6L0aVzp(W2`&Fm9 z#4@ad)b1W6G8Jj}u(QL?5PRl4Yjw7pO0j#8P{vfG6}yN1CThQQng>rvox7$|>>ebP zF%@aW?h(7B?On~>GOSD0?j9sE6>0a-znDim!#NVg?vW66go}BQuKU6&fLPu3ZP!k; zzEt~|O0j#8P{vfG6}w05lIqrVu_n!(^{(38gG8pXe%0J?!kLZ*E&FJweQ)wlC%p&zuC;owQ>2h}CT~kJYX5;vDSF zPwhSr68!2*McO^AezP~%o`+5Y%kX}#_QhH)h)hM=J?sp$O2a(#g@dZCkJ{aX1b;P< zinM!J{bp~jdFV@ReencP>>ebPvA#$vc8|Kc_Wx=&kGU!t%kcZ5cK0BWsYts=oH*@n z5bp7u+y6VZ=O5qp)?U8`eyTM>8S9I*V)uxh*q*uGC>w9=zUHCW=Rral>x;Bv_lSLE zYt`uqS|;{Vd**6)4-%P*w0p!ZY4gxdv`p-hwmxci4-%P*w0p!ZY3HC$HS3Ejs@Oe9 MC}Vw*R_q@C0zn4uYXATM diff --git a/test_competitor/meshes/battery.stl b/test_competitor/meshes/battery.stl deleted file mode 100644 index e36b9d48099450a1096cedc64fdb9de3310042c3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 5484 zcmbtX-;0%16g~9gB@im#YjBv9VCX?Yow;%`dTUTv-iLgbN}z><%FNrF+cL3l|9Gzubp~wbl(s2BgjV{ynA%wn;TWec^~0g zj)n}htgmiD3LjJ4oPFph{7vOga*G(!a&+T^u>g@-E#oHQScVUsD-QT6(GO`kT0Yub zWgiW9Av}&2fg5Q#8glWmd-qeH<=^$pY%^HVhi4(;;CV`pa4knep0T<*{_za#zxmUb z+kSs~@BWJyE=(t1Kb&*+)&}JKxukQ*DLGe^|sBz>hj&Kth9jUKy-uNJ5NXyYPAE+X_QI*1t@*`Q@ zQ!*ulYdIRS@|M!Q>yw3jD}OGa?x^kBsjbl$H_y+bnpvK%Z~y+&kVjeG(IQ;S(IHQ- zd^bN|Ywq=6u7V-+6Rg@?c|95}MV{lJrIe1p_x`TG?sbBONBNWe+KDORaD;R3 zqI2id5_^i(gA{zZKSE}YNmk917Id@Tio%K# z7HimhFKlsMp}-Btn-vEo%~qyp*YBQsG6mMj_7JBT8}TK#lJsP z?`oHD1E?$W=aO=Wz0x9FOZmb62;WnnXK`a+DZ-q0?L@>x8IeP75y~QN9IdmbTLHs9H6|Ksqgzs3?5#P74t6s`o4ta{ZkYkTH!nGV7@)Wa@($dB4 zksfn=H9zI5p3~N%UjeCuI1>E8BT9HEVjPVAzQH&{w(+qKL5Z$ep=Euk|K-v;ynxkU_V!Jp2{MBRAfEMGF8+WTF}p;|Qt@$hQeT#OkzxBVa#YRHeMHv>swTAL(ur?>K0b~9Qvbjf z(9#MyvI|WiT+7kw%ED@T*I~T?^zE#de(gS4Ld{Zg1a8!$$PZYd`;@DEpDXyxV?Xq&U$FE&RzqYt7A&dv3Zbb*;38LMCZi^YA!_BB6n#yy3|~o2cN;JW)wc4<><<4 zeCeN8cRJtDYNo4UMZ}Pn`QWUSeY@lE{p*M)+t|pRK~@Os5FR~xEwzRzyy8sR!&IyLDs%pW=)EsvRv> zuvB~q;aZLkng6TT$QiwE5V_D`f^)@J5y%x;AwO9|Z*~5^+-HUA-MqfIt{byUw9a9v x=8A~$-e@_xBC>+C8`orIC2TQ26v}-C#X?AnSV61V1}sHNYNZsVQVPy<&UxM^tC&_zIfM{mv$Zc(!#F2d-s2K*Z=qVLrUpacYJgnPfC;a$IHu2 ze!8o_cmw1Pzl&_`BZi`s4cXW!edfixkD>o;XO;cF=g2LFAPV1j&^}`HeGS+h**OsV zOs0q>`qDT$v1js#fxZgVIC8{*SR#(z-+?`}_clfhdNpK6uCk3Oe2X~Xm1)o?`=tyI zfgLdz$B^B4cHuuBFwsAr$j{J5ouMdYLpIwpuOMw+wc7pl`p24A9{7A4a(8m!U_%-0 zRfeLJ-GH6a_a5G8-ui{5=?xFuG&m9=UOlyW zUwiutE8Tnd9$7ti)oPRDFt7e{;Y9Q23nx0DD7#n6pav*qQ+691LFC|vIR@q`WXmY! zEg6ck$k~%ej=udfY0qcB>3eN#Xk*4ul(IuMdqm^Aw_zG({OtG%%mvF^$xw|l($81A z|Kt%Jd!-s>UU`pkmA`4Ap4A?alkQu%Qe^LC!O@X2Zv$fxp99nd7kbGFJsk z*pb5uLp3^Tn)T8~L|gR=D{Y)zWkePX6_H9+s-@T%56 z&Rx_G${vR@s2NsH_T<&)xto^SuYP}{`|pLLtDGgM06PAZy)r0+8l8ziu_yQH-H&c` z-#xh0{%HN}yYe_U$Rbd$=9&v6P?X(Xp+>Z9*(>vO07uLGfo$Cb({h}SVebr*ege82|Ga4F^&68+1+t7^|ic+@O1_K{w zVFXdtU>)NRIaXA|cE^)3K=o{lKN!V|vRG83_9z!~cS{CPWfgV5JPy@pzgmyWI5Mh_ z5G$CzvwiGU4TZd6eK2pO4b`yUVWvw4QLwJ|P%&Vms3n#wWhe@@1PqVko<}yC^<1gn z{Qd6TTv5&~9M_K`C;MisSvJ95=w_C*$~JtbufQHUHFMLT4lyKg(VG=25hM)%J9kFMtPW`hUT z^&Cyvy;24>h)~&-J@WTov8TQG%Wd1wUAd>_dX8#fYaX*CR|d%TmXVS&LSw5|*{m2| zuWq|?&-BqN_DnGztZJ+{xe{6Zh^k~2BPC@}Gi0+Tw5N3b?wt}*Z>M4q7eD`lGhKhcj;oh1&V2DN; zS(Xu;ak7n)(W7!ZR&CuzuhD+_X6e~g8>&$T-#Nz&)l?YGn2}L^-Z^SO1-rr+pl0lt zJ!2a>f7D)#Y{iCZDvU~Y)nD~6#-n0mz_3~i-y`dnVJIrHei??M!gj>*t<7s}joD;Gj;>Dp&IQs&Z74AQpSPVKu>hfVEfps8VWl=jECi|w4oaI3x?*fWDo`G zYESpatr?1n_m7A?-*z?m4r{vj;PRZ-64*nHf<4N!Cl>2hy(;%} zWhhG7WgLh-@19fawOBdW`?wEsFUjpVoyR^nV4z0XS{tzYs+|(NTKwmMHLgMA2d4=g z%~)wM=E_i1tUF`tX!f@T&7rl!^Q?);qEb0*Io5jXCGw5r*DwR z*rZjIvNgW`t!T=(B4(E-3y#ffJmQowZEjtj%k#>tOB_BEegBA>STI6^oD}Tn$#eBu zK9S|eDk;xYh-x_IJUOML3`HrMvY*J%pMUHG(61t$uR4#3#`mOEl&&tws7fBkjN>+_ zQAW82w_+$t?`mdvc%>R;#5uU)m1-)CX3IEC6Zz@o9AAZ@sK}3@A`2CUYLvlqd)=$N z4y-RlMNAbNs!;~t8OAo0p(xE;IUdST6wh{-wxKAVIGho+yqfKgQE~56L=IaM?#K;xukf&!QA_H(8rC^9v6J;O+x?ya7I-rO zjod9XyqSOvzs14L1R1f@_b6pU_M3@UzP-_XGT%;|IrWjpwSMF*UgL?N2^__T=WnXKvI}xMo+!CM9JIQQ0e-S7?Xqk$)xs9p@YWI2D4k=FVM9mI-won)*Tl&JCqTOwWjjWJpPn&7DVzJbqrqjh#0;-g(Kwf2 z2GUXmWtEdX`FQ*;-yh@o;4?dTQ&hx3uV%L&af@Si7$Hz`zOu+EdmPH323{(gvU~r5 zcWwUk9r>|%;wBz5j<tXcec9?|LOaA4HebL z*lr`{4YC!BBYl`<5CyvcY#juUeHhfs6(%ZKNALYDo*&c56Y(4PN0!T$@% z7}Q{&P&Q@XKTa-|aB{0y@bkY&?k!Zot1QlVpm`&xp ziI(YV=uMI(lQ1wt%)7VAN=OtE!`1AJoW-n4qBfoHJ@0$&_ndp9MQ?Suzu)Kl`JLat z^R^#pzV+&%q0!#$J6`O4Zu1iZz5nkwZj6~a^K9Rho>j@E>%++_E2Et+)=o5%BfFEu z_YT&3Rwf#?JwwS}`<>&EP%WjuvovQi=KQXo+jfoAu8~4(=ceP?u}9YBYs}p_k$qaf zFJFV>kiZ(0D2+8ZlRbzzmh>-tb9S-k<|iNBeX9P!@?_)J=IUg<9bbLs<80N|!+wnK zA)#6^jhR3jW9Oa)hqaWLvpa#-O;!tItcP((sFu&67N=%HonDxq2!SNm>EPz!g%EMcp`%E5Ny#%S`@wJ$p(whmV9Bs_-_ zx)Y@_Mhv=h%)T~Xd;X7|$^KhzO{j%$-R(&aaeEEv3~fjY(&k zweEe?X+>l*wGXZCGMo~srL@}3n6Z%>)=2H0*FmrfLyPqsYj7MAsuk0m$(a7RlMlZ0 z()-D!spUZs^gi7?#{3=MQ%U3=X!0&x)Yrb{~XIR06H(!?P06Mip=iJ)pI3O zi#Zrt%tWqoqIdh@Gc*WKXC>30S1ayLxh!#6*$X zV}g>HsPSY_;bVr-y-zgvUQbEEqZ9#x=N@PeYb2~B~*(Z>faxg=TI$- ziyEcY)iqQL(oQ2MIi9PRxPbQA)#7I>u$rUrky4KH9&6H zu0JGHOKJUI!9#)qpoN~o67>`NRmc-MTp+f%2s3Gyzi zz+6j;o%soWBh`p;}6_*K&P{^^(uJ zezl(EluvWmsP|`>>}j|7WRMN{9JMbE3X@HREU| z3Ui`rDGlFcJ~qa_CfeQiN2*cF&Kq4AI|J8|DWO_Q({8>OO>q|3OixbxAnrJJwnF?V z!Iz2BrE1`#mKH@m*LgWE$2lcfU1^S8oDXbzVA|TMZr^GtPR?GG9)r`ewmVxXp;}7Q zubi!B-@JjSMg2$-AM8b04G^iC?Tk@EwUlQ4aB<#RK7pvkyLJH)#aY5FL_E$UhJ{IO0dCD5eMr1WPR>S}8VB5N(23id+%PApQb{gcuuZKf(nC zD?15TSQ#u+TB|dYnajAjVBx~S%ig?sJMZnKxW98)+FdVu#kFN`VSZ`W``;P|T0y(_ zeVp1#dj95VA-tJ5$&J1GS-~OVmciLrhOM$}!YzQa+e8y}@`di@Om8(4BE|;*agH$Q zDTW}?7-HBe%Vtb$fQz|Gl{6~bA2nx=oXn(i$f+DgTot*X`>25))u=z}w9OmsOLHg| z4TnMQYL)gS24UU@*tknhQ-Ad8N<6{o*(;is`jS_S0Xp>|hOI(ofKTK!v*~w`FF!3# zy{#N*9$3|>OJ6#PYGAki8e-Tg%N}U7y@!|d*U@=X=a9Zwjrn8Fvd^7JC2$$Giu-D( z{+Ter6tOT9y1VKkrRF+K8h8^OMqHI;W9H-)ejimz(#hQFDMpPL22U8n=9vJl_Y#x$ zkI!jl4u2o$*C79X<_?1#wN;@oXrDa<_$+1o#(UQ%4dYLgGi4+Daw^NPH^Z`JCY~== zgL_%F;J-E0(deInPWW3xZUd{^lgR#P4SpjG#dH^97`DoueYA$d;B3U*XeNLkb#4it diff --git a/test_competitor/meshes/kit_tray_table.stl b/test_competitor/meshes/kit_tray_table.stl deleted file mode 100644 index 8c20cb9fe5139ecbad33968d4478a671e1c15f5a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 35084 zcmbuId9)o>mBtGhA`pfK6vS=`F35W~;D`g$t5O+)iijwK0@@7?jlx=pLIWK%Y3PXJ zfZd?Tup}zvy~`9es5J52Qa}a;ZH$6QTp%noASw!NU^&qHJNtZfzf*PY<&Vxk+3W26 z+xyH@Ro(Z_6OTLf*b@%@;JkOf>%e*Y?(^zb&inuU`}XsS;^x_Fr=Q$p_sODIQs1+9 z>uR^%R+c*~-L#rHbDL_>HrE9i={NuHOpsHeZAJQxTfe;VG@4MWN6y)$iI)=4Snfjf zPdqhQeRI3B8ky-Sp;k&G9_f@&jS{1C)h1Y1TBbAx>Wa0fHL7)36!-4_&IR|bJlaZJ z)aI+U{0`6_R;K!{I}aQb#k?RhJtfpiX~?9FF#pHiv+-4kVL7SUYWH6=r$!DD1L@3d zO0=z%h739o^|AXd3^9xdy&?K-Y60ntml9e(ZJO~c_y2lV-+DlG(QcPcvKAjd<%QGQ zYLt&WW2fnqphkN;XnI0=QS_#cxr*%(`&DQUv@>fI@~}OWP%EV&leS#h*eh;dG4tsA zZk<3&SqTRR{Bq9*JVOLc%zHyvj4*<` zxuT{`NSmXsPU!U?|LEfR)`B(19b}7NF4_IW3CdDeLa3F}kn6WDo+yf=H#=trW%=X2 z+x9Fc=)0jk&?_OwdTZpA$XTHcVm}`E_|O=t(RkZjcUy^gT|zZE!rOe+`pcF-dCK}> zZfA`m?^D>Lz{puOGymC`8T*wZL@9MX|@JUzz^M>0dK;No~(}0{LyM(3eP44kfRSJ@5Etjbi=8c%igt^Q5jCqHU$L)RnCr#T!03`Ir5^JcChW z?VOD5IEzYqScww?(P^bL=i@NLjU;G=*eJP=+Dr`2oXwg6!st+M*%(R$t609!r^gfQ z?aY<8`Gw2Q#X4wnY*C}PGrCzdqew?VQ7F;2Qkt@PKKON*cdM%p+oBHwb%N;H$6}ka z973p-(v)NEJ_o_pl=eu8&{F8H<~PX5VH~xYT&b1iutRrqMFJWnw0uQzOz-ODmwn+i zwpuMgdo+F5W*-}&O9sdl(kZ7z+e-T`<&?k-z)=)S8)86adP=C3(&=2HuRU`ACl@)Jkh)Ji!`8>|tG%_iR72@^y8X6XQ4t`8B^bX1qO&z*vdpP(rPg zhD^FBl(4$O{2F{&$+5mRGSgE+t(1n0dCbST#z20lK@J&XG@({Xk1sj>s+HDAS`Lk_ z@w9w4`<(WTX1+4N&1{J=VQo;Nv`D9%5^XEaQAnp;6i58*{>h1Fra10qp)*=SmVDLQKYk_Ck3RFWSEfYH3MFT{xA|84tv7Ss z7QbaVni9r(V-ArXbEQPas^t90G8M%Oet&cM-g8c>AE>rk@R8qKSRVS-Ev;wIs}7#} zOu2Nct!m76l#!ki)WAw<%07!;_2GxgQ!hTQ{==S|EtvPvY`Nw9Z-o3F@XH&^*MI4o zVWp<*F_fSNzLciyF@Cou8)8uQ7)nqBUrJN<7<;~HezmUuruwXh z*7e@^xn<>*AD$c9+OS%-|9k3#HrQkVa|K`8GbnouC8&Wfr73%i!8VJk zo4&VjF#ngA_rCw+b>$_y-WuBDQwLsG9&_3qp*<*j3?-<6FQqAa3>zDjjgi4#{h8hl ztFJD9GR?#Y7_-}R{;dm?;kPAGc}C8&Wfr73$1Ym3rqxPD^p?|a%y z9@^}?<=Ia+ZoT%@+j{@_+H=d#J?8VEt@w8C-#WBE(>;a~IV<<;w97UhRyNBG4*9@O zdyCKhT3Hm|3H5WtoUfMm6*q@Ar|dD5pa#B_rtC3nKCEn(t52I+)4S=tE6Y7zKO17G z6=jd11U2xbG-Z$RtqZOw*Z;tW>YL8nbOBomzI1k`>@k#}2ELT0>@jRUtZbI6PgrqF zk1YsaI;&Ck7)nqBUrJN<7!QAcvbx}mH`IUsyQg}(8vNygZYLa#kPknp=B}4qw`8ls$$L)M(ye zMNb~X=8@87hx&+pHeaAS038!d_ZUj#tl*dF9)q)J|KU}=V#5mu7wrF&!BOYGwAyyN ziw3uT{r)gMC?mZnl%OW~5}KYo2G&{htM!&WFloJHfVCDH>lp*gCCEF|y&Ov9tdwqI zpzl`K9CqK#yMKKC;OZZ4P$Rz=U2@)FX3d-skFu8|_T4~G17Av0_86Gg(H@5%I_SN4 z@@0dQSKk=wYTa#n58nEu?|mqH3?-<6FQqAa49tg^<<=g$r2pnael&aDxle{Ru=Qzn zho!#>@tE#0l*n1ZFVj6ntletWCXe(_`sWRr`uWkLPtLO5nBLSCS}+hfEBIx)$3Q)y z^won8&LF?npZ#zs$9+G(XBPbmewpqul*n1ZFVj5+_PUtczqi*MjPn8KbDL3Z9%<(J zkna6TiJTSuGTmcfZ-pJx!CS2EYYAVy!^5*b{ipRpA7i?gLy4Rf{4za>QQy7yEtBl+ z(Aa7@z}^RWXS$ao#t1~tN@0^JIwq4PHdqbO12eM}j>0S;ca#rxmbdSMZ#@So0?qPKP`lP!;UrJ^iorwx1 z)Jkc}9s}zv){l$V{dK-ZXCFh}neOFKB4-7^O!pW(vFv-;lHOiVe_?>}fqK8>;*0Gj z;x}QeQ1FegIu@S7J6mi}rs#9+F|P$Flgw8y|c6g~dVPoL8}`m!yXa(wgcFKJ>h z-D4<`vw~lydkpkj^v3-@`Bcw#MFZJK59F*CWN&jNY`5K5DNWg9;Ov3(;5E13f_;1A z%T9Cy+%v(7vd2(@8dxb!*<)Z&g8kKH3txqO$w2m0138}u*<&bSJFmt{Y04fWuAKE* ziyr8)H=y2i6jAo}P=XrxQkt^Iz`Tyx`jW5T+q2t&0q!r5U)#wJaIXO?%3cm7sDYKz zlsyK{o3@*&A9(7^J?#x#k&$<%dpVTIS-~&UJqG3x%npA!{J|b`h5TyIpzP&Pf*SZz znzF~heGJYYv;Bi`#tCa5G`7ne$XTRW6N^HLoR!iZBknS0uUme@jGaaYyoa$<@j&ip zLb{hjiJTSuGTmdu-D_q0t3Kw8e|fRr!+hoLqXuVgxGSlPVWM~`D&p}(&A&$*$ln4a|AK;*38m+2mZJF;h2E}XHOi2?3p zpke!?ewglcr9{q3X^&xRRE_=UcQ;?!W9>qt=K-dB3?*__N_&jB;#4-y`&e;UKkQ4e zqTJLKZfgTU4Xl)=>@hIsVkSNKvi)%f5XK!eoH>zqrh7S*$XO}vG2%|9+WUiZX4p%R zU(Bf3x5J9EmqQ6^f-miT(%(W)tkJi&ocr-f?kquQJ2TzOffVgsDUq~DSM3A+bio=eXfx3g`Uwn%KI^8kD&xL@TD|mj}cdc za=&es^>K2?ItY#4UohQcD3P;Lx``3)+bu?qw^8t=Cn3sBjE0~FzLciyF=8*NY%b}u zY#fGSB3&qUBldkiJ0fiI;gdkmauaZ>&2K4;EgN6%+zyaU2~&UByK zmB?AaFVj6n-0#+pulZPyvp#%ruMR899zzLgV5Kx=kAX83PDW3k{b@X#XwJxbYNYHj zl%NK_l&0)4cq-cV{yl9j@jG2;^e&X?-X2Qitd#Z`Hj840EA8%elB)rH=?Q|e$54VA z_)?m($9UwgmxuRiuYLX@c(&c#U&Obx!dac^URO$}72X5kTQ>S`OHnAHRyn$xD-zHs z!CX19?(B;uRxEx5?=5vo+rE?Yx@r@&V*RkDdCOfCN^oU?Rhwwei6aTNO~{Gu+xfI6 zjlo+^=A`kfyZ$7G677>kbHkbtyv1Qoc$VcUBczWc)Qa~$T|zZ&0?%~D6QF6uxw1>B zrcH3~o7S!pkZHwvw;Mw>ZGvafG=>t8X~p%t8$&g10(S%B3DC4kcNyAuRnsPTKa-Y2 z3COg<-UlbLZmv|*Ca^CZPjF8KtMpW)M-zHB z#L0_0gOpHBn_!O_P3Vb^>wLF%RnsQe^V1kgK&BP<4BZ&2X%ig5X$&PG(@OV`DWRG+ z!DpVM3DC6SKCxR4)wBuDk7*1gbT^sqUbXM4rcE^Sd@}DU!Tl%lu6v0zSE^|fIDd9a zt^{OSr8}85$6vbhs&D!CIC#qwC+kf%d~#>Ljfrig8$&&H9b^cA4 z_g=8~eyGJ7xx%}^kU>Wxr00pYmD1kkD-M0d{9p7=m_T`~gzy!4$=|69KoYmTr+G?>vtKkb-D-%X)dEXmf z!BJbTJ?=%*MWIC7inQf0c=9iDM-gfTPpGTK5AKnPL5&iSk-q!f$_SIH-Bw<-VEY6A zygb)4Q1Y~wv=(Xwe)h77Nh@IuqMa+I`*~PAjE7hULhqyPxnQk1kd`2uC&SMiNVuYqOsyz7G4gGp&IqeXZKw~HA?WQ zewR><61;Qj5~@*xw^3a}HA?8S$+WIiqeS|gQ|n4KO7N|PZmv|L1mC9W5~@)mePc^= zr5Yu81KEwC8YQf^huhCCp&BKu_mp_QXgr}-@WeMR()Lh|64u|E?*c{>YNgNaQ$jUL z=#$@+P>mAlQ)F!q)hLm^1F3{+l+gDB(p;%ViS%6qjiDMPc(>ebLDeY1TjwsJ8YOJ} zH(zt5ImTd=b-)p&BKuFNALi zyM$_#u$iyLal5xH(&6jxl)Z1 zHe-bEn7T1kqlC>$;Y+kGp&BJ@K5q6zX|9w|E1N5uFW*u^HA>ig9KMO{=1NxpU2$wr zWc@9-;;2Rm$aw-e;aa3Q(H#1TzN)JJ`)~j6@O)5qea&iVgsf`V?I|I9!(bJ6mVx$t z;#=lE5AS@`*WP^z{%=MUXCk-K8LmZyxi4BN9b`{`^ZSqNANBG5!f!NiHGqcCaUiE} zSty}aN^_Oy#!y17_!Ov1s747s)9DhbQG)BrctWlCgr-ZVMhQM|=@P0@f@{}!Laq3` zrAw$riL}RP3o4;jeBRQHk!rm6!HQ2$x8|M5-AQyshsNsb)y< z?!8N-njv9zSn^HaqR_spgj(^oz8ga|O4wW+zC`O1s!_t)sl@ZB@q}7gJC%5DG@ei^ zYo`)VfyNVRW$iS@w`SA2(sHPkwbK;e0~$@JmDTzb-+CKOsFl_F6yNb1O{kUSc8XsN zrG&PDj%^#8CEry|iBvP}S2kmmd@C>|Qq7RCS!qO!R5K)O<{Qy>Q_YaD*>yy}N;N~m zX7CaHD%A`L?zOx9N(r^HwQEFARE-k0!Zp7ElGd&gYGo^GneV%*QNq^tuy5~{Ly5#{ z!wbuNZ?`=|qfx?kA7#F`t40Z)0J^zSLal61ROZKrYLu`YncWim7jzHrvKxf(9b2~? z8bht@bl-ekH9Ce`@t$#f47K7t<9I@?uwUW(`)RIJqXcgzyD^kdE8a|wC)5i2E`If$ z=1Mh6@Gi3(LkYFAvwHJY^XM3AW#{Pdt?l?2YK0vq-eKtys!_tuxXrh{X|9w|EA(_* zr=BB*T4Cpmw_durQjHSWo%4;>G*_xo0;deT_uP%48YOU&;ak&b4Am$BndjZ{L}L}- z>+9wUXEnZ0t$ukYJDyN0-pP(9)Cwm{{vJeH4%H~ZTik98CDe+yxZ?@6!U>tbERg0( zHA?W_w;MwVwHke*SB(;Sx=(YZ^RCWBI4$Mp5+&5i_I4vitrBX*+nsL7RigyXSNSoh zgj!jd%8@g5qfr7Uvizu3UuuOtYQC;iqXf=y`Ejmxws!_t^5vMyHgF04lzRHgeCDe*H#od`fHA>(_ zmhXv5s1;6R`Cg)gTJiZ;H&?1r0w=P3zfwZ2_%y2&8%x5~EL$N~jf|v2|mpMhQOu>Jq9^LeIGAOs#}k@!3>2hH8}H)3fn}TJh=GctWlC zENncXR<`#I&(*qwYLwv9v+;yljlKiW{guwBb_Woil67M!p;j@y97zO?5>d_&romTK zL1TV#Gof^bXjwIM^fbukO8tTeR?$<2(EMsVlW}7abH%S3w>@d&8SWYIj=AmzDEqtT zN>GFRDoxqTVP7?d-+Ntl>houm;Qm45c?>0TR`APokHKHLS#QS;W~hNL-EmO%7)ns1 zeqlvV9wYvuL;bhyo|z^AE8U||_83Y~17Av0_89o(3w%9!+g%S&lYlSX)ll{rN>Bq| zN>lb2@r%R3=YMtkGzs|9eGp}jp#(MXr8H%a5xBq|N>ff^ zO#NWvrPCx>4&GCO;1>}^5n}{`8u(J0vd7@pnNKXePV(d_`<1(<)GZeXb5W5FRbXvV_3=Y`=Zsq@3Y>d5;#Y(J(%t>l*n1Z zFVj6n{3^aWXuUa;s=+ym-wZO{VrlhnW$&QTm6ls$$L)Tm!r z(UZrp5ryB4s%$-WIUzE0$mv5*V5MW2@=rY9K{uf>0S;ca#l)v3>$ypw{dMfpCADZPet&{bdRA#&PwSd#;mR9 z6C|L4Uz{`Hm+482K;*2H_876`16!R#Ie30W{ouUJaxh*|D3P;*U#5GE*yaOUo#!W- zcs@rArh7S*;JII|f?qk^7x-n>@LMmop6^ToR_MpLzX*OMUP|Pwl=gDOF<9Grz8oWp zcP~m)?h@1>CrVTHlE;~$w)K3P8u-Hef%_z-yF|`PY0p)h;c8pYr2^C4V)5=^jIgoE7{sy@@e}*}>Lx+qVz>VxPcrP;O#01U2dxR`ldC zFdt&%+Il{t1a>cM52kwzC305q%XE)nEg1f;jIHN0s=>Yu{R)1W?lF|eS-~&UJ%+tI zRQ>R-Pfb2CICi;xWrlCC!uPI_@m^8D%L{ya<&^obEWj~?_jlKuuj?~N8j?d zyq|*zR-lJ}+i4s@t5}ZEnnm%0JByv`z)$4a#qL{)4RD+4gG?M{W#>dC^Uv@lrXvJ(t+ae8p_(`|gjUl!EQ%Xf%uKJ{^eLOU{BLJ+ zbXvToHKQCxSBFq5l)M>(>IG!jpz* zrF3|1;Kb5{3M)^an&=mF^8CYd zg%Y<2wbGKu=O54f?c4J~SveNO7@l=iy1;NLEu*!8MMZ5~<@Zp?Y^=Z(4Vko|oI-;1^ho_KjT4J+Is^Hf|E zN~oqyH1`@K3EoX32JVpYG`yQD)wGG>Z#rqNl;GVo{NfI|ITa_lQcat{ol`egO7LzP zR=7jP3AszCrcLm^EUjH7csC8dxI^ZtI3-lmCip90qY2(kBL>eO-Eyd=P4NCQjiCf& zTBWBVZ9&zv3Esn|F_eHztMqiIF;vqghQC2KiU3V3oC|m=PIIN2Ho<%G(FAB(rKfGp zm1^1qbC||Z0y3?52heSE)wBuLbQ(hm$h1mtKeQaGX%p-*X$&PG(~5U8-Eyd=P2|Ui z5|C-dTdHmh)wGHHI9CEPt@Q3JExBsiM1H_;{$31~+q(CmoNkc|_7 zbUKucssmXSvTkI%85k@B3Y39%Yy$ZOreikHc4R++>;TyZ5=Yg6tO{8-%npb@K<)>H zm$c0lHDn#gZb!8PSqHKzWZfV(O6Y?8upi_Xm`h;c19LmFN#OW^>p-?0SvQOg@i{1r lfd1HIZvRLb=0i|CBddb3L3RU^36c(&49F~G-7q#JtN<%!J~03Q diff --git a/test_competitor/meshes/regulator.stl b/test_competitor/meshes/regulator.stl deleted file mode 100644 index 876f933849de7d2bd2a44ea0991c535c9736251e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 26484 zcmbuHf2gO&RmNYFG}eL`l2#hT8oNcaqzx*mrn&d-t)@X}QZyzZT0$wUg_v5DT1C;+ zmD)cv%C=AtgtV~t-mfIumL?&hO%^m3ify8@5QvEe?W*T<&OGn;nR&l= zQ*hyCPu_W+IWu#9%$a%LySKdeJs)`Yowxns6}Q}a(-qfT`vY&f;{W&Ok_)Ekw#V;Y z-S+solc!F-?BKCSAKg28$9L?<&(3Xs_cwd5ICEtGuO9r@ZJs0Zyl0FkXCuR&rfV+0 zarc_bA35@+GvBp(_-{Y5P0NXAet-MZcl_lx{hqz~bK5*e=6TN;QO-t&ouk@~c^rv( z9Ex__3 zgQ|X~2|0e2j2Y$Z^XixxjtY5i^QgSV8L~}zBIDOQoZ+aDQ^%6wEzWSCE)CM*j1k2= ze)>%h3~L2nEzWrFZ{3mJ^ArE@*@59L&UpS@`PS-n z)5%!Ipfem5o}Y|mM0txdBGSGIhNHw@#^5F$ExJ<2G6ppdTD)Ja!ZHS(;V4;l8H3)^ zF`5N8#9;G8`jQ1f$^Oe2bRCY8S1x1F8IIE4wTwY;afV`L8H3Jn)EL3ERc?2NqqMhI zMFxHo!%^Djmke)lhH4a5YHJU@(6;aXmLDah^sdq#BY0blD>#LCU?#ytMvymB9&BM+( zyNpWO-4lJZ-5IAlO^j@Ms)(|)8f%rdWwxHXJjZlrHBLoywj9~$h@aR8t0XQ#z=X|2<;t8{qkJ;(t@iv*PbM@yv3u-d+He@O8xR2xuP~A>LAX8e}e6aC|Y9e ziKyB8ka=F7=nO|W+hemkrFQ3YS&(cX3sz>ZlW?9O@rIUt(#|dWr1#W3IM258Irc~t z>mFX2s2Kko=VcFPj3{O+HZtsKdidFwt*-j&1*^~e-qpKzJ@8Dd&&mAJEt?)`-F0K^pZWj`iUP39Z!G##VheCJThj@d4CVDW(2G@F@db-J;PDX zMizUTZg}yAqxcj#g%&)54B-7D>v_*`l(SvmG`;8G#Z$z(<`LSrZ0y@Mr+nTsnvO)Z zu_vnO$mi3v{f2vw5{Kxag;*dX!zL!lT|AfEGe(rNB`4#4=70_u(Xx$>*6%htT2CqW zj1lE*$pyWn-pKCeeZj^vdB%_Ao-v~Eew%q4)z7`<#$9~(oj-f(AS%6WJdtYq zD?j?0h>w%|yANHov;WWEp7M!@UWmQc8IE#x)TP-|RpI9CD|tKT40-#HJo2?|#YdeD zoZ%>ED?Wbd^Jl68P`=t#mZ4(7yJL;Ce`M~e{loGV`^SuOwsKe1J`o@HlYyvxSlx(& zZL&<+;HGUd5wf0Ve9R0-IlJss>}d)+9K#;5%B!eYx3CANxR~u0_F%T3_@kpKzxxj! zkC{8eQO=Ha%bx0V_wv6udJ3Pyx6nRK-}pXe$Ot_ir|FCNe43o$C}$&!-9^*^EyNm` z6E;!AKFD*)J!3>UTXN@#$K-LGo#e0dFAu%!KQ51Xn(rQSl(Xf>b&d*8%=n1hj_=N^ z7+#q+z;}Jsb2pe7j&gQ*d&Wn3JGO2yc#P8)Hb;AmeGi*6_84u-%qizFGaTh?&W7|< zXHNExR*R;$M?V=8>=&`xSs@|wys9N&uH3bl(Qx0>MG8$N5=MO55;+{ zZrEdH4EE^QYYW;rDq}Dr=d|{yh^Re_$9&$MqjHZDiggnkSaIFX z_&l89D35}ERNmr@h^WX)b$U6R;ix_aeN^7!jIep?z_)uH&Tv%7dyHzy@D^uCUsbO@ zDrY!Kx|a-Zafa3{dOTzvt^Ul}g3_ul8Q$Xk%KlUa{aQK0QSyZ)!&{soKdvgn*UA}= z3ORju$?z6uD4La(&{A>k3`Z&Emke)l27N|vZO5@Xao?*uS(81(eW9xp_Z{VI-%FB_ zCl<14wB*TzJu3R0JzO4BdsK{SW;n{(p)Y$XZ?_#MvnYF!&VcMPI!8fb+wObDt6?)l5xg}inYti ziRZ|*pL37MsJI33z7$bpPI-RiU1vDT*{-|0HHiJZb{68mtC!v@?Ve%RQO;IvSM@G; zaP2Pq7O^AZ+RcS+i;e z?tR2?l(UgxR|UvDA5S;r_GVD6hQ+>Dh2E6&1`*j=iI-D`0G%B}jp6IV+g~J{hwK};6dajm`bsx|( z92GMSRGs^gRcm$mD(=|VSw=aE{VEu-y4q_kUv)P^&X%3(zQiIb&r$dl=P1P;`;sE9 zM(PYl$uq`NSl&z=Wt~_r`iPS#HpzQ9-E#3lPX=(d za=Uit;~D2I`n%O-rFDD0UftOJ7Yw0ba2wU;mp7jDd^m$h(w>N1IdNpD#rs8G+v9u; zp~R?MGBQBXB9v~t)?RWxhETBMUO9bwIdipua>k?Ydf(p1{_TJA{L!zKT1LiwPu#lq znIHe^28LQj#!r6X4SRp~N_kHm6 z*^5tY7?oN^hVtWbb)6v;?2=Dz(4m$!iVW&^OXJL)5%b8qUKpW!>(F^*i}#BhE3$Ry zT4l?~2#?9Tv*pay7Yw10!%o|W+B{oEMtINmp?;MuBO~G_E7|3!)E5k)ki)095B2tJ z85t2b+ho(FjxnOx^&2~*dC4H|gmN~K_VVk$XM+xB2!#$J?bly<^-C~>f%ug2LqD(`$7#%W7;K%AF#=M=si`=B!%X&HZoOUNn+)v1nNTyYb@%sNbu&@FkK+uOtS&9iMwT^eR56DY zb755M;nW|DB-SZwb)LshtJ|5e7$eHr+%?wiAdH{6?vmBTxBt@teX)mAdBJA2i94QC z4OD&Qj1lE*tXMZ*Fz6pWHQuVWVGHHDYvy8%s2EY!>OALsSZ_?w(QKYNuvE0>Jg`4E zg6P2JRqr~3z8vLjWb)$dB|E`P9qh>T1zRX|h}|Y^9&bgZmd;^{xBAaOs6x{LK&E$=|f`yt}zu(Mn&< zcXK3ozI0R+a8ClV#m9_tHnO4sxiTqPw0TL^IH7BBf5KBs5?tj`9%R+70-M2>m1oVbZ0l)txF@!<~@}c&~ z7NKO1u_I=2zJ-jPU8$L8-z=9rP=PP+}Y!A7q-ehckF9P<6?u z1C$s-p#zy{UXDsFLeVdB^wQ2aQ~;5sk45gyI#Fvyc4l|xv<*Xcs!T1$h;sINk7`v0 zHG^u=v^nhZ@SA3@EZ=p;h|)-0ca5r5OQ}O~4k|{Hw<^d~XLX`ShZv4xwqn;>Rs2tr zW$HZLk*V*!{vI-OX6K`KoJg6h&2#LbKgV|m)IR^;7|5~XoLDj(Mc;{PJtcd#$AMWe z^Hp1)(^$~w*E1aDvu!rd98-)|As?e9JTY@SY^^9VJ9XWDu)1P6%GtdSg}@{EfHa5o7h*p111N8Tb)D zto8D4MwBC2j;gI}GPJrHmCYkpx^}gu zV+9i(rRM~rNLf1g|C$G9W@gWgk?et2hxero(8Ke!w>x7*VFR&|Vds9HBVu%#xn(7=&TfOTHN3|Uw*)3Bi zyP}nO5Q~2I;wNlBdG`!QIU5n?Hp&^3z78n22%D_a*poKA)z6;i#Mk z9z;)K*F2(n<$R!emt8VPsGBrKs8e6hV789ZxsB&hA-DH~%$)ww<9&hB*{N1RCt+ud zD2>E*$G<3c^^=!i#kN+=+;d4p>0G0vykMklg>ts!nt7W?*1L?Ix>j?tJDGL|VDnfx zN^^p(dFTs>`Cc@2u)4IAO-diKo-^V#!%^O^>+3A2w*aYw5vs=Z(e?~SjXJQozRRdP zMRH2ho@anI_I&HmM%&H?Z65rNdwvfWGfb4To4$OW6VLlf-mj^J6EFShe8zLhJ!3?n zPi)D>;HEQ1wa}r{w&tYw=kv_-U8+z%LS#KJ9aOMXWb~y|LmT`2Jxjm3VvHzfOD+qx zyNtAlzmMQ9Lw8HuIn=ued{+!?E)@F2Zu7_}sCiJcvqyOga}ontanmy#KVs_~g${k4ux5-XXG@-E zp8sk@&7*E^U~>|-Z$IYmWNJ^x?v8Rcve=~qza>6kkTdYnHmA(aJjbu}B}cv2Rz#^~ zWUHr6M@B6Soqj3R%xf zNBC9#iw^Wf-n*z~+-)C^xp6#9g!`PK7ckfq6sw1oCOtOh%*IcIIjJ!3>UTXHd4oM#Ni`edxt z2rE)+R35crj3{SIE*+e@U}?5^25i06MP?P&ekF#ZoQ*8@tRug=_m zXWrgCF*97WcC{#Zke=|dE+^vcnMKtC%Gr`@9xW$|k@*#dtj4G$_lyyR4Qy1FsoUzN z@9M@JwmcD8&*#;h|Gf;M7>Q8GdOkC-hvH+-wPG}V`MhVi_SSErTwmQ;Qj6Yo@9wGC zU8v9V)IgoCYEsYFr&KXEq1xFw*~shLl_$TrbE;MJJ8%Bz?@-xiC989^^!1EQC}%Hq zh|xxpdSFO*$5@@a<(?}WBhO9^m1lB2pJ$#mJB&>z)0aI>(`6sL^;6s>Ui#$gItJfW zM&J2wUPVu=Rl5Ul#(-iTiQ>CRWPZ!I*WoQbLcaBb!58>F!&{uemwPby#;<31i!=Cg z4+dZC^$c%uhUBt`&pcY@HStxz!_g7G{Y~hgU*4y}h@bF8*WoQb5{>q`{LL_Dc#AW% zBK6gzGrYwaTJ`$E)EVC53|Y2*2g@1W;tZ`@ee3KDZ*hiJw=#q?0yWzsR$)(e*7KzW zH=&54SdnCJ^sBi={lbV9$>$q=cW!Y;tVllJ7$0+sGh#*T-=OoEdy6x)R!dJ5iZ$_7 z2!Bi6OHXu$qr_f%qB9(&)m?g`GaRMWU3#K39HrG=dZIHNr8Qi7qBDAw|9kOn4`;{* z_-j1R{8*mI%?dwH-}x@`Zu%}Q_$~k6cAVL#=Y234n^4X!9sV5vHdVjK*hK$^5YIO= zMiirhjeROUMjdJy^IJV5Nyvu{ZnffJzUCf%jBq8112!*6+-~ zLzlm40ZJ_+gD6=3h5{(Hj0_^JvgkM}wTz6zJrS>#@A@j}w+fc_aE4Im!xNXkiPT2o zIugZm{B85i+=hUnvIw diff --git a/test_competitor/meshes/sensor.stl b/test_competitor/meshes/sensor.stl deleted file mode 100644 index de6b1b7c3ce7657a8092d5945bf752f9d09d41cd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17184 zcmbuGdvw*+mB$Z;0BR8=5d;K5C8sg^>Eykwk; z6lJIaB2}pta1co@1i49WO?(s#gJOW%vD0xuK&llVXzfU0zTaQ6`F+nXB>xO+-Njz# z?9X|hefHVsmx*^xyYB~g{L?*sCQcgHCze;x=l}U{avSG*y?;is?r3&$^CQEPD^{*d z{C)r0qT;=IiGig{6a7|qEskf!1NqR2`l9)>5(%GBEu|^nu;p;kyAy_i=!rURn7Xd| zC!@zENu2SI`t(AS^#rtPRu4|U_EAAl$AfcA)8pS96(OQpN?)*VZ90M0I#+}`uAaKC zv-V(WBG3&-6xo1TCC4#$`e61w(7p;vtoPp_sk6MJ~q<Qd_#_D>^$)M6siM`qN>4)n^ z6rYK5m(-1`{IKHY;($6L)!&tRNx$h>1#a^np zKhY26u4!LibkCLl63ElXHKzN7YAKzKl8kn4_=e%B1?#e_CpXQ9&a=&dKj%v5OzHgV z+&EV_sY5yDJHvTr&VqkTsvM6#khb;O9`byok3+gosFu={Z5KFq(Z3E!UEO_ba^wD< z$%|1&=eHj%e)`oLf;x_sZZ5uO>8M11q?_y$tiy7mG)sC0B{|pk?&Y!GsN*o!7Uy<9 z%FUUuEtvDeZ#`OEd3XudSf>Q#G-RLPoSRmZW=XjyDOe>btg8~NtEA2pTXpZQam6dn zy2G1)pHMBODU)6a+PU}7nwLT!lI)A-XwS7IpHMBOIfmg_&6RPY&6QVT7jbtn{a~;Ml~667 z07g?L{heSBUV?sn-uLFr162)GY}G%1yLaWZnzq3?Xts+Z@&wM>g5_&4 z#lC)(*P?aK6{C)x)eTjLQ4hze^KnlF zP&v;)u3xswPI|6N+fkIwfA9&_QkrtO2c4UjocXO^mv8tq6Qd39x97FWUKK}*N#{cD z`=>^~WO-d(<&8nll~66E{rPB#P%T=bX_&!Zz|K4T3tOc*N>H}>Z=EnLmUKVX)t2Bi zh}K~_S}ymdc>~SsFhaHbb5QHhoG9@)R$@F@iBZCGq6GJR5A1orRjk8ul%Q;usCB}$ zI0~VErLEE&Etl!$Yx#9(PL!yDjd=_C1MVzgCCm=^1PZ(@D^D(ubXby$v; zOF2A0qID=?TCC$dc&|%O+Yigpa!K$u(J$~tqjgx05|lf_11Jl8(bfplVjbt>gi8k= za+I(fEtkaA_m3>jX|}F3M+vUCnU&*GFg0KEtcu1}9bJJ3qcFa6kzg1eUYEcgDu8wY$FfIDSDEfV2fxGxoiFfHA|e$SOqEz03Ns-;$`7E7{QU-LRFM+w@ohxWWy+^{uza%x$s z#hyP7A{%~?Ux(&IiFWYciUND}{A0h1?fJO2WCr34Y>8QCz3)(F`b&XrS3NaPvAvsFqp@f1jgODfFWn{Ya89E2@NFhZ0dO zqqz%AcJ7C%LuI@RvM(H6^F?{@H~*720H$96AJr#ROKHxm(Se7Y&$}SwOM0r&SLc~n z)f8K@^U)`wT6S78opRUU^t|oPLD}wY&MBcjF*jB^F3 z<2l4#U&Vb;Yp2$gh@Q^D-B4TW6HzT|JJW|9>{z@vxDO6S9mo3KoGhzq2<}d_GPl9X zQ0wQok2BW8)^g8Z1=%?zqFPE{KdN_e!I#}ZV9xWnZW#yQ405lv#a`nK+AiXpo9q*+ z6{W*nI~@J^1pR1>`C0ci5Ed0laQ{Rnx7}=hW=<=&g@o$YxgppK6&y8T)O-pIkVUc^pkWX$- z6~y`$(Ebi=H^1t?hP1CrfZQ-Bx9Tt7Ye-)Pc@Wau-P(|jL-q;PQu^y5?Gw9D(x_Gm z)jD%%`>JgRW~WCsBQ!^e4j_u!%}?}cMre)_Z*|E^?;Vtz7}ku?93^Uhx$^S7q3siw zG$S-eiR0*b{%MJws6B!9UWBr=g<3b}eABXgo*lHkv{x48P&=qm5uZ>krPV$<*B`5d zYw9MfEv^#Xsd=ylm!Mv*l5p25!5q_4nlgQq4uL0gG3vMzHgP8EATb6}r*EQMYFQMR z>C+Lbxh${@N<_7kwy216{T_TN#kdmV$c%E@{ajBOJ-d8DwW73@f~Gx)bSi*S0`YzqAytbVq&p=pWs-TmeQ2lO!_3h^MsMu zwTnBpiSb>6aZ9zY>d7z;M{nt1;4LYkT1u-ucJ8UsBlGYsk>UFUTV-b-wd~$T*tR>G)7K>l&<*7~drrx1{G~_gD4XD2ML` zSc#_y)lyphx!@fqhH)z4cyV+sYQPm^(LttL{J|%pT1w|Oy14caH7S~i1Bx7QQr5?EJ@;ZGlFdN;7$NiB=~vku$y zJ`vSYI{emvzS~2c3(D3brcc6d4V3MCr5wIyLvNFtLR>&f9t|Ui89E}eN>l|xo}rb<~O@?k^^w7KBKf# z)r(gwPUzivd+$ZTEqoKwojcJsp823{JUOkjx{&wqxA(41KaZU87cQ#0b#g=c+VirC z(jYqEe$I5CP%WjmqohH*)~3gR4!rO(eosVylwO}jmpMKb>tVXZdVM0QrL^5Fom*d# zlY9}sYrevm;oLreJMt^JSw;1O+b15xoq6`Eov|HAJM)%Ki4@YED?NF#de+I4CAVb9 zlNx8Z6l3=H6{FJ|(A&vNx>aq(h*0(k)lyoc3Ah#Qcyh~fjP3-~p-~PkSI5mKqFNRs z(faVc&nr2Z+E;Rt-#D0)(Q&WD9L$;8kba@VhM3Nib4jd_FR@Z)+=cK(iWb4 zq+8XXch{!h4pzy&2~*1Xr6ImJ3_LP>kKp{4dnKix7)ERH71Ak!Ii^LQ%4o`V3wQ3-)ive(%8B)`CH7v!7FtWRg+3A0QabeX zufUpGH+^6HH&|B}VYM?BwiRnsqeB`sf-Mn3wUnk;%h=Mh!D`1Brun!@BzdFYxZCS1 zdu#Dsj)=vqgD*@^5vmoXEgI=uXRN?%*s4|d>Hg3?;ai_MuvY^wJrTbXJ=eW99BY`- zx_?L8b#FSSglZ|R@v`6?%shGbl!}jz^eoh}#=`FI-a9(64*TN|-|bZO-@hK6ej`}B zI0sJ=s-?8n?bo4%YN?%f?o0IiQ_SL~&$24iGH6Tu9i@b7DXlFGzO9YntITSCoyobi z=m2Na?to0UyP;1+wUiF;ANcw+pI`g39gw2j{|@mp@6&>SVy`|t@J6CDLVwxs>gI<#D-+jpIQ9hws*_`=KYVVe_{ z6D7h|Zo2 z%$7t6(_*@P9p7>tszurUMxb@VwAgctS+`867G-;nZk;eKU7-Py$zD@eKDYbgLcPVQ zcdtIYPpFpCzJK*O&L{qc5C3e-`oZhV<}T@$Sc!8<&y4imm1PrVI0`3`a|3EJTeyBRx8GVw z@9sZ)FG@UyeylzxD}6QY%eiQ~e|J_wwUp)?>su&kIZAS_W6yIdmi#T2ys=9>qdnI< zXZ4*cFJB0c;Q5?4agS0$wUpK~xN()tnfJ`rF_)fw-k5%u6 zKA~Dlt0#*uYtWA@^dsxQwS_u^+FKn3pHMBO{W<@`Uo&~Lv2JIe9*wZ*ZA+sq&MBc< zQTiBe6uiSZHw5KxpBhV5X2&zSYjscf`&u zw7<{4LR8{7q7&N=Tch7n8jIL_kMHBmHr3c_Ei(h`R8#_t~ z)lyn_mvjFEpZyS`A0NPP*ON@=hH^L`%ig_f%j17V^y7kfMpuT$ta; zg|SjXwUpMoXv>6Z`M1qrPWsAWfUv$&sdF{@V{Cg zwK)1!PY@}}XY7i9NynS8y&URYmiP1UFD(#_Iz_0K(t5`Y{^~cz7##1<)UwDH%eC(S zC|e}VCsa#m%J#bG+yU&r9_aOE{PcX)=!YMZS#bWCEsJ2^r@&t6m?+&cp;}7oNIUl! z?C!KKug52&4)u^UGUaua# z`BkdtgC+b1(zA(b2h|??gxV`T+x_!g>rgGFeS4*Z<|yIY#FhxvYPla;hid64_&rxb pwe*~CnNTgY!Ok7Wn)(oXxz~G%`#+K7R3!ia diff --git a/test_competitor/package.xml b/test_competitor/package.xml deleted file mode 100644 index d8925e11d..000000000 --- a/test_competitor/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - test_competitor - 2024.1.0 - Example competitor package for the ARIAC simulation - justin - NIST - - ament_cmake - - rclcpp - std_msgs - ariac_msgs - shape_msgs - std_srvs - geometry_msgs - moveit_msgs - tf2_geometry_msgs - tf2_ros - orocos_kdl_vendor - - - ament_cmake - - diff --git a/test_competitor/rviz/test_competitor.rviz b/test_competitor/rviz/test_competitor.rviz deleted file mode 100644 index 099da382c..000000000 --- a/test_competitor/rviz/test_competitor.rviz +++ /dev/null @@ -1,654 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PlanningScene1 - - /PlanningScene1/Scene Geometry1 - - /PlanningScene1/Scene Robot1 - - /TF1 - - /TF1/Frames1 - Splitter Ratio: 0.5823529362678528 - Tree Height: 363 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: -8 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 16 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: /monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 206; 92; 0 - Scene Display Time: 9.999999747378752e-05 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 78; 154; 6 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - agv1_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv1_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv1_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv2_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv2_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv2_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv3_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv3_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv3_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv4_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv4_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv4_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_base: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_base_link_inertia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_ft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_base: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_base_link_inertia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_ft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - long_rail_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - long_rail_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_base: - Alpha: 1 - Show Axes: false - Show Trail: false - slide_bar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - small_rail: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_main: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - agv1_base: - Value: false - agv1_track: - Value: false - agv1_tray: - Value: true - agv2_base: - Value: false - agv2_track: - Value: false - agv2_tray: - Value: true - agv3_base: - Value: false - agv3_track: - Value: false - agv3_tray: - Value: true - agv4_base: - Value: false - agv4_track: - Value: false - agv4_tray: - Value: true - as1_insert_frame: - Value: true - as1_table_frame: - Value: false - as2_insert_frame: - Value: true - as2_table_frame: - Value: false - as3_insert_frame: - Value: true - as3_table_frame: - Value: false - as4_insert_frame: - Value: true - as4_table_frame: - Value: false - bin1_frame: - Value: true - bin2_frame: - Value: true - bin3_frame: - Value: true - bin4_frame: - Value: true - bin5_frame: - Value: true - bin6_frame: - Value: true - bin7_frame: - Value: true - bin8_frame: - Value: true - ceiling_base: - Value: false - ceiling_base_link: - Value: false - ceiling_base_link_inertia: - Value: false - ceiling_flange: - Value: false - ceiling_forearm_link: - Value: false - ceiling_ft_frame: - Value: false - ceiling_gripper: - Value: true - ceiling_shoulder_link: - Value: false - ceiling_tool0: - Value: false - ceiling_upper_arm_link: - Value: false - ceiling_wrist_1_link: - Value: false - ceiling_wrist_2_link: - Value: false - ceiling_wrist_3_link: - Value: false - conveyor_belt_base_frame: - Value: false - conveyor_belt_part_spawn_frame: - Value: false - floor_base: - Value: false - floor_base_link: - Value: false - floor_base_link_inertia: - Value: false - floor_flange: - Value: false - floor_forearm_link: - Value: false - floor_ft_frame: - Value: false - floor_gripper: - Value: true - floor_shoulder_link: - Value: false - floor_tool0: - Value: false - floor_upper_arm_link: - Value: false - floor_wrist_1_link: - Value: false - floor_wrist_2_link: - Value: false - floor_wrist_3_link: - Value: false - kts1_camera_frame: - Value: false - kts1_table_frame: - Value: false - kts1_tool_changer_parts_frame: - Value: true - kts1_tool_changer_trays_frame: - Value: true - kts2_camera_frame: - Value: false - kts2_table_frame: - Value: false - kts2_tool_changer_parts_frame: - Value: true - kts2_tool_changer_trays_frame: - Value: true - left_bins_camera_frame: - Value: false - long_rail_1: - Value: false - long_rail_2: - Value: false - right_bins_camera_frame: - Value: false - robot_base: - Value: false - slide_bar: - Value: false - small_rail: - Value: false - torso_base: - Value: false - torso_main: - Value: false - torso_tray: - Value: false - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: false - Show Axes: true - Show Names: false - Tree: - world: - agv1_track: - agv1_base: - agv1_tray: - {} - agv2_track: - agv2_base: - agv2_tray: - {} - agv3_track: - agv3_base: - agv3_tray: - {} - agv4_track: - agv4_base: - agv4_tray: - {} - as1_table_frame: - as1_insert_frame: - {} - as2_table_frame: - as2_insert_frame: - {} - as3_table_frame: - as3_insert_frame: - {} - as4_table_frame: - as4_insert_frame: - {} - bin1_frame: - {} - bin2_frame: - {} - bin3_frame: - {} - bin4_frame: - {} - bin5_frame: - {} - bin6_frame: - {} - bin7_frame: - {} - bin8_frame: - {} - conveyor_belt_base_frame: - conveyor_belt_part_spawn_frame: - {} - kts1_camera_frame: - {} - kts1_table_frame: - kts1_tool_changer_parts_frame: - {} - kts1_tool_changer_trays_frame: - {} - kts2_camera_frame: - {} - kts2_table_frame: - kts2_tool_changer_parts_frame: - {} - kts2_tool_changer_trays_frame: - {} - left_bins_camera_frame: - {} - long_rail_1: - long_rail_2: - small_rail: - torso_base: - torso_main: - ceiling_base_link: - ceiling_base: - {} - ceiling_base_link_inertia: - ceiling_shoulder_link: - ceiling_upper_arm_link: - ceiling_forearm_link: - ceiling_wrist_1_link: - ceiling_wrist_2_link: - ceiling_wrist_3_link: - ceiling_flange: - ceiling_tool0: - {} - ceiling_ft_frame: - {} - ceiling_gripper: - {} - torso_tray: - {} - right_bins_camera_frame: - {} - slide_bar: - robot_base: - floor_base_link: - floor_base: - {} - floor_base_link_inertia: - floor_shoulder_link: - floor_upper_arm_link: - floor_forearm_link: - floor_wrist_1_link: - floor_wrist_2_link: - floor_wrist_3_link: - floor_flange: - floor_tool0: - {} - floor_ft_frame: - {} - floor_gripper: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 8.921540260314941 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -2.3044843673706055 - Y: -3.5008909702301025 - Z: 0.5595264434814453 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5853981971740723 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.978596210479736 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 716 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000176000001a90000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039c0000022e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 924 - X: 970 - Y: 57 diff --git a/test_competitor/src/competitor.cpp b/test_competitor/src/competitor.cpp deleted file mode 100644 index 8aeadfe24..000000000 --- a/test_competitor/src/competitor.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include - -#include - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - - auto test_competitor = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(test_competitor); - std::thread([&executor]() { executor.spin(); }).detach(); - - // Start Competition - test_competitor->StartCompetition(); - - test_competitor->AddModelsToPlanningScene(); - - sleep(2); - - // Move Robots to Home Poses - test_competitor->FloorRobotSendHome(); - test_competitor->CeilingRobotSendHome(); - - // Complete Orders - test_competitor->CompleteOrders(); - - test_competitor->EndCompetition(); - - rclcpp::shutdown(); -} \ No newline at end of file diff --git a/test_competitor/src/moveit_test.cpp b/test_competitor/src/moveit_test.cpp deleted file mode 100644 index 42395a6da..000000000 --- a/test_competitor/src/moveit_test.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - auto test_competitor = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(test_competitor); - std::thread([&executor]() { executor.spin(); }).detach(); - - // Start Competition - test_competitor->StartCompetition(); - - // Send Robots to home position - test_competitor->FloorRobotSendHome(); - test_competitor->CeilingRobotSendHome(); - - // Shutdown ROS - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/test_competitor/src/test_competitor.cpp b/test_competitor/src/test_competitor.cpp deleted file mode 100644 index c14939824..000000000 --- a/test_competitor/src/test_competitor.cpp +++ /dev/null @@ -1,1984 +0,0 @@ -#include - -TestCompetitor::TestCompetitor() - : Node("test_competitor"), - floor_robot_(std::shared_ptr(std::move(this)), "floor_robot"), - ceiling_robot_(std::shared_ptr(std::move(this)), "ceiling_robot"), - planning_scene_() -{ - // Use upper joint velocity and acceleration limits - floor_robot_.setMaxAccelerationScalingFactor(1.0); - floor_robot_.setMaxVelocityScalingFactor(1.0); - floor_robot_.setPlanningTime(10.0); - floor_robot_.setNumPlanningAttempts(5); - floor_robot_.allowReplanning(true); - floor_robot_.setReplanAttempts(5); - - ceiling_robot_.setMaxAccelerationScalingFactor(1.0); - ceiling_robot_.setMaxVelocityScalingFactor(1.0); - ceiling_robot_.setPlanningTime(10.0); - ceiling_robot_.setNumPlanningAttempts(5); - ceiling_robot_.allowReplanning(true); - ceiling_robot_.setReplanAttempts(5); - // Subscribe to topics - rclcpp::SubscriptionOptions options; - - topic_cb_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - options.callback_group = topic_cb_group_; - - orders_sub_ = this->create_subscription("/ariac/orders", 1, - std::bind(&TestCompetitor::orders_cb, this, std::placeholders::_1), options); - - competition_state_sub_ = this->create_subscription("/ariac/competition_state", 1, - std::bind(&TestCompetitor::competition_state_cb, this, std::placeholders::_1), options); - - kts1_camera_sub_ = this->create_subscription( - "/ariac/sensors/kts1_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::kts1_camera_cb, this, std::placeholders::_1), options); - - kts2_camera_sub_ = this->create_subscription( - "/ariac/sensors/kts2_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::kts2_camera_cb, this, std::placeholders::_1), options); - - left_bins_camera_sub_ = this->create_subscription( - "/ariac/sensors/left_bins_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::left_bins_camera_cb, this, std::placeholders::_1), options); - - right_bins_camera_sub_ = this->create_subscription( - "/ariac/sensors/right_bins_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::right_bins_camera_cb, this, std::placeholders::_1), options); - - conveyor_camera_sub_ = this->create_subscription( - "/ariac/sensors/conveyor_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::conveyor_camera_cb, this, std::placeholders::_1), options); - - breakbeam_sub_ = this->create_subscription( - "/ariac/sensors/conveyor_breakbeam/change", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::breakbeam_cb, this, std::placeholders::_1), options); - - conveyor_parts_sub_ = this->create_subscription( - "/ariac/conveyor_parts", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::conveyor_parts_cb, this, std::placeholders::_1), options); - - floor_gripper_state_sub_ = this->create_subscription( - "/ariac/floor_robot_gripper_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::floor_gripper_state_cb, this, std::placeholders::_1), options); - - ceiling_gripper_state_sub_ = this->create_subscription( - "/ariac/ceiling_robot_gripper_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::ceiling_gripper_state_cb, this, std::placeholders::_1), options); - - as1_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_1_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as1_state_cb, this, std::placeholders::_1), options); - - as2_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_2_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as2_state_cb, this, std::placeholders::_1), options); - - as3_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_3_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as3_state_cb, this, std::placeholders::_1), options); - - as4_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_4_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as4_state_cb, this, std::placeholders::_1), options); - - agv1_status_sub_ = this->create_subscription( - "/ariac/agv1_status", 10, - std::bind(&TestCompetitor::agv1_status_cb, this, std::placeholders::_1), options); - - agv2_status_sub_ = this->create_subscription( - "/ariac/agv2_status", 10, - std::bind(&TestCompetitor::agv2_status_cb, this, std::placeholders::_1), options); - - agv3_status_sub_ = this->create_subscription( - "/ariac/agv3_status", 10, - std::bind(&TestCompetitor::agv3_status_cb, this, std::placeholders::_1), options); - - agv4_status_sub_ = this->create_subscription( - "/ariac/agv4_status", 10, - std::bind(&TestCompetitor::agv4_status_cb, this, std::placeholders::_1), options); - - // Initialize service clients - quality_checker_ = this->create_client("/ariac/perform_quality_check"); - pre_assembly_poses_getter_ = this->create_client("/ariac/get_pre_assembly_poses"); - floor_robot_tool_changer_ = this->create_client("/ariac/floor_robot_change_gripper"); - floor_robot_gripper_enable_ = this->create_client("/ariac/floor_robot_enable_gripper"); - ceiling_robot_gripper_enable_ = this->create_client("/ariac/ceiling_robot_enable_gripper"); - - RCLCPP_INFO(this->get_logger(), "Initialization successful."); -} - -TestCompetitor::~TestCompetitor() -{ - floor_robot_.~MoveGroupInterface(); - ceiling_robot_.~MoveGroupInterface(); -} - -void TestCompetitor::orders_cb( - const ariac_msgs::msg::Order::ConstSharedPtr msg) -{ - orders_.push_back(*msg); -} - -void TestCompetitor::competition_state_cb( - const ariac_msgs::msg::CompetitionState::ConstSharedPtr msg) -{ - competition_state_ = msg->competition_state; -} - -void TestCompetitor::kts1_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!kts1_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from kts1 camera"); - kts1_camera_recieved_data = true; - } - - kts1_trays_ = msg->tray_poses; - kts1_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::kts2_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!kts2_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from kts2 camera"); - kts2_camera_recieved_data = true; - } - - kts2_trays_ = msg->tray_poses; - kts2_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::left_bins_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!left_bins_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from left bins camera"); - left_bins_camera_recieved_data = true; - } - - left_bins_parts_ = msg->part_poses; - left_bins_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::right_bins_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!right_bins_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from right bins camera"); - right_bins_camera_recieved_data = true; - } - - right_bins_parts_ = msg->part_poses; - right_bins_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::conveyor_parts_cb( - const ariac_msgs::msg::ConveyorParts::ConstSharedPtr msg -) -{ - if (!conveyor_parts_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from conveyor parts"); - conveyor_parts_recieved_data = true; - } - - conveyor_parts_expected_ = msg->parts; -} - -void TestCompetitor::conveyor_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!conveyor_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from conveyor camera"); - conveyor_camera_recieved_data = true; - } - - conveyor_part_detected_ = msg->part_poses; - conveyor_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::breakbeam_cb( - const ariac_msgs::msg::BreakBeamStatus::ConstSharedPtr msg) -{ - if (!breakbeam_received_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from conveyor breakbeam"); - breakbeam_received_data = true; - breakbeam_pose_ = FrameWorldPose(msg->header.frame_id); - } - - breakbeam_status = msg->object_detected; - ariac_msgs::msg::PartPose part_to_add; - auto detection_time = now(); - float prev_distance = 0; - int count = 0; - - if (breakbeam_status) - { - // Lock conveyor_parts_ mutex - std::lock_guard lock(conveyor_parts_mutex); - - for (auto part : conveyor_part_detected_) - { - geometry_msgs::msg::Pose part_pose = MultiplyPose(conveyor_camera_pose_, part.pose); - float distance = abs(part_pose.position.y - breakbeam_pose_.position.y); - if (count == 0) - { - part_to_add = part; - detection_time = now(); - prev_distance = distance; - count++; - } - else - { - if (distance < prev_distance) - { - part_to_add = part; - detection_time = now(); - prev_distance = distance; - } - } - } - conveyor_parts_.emplace_back(part_to_add, detection_time); - } -} - -void TestCompetitor::floor_gripper_state_cb( - const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg) -{ - floor_gripper_state_ = *msg; -} - -void TestCompetitor::ceiling_gripper_state_cb( - const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg) -{ - ceiling_gripper_state_ = *msg; -} - -void TestCompetitor::as1_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS1, *msg); -} - -void TestCompetitor::as2_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS2, *msg); -} - -void TestCompetitor::as3_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS3, *msg); -} - -void TestCompetitor::as4_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS4, *msg); -} - -void TestCompetitor::agv1_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[1] = msg->location; -} - -void TestCompetitor::agv2_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[2] = msg->location; -} - -void TestCompetitor::agv3_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[3] = msg->location; -} - -void TestCompetitor::agv4_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[4] = msg->location; -} - -geometry_msgs::msg::Pose TestCompetitor::MultiplyPose( - geometry_msgs::msg::Pose p1, geometry_msgs::msg::Pose p2) -{ - KDL::Frame f1; - KDL::Frame f2; - - tf2::fromMsg(p1, f1); - tf2::fromMsg(p2, f2); - - KDL::Frame f3 = f1 * f2; - - return tf2::toMsg(f3); -} - -void TestCompetitor::LogPose(geometry_msgs::msg::Pose p) -{ - tf2::Quaternion q( - p.orientation.x, - p.orientation.y, - p.orientation.z, - p.orientation.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - roll *= 180 / M_PI; - pitch *= 180 / M_PI; - yaw *= 180 / M_PI; - - RCLCPP_INFO(get_logger(), "(X: %.2f, Y: %.2f, Z: %.2f, R: %.2f, P: %.2f, Y: %.2f)", - p.position.x, p.position.y, p.position.z, - roll, pitch, yaw); -} - -geometry_msgs::msg::Pose TestCompetitor::BuildPose( - double x, double y, double z, geometry_msgs::msg::Quaternion orientation) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; - pose.orientation = orientation; - - return pose; -} - -geometry_msgs::msg::Pose TestCompetitor::FrameWorldPose(std::string frame_id) -{ - geometry_msgs::msg::TransformStamped t; - geometry_msgs::msg::Pose pose; - - try - { - t = tf_buffer->lookupTransform("world", frame_id, tf2::TimePointZero); - } - catch (const tf2::TransformException &ex) - { - RCLCPP_ERROR(get_logger(), "Could not get transform"); - } - - pose.position.x = t.transform.translation.x; - pose.position.y = t.transform.translation.y; - pose.position.z = t.transform.translation.z; - pose.orientation = t.transform.rotation; - - return pose; -} - -double TestCompetitor::GetYaw(geometry_msgs::msg::Pose pose) -{ - tf2::Quaternion q( - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - return yaw; -} - -geometry_msgs::msg::Quaternion TestCompetitor::QuaternionFromRPY(double r, double p, double y) -{ - tf2::Quaternion q; - geometry_msgs::msg::Quaternion q_msg; - - q.setRPY(r, p, y); - - q_msg.x = q.x(); - q_msg.y = q.y(); - q_msg.z = q.z(); - q_msg.w = q.w(); - - return q_msg; -} - -moveit_msgs::msg::CollisionObject TestCompetitor::CreateCollisionObject( - std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose) -{ - moveit_msgs::msg::CollisionObject collision; - - collision.id = name; - collision.header.frame_id = "world"; - - shape_msgs::msg::Mesh mesh; - shapes::ShapeMsg mesh_msg; - - std::string package_share_directory = ament_index_cpp::get_package_share_directory("test_competitor"); - std::stringstream path; - path << "file://" << package_share_directory << "/meshes/" << mesh_file; - std::string model_path = path.str(); - - shapes::Mesh *m = shapes::createMeshFromResource(model_path); - shapes::constructMsgFromShape(m, mesh_msg); - - mesh = boost::get(mesh_msg); - - collision.meshes.push_back(mesh); - collision.mesh_poses.push_back(model_pose); - - collision.operation = collision.ADD; - - return collision; -} - -void TestCompetitor::AddModelToPlanningScene( - std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose) -{ - planning_scene_.applyCollisionObject(CreateCollisionObject(name, mesh_file, model_pose)); -} - -void TestCompetitor::AddModelsToPlanningScene() -{ - std::vector objects; - - // Add bins - std::map> bin_positions = { - {"bin1", std::pair(-1.9, 3.375)}, - {"bin2", std::pair(-1.9, 2.625)}, - {"bin3", std::pair(-2.65, 2.625)}, - {"bin4", std::pair(-2.65, 3.375)}, - {"bin5", std::pair(-1.9, -3.375)}, - {"bin6", std::pair(-1.9, -2.625)}, - {"bin7", std::pair(-2.65, -2.625)}, - {"bin8", std::pair(-2.65, -3.375)}}; - - geometry_msgs::msg::Pose bin_pose; - for (auto const &bin : bin_positions) - { - bin_pose.position.x = bin.second.first; - bin_pose.position.y = bin.second.second; - bin_pose.position.z = 0; - bin_pose.orientation = QuaternionFromRPY(0, 0, 3.14159); - - objects.push_back(CreateCollisionObject(bin.first, "bin.stl", bin_pose)); - } - - // Add assembly stations - std::map> assembly_station_positions = { - {"as1", std::pair(-7.3, 3)}, - {"as2", std::pair(-12.3, 3)}, - {"as3", std::pair(-7.3, -3)}, - {"as4", std::pair(-12.3, -3)}, - }; - - geometry_msgs::msg::Pose assembly_station_pose; - for (auto const &station : assembly_station_positions) - { - assembly_station_pose.position.x = station.second.first; - assembly_station_pose.position.y = station.second.second; - assembly_station_pose.position.z = 0; - assembly_station_pose.orientation = QuaternionFromRPY(0, 0, 0); - - objects.push_back(CreateCollisionObject(station.first, "assembly_station.stl", assembly_station_pose)); - } - - // Add assembly briefcases - std::map> assembly_insert_positions = { - {"as1_insert", std::pair(-7.7, 3)}, - {"as2_insert", std::pair(-12.7, 3)}, - {"as3_insert", std::pair(-7.7, -3)}, - {"as4_insert", std::pair(-12.7, -3)}, - }; - - geometry_msgs::msg::Pose assembly_insert_pose; - - for (auto const &insert : assembly_insert_positions) - { - // assembly_insert_pose.position.x = insert.second.first; - // assembly_insert_pose.position.y = insert.second.second; - // assembly_insert_pose.position.z = 1.011; - // assembly_insert_pose.orientation = QuaternionFromRPY(0, 0, 0); - - std::string frame_name = insert.first + "_frame"; - - objects.push_back(CreateCollisionObject(insert.first, "assembly_insert.stl", FrameWorldPose(frame_name))); - } - - geometry_msgs::msg::Pose conveyor_pose; - conveyor_pose.position.x = -0.6; - conveyor_pose.position.y = 0; - conveyor_pose.position.z = 0; - conveyor_pose.orientation = QuaternionFromRPY(0, 0, 0); - - objects.push_back(CreateCollisionObject("conveyor", "conveyor.stl", conveyor_pose)); - - geometry_msgs::msg::Pose kts1_table_pose; - kts1_table_pose.position.x = -1.3; - kts1_table_pose.position.y = -5.84; - kts1_table_pose.position.z = 0; - kts1_table_pose.orientation = QuaternionFromRPY(0, 0, 3.14159); - - objects.push_back(CreateCollisionObject("kts1_table", "kit_tray_table.stl", kts1_table_pose)); - - geometry_msgs::msg::Pose kts2_table_pose; - kts2_table_pose.position.x = -1.3; - kts2_table_pose.position.y = 5.84; - kts2_table_pose.position.z = 0; - kts2_table_pose.orientation = QuaternionFromRPY(0, 0, 0); - - objects.push_back(CreateCollisionObject("kts2_table", "kit_tray_table.stl", kts2_table_pose)); - - if (!planning_scene_.applyCollisionObjects(objects)) { - RCLCPP_WARN(get_logger(), "Unable to add objects to planning scene"); - } -} - -geometry_msgs::msg::Quaternion TestCompetitor::SetRobotOrientation(double rotation) -{ - tf2::Quaternion tf_q; - tf_q.setRPY(0, 3.14159, rotation); - - geometry_msgs::msg::Quaternion q; - - q.x = tf_q.x(); - q.y = tf_q.y(); - q.z = tf_q.z(); - q.w = tf_q.w(); - - return q; -} - -bool TestCompetitor::FloorRobotMovetoTarget() -{ - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = static_cast(floor_robot_.plan(plan)); - - if (success) - { - return static_cast(floor_robot_.execute(plan)); - } - else - { - RCLCPP_ERROR(get_logger(), "Unable to generate plan"); - return false; - } -} - -bool TestCompetitor::FloorRobotMoveCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = floor_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return false; - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(floor_robot_.getCurrentState()->getRobotModel(), "floor_robot"); - rt.setRobotTrajectoryMsg(*floor_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return static_cast(floor_robot_.execute(trajectory)); -} - -std::pair TestCompetitor::FloorRobotPlanCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = floor_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return std::make_pair(false, trajectory); - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(floor_robot_.getCurrentState()->getRobotModel(), "floor_robot"); - rt.setRobotTrajectoryMsg(*floor_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return std::make_pair(true, trajectory); -} - -void TestCompetitor::FloorRobotWaitForAttach(double timeout) -{ - // Wait for part to be attached - rclcpp::Time start = now(); - std::vector waypoints; - geometry_msgs::msg::Pose starting_pose = floor_robot_.getCurrentPose().pose; - - while (!floor_gripper_state_.attached) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for gripper attach"); - - waypoints.clear(); - starting_pose.position.z -= 0.001; - waypoints.push_back(starting_pose); - - FloorRobotMoveCartesian(waypoints, 0.01, 0.01, true); - - usleep(500); - - if (now() - start > rclcpp::Duration::from_seconds(timeout)) - { - RCLCPP_ERROR(get_logger(), "Unable to pick up object"); - return; - } - } -} - -void TestCompetitor::FloorRobotSendHome() -{ - // Move floor robot to home joint state - RCLCPP_INFO_STREAM(get_logger(), "Moving Floor Robot to home position"); - floor_robot_.setNamedTarget("home"); - FloorRobotMovetoTarget(); -} - -bool TestCompetitor::FloorRobotSetGripperState(bool enable) -{ - if (floor_gripper_state_.enabled == enable) - { - if (floor_gripper_state_.enabled) - RCLCPP_INFO(get_logger(), "Already enabled"); - else - RCLCPP_INFO(get_logger(), "Already disabled"); - - return false; - } - - // Call enable service - auto request = std::make_shared(); - request->enable = enable; - - auto future = floor_robot_gripper_enable_->async_send_request(request); - future.wait(); - - if (!future.get()->success) - { - RCLCPP_ERROR(get_logger(), "Error calling gripper enable service"); - return false; - } - - return true; -} - -bool TestCompetitor::FloorRobotChangeGripper(std::string station, std::string gripper_type) -{ - // Move gripper into tool changer - RCLCPP_INFO_STREAM(get_logger(), "Changing gripper to " << gripper_type << " gripper"); - - auto tc_pose = FrameWorldPose(station + "_tool_changer_" + gripper_type + "_frame"); - - std::vector waypoints; - waypoints.push_back(BuildPose(tc_pose.position.x, tc_pose.position.y, - tc_pose.position.z + 0.4, SetRobotOrientation(0.0))); - - waypoints.push_back(BuildPose(tc_pose.position.x, tc_pose.position.y, - tc_pose.position.z, SetRobotOrientation(0.0))); - - if (!FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true)) - return false; - - // Call service to change gripper - auto request = std::make_shared(); - - if (gripper_type == "trays") - { - request->gripper_type = ariac_msgs::srv::ChangeGripper::Request::TRAY_GRIPPER; - } - else if (gripper_type == "parts") - { - request->gripper_type = ariac_msgs::srv::ChangeGripper::Request::PART_GRIPPER; - } - - auto future = floor_robot_tool_changer_->async_send_request(request); - future.wait(); - if (!future.get()->success) - { - RCLCPP_ERROR(get_logger(), "Error calling gripper change service"); - return false; - } - - waypoints.clear(); - waypoints.push_back(BuildPose(tc_pose.position.x, tc_pose.position.y, - tc_pose.position.z + 0.4, SetRobotOrientation(0.0))); - - if (!FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true)) - return false; - - return true; -} - -bool TestCompetitor::FloorRobotPickandPlaceTray(int tray_id, int agv_num) -{ - RCLCPP_INFO_STREAM(get_logger(), "Attempting to pick up kit tray " << tray_id << " and place on AGV " << agv_num); - - // Check if kit tray is on one of the two tables - geometry_msgs::msg::Pose tray_pose; - std::string station; - bool found_tray = false; - - // Check table 1 - for (auto tray : kts1_trays_) - { - if (tray.id == tray_id) - { - station = "kts1"; - tray_pose = MultiplyPose(kts1_camera_pose_, tray.pose); - found_tray = true; - break; - } - } - // Check table 2 - if (!found_tray) - { - for (auto tray : kts2_trays_) - { - if (tray.id == tray_id) - { - station = "kts2"; - tray_pose = MultiplyPose(kts2_camera_pose_, tray.pose); - found_tray = true; - break; - } - } - } - if (!found_tray) - return false; - - double tray_rotation = GetYaw(tray_pose); - - RCLCPP_INFO_STREAM(get_logger(), "Found kit tray " << tray_id << " on ktting tray station " << station << " moving to pick location"); - - // Move floor robot to the corresponding kit tray table - if (station == "kts1") - { - floor_robot_.setJointValueTarget(floor_kts1_js_); - } - else - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - } - FloorRobotMovetoTarget(); - - // Change gripper to tray gripper - if (floor_gripper_state_.type != "tray_gripper") - { - FloorRobotChangeGripper(station, "trays"); - } - - // Move to tray - std::vector waypoints; - - waypoints.push_back(BuildPose(tray_pose.position.x, tray_pose.position.y, - tray_pose.position.z + 0.2, SetRobotOrientation(tray_rotation))); - waypoints.push_back(BuildPose(tray_pose.position.x, tray_pose.position.y, - tray_pose.position.z + pick_offset_, SetRobotOrientation(tray_rotation))); - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - FloorRobotSetGripperState(true); - - FloorRobotWaitForAttach(3.0); - - RCLCPP_INFO_STREAM(get_logger(), "Picked kit tray " << tray_id); - - // Add kit tray to planning scene - std::string tray_name = "kit_tray_" + std::to_string(tray_id); - AddModelToPlanningScene(tray_name, "kit_tray.stl", tray_pose); - floor_robot_.attachObject(tray_name); - - order_planning_scene_objects_.push_back(tray_name); - - // Move up slightly - waypoints.clear(); - - waypoints.push_back(BuildPose(tray_pose.position.x, tray_pose.position.y, - tray_pose.position.z + 0.2, SetRobotOrientation(tray_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.05, 0.05, true); - - if (station == "kts1") - { - floor_robot_.setJointValueTarget(floor_kts1_js_); - } - else - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - } - FloorRobotMovetoTarget(); - - RCLCPP_INFO_STREAM(get_logger(), "Moving tray to AGV " << agv_num); - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_["agv" + std::to_string(agv_num)]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - - FloorRobotMovetoTarget(); - - auto agv_tray_pose = FrameWorldPose("agv" + std::to_string(agv_num) + "_tray"); - auto agv_rotation = GetYaw(agv_tray_pose); - - waypoints.clear(); - waypoints.push_back(BuildPose(agv_tray_pose.position.x, agv_tray_pose.position.y, - agv_tray_pose.position.z + 0.3, SetRobotOrientation(agv_rotation))); - - waypoints.push_back(BuildPose(agv_tray_pose.position.x, agv_tray_pose.position.y, - agv_tray_pose.position.z + kit_tray_thickness_ + drop_height_, SetRobotOrientation(agv_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true); - - FloorRobotSetGripperState(false); - - floor_robot_.detachObject(tray_name); - - LockAGVTray(agv_num); - - waypoints.clear(); - waypoints.push_back(BuildPose(agv_tray_pose.position.x, agv_tray_pose.position.y, - agv_tray_pose.position.z + 0.3, SetRobotOrientation(agv_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, false); - - return true; -} - -bool TestCompetitor::FloorRobotPickBinPart(ariac_msgs::msg::Part part_to_pick) -{ - RCLCPP_INFO_STREAM(get_logger(), "Attempting to pick a " << part_colors_[part_to_pick.color] << " " << part_types_[part_to_pick.type] << " from the bins"); - - // Check if part is in one of the bins - geometry_msgs::msg::Pose part_pose; - bool found_part = false; - std::string bin_side; - - // Check left bins - for (auto part : left_bins_parts_) - { - if (part.part.type == part_to_pick.type && part.part.color == part_to_pick.color) - { - part_pose = MultiplyPose(left_bins_camera_pose_, part.pose); - found_part = true; - bin_side = "left_bins"; - break; - } - } - // Check right bins - if (!found_part) - { - for (auto part : right_bins_parts_) - { - if (part.part.type == part_to_pick.type && part.part.color == part_to_pick.color) - { - part_pose = MultiplyPose(right_bins_camera_pose_, part.pose); - found_part = true; - bin_side = "right_bins"; - break; - } - } - } - if (!found_part) - { - RCLCPP_INFO(get_logger(), "Unable to locate part in the bins"); - return false; - } - - RCLCPP_INFO_STREAM(get_logger(), "Found part in " << bin_side << " checking gripper type and moving to pick location"); - - double part_rotation = GetYaw(part_pose); - - // Change gripper at location closest to part - if (floor_gripper_state_.type != "part_gripper") - { - std::string station; - if (part_pose.position.y < 0) - { - station = "kts1"; - } - else - { - station = "kts2"; - } - - // Move floor robot to the corresponding kit tray table - if (station == "kts1") - { - floor_robot_.setJointValueTarget(floor_kts1_js_); - } - else - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - } - FloorRobotMovetoTarget(); - - FloorRobotChangeGripper(station, "parts"); - } - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_[bin_side]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - std::vector waypoints; - waypoints.push_back(BuildPose(part_pose.position.x, part_pose.position.y, - part_pose.position.z + 0.5, SetRobotOrientation(part_rotation))); - - waypoints.push_back(BuildPose(part_pose.position.x, part_pose.position.y, - part_pose.position.z + part_heights_[part_to_pick.type] + pick_offset_, SetRobotOrientation(part_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - FloorRobotSetGripperState(true); - - FloorRobotWaitForAttach(3.0); - - RCLCPP_INFO_STREAM(get_logger(), "Picked Up the " << part_colors_[part_to_pick.color] << " " << part_types_[part_to_pick.type] << " from the bins"); - - // Add part to planning scene - std::string part_name = part_colors_[part_to_pick.color] + "_" + part_types_[part_to_pick.type]; - AddModelToPlanningScene(part_name, part_types_[part_to_pick.type] + ".stl", part_pose); - floor_robot_.attachObject(part_name); - floor_robot_attached_part_ = part_to_pick; - - order_planning_scene_objects_.push_back(part_name); - - // Move up slightly - waypoints.clear(); - waypoints.push_back(BuildPose(part_pose.position.x, part_pose.position.y, - part_pose.position.z + 0.2, SetRobotOrientation(part_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true); - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_[bin_side]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - return true; -} - -bool TestCompetitor::FloorRobotPickConveyorPart(ariac_msgs::msg::Part part_to_pick) -{ - if (conveyor_parts_expected_.empty()) - { - RCLCPP_INFO(get_logger(), "No parts expected on the conveyor"); - return false; - } - for (auto parts : conveyor_parts_expected_){ - if (parts.part.type == part_to_pick.type && parts.part.color == part_to_pick.color){ - RCLCPP_INFO_STREAM(get_logger(), "Attempting to pick a " << part_colors_[part_to_pick.color] << " " << part_types_[part_to_pick.type] << " from the conveyor"); - break; - } - else if (parts == conveyor_parts_expected_.back()){ - RCLCPP_INFO(get_logger(), "Unable to locate part on the conveyor"); - return false; - } - } - - bool found_part = false; - bool part_picked = false; - int num_tries = 0; - geometry_msgs::msg::Pose part_pose; - rclcpp::Duration elapsed_time(0, 0); - builtin_interfaces::msg::Duration time_to_pick; - rclcpp::Time detection_time; - - // Change gripper at Kitting Tray Station 2 - if (floor_gripper_state_.type != "part_gripper") - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - FloorRobotMovetoTarget(); - FloorRobotChangeGripper("kts2", "parts"); - } - RCLCPP_INFO_STREAM(get_logger(), "Moving Floor Robot to Conveyor pick location"); - while(!part_picked && num_tries < 3){ - // Move robot to predefined pick location - floor_robot_.setJointValueTarget(floor_conveyor_js_); - FloorRobotMovetoTarget(); - - // Find the requested part on the conveyor - do { - { - // Lock conveyor_parts_ mutex - std::lock_guard lock(conveyor_parts_mutex); - auto it = conveyor_parts_.begin(); - for (; it != conveyor_parts_.end(); ) { - auto part = it->first.part; - if (part.type == part_to_pick.type && part.color == part_to_pick.color) - { - part_pose = MultiplyPose(conveyor_camera_pose_, it->first.pose); - detection_time = it->second; - - elapsed_time = rclcpp::Time(now()) - detection_time; - auto current_part_position_ = part_pose.position.y - (elapsed_time.seconds() * conveyor_speed_); - // Check if part hasn't passed the pick location - if (current_part_position_ > 0) - { - time_to_pick.sec = current_part_position_ / conveyor_speed_; - // Check if part has more than 5 seconds to arrive at pick location - if (time_to_pick.sec > 5.0) - { - found_part = true; - conveyor_parts_.erase(it); - break; - } - } - it = conveyor_parts_.erase(it); - } - else{ - ++it; - } - } - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 20000, "Waiting for %s %s to arrive on the conveyor", part_colors_[part_to_pick.color].c_str(), part_types_[part_to_pick.type].c_str()); - } // End lock_guard scope - } while (!found_part); - - // Correct robot position to account for part offset on conveyor - double part_rotation = GetYaw(part_pose); - geometry_msgs::msg::Pose robot_pose = floor_robot_.getCurrentPose().pose; - - std::vector waypoints; - waypoints.push_back(BuildPose(part_pose.position.x, robot_pose.position.y, - part_pose.position.z + 0.15, SetRobotOrientation(part_rotation))); - FloorRobotMoveCartesian(waypoints, 0.5, 0.5, true); - - auto elapsed_time_ = rclcpp::Time(now()) - detection_time; - auto current_part_position_ = part_pose.position.y - (elapsed_time.seconds() * conveyor_speed_); - // Check if part hasn't passed the pick location - if (current_part_position_ < 0) - { - RCLCPP_INFO(get_logger(), "Part has passed the pick location"); - found_part = false; - num_tries++; - continue; - } - - // Plan trajectory to pickup part - waypoints.clear(); - waypoints.push_back(BuildPose(part_pose.position.x, robot_pose.position.y, - part_pose.position.z + part_heights_[part_to_pick.type],SetRobotOrientation(part_rotation))); - - auto trajectory = FloorRobotPlanCartesian(waypoints, 0.5, 0.5, true); - if (!trajectory.first) - { - found_part = false; - num_tries++; - continue; - } - - // Wait for part to arrive at pick location - auto trajectory_time = trajectory.second.joint_trajectory.points.back().time_from_start; - while (rclcpp::Time(now()).nanoseconds() < (detection_time + elapsed_time + time_to_pick - trajectory_time).nanoseconds() - 2.75e8) - { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 20000, "Waiting for part to arrive at pick location"); - } - - // Execute trajectory to pickup part - FloorRobotSetGripperState(true); - floor_robot_.execute(trajectory.second); - - // Move up slightly - waypoints.clear(); - waypoints.push_back(BuildPose(part_pose.position.x, robot_pose.position.y, - part_pose.position.z + 0.1, SetRobotOrientation(0))); - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - if(floor_gripper_state_.attached) - { - // Add part to planning scene - std::string part_name = part_colors_[part_to_pick.color] + "_" + part_types_[part_to_pick.type]; - AddModelToPlanningScene(part_name, part_types_[part_to_pick.type] + ".stl", robot_pose); - RCLCPP_INFO_STREAM(get_logger(), "Attached " << part_name << " to robot"); - floor_robot_.attachObject(part_name); - order_planning_scene_objects_.push_back(part_name); - floor_robot_attached_part_ = part_to_pick; - part_picked = true; - } - - // Move up - waypoints.clear(); - waypoints.push_back(BuildPose(robot_pose.position.x, robot_pose.position.y, - robot_pose.position.z + 0.3, SetRobotOrientation(0))); - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - num_tries++; - } - - return true; -} - -bool TestCompetitor::FloorRobotPlacePartOnKitTray(int agv_num, int quadrant) -{ - if (!floor_gripper_state_.attached) - { - RCLCPP_ERROR(get_logger(), "No part attached"); - return false; - } - - RCLCPP_INFO_STREAM(get_logger(), "Placing the " << part_colors_[floor_robot_attached_part_.color] << " " << part_types_[floor_robot_attached_part_.type] << " on AGV " << agv_num << " in quadrant " << quadrant); - - // Move to agv - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_["agv" + std::to_string(agv_num)]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - // Determine target pose for part based on agv_tray pose - auto agv_tray_pose = FrameWorldPose("agv" + std::to_string(agv_num) + "_tray"); - - auto part_drop_offset = BuildPose(quad_offsets_[quadrant].first, quad_offsets_[quadrant].second, 0.0, - geometry_msgs::msg::Quaternion()); - - auto part_drop_pose = MultiplyPose(agv_tray_pose, part_drop_offset); - - std::vector waypoints; - - waypoints.push_back(BuildPose(part_drop_pose.position.x, part_drop_pose.position.y, - part_drop_pose.position.z + 0.3, SetRobotOrientation(0))); - - waypoints.push_back(BuildPose(part_drop_pose.position.x, part_drop_pose.position.y, - part_drop_pose.position.z + part_heights_[floor_robot_attached_part_.type] + kit_tray_thickness_ + drop_height_, - SetRobotOrientation(0))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.2, true); - - // Drop part in quadrant - FloorRobotSetGripperState(false); - - std::string part_name = part_colors_[floor_robot_attached_part_.color] + - "_" + part_types_[floor_robot_attached_part_.type]; - floor_robot_.detachObject(part_name); - - waypoints.clear(); - waypoints.push_back(BuildPose(part_drop_pose.position.x, part_drop_pose.position.y, - part_drop_pose.position.z + 0.2, - SetRobotOrientation(0))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, false); - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_["agv" + std::to_string(agv_num)]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - return true; -} - -void TestCompetitor::CeilingRobotSendHome() -{ - // Move ceiling robot to home joint state - RCLCPP_INFO_STREAM(get_logger(), "Moving ceiling robot to home position"); - ceiling_robot_.setNamedTarget("home"); - CeilingRobotMovetoTarget(); -} - -bool TestCompetitor::CeilingRobotSetGripperState(bool enable) -{ - if (ceiling_gripper_state_.enabled == enable) - { - if (ceiling_gripper_state_.enabled) - RCLCPP_INFO(get_logger(), "Already enabled"); - else - RCLCPP_INFO(get_logger(), "Already disabled"); - - return false; - } - - // Call enable service - auto request = std::make_shared(); - request->enable = enable; - - auto future = ceiling_robot_gripper_enable_->async_send_request(request); - future.wait(); - - if (!future.get()->success) - { - RCLCPP_ERROR(get_logger(), "Error calling gripper enable service"); - return false; - } - - return true; -} - -void TestCompetitor::CeilingRobotWaitForAttach(double timeout) -{ - // Wait for part to be attached - rclcpp::Time start = now(); - std::vector waypoints; - geometry_msgs::msg::Pose starting_pose = ceiling_robot_.getCurrentPose().pose; - - while (!ceiling_gripper_state_.attached) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for gripper attach"); - - waypoints.clear(); - starting_pose.position.z -= 0.001; - waypoints.push_back(starting_pose); - - CeilingRobotMoveCartesian(waypoints, 0.01, 0.01, false); - - usleep(500); - - if (now() - start > rclcpp::Duration::from_seconds(timeout)) - { - RCLCPP_ERROR(get_logger(), "Unable to pick up object"); - return; - } - } -} - -bool TestCompetitor::CeilingRobotWaitForAssemble(int station, ariac_msgs::msg::AssemblyPart part) -{ - // Wait for part to be attached - rclcpp::Time start = now(); - - auto rotation = GetYaw(FrameWorldPose("as"+std::to_string(station)+"_insert_frame")); - - bool assembled = false; - - double R[3][3] = {cos(rotation), -sin(rotation), 0, - sin(rotation), cos(rotation), 0, - 0, 0, 1}; - - double dx = R[0][0] * part.install_direction.x + R[0][1] * part.install_direction.y + R[0][2] * part.install_direction.z; - double dy = R[1][0] * part.install_direction.x + R[1][1] * part.install_direction.y + R[1][2] * part.install_direction.z; - double dz = R[2][0] * part.install_direction.x + R[2][1] * part.install_direction.y + R[2][2] * part.install_direction.z; - - geometry_msgs::msg::Pose pose = ceiling_robot_.getCurrentPose().pose; - - pose.position.x += dx * (assembly_offset_ + 0.005); - pose.position.y += dy * (assembly_offset_ + 0.005); - pose.position.z += dz * (assembly_offset_ + 0.005); - - std::vector waypoints; - waypoints.push_back(pose); - - auto ret = CeilingRobotPlanCartesian(waypoints, 0.01, 0.01, false); - - if (ret.first) { - ceiling_robot_.asyncExecute(ret.second); - } else { - return false; - } - - while (!assembled) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for part to be assembled"); - - // Check if part is assembled - switch (part.part.type) - { - case ariac_msgs::msg::Part::BATTERY: - assembled = assembly_station_states_[station].battery_attached; - break; - case ariac_msgs::msg::Part::PUMP: - assembled = assembly_station_states_[station].pump_attached; - break; - case ariac_msgs::msg::Part::SENSOR: - assembled = assembly_station_states_[station].sensor_attached; - break; - case ariac_msgs::msg::Part::REGULATOR: - assembled = assembly_station_states_[station].regulator_attached; - break; - default: - RCLCPP_WARN(get_logger(), "Not a valid part type"); - return false; - } - } - - ceiling_robot_.stop(); - - RCLCPP_INFO(get_logger(), "Part is assembled"); - - return true; -} - -bool TestCompetitor::CeilingRobotMovetoTarget() -{ - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = static_cast(ceiling_robot_.plan(plan)); - - if (success) - { - return static_cast(ceiling_robot_.execute(plan)); - } - else - { - RCLCPP_ERROR(get_logger(), "Unable to generate plan"); - return false; - } -} - -bool TestCompetitor::CeilingRobotMoveCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = ceiling_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return false; - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(ceiling_robot_.getCurrentState()->getRobotModel(), "ceiling_robot"); - rt.setRobotTrajectoryMsg(*ceiling_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return static_cast(ceiling_robot_.execute(trajectory)); -} - -std::pair TestCompetitor::CeilingRobotPlanCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = ceiling_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return std::make_pair(false, trajectory); - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(ceiling_robot_.getCurrentState()->getRobotModel(), "ceiling_robot"); - rt.setRobotTrajectoryMsg(*ceiling_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return std::make_pair(true, trajectory); -} - -bool TestCompetitor::CeilingRobotMoveToAssemblyStation(int station) -{ - RCLCPP_INFO_STREAM(get_logger(), "Moving ceiling robot to assembly station " << station); - switch (station) - { - case 1: - ceiling_robot_.setJointValueTarget(ceiling_as1_js_); - break; - case 2: - ceiling_robot_.setJointValueTarget(ceiling_as2_js_); - break; - case 3: - ceiling_robot_.setJointValueTarget(ceiling_as3_js_); - break; - case 4: - ceiling_robot_.setJointValueTarget(ceiling_as4_js_); - break; - default: - RCLCPP_WARN(get_logger(), "Not a valid assembly station"); - return false; - } - - return CeilingRobotMovetoTarget(); -} - -bool TestCompetitor::CeilingRobotPickAGVPart(ariac_msgs::msg::PartPose part) -{ - RCLCPP_INFO_STREAM(get_logger(), "Determining waypoints to pick " << part_types_[part.part.type]); - double part_rotation = GetYaw(part.pose); - std::vector waypoints; - - double dx = 0; - double dy = 0; - - if (part.part.type == ariac_msgs::msg::Part::BATTERY) - { - dx = battery_grip_offset_ * cos(part_rotation); - dy = battery_grip_offset_ * sin(part_rotation); - } - - waypoints.push_back(BuildPose(part.pose.position.x + dx, part.pose.position.y + dy, - part.pose.position.z + 0.4, SetRobotOrientation(part_rotation))); - - waypoints.push_back(BuildPose(part.pose.position.x + dx, part.pose.position.y + dy, - part.pose.position.z + part_heights_[part.part.type] + pick_offset_, SetRobotOrientation(part_rotation))); - - RCLCPP_INFO_STREAM(get_logger(), "Moving ceiling robot to pick " << part_types_[part.part.type]); - CeilingRobotMoveCartesian(waypoints, 0.7, 0.7, false); - - CeilingRobotSetGripperState(true); - - CeilingRobotWaitForAttach(5.0); - - // Add part to planning scene - std::string part_name = part_colors_[part.part.color] + "_" + part_types_[part.part.type]; - AddModelToPlanningScene(part_name, part_types_[part.part.type] + ".stl", part.pose); - ceiling_robot_.attachObject(part_name); - ceiling_robot_attached_part_ = part.part; - - // Move up slightly - waypoints.clear(); - waypoints.push_back(BuildPose(part.pose.position.x, part.pose.position.y, - part.pose.position.z + 0.3, - SetRobotOrientation(0))); - - CeilingRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - return true; -} - -bool TestCompetitor::CeilingRobotAssemblePart(int station, ariac_msgs::msg::AssemblyPart part) -{ - // Check that part is attached and matches part to assemble - if (!ceiling_gripper_state_.attached) - { - RCLCPP_WARN(get_logger(), "No part attached"); - return false; - } - - if (part.part != ceiling_robot_attached_part_) - { - RCLCPP_WARN(get_logger(), "Incorrect part attached for this assembly"); - return false; - } - - RCLCPP_INFO_STREAM(get_logger(),"Attempting to assemble a " << part_colors_[part.part.color] << " " << part_types_[part.part.type]); - // Calculate assembled pose in world frame - std::string insert_frame_name; - switch (station) - { - case 1: - insert_frame_name = "as1_insert_frame"; - break; - case 2: - insert_frame_name = "as2_insert_frame"; - break; - case 3: - insert_frame_name = "as3_insert_frame"; - break; - case 4: - insert_frame_name = "as4_insert_frame"; - break; - default: - RCLCPP_WARN(get_logger(), "Not a valid assembly station"); - return false; - } - - // Calculate robot positions at assembly and approach - KDL::Vector install(part.install_direction.x, part.install_direction.y, part.install_direction.z); - - KDL::Frame insert; - tf2::fromMsg(FrameWorldPose(insert_frame_name), insert); - - KDL::Frame part_assemble; - tf2::fromMsg(part.assembled_pose.pose, part_assemble); - - KDL::Frame part_to_gripper; - - // Build approach waypoints - std::vector waypoints; - if (part.part.type == ariac_msgs::msg::Part::BATTERY) - { - tf2::fromMsg(BuildPose(battery_grip_offset_, 0, part_heights_[part.part.type], QuaternionFromRPY(M_PI, 0, M_PI)), part_to_gripper); - KDL::Vector up(0, 0, 0.1); - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(up) * KDL::Frame(install * -0.06) * part_assemble * part_to_gripper)); - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(install * -0.06) * part_assemble * part_to_gripper)); - } - else - { - tf2::fromMsg(BuildPose(0, 0, part_heights_[part.part.type], QuaternionFromRPY(M_PI, 0, M_PI)), part_to_gripper); - - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(install * -0.1) * part_assemble * part_to_gripper)); - } - - // Move to approach position - CeilingRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - // Move to just before assembly position - waypoints.clear(); - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(install * -assembly_offset_) * part_assemble * part_to_gripper)); - CeilingRobotMoveCartesian(waypoints, 0.1, 0.1, true); - - CeilingRobotWaitForAssemble(station, part); - - CeilingRobotSetGripperState(false); - - std::string part_name = part_colors_[ceiling_robot_attached_part_.color] + - "_" + part_types_[ceiling_robot_attached_part_.type]; - ceiling_robot_.detachObject(part_name); - - waypoints.clear(); - KDL::Vector away; - if (part.part.type == ariac_msgs::msg::Part::REGULATOR) { - away = KDL::Vector(-0.1, 0, 0); - } else { - away = KDL::Vector(0, 0, 0.1); - } - - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(away) * part_assemble * part_to_gripper)); - - CeilingRobotMoveCartesian(waypoints, 0.1, 0.1, false); - - return true; -} - -bool TestCompetitor::CompleteOrders() -{ - // Wait for first order to be published - while (orders_.size() == 0) - { - } - - bool success; - while (true) - { - if (competition_state_ == ariac_msgs::msg::CompetitionState::ENDED) - { - success = false; - break; - } - - if (orders_.size() == 0) - { - if (competition_state_ != ariac_msgs::msg::CompetitionState::ORDER_ANNOUNCEMENTS_DONE) - { - // wait for more orders - RCLCPP_INFO(get_logger(), "Waiting for orders..."); - while (orders_.size() == 0) - { - } - } - else - { - RCLCPP_INFO(get_logger(), "Completed all orders"); - success = true; - break; - } - } - - current_order_ = orders_.front(); - orders_.erase(orders_.begin()); - // int kitting_agv_num = -1; - - if (current_order_.type == ariac_msgs::msg::Order::KITTING) - { - RCLCPP_INFO_STREAM(get_logger(), "Current order is: " << current_order_.id << " of type KITTING"); - TestCompetitor::CompleteKittingTask(current_order_.kitting_task); - } - else if (current_order_.type == ariac_msgs::msg::Order::ASSEMBLY) - { - RCLCPP_INFO_STREAM(get_logger(), "Current order is: " << current_order_.id << " of type ASSEMBLY"); - TestCompetitor::CompleteAssemblyTask(current_order_.assembly_task); - } - else if (current_order_.type == ariac_msgs::msg::Order::COMBINED) - { - RCLCPP_INFO_STREAM(get_logger(), "Current order is: " << current_order_.id << " of type COMBINED"); - TestCompetitor::CompleteCombinedTask(current_order_.combined_task); - } - - - TestCompetitor::SubmitOrder(current_order_.id); - } - return success; -} - - -bool TestCompetitor::CompleteKittingTask(ariac_msgs::msg::KittingTask task) -{ - FloorRobotSendHome(); - - if(agv_locations_[task.agv_number] != ariac_msgs::msg::AGVStatus::KITTING) - { - MoveAGV(task.agv_number,ariac_msgs::srv::MoveAGV::Request::KITTING); - } - - FloorRobotPickandPlaceTray(task.tray_id, task.agv_number); - - bool found; - for (auto kit_part : task.parts) - { - found = FloorRobotPickBinPart(kit_part.part); - if (!found) - { - FloorRobotPickConveyorPart(kit_part.part); - } - FloorRobotPlacePartOnKitTray(task.agv_number, kit_part.quadrant); - } - - // Check quality - auto request = std::make_shared(); - request->order_id = current_order_.id; - auto future = quality_checker_->async_send_request(request); - future.wait(); - - if (!future.get()->all_passed) - { - RCLCPP_ERROR(get_logger(), "Issue with shipment"); - } - - //Remove objects from planning scene - planning_scene_.removeCollisionObjects(order_planning_scene_objects_); - - order_planning_scene_objects_.clear(); - - MoveAGV(task.agv_number, task.destination); - - return true; -} - -bool TestCompetitor::CompleteAssemblyTask(ariac_msgs::msg::AssemblyTask task) -{ - // Send AGVs to assembly station - for (auto const &agv : task.agv_numbers) - { - int destination; - if (task.station == ariac_msgs::msg::AssemblyTask::AS1 || task.station == ariac_msgs::msg::AssemblyTask::AS3) - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_FRONT; - } - else if (task.station == ariac_msgs::msg::AssemblyTask::AS2 || task.station == ariac_msgs::msg::AssemblyTask::AS4) - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_BACK; - } - - LockAGVTray(agv); - MoveAGV(agv, destination); - } - - CeilingRobotMoveToAssemblyStation(task.station); - - // Get Assembly Poses - RCLCPP_INFO_STREAM(get_logger(), "Getting pre assembly poses for order " << current_order_.id); - auto request = std::make_shared(); - request->order_id = current_order_.id; - auto future = pre_assembly_poses_getter_->async_send_request(request); - - RCLCPP_INFO(get_logger(), "Waiting for pre assembly poses"); - - future.wait(); - - RCLCPP_INFO(get_logger(), "Recieved pre assembly poses"); - - std::vector agv_part_poses; - - auto result = future.get(); - - if (result->valid_id) - { - RCLCPP_INFO(get_logger(), "Valid id"); - - try { - int len = result->parts.size(); - RCLCPP_INFO_STREAM(get_logger(), "There are " << std::to_string(len) << " parts"); - } - catch (...) { - RCLCPP_INFO(get_logger(), "Unable to access future result"); - } - - agv_part_poses = result->parts; - - if (agv_part_poses.size() == 0) - { - RCLCPP_WARN(get_logger(), "No part poses recieved"); - return false; - } - } - else - { - RCLCPP_WARN(get_logger(), "Not a valid order ID"); - return false; - } - - for (auto const &part_to_assemble : task.parts) - { - // Check if matching part exists in agv_parts - bool part_exists = false; - ariac_msgs::msg::PartPose part_to_pick; - part_to_pick.part = part_to_assemble.part; - for (auto const &agv_part : agv_part_poses) - { - if (agv_part.part.type == part_to_assemble.part.type && agv_part.part.color == part_to_assemble.part.color) - { - part_exists = true; - part_to_pick.pose = agv_part.pose; - break; - } - } - - if (!part_exists) - { - RCLCPP_WARN_STREAM(get_logger(), "Part with type: " << part_to_assemble.part.type << " and color: " << part_to_assemble.part.color << " not found on tray"); - continue; - } - - // Pick up part - CeilingRobotPickAGVPart(part_to_pick); - - CeilingRobotMoveToAssemblyStation(task.station); - - // Assemble Part to insert - CeilingRobotAssemblePart(task.station, part_to_assemble); - - CeilingRobotMoveToAssemblyStation(task.station); - } - - return true; -} - -bool TestCompetitor::CompleteCombinedTask(ariac_msgs::msg::CombinedTask task) -{ - RCLCPP_INFO_STREAM(get_logger(),"Starting Combined Task"); - // Decide on a tray to use - int id; - if (kts1_trays_.size() != 0) - { - id = kts1_trays_[0].id; - RCLCPP_INFO_STREAM(get_logger(), "Using tray " << id << " from kts1"); - } - else if (kts2_trays_.size() != 0) - { - id = kts2_trays_[0].id; - RCLCPP_INFO_STREAM(get_logger(), "Using tray " << id << " from kts2"); - } - else - { - RCLCPP_ERROR(get_logger(), "No trays available."); - return false; - } - - // Decide which AGV to use - int agv_number; - if (task.station == ariac_msgs::msg::CombinedTask::AS1 or task.station == ariac_msgs::msg::CombinedTask::AS2) - { - agv_number = 1; - RCLCPP_INFO_STREAM(get_logger(),"Using AGV " << agv_number << " for assembly since station is AS1 or AS2"); - } - else - { - agv_number = 4; - RCLCPP_INFO_STREAM(get_logger(),"Using AGV " << agv_number << " for assembly since station is AS3 or AS4"); - } - - MoveAGV(agv_number, ariac_msgs::srv::MoveAGV::Request::KITTING); - - FloorRobotPickandPlaceTray(id, agv_number); - - int count = 1; - bool found; - for (auto assembly_part : task.parts) - { - found = FloorRobotPickBinPart(assembly_part.part); - if (!found) - { - FloorRobotPickConveyorPart(assembly_part.part); - } - FloorRobotPlacePartOnKitTray(agv_number, count); - count++; - } - - int destination; - if (task.station == ariac_msgs::msg::CombinedTask::AS1 or task.station == ariac_msgs::msg::CombinedTask::AS3) - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_FRONT; - } - else - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_BACK; - } - - //Remove objects from planning scene - planning_scene_.removeCollisionObjects(order_planning_scene_objects_); - - order_planning_scene_objects_.clear(); - - MoveAGV(agv_number, destination); - - CeilingRobotMoveToAssemblyStation(task.station); - - // Get Assembly Poses - RCLCPP_INFO_STREAM(get_logger(), "Getting pre assembly poses for order " << current_order_.id); - auto request = std::make_shared(); - request->order_id = current_order_.id; - auto future = pre_assembly_poses_getter_->async_send_request(request); - - future.wait(); - - auto result = future.get(); - - std::vector agv_part_poses; - if (result->valid_id) - { - agv_part_poses = result->parts; - - if (agv_part_poses.size() == 0) - { - RCLCPP_WARN(get_logger(), "No part poses recieved"); - return false; - } - } - else - { - RCLCPP_WARN(get_logger(), "Not a valid order ID"); - return false; - } - - for (auto const &part_to_assemble : task.parts) - { - // Check if matching part exists in agv_parts - bool part_exists = false; - ariac_msgs::msg::PartPose part_to_pick; - part_to_pick.part = part_to_assemble.part; - for (auto const &agv_part : agv_part_poses) - { - if (agv_part.part.type == part_to_assemble.part.type && agv_part.part.color == part_to_assemble.part.color) - { - part_exists = true; - part_to_pick.pose = agv_part.pose; - break; - } - } - - if (!part_exists) - { - RCLCPP_WARN_STREAM(get_logger(), "Part with type: " << part_types_[part_to_assemble.part.type] << " and color: " << part_colors_[part_to_assemble.part.color] << " not found on tray"); - continue; - } - - // Pick up part - CeilingRobotPickAGVPart(part_to_pick); - - CeilingRobotMoveToAssemblyStation(task.station); - - // Assemble Part to insert - CeilingRobotAssemblePart(task.station, part_to_assemble); - - CeilingRobotMoveToAssemblyStation(task.station); - } - - return true; -} - -bool TestCompetitor::StartCompetition() -{ - // Wait for competition state to be ready - while (competition_state_ != ariac_msgs::msg::CompetitionState::READY) - { - } - - RCLCPP_INFO_STREAM(get_logger(), "Starting competition"); - - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/start_competition"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::EndCompetition() -{ - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/end_competition"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - RCLCPP_INFO(get_logger(), "Ending competition."); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::SubmitOrder(std::string order_id) -{ - RCLCPP_INFO_STREAM(get_logger(), "Submitting order " << order_id); - rclcpp::Client::SharedPtr client; - std::string srv_name = "/ariac/submit_order"; - client = this->create_client(srv_name); - auto request = std::make_shared(); - request->order_id = order_id; - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::LockAGVTray(int agv_num) -{ - RCLCPP_INFO_STREAM(get_logger(), "Locking Tray to AGV" << agv_num); - - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/agv" + std::to_string(agv_num) + "_lock_tray"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::UnlockAGVTray(int agv_num) -{ - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/agv" + std::to_string(agv_num) + "_unlock_tray"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::MoveAGV(int agv_num, int destination) -{ - RCLCPP_INFO_STREAM(get_logger(), "Moving AGV" << agv_num << " to " << agv_destination_[destination]); - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/move_agv" + std::to_string(agv_num); - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - request->location = destination; - - auto future = client->async_send_request(request); - - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(20); - - rclcpp::Time start = get_clock()->now(); - - while (get_clock()->now() - start < timeout){ - if (agv_locations_[agv_num] == destination) { - return true; - } - } - - RCLCPP_INFO_STREAM(get_logger(), "Unable to move AGV" << agv_num << " to " << agv_destination_[destination]); - return false; -} \ No newline at end of file