From 06c2d01b4a0b4c6c537a326c4a0e83c21e4ac60a Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 2 Mar 2026 07:50:30 +0000 Subject: [PATCH] update --- config/costmap_common_params.yaml | 2 +- config/costmap_global_params.yaml | 2 +- config/costmap_local_params.yaml | 2 +- config/mprim/ekf.yaml | 2 +- config/mprim/localization.yaml | 2 +- config/mprim/ros_diff_drive_controller.yaml | 2 +- config/pnkx_local_planner_params.yaml | 6 +- examples/NavigationExample/NavigationAPI.cs | 292 +++++++ .../NavigationExample.csproj | 2 +- examples/NavigationExample/Program.cs | 597 ++++---------- examples/NavigationExample/TF3API.cs | 133 +++ examples/NavigationExample/libnav_c_api.so | Bin 144464 -> 300144 bytes examples/NavigationExample/msgs/CommonMsgs.cs | 274 +++++++ .../msgs/ProtocolMsgsTypes.cs | 109 +++ examples/NavigationExample/msgs/SensorMsgs.cs | 0 pnkx_nav_core.sln | 29 + robotapp.db | 0 src/APIs/c_api/CMakeLists.txt | 3 + src/APIs/c_api/include/convertor.h | 46 ++ src/APIs/c_api/include/nav_c_api.h | 114 +-- src/APIs/c_api/include/protocol_msgs/Action.h | 30 + .../include/protocol_msgs/ActionParameter.h | 21 + .../include/protocol_msgs/ControlPoint.h | 22 + src/APIs/c_api/include/protocol_msgs/Edge.h | 42 + src/APIs/c_api/include/protocol_msgs/Error.h | 28 + .../include/protocol_msgs/ErrorReference.h | 21 + src/APIs/c_api/include/protocol_msgs/Info.h | 28 + .../include/protocol_msgs/InfoReference.h | 21 + .../c_api/include/protocol_msgs/Information.h | 22 + src/APIs/c_api/include/protocol_msgs/Node.h | 28 + .../include/protocol_msgs/NodePosition.h | 26 + src/APIs/c_api/include/protocol_msgs/Order.h | 33 + .../c_api/include/protocol_msgs/Trajectory.h | 25 + src/APIs/c_api/src/convertor.cpp | 435 ++++++++++ src/APIs/c_api/src/nav_c_api.cpp | 774 ++++++++++-------- .../Libraries/kalman/src/kalman.cpp | 10 +- .../mkt_algorithm/diff/diff_go_straight.h | 4 +- .../diff/diff_predictive_trajectory.h | 9 +- .../mkt_algorithm/diff/diff_rotate_to_goal.h | 4 +- .../src/diff/diff_go_straight.cpp | 7 + .../src/diff/diff_predictive_trajectory.cpp | 42 + .../src/diff/diff_rotate_to_goal.cpp | 8 + .../mkt_plugins/kinematic_parameters.h | 1 + .../mkt_plugins/limited_accel_generator.h | 9 + .../mkt_plugins/standard_traj_generator.h | 10 + .../include/mkt_plugins/xy_theta_iterator.h | 7 +- .../mkt_plugins/src/goal_checker.cpp | 7 +- .../mkt_plugins/src/kinematic_parameters.cpp | 42 +- .../src/limited_accel_generator.cpp | 10 + .../mkt_plugins/src/simple_goal_checker.cpp | 9 +- .../src/standard_traj_generator.cpp | 18 + .../mkt_plugins/src/xy_theta_iterator.cpp | 15 + .../src/two_points_planner.cpp | 12 +- .../pnkx_go_straight_local_planner.h | 1 - .../src/pnkx_docking_local_planner.cpp | 5 +- .../src/pnkx_go_straight_local_planner.cpp | 1 + .../src/pnkx_local_planner.cpp | 16 +- .../src/pnkx_rotate_local_planner.cpp | 1 + .../include/move_base_core/navigation.h | 3 +- .../move_base/include/move_base/move_base.h | 1 + .../Packages/move_base/src/move_base.cpp | 521 +++++++++--- 61 files changed, 2934 insertions(+), 1012 deletions(-) create mode 100644 examples/NavigationExample/NavigationAPI.cs create mode 100644 examples/NavigationExample/TF3API.cs create mode 100644 examples/NavigationExample/msgs/CommonMsgs.cs create mode 100644 examples/NavigationExample/msgs/ProtocolMsgsTypes.cs create mode 100644 examples/NavigationExample/msgs/SensorMsgs.cs create mode 100644 pnkx_nav_core.sln create mode 100644 robotapp.db create mode 100644 src/APIs/c_api/include/protocol_msgs/Action.h create mode 100644 src/APIs/c_api/include/protocol_msgs/ActionParameter.h create mode 100644 src/APIs/c_api/include/protocol_msgs/ControlPoint.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Edge.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Error.h create mode 100644 src/APIs/c_api/include/protocol_msgs/ErrorReference.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Info.h create mode 100644 src/APIs/c_api/include/protocol_msgs/InfoReference.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Information.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Node.h create mode 100644 src/APIs/c_api/include/protocol_msgs/NodePosition.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Order.h create mode 100644 src/APIs/c_api/include/protocol_msgs/Trajectory.h diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 22cba75..429860c 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -5,7 +5,7 @@ rotate_planner_name: PNKXRotateLocalPlanner base_local_planner: LocalPlannerAdapter base_global_planner: CustomPlanner -robot_base_frame: base_footprint +robot_base_frame: base_link transform_tolerance: 1.0 obstacle_range: 3.0 #mark_threshold: 1 diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml index 5c88dc6..811ab13 100755 --- a/config/costmap_global_params.yaml +++ b/config/costmap_global_params.yaml @@ -1,6 +1,6 @@ global_costmap: library_path: libplugins - robot_base_frame: base_footprint + robot_base_frame: base_link global_frame: map update_frequency: 1.0 publish_frequency: 1.0 diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index 6af3ebb..ecd9e9d 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -1,7 +1,7 @@ local_costmap: library_path: libplugins global_frame: odom - robot_base_frame: base_footprint + robot_base_frame: base_link update_frequency: 6.0 publish_frequency: 6.0 rolling_window: true diff --git a/config/mprim/ekf.yaml b/config/mprim/ekf.yaml index 4bf871d..ff05f12 100755 --- a/config/mprim/ekf.yaml +++ b/config/mprim/ekf.yaml @@ -62,7 +62,7 @@ publish_acceleration: false # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified -base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified +base_link_frame: $(arg tf_prefix)base_link # Defaults to "base_link" if unspecified world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry, diff --git a/config/mprim/localization.yaml b/config/mprim/localization.yaml index 677cb96..2ed52b8 100644 --- a/config/mprim/localization.yaml +++ b/config/mprim/localization.yaml @@ -25,7 +25,7 @@ Amcl: update_min_d: 0.05 update_min_a: 0.05 odom_frame_id: odom - base_frame_id: base_footprint + base_frame_id: base_link global_frame_id: map resample_interval: 1 transform_tolerance: 0.2 diff --git a/config/mprim/ros_diff_drive_controller.yaml b/config/mprim/ros_diff_drive_controller.yaml index e498438..90b2db6 100755 --- a/config/mprim/ros_diff_drive_controller.yaml +++ b/config/mprim/ros_diff_drive_controller.yaml @@ -25,7 +25,7 @@ wheel_radius_multiplier : 1.0 # default: 1.0 cmd_vel_timeout: 1.0 # frame_ids (same as real MiR platform) -base_frame_id: base_footprint # default: base_link base_footprint +base_frame_id: base_link # default: base_link base_link odom_frame_id: odom # default: odom # Velocity and acceleration limits diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 9c205af..9b3aa9d 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -1,4 +1,4 @@ -yaw_goal_tolerance: 0.017 +yaw_goal_tolerance: 0.03 xy_goal_tolerance: 0.02 min_approach_linear_velocity: 0.05 @@ -41,8 +41,8 @@ PNKXRotateLocalPlanner: LimitedAccelGenerator: library_path: libmkt_plugins_standard_traj_generator - max_vel_x: 1.2 - min_vel_x: -1.2 + max_vel_x: 0.2 + min_vel_x: -0.2 max_vel_y: 0.0 # diff drive robot min_vel_y: 0.0 # diff drive robot diff --git a/examples/NavigationExample/NavigationAPI.cs b/examples/NavigationExample/NavigationAPI.cs new file mode 100644 index 0000000..e6e87b3 --- /dev/null +++ b/examples/NavigationExample/NavigationAPI.cs @@ -0,0 +1,292 @@ +using System; +using System.Runtime.InteropServices; +using NavigationExample; + +namespace NavigationExample +{ + /// + /// C# P/Invoke wrapper for Navigation C API + /// + public class NavigationAPI + { + private const string DllName = "libnav_c_api.so"; // Linux + // For Windows: "nav_c_api.dll" + // For macOS: "libnav_c_api.dylib" + + // ============================================================================ + // Enums + // ============================================================================ + public enum NavigationState + { + Pending = 0, + Active = 1, + Preempted = 2, + Succeeded = 3, + Aborted = 4, + Rejected = 5, + Preempting = 6, + Recalling = 7, + Recalled = 8, + Lost = 9, + Planning = 10, + Controlling = 11, + Clearing = 12, + Paused = 13 + } + + [StructLayout(LayoutKind.Sequential)] + public struct NavFeedback + { + public NavigationState navigation_state; + public IntPtr feed_back_str; // char*; free with nav_c_api_free_string + public Pose2D current_pose; + [MarshalAs(UnmanagedType.I1)] + public bool goal_checked; + [MarshalAs(UnmanagedType.I1)] + public bool is_ready; + } + + + /// Planner data output (plan, costmap, footprint). + [StructLayout(LayoutKind.Sequential)] + public struct PlannerDataOutput + { + public Path2D plan; + public OccupancyGrid costmap; + public OccupancyGridUpdate costmap_update; + [MarshalAs(UnmanagedType.I1)] + public bool is_costmap_updated; + public PolygonStamped footprint; + } + + [StructLayout(LayoutKind.Sequential)] + public struct NavigationHandle + { + public IntPtr ptr; + } + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern Header header_create(string frame_id); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern Header header_set_data( + uint seq, + uint sec, + uint nsec, + string frame_id); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern Time time_create(); + + /// Free a string allocated by the API (strdup). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void nav_c_api_free_string(IntPtr str); + + /// Convert NavigationState to string; caller must free with nav_c_api_free_string. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern IntPtr navigation_state_to_string(NavigationState state); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); + + + /// Helper: copy unmanaged char* to managed string; does not free the pointer. + public static string MarshalString(IntPtr p) + { + if (p == IntPtr.Zero) return string.Empty; + return Marshal.PtrToStringAnsi(p) ?? string.Empty; + } + + /// Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done. + public static void navigation_free_feedback(ref NavFeedback feedback) + { + if (feedback.feed_back_str != IntPtr.Zero) + { + nav_c_api_free_string(feedback.feed_back_str); + feedback.feed_back_str = IntPtr.Zero; + } + } + + // ============================================================================ + // Navigation Handle Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern NavigationHandle navigation_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_destroy(NavigationHandle handle); + + /// Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count); + + /// Send a goal for the robot to navigate to (global frame). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal); + + /// Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal); + + /// Goal is passed by reference to match C API: navigation_dock_to_order(..., const PoseStamped &goal). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, ref PoseStamped goal); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance); + + /// Rotate in place to align with target orientation (radians). + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_pause(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_resume(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_cancel(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist); + + // ============================================================================ + // Navigation Data Management + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_static_maps(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_remove_all_data(NavigationHandle handle); + + /// Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed. + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data); + } +} \ No newline at end of file diff --git a/examples/NavigationExample/NavigationExample.csproj b/examples/NavigationExample/NavigationExample.csproj index 48dbb2c..ad63abd 100644 --- a/examples/NavigationExample/NavigationExample.csproj +++ b/examples/NavigationExample/NavigationExample.csproj @@ -1,7 +1,7 @@ Exe - net6.0 + net10.0 linux-x64 true diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 3b1a84b..ceedc3e 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -6,6 +6,8 @@ using System.Runtime.CompilerServices; using System.Text.RegularExpressions; using SixLabors.ImageSharp; using SixLabors.ImageSharp.PixelFormats; +using NavigationExample; + namespace NavigationExample { @@ -19,366 +21,6 @@ namespace NavigationExample public double OccupiedThresh; public double FreeThresh; } - /// - /// C# P/Invoke wrapper for Navigation C API - /// - public class NavigationAPI - { - private const string DllName = "libnav_c_api.so"; // Linux - // For Windows: "nav_c_api.dll" - // For macOS: "libnav_c_api.dylib" - - // ============================================================================ - // Enums - // ============================================================================ - public enum NavigationState - { - Pending = 0, - Active = 1, - Preempted = 2, - Succeeded = 3, - Aborted = 4, - Rejected = 5, - Preempting = 6, - Recalling = 7, - Recalled = 8, - Lost = 9, - Planning = 10, - Controlling = 11, - Clearing = 12, - Paused = 13 - } - - // ============================================================================ - // Structures - // ============================================================================ - [StructLayout(LayoutKind.Sequential)] - public struct Point - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Pose2D - { - public double x; - public double y; - public double theta; - } - - [StructLayout(LayoutKind.Sequential)] - public struct NavFeedback - { - public NavigationState navigation_state; - public IntPtr feed_back_str; // char*; free with nav_c_api_free_string - public Pose2D current_pose; - [MarshalAs(UnmanagedType.I1)] - public bool goal_checked; - [MarshalAs(UnmanagedType.I1)] - public bool is_ready; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist2D - { - public double x; - public double y; - public double theta; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Quaternion - { - public double x; - public double y; - public double z; - public double w; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Position - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Pose - { - public Position position; - public Quaternion orientation; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Vector3 - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist - { - public Vector3 linear; - public Vector3 angular; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Header - { - public uint seq; - public uint sec; - public uint nsec; - public IntPtr frame_id; // char* - } - - [StructLayout(LayoutKind.Sequential)] - public struct PoseStamped - { - public Header header; - public Pose pose; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist2DStamped - { - public Header header; - public Twist2D velocity; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct NavigationHandle - { - public IntPtr ptr; - } - - [StructLayout(LayoutKind.Sequential)] - public struct TFListenerHandle - { - public IntPtr ptr; - } - - [StructLayout(LayoutKind.Sequential)] - public struct LaserScan - { - public Header header; - public float angle_min; - public float angle_max; - public float angle_increment; - public float time_increment; - public float scan_time; - public float range_min; - public float range_max; - public IntPtr ranges; - public UIntPtr ranges_count; - public IntPtr intensities; - public UIntPtr intensities_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct PoseWithCovariance - { - public Pose pose; - public IntPtr covariance; - public UIntPtr covariance_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct TwistWithCovariance { - public Twist twist; - public IntPtr covariance; - public UIntPtr covariance_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Odometry - { - public Header header; - public IntPtr child_frame_id; - public PoseWithCovariance pose; - public TwistWithCovariance twist; - } - - [StructLayout(LayoutKind.Sequential)] - public struct OccupancyGrid - { - public Header header; - public MapMetaData info; - public IntPtr data; - public UIntPtr data_count; - } - - [StructLayout(LayoutKind.Sequential)] - public struct MapMetaData - { - public Time map_load_time; - public float resolution; - public uint width; - public uint height; - public Pose origin; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Time - { - public uint sec; - public uint nsec; - } - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern Header header_create(string frame_id); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern Header header_set_data( - uint seq, - uint sec, - uint nsec, - string frame_id); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern Time time_create(); - - /// Free a string allocated by the API (strdup). - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void nav_c_api_free_string(IntPtr str); - - /// Convert NavigationState to string; caller must free with nav_c_api_free_string. - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern IntPtr navigation_state_to_string(NavigationState state); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); - - - /// Helper: copy unmanaged char* to managed string; does not free the pointer. - public static string MarshalString(IntPtr p) - { - if (p == IntPtr.Zero) return string.Empty; - return Marshal.PtrToStringAnsi(p) ?? string.Empty; - } - - /// Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done. - public static void navigation_free_feedback(ref NavFeedback feedback) - { - if (feedback.feed_back_str != IntPtr.Zero) - { - nav_c_api_free_string(feedback.feed_back_str); - feedback.feed_back_str = IntPtr.Zero; - } - } - - // ============================================================================ - // TF Listener Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern TFListenerHandle tf_listener_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void tf_listener_destroy(TFListenerHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool tf_listener_set_static_transform( - TFListenerHandle tf_handle, - string parent_frame, - string child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw); - - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern NavigationHandle navigation_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_destroy(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_pause(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_resume(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_cancel(NavigationHandle handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist); - // ============================================================================ - // Navigation Data Management - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid); - - } // ============================================================================ // Example Usage @@ -497,69 +139,53 @@ namespace NavigationExample static void Main(string[] args) { - // Create TF listener (required for costmap and local planner; must be valid when calling navigation_initialize) - NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create(); - if (tfHandle.ptr == IntPtr.Zero) + // Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init) + IntPtr tf3Buffer = TF3API.tf3_buffer_create(10); + if (tf3Buffer == IntPtr.Zero) { - LogError("Failed to create TF listener"); + LogError("Failed to create tf3 buffer (libtf3.so may be missing)"); return; } - Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}"); + Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}"); + string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?"; + Console.WriteLine($"[TF3] {version}"); - // Inject a static TF so costmap can immediately canTransform(map <-> base_link). - // If you already publish TF from localization/odometry, you can remove this call. - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom", - 0, 0, 0, - 0, 0, 0, 1)) + // Inject static transforms: map -> odom -> base_footprint -> base_link + var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1); + var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1); + var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1); + if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) || + !TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) || + !TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true)) { - LogError("Failed to inject static TF map -> odom"); - NavigationAPI.tf_listener_destroy(tfHandle); + LogError("Failed to set static TF"); + TF3API.tf3_buffer_destroy(tf3Buffer); return; } - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint", - 0, 0, 0, - 0, 0, 0, 1)) - { - LogError("Failed to inject static TF map -> base_link"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link", - 0, 0, 0, - 0, 0, 0, 1)) - { - LogError("Failed to inject static TF map -> base_link"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Create navigation instance + // Create navigation instance and initialize with tf3 buffer NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create(); if (navHandle.ptr == IntPtr.Zero) { LogError("Failed to create navigation instance"); - NavigationAPI.tf_listener_destroy(tfHandle); + TF3API.tf3_buffer_destroy(tf3Buffer); return; } - // Initialize navigation (passes TF to move_base; navigation keeps its own copy, so tfHandle can be destroyed later) - if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) + if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer)) { - LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')"); + LogError("Failed to initialize navigation with tf3 buffer"); NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); + TF3API.tf3_buffer_destroy(tf3Buffer); return; } - while(true) + while (true) { - // Get navigation feedback NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) { - if(feedback.is_ready) + if (feedback.is_ready) { Console.WriteLine("Navigation is ready"); break; @@ -571,22 +197,23 @@ namespace NavigationExample } System.Threading.Thread.Sleep(100); } - Console.WriteLine("[NavigationExample] Navigation initialized with TF successfully"); + Console.WriteLine("[NavigationExample] Navigation initialized successfully"); + // Set robot footprint - NavigationAPI.Point[] footprint = new NavigationAPI.Point[] + Point[] footprint = new Point[] { - new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 }, - new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 }, - new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 }, - new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 } + new Point { x = 0.3, y = -0.2, z = 0.0 }, + new Point { x = 0.3, y = 0.2, z = 0.0 }, + new Point { x = -0.3, y = 0.2, z = 0.0 }, + new Point { x = -0.3, y = -0.2, z = 0.0 } }; NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan"); - NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); - NavigationAPI.LaserScan fscanHandle; + Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); + LaserScan fscanHandle; fscanHandle.header = fscanHeader; fscanHandle.angle_min = -1.57f; fscanHandle.angle_max = 1.57f; @@ -604,8 +231,8 @@ namespace NavigationExample NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan", fscanHandle); IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan"); - NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId)); - NavigationAPI.LaserScan bscanHandle; + Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId)); + LaserScan bscanHandle; bscanHandle.header = bscanHeader; bscanHandle.angle_min = 1.57f; bscanHandle.angle_max = -1.57f; @@ -623,8 +250,8 @@ namespace NavigationExample NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan", bscanHandle); IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom"); - NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId)); - NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry(); + Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId)); + Odometry odometryHandle = new Odometry(); odometryHandle.header = odometryHeader; IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint"); odometryHandle.child_frame_id = childFrameId; @@ -695,13 +322,13 @@ namespace NavigationExample Console.WriteLine("maze.yaml not found, using default map 3x10"); } - NavigationAPI.Time mapLoadTime = NavigationAPI.time_create(); - NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData(); + Time mapLoadTime = NavigationAPI.time_create(); + MapMetaData mapMetaData = new MapMetaData(); mapMetaData.map_load_time = mapLoadTime; mapMetaData.resolution = mapConfig.Resolution; mapMetaData.width = (uint)mapWidth; mapMetaData.height = (uint)mapHeight; - mapMetaData.origin = new NavigationAPI.Pose(); + mapMetaData.origin = new Pose(); mapMetaData.origin.position.x = mapConfig.OriginX; mapMetaData.origin.position.y = mapConfig.OriginY; mapMetaData.origin.position.z = mapConfig.OriginZ; @@ -709,7 +336,7 @@ namespace NavigationExample mapMetaData.origin.orientation.y = 0.0; mapMetaData.origin.orientation.z = 0.0; mapMetaData.origin.orientation.w = 1.0; - NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid(); + OccupancyGrid occupancyGrid = new OccupancyGrid(); IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map"); occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); occupancyGrid.info = mapMetaData; @@ -718,26 +345,148 @@ namespace NavigationExample occupancyGrid.data_count = new UIntPtr((uint)data.Length); Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count); Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}", - Marshal.SizeOf(), - Marshal.OffsetOf("data"), - Marshal.OffsetOf("data_count")); + Marshal.SizeOf(), + Marshal.OffsetOf("data"), + Marshal.OffsetOf("data_count")); NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid); - NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped(); + Twist2DStamped twist = new Twist2DStamped(); if (NavigationAPI.navigation_get_twist(navHandle, ref twist)) { Console.WriteLine( "Twist: {0}, {1}, {2}, {3}", NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta); - } - // Cleanup - NavigationAPI.navigation_move_straight_to(navHandle, 1.0); + } + + // // Build order (thao cách bom order): header + nodes + edges giống C++ + // Order order = new Order(); + // order.headerId = 1; + // order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00"); + // order.version = Marshal.StringToHGlobalAnsi("1.0.0"); + // order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer"); + // order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number"); + // order.orderId = Marshal.StringToHGlobalAnsi("Order ID"); + // order.orderUpdateId = 1; + + // // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); } + // int nodeCount = 1; + // order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf() * nodeCount); + // order.nodes_count = new UIntPtr((uint)nodeCount); + // Node node1 = new Node(); + // node1.nodeId = Marshal.StringToHGlobalAnsi("node-1"); + // node1.sequenceId = 0; + // node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node"); + // node1.released = 0; + // node1.nodePosition.x = 1.0; + // node1.nodePosition.y = 1.0; + // node1.nodePosition.theta = 0.0; + // node1.nodePosition.allowedDeviationXY = 0.1f; + // node1.nodePosition.allowedDeviationTheta = 0.05f; + // node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map"); + // node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi(""); + // node1.actions = IntPtr.Zero; + // node1.actions_count = UIntPtr.Zero; + // Marshal.StructureToPtr(node1, order.nodes, false); + + // // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...) + // order.edges = IntPtr.Zero; + // order.edges_count = UIntPtr.Zero; + // order.zoneSetId = Marshal.StringToHGlobalAnsi(""); + + // PoseStamped goal = new PoseStamped(); + // goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); + // goal.pose.position.x = 1.0; + // goal.pose.position.y = 1.0; + // goal.pose.position.z = 0.0; + // goal.pose.orientation.x = 0.0; + // goal.pose.orientation.y = 0.0; + // goal.pose.orientation.z = 0.0; + // goal.pose.orientation.w = 1.0; + + // NavigationAPI.navigation_move_to_order(navHandle, order, goal); + + NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0); + NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2); + + // NavigationAPI.navigation_move_straight_to(navHandle, 1.0); + + // while (true) + // { + // System.Threading.Thread.Sleep(100); + // NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); + // if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) + // { + // if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded) + // { + // Console.WriteLine("Navigation is Succeeded"); + // break; + // } + // } + + // NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput(); + // if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData)) + // { + // int n = (int)(uint)globalData.plan.poses_count; + // int poseSize = Marshal.SizeOf(); + // for (int i = 0; i < n; i++) + // { + // IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize); + // Pose2DStamped p = Marshal.PtrToStructure(posePtr); + // double p_x = p.pose.x; + // double p_y = p.pose.y; + // double p_theta = p.pose.theta; + // Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta); + // } + // if(globalData.is_costmap_updated) { + // for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) { + // byte cellValue = Marshal.ReadByte(globalData.costmap.data, i); + // Console.WriteLine("Costmap: {0} {1}", i, cellValue); + // } + // } + // else { + // Console.WriteLine("Global Costmap is not updated"); + // } + // } + + // NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput(); + // if(NavigationAPI.navigation_get_local_data(navHandle, ref localData)) + // { + // int n = (int)(uint)localData.plan.poses_count; + // int poseSize = Marshal.SizeOf(); + // for (int i = 0; i < n; i++) + // { + // IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize); + // Pose2DStamped p = Marshal.PtrToStructure(posePtr); + // double p_x = p.pose.x; + // double p_y = p.pose.y; + // double p_theta = p.pose.theta; + // Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta); + // } + // if(localData.is_costmap_updated) { + // for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) { + // byte cellValue = Marshal.ReadByte(localData.costmap.data, i); + // Console.WriteLine("Costmap: {0} {1}", i, cellValue); + // } + // } + // else { + // Console.WriteLine("Local Costmap is not updated"); + // } + // } + // } + // Cleanup (destroy nav first, then tf3 buffer) NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); + TF3API.tf3_buffer_destroy(tf3Buffer); Console.WriteLine("Press any key to exit..."); - Console.ReadKey(); + try + { + Console.ReadKey(intercept: true); + } + catch (InvalidOperationException) + { + // Running without a real console (e.g. redirected/automated run). + } } } } diff --git a/examples/NavigationExample/TF3API.cs b/examples/NavigationExample/TF3API.cs new file mode 100644 index 0000000..c059fa4 --- /dev/null +++ b/examples/NavigationExample/TF3API.cs @@ -0,0 +1,133 @@ +using System; +using System.Runtime.InteropServices; + +namespace NavigationExample +{ + + // ============================================================================ + // TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore) + // ============================================================================ + public static class TF3API + { + private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib + + public enum TF3_ErrorCode + { + TF3_OK = 0, + TF3_ERROR_LOOKUP = 1, + TF3_ERROR_CONNECTIVITY = 2, + TF3_ERROR_EXTRAPOLATION = 3, + TF3_ERROR_INVALID_ARGUMENT = 4, + TF3_ERROR_TIMEOUT = 5, + TF3_ERROR_UNKNOWN = 99 + } + + [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)] + public struct TF3_Transform + { + public long timestamp_sec; + public long timestamp_nsec; + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)] + public string frame_id; + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)] + public string child_frame_id; + public double translation_x; + public double translation_y; + public double translation_z; + public double rotation_x; + public double rotation_y; + public double rotation_z; + public double rotation_w; + } + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr tf3_buffer_create(int cache_time_sec); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf3_buffer_destroy(IntPtr buffer); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_set_transform( + IntPtr buffer, + ref TF3_Transform transform, + string authority, + [MarshalAs(UnmanagedType.I1)] bool is_static); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_lookup_transform( + IntPtr buffer, + string target_frame, + string source_frame, + long time_sec, + long time_nsec, + out TF3_Transform transform, + out TF3_ErrorCode error_code); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_lookup_transform_full( + IntPtr buffer, + string target_frame, + long target_time_sec, + long target_time_nsec, + string source_frame, + long source_time_sec, + long source_time_nsec, + string fixed_frame, + out TF3_Transform transform, + out TF3_ErrorCode error_code); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_can_transform( + IntPtr buffer, + string target_frame, + string source_frame, + long time_sec, + long time_nsec, + System.Text.StringBuilder error_msg, + int error_msg_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf3_clear(IntPtr buffer); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf3_get_current_time(out long sec, out long nsec); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len); + + [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr tf3_get_version(); + + /// Helper: create TF3_Transform for static transform (identity or given pose). + public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame, + double tx = 0, double ty = 0, double tz = 0, + double qx = 0, double qy = 0, double qz = 0, double qw = 1) + { + var t = new TF3_Transform(); + t.timestamp_sec = 0; + t.timestamp_nsec = 0; + t.frame_id = parentFrame ?? ""; + t.child_frame_id = childFrame ?? ""; + t.translation_x = tx; + t.translation_y = ty; + t.translation_z = tz; + t.rotation_x = qx; + t.rotation_y = qy; + t.rotation_z = qz; + t.rotation_w = qw; + return t; + } + } +} \ No newline at end of file diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 883057967807b2b92a894e6983b9260b4d2de280..fc78ee20b66d82a3a7862d280f54c7e69c8bd217 100755 GIT binary patch literal 300144 zcmeFa3w%_?`8R$N0!9UPQPI>(T{P6-1$RS$jY{1BfwQu~m4Hp9n1ozNB$qYWgrK5} z3COyQjn)gTw#HUkTD69HDc*xz61+85t$0C$ik@vmBB;a*@c%wDb53T5U5owo_xZo? z`*HPf&htGp&ph+YGtbOCGrPGi*Eb_MDQSo%K0~x0Ye^=7yGJ|+ex72X9?h*CjW-VM zRF-z|jg`cDaQvP>ns?34X)0qVJ$7(fy=VW5OJUySe5^cwGS8^oeZSzad6)AI!z*VW z&*%I)_U`xr&lft8-_3lKit>fN;ps!exvI>&QoNQbvXhK-t>xJcxjRHU^RDLW7WveB zy2xkVsSNt$?+ljyC*}D?KE0V2sNSa!=lspPS>7ecM|wHjk7#ynjwmltJMm_Myq4mQFoY zo9@45l~$}dLSs%#NqaT*bH+;wr;cj%yLF3S5=A z=yL_XX^X|Z3ildZwYaXtRfnq{R}h!{X#}wZSCas*!u=Xt*W$VkR|wZ~T-W2G&q`c3 z;97+%g6k$+ZMbg1MW0{dx((OuM2OEF;{Gdf|FyW^h5K*BvwFK5z&!%|ow(mC?rU&g zi|fz0?#J~PT=X$r);c^t#1Kt;823kU{SDW8fjxoy)3}}y^agR?DDGs7Tf}oW?ptv^ zhwJYGqg#)7eg*f}#PjR8Z^QK#uI&PQ2lscy^A2&R+xxh7N;ExwDCm#G-4J)W{R>wg zu3bdn^9ingTzhcs#q~L^FLBZ5D_jTYAqfVKYX~m$lOjD6HWc(QfgK^eOU3h%xJKZz z3H%t`kHa+@*YUVcz-4~Y@O%=klX3kJmjjpi86!QPCOy;p(?O5L^&?zo;L5-?4i|mS z#QiK>=inNTtEIBm*E{CwtM9yUP1=Vu<{fPQGVA0iSFPM#dsEJVA688?Bq8-dG3wo<@znT$FAO6 z{p)KAvhSQUfBUD4mprkgx%dyqEUMq~!T3j>UDN&Hw&HjGQ1yfS-iMFxop!~@FJH+S zR(R^dJ!|iB=U?^PD~~*Nbnlw8uPtf(9|Wx|mo?l9Jzy=1|{yWH*J3m-*;Oe|B;zuQyUmc$`X2tBg zuKDe=kG$MtU-#l&Z@xbImd7i9{BBj#RZpC7(rwx2|7lD6d8gm>!rQ;Sa9{5~w^sJ9 z7`T1n@+C=EKb}56bMKC~7G)Kjzb5Thcii~L=)l+eXWxC!HxJ(S((T?qop<`Wr+=D! z+^spU-FnT{!x}I6Q{cwRNtc{-n*YOR3Z5?tHZHnwQ+2KHl)LV~t=m80q;r=o`sCz? zUbuX6&HBZKfhk7MJMX+WeAuh|mL53q)4R^}f4KLlGRGe`ZuP%0Vr<&&lQL#r{lc+Z z)0R)Y^UkBj+x}Ae^UH>Q(ss+xjZI%4eWI3j)@xrpGj;!xdzPJiebTR9y8rgSydJ8V zpOt^rgoiwDw>&+6N7jR$=xZe&Y5^KUj12Ir~>Vmby6g`gI30o||^= zgUPR~Yky_F{?m)A%fAWU^Y#z-JXXB$u6y2huQKkw;Fh(&z5I=rjxTZNAAI+*IUk-_ zJ?+G&9CM~j`|8Bc&fT`t|I+;@FYbBpu{SOsv3L9Fvp)UJkGnQp+|b-LYDG?J(;t8M z;ooOGGpqWeu7`H5J890XpY3~n+9hX?bRN-m%5fiE_P5fXKk%z-HxK`5>B9$~{O#}0 z-@otCzl^x&&DZ<@u6JIBVyAH{?u9-kSg z>uGW7yJTqm^vBxb<7>{1kH46pUVlj-&+L)$`Ao@(k7p-HzdAws9|E_>sqdcA@%jHO zf&7JM$4~zT+G|uC{#z61^FRWGAnD zC(!@S1oiq;R(w9LiSh9r3HsOF3Ch(I*vT~s>f4Bkd%XVQJT^XlZG!Z_PSD?7@IvwU z9|t_1{T!7*{(NVAK0iRGj>o4A>&bX{dgf0Rd7mtKAr^j z)|sHaiWAt?jc3H?e`5muzn`F9BgVz&GdY2MdJ?o(cLKk+EdjnKfqxj0ApPr;vidodO4f&r$4OP5bC{ zk^V;^GpH4OrY4a8DUrVIRh}@Lv(tu;7y8-9@fM+n zo8TwMV5Z=?gab&%K2O45k)GWm?xqVoI9%vwF6Tq*NBZ;$`Q3{AoA8eG7PyEfoWN3J zZGR4zr|mP2n`_hs$8$Ysf{!_e@qj<^>Hj%Te**d^eNy3v3Af+EakJek;fD#=oV;G| z4CCoP$>8ON9_8utg&t0YKOsFhujc@UKlY(DGx6yV@|*U#5XlJl%;J1zi1bNr&Zk%K zDG+#yD7WJwp1wEpni=|}X(pKw^)!}uJpwTu2`>i;Xz@3tv;B@{yauJ;4Z;B`^% z8PGTJ4_wRvQ$NE*`}Q9fYv+Gq^Gx)|ojiTH(9e`p1)m>sV5yMvNKvj`l)FXX%PI8Iw9LlSw^eXC=ZN$-!w$)Q zdPQK`!Lw=CrSSAcKA!$zf!}sI$L)%LD><6$v;AK@;bNXmdqMQO{&^f2CdxhKOrE~; zagL`6+y!|_4?V&@vqibT7Ji^TnJ0W*)axu!ug)^gXQSX#2tPo4I)#6Ry4Z)}(u6yO zA6O&sCoxVD?wP_Fj1YXvMSl;Ff#GBJ_vg?+WIrLDC!8YqoDKbuK10hnZpwcN<{?a; z861C@v(x%c;Qb<1=@&O9NbeE$*1LlFZ!duhvTOHyM(=Ue#ixMi*i$ho%9TF zJSzCyJ%Y;_x{N1$Q~0a>=ujko$CVtvOQb(2>_-#*7u}zI&c{GN`sols#`gq%%NWil zkj@!UERQ~O(VpZViiDlR?Xb_g!f$j7{pbRJjrD5?CHxiGvQJ3ZRj89QzDmgXGz265 zsYh_bG5Mqky%jyr)0=)cUFg~WF2}=y&o2dkZ6n7|M?axYwZI+Ea9o(XwoQ!79x+~1 z44XceKy=bikqCS~A^1PS^m)A2ox9#y**Bes#RIP1qr-$v%(5&yzgre7IAj?^n_Z4Uf-VgIPKxp)~-n~?`?;@ket2Be6LLC zxxJV3spmH>SJ;U<@7*uPadqB%PNry=Xa#7qP?nEi-} z@M&TI`^@y8TX}jhY-krv5amwi@_fx-YX>n-(Rk+(_H6q9`-L9}DSqISGkCdeVm@j1 zuTIEC^7OyOE3{77^P6H`o0~=($a4-{VC4s`JTA=I4*sI+XNL(ck+=@%lb1_}{_e7-%ms5q?bQAxEU|{e~xe zT#Vyu?7UtMv9NCD?6gmX{dau9)0^|vQuKGyL)!|D?-2D(Mu#PRb{@+atP^~Ong01X zp5F8WFA4wP?BcjN&#{f<{ELb>{y4vB&eJ%q&i8&K+Ocg17i58u=cTi`JWhi%5X)Wd zB<4@-T8C&~m=pW#7U}z=JiXc9Z-jkPeFHaee6^_8DxsgyUXGjknKg#@qc&j=rk!tu ze9HOp$F{)j+=U$E#&NdlH=Ke&v2m!`^OwN_4ce7@A`$mT`B5& zEXIAZ6HQD&%z4^4!N>VYET0zz-u^kqO+B9leNesZ%Dm_aQ7^5Or#J1V73EXUN(Ey#D%rB^}$K?OTngk zQfXPRxUwoUb5U7MSzTpGVOd>WZCznWajzpOM~QnPaLh0L5#T2@}%P!%+V zudOMq3|7|GFjkk%j;ANrq{{k2Q&kYV0V3v306=oa&ESM83oB}?&~o0!+{}q)W>dq3 zl{IMLnv$~I!)1{rR34v0+~##<)(1-uyHR6PPp&SjuPv5BY+ zN~`>h)g(%O&^4j1p(a>aT^7UQ+ZarD@y-(06k-}7fjmNUu}#2CBR@Ey@X}1x`URU< znc_0}4x%~Z^?0@9SmmP?Yf1u3VH@Oc7Q#u@kiC^u2&Alr*&m}9H8m9m%IfNC$teYw z7B)H$ZF#k{7)MblJjF!QQJ#G;UM`&WN}FWU38p-j%tqxE{U{?<#8+ zc}rj!u9Ax4y24;xab>XHTaue!jDK>o3-by~>dN3dbLUi7Yt?1dOeL2geRg4Cu%fPZ zNnus(BGb$IXO$!tIKIy^l|Cr%zp?tmYRYf-gy;wps*3|&|EyRmQ=2Fk)pR!0R9@Lo zRu~A@d1vRl3gcQLCCoB?Z)sUoS+LBTKe;eBcXs}yLRTg^Oyn=Dg5M#<<_EI^#g%p5 zS@~Io`8i0IYa0Fkd4f^*uP&~ts->27WtY^}G?vu`Gp7Xt7njynmj&yV&Y9&lAOABW zb2(vUb!CC7VvHHp)i5G>t9dm`D$&E{l+`y>(=UcIas<0a+I;zH2yMdeTCZ{%#56RaZVF2U3&bGlX6*{;bvdrfg;VJ7F`ni!iZ zhSi|fE-oo)2w+%VI9XUb%bnwj2NVB675ZzdmM*HTkyIGh3?{~=8x-1=75j$j zKPbJgxW23|zofW^WELu@FRQ7?jFTzVX(brsm~COiYp_fCYb$Glm;*Pk=83}$hHj=9 zZLqeawu*PVoD!NCA0qnUr2RI&6K7+Re^^{&Q+_MY+|osd$ulP9xAB~GaUJ~8 zVF{`v7^gB_SSk%7h2U2NuDt(FyAUROpw9pmt%#8Fyc#b8(bE$jwWgF!OZ>mKS$XHR4f-Hllk7Ea~0xsd*l6{l^%ATlv#o0%=oOpB4hPgiuNl#Og+W3Y1f2lwKc)I+A3Dr!U|_cCJa1#W6=VXVye^=MC*HoU;%snXlai0>GQx%q%#&MP)$>m|#v_Xs$GB z>vQK|-M~DY%c=|9B35G62rKzf%phDt0}fVm!l7C({?D$vYZB@nQ*WlN-290!`XF4G zXu-lt^n+p=AxN1Eb1$CD^^%!An1XAs7^{W4vW3M}6na{iKu3ti&B-SY@q>o3L5go* zM9#c4L40gtuomm;!Qhk`1yllsxw%<1xrHqMQ$CZ~B)O?j3t&Nxbw*)zL$Is~>++Jt z{{`+`%2?J7HJmvMjVx*?t}87pEd-B?mHy8)=bB^&9nDzd zf1#prnf@1KohX8K|4mWj@RVmma+r_;&iTP?bfap-%!6f#L*KF19}}ONGmBiC8JsFD zV^M4ozq8uH4BxU?+gxvKY$bl&1hOGv{6JtX%%^lI!u5zQl@!;*nhvYi#>kDh8U$CP zYQ!l&=&r1-XO1(=TT>aFCT7bjE`p3?2NQzj6SC$aco3*5(I4I(k5I#0yE4|DQ{wL8qF{$Q3mdobm$F|ZT5irz5 zZOOQ4E}e~$k7A3(3o9F4@ZW_cRmJu7g~6qPGCr>*PD*M2CX`8PR0}_xEU~iC8lk@O zyNDA%Q2A}rd`ph_RgUdyjJ1+5)+XW=9xufq_ChK&ZBuNJ#c4n0vjYuvWrdBEb-{*W z+5l0r9iHanZ4+cBR?lp7&v=^vEF^g7HjOn>@;yG+#QH#CNiB9X%1T*Jo?Kdnz+LT9 z-g|^BBCT6WTUi~bDh!l*{j>86CzMuIxw7gJ{05V%%7t~sbxSd8!?xW72AR#{R_vC99QVny6# z&>sG~waV2>t4gX6JN@6QfN<^qEA}v5>__~sFrQpnSs#FE_=s9j<1Q3U>~b!tD-M)3 zm6Qd77(WZmt{t;Mh;0R4bF1kNGaK_Zrq{^krO+-id4-Ca$(jF|6yH&FzPtGyGsU#@ zVb)f(;D1yz-uTG+om(b$8|yGdorsQt=><*U2HSL!)j;qGbo9aHWXYIEq&CA`N0~)T z%Os!0)7GB{Zht*Q)1iH0t0UL7(L95POd#TXwa%`GW&Cn($8&NJgr zR^(mmT_sK^^rdoi#ot>>#5YlLvo%=6^bqr$?8nj{M}DRk2a7Lm2nHI0CVw7cJ)8_? z3G6@-%?!*o&yJlS6w(aBjfhMwf)m&q$2lJy^C`!%9lrEJP$^i!8bX|2Ipn3A5+8CF z>5ybDR{#@}8tfkv!$xgMNj=-lFI>X+;_1lBV0#~uz4THtESKo%3Dyfs(2-+y|Cdk% zZpk6L|CU)DQq1HM1ffbA#j1r5g>oXQ1)}r(Z~+ptJ!C|>mtamc2kt3>UUu-;-8 z>RnReaupUYo#bhr|+tCa#n_d@Iqi$I==gNhs)<`QYhG_;< zi>(UW*a|s*dzhA#$wD;sORK9YYZf!haTaoOq1mW8kjJo(t9X;&q36vwH=Hf_j~GrO zH8vmw$A?@}^sF|U*yd1kag^Ox2L}Ze; zcbS$!rwjOQ`kYyl_yg(myW0sJU%@K0a#0OgOcv_~+!?TS{Dhj?CFoNacS>sOsRWlh zW_P_ISXqSw0yG25oIbafBq6n!8(lHmF^6pbORAA;3N_KOg@mPLQ)&4;5ic@xdcr~z z9A1!$77HR)T$~-{L${|*pBZmgHco2cFyTQa73Rk-#=O4SI3u*Erh&o&uFOIo;*o`c z+N#QurMa0{Nbyr2EGURZkYuhzIHppZ?ko(zn1bcp&9P-_T^T!eBxj&;ls8;?ZEcX{ zkYBTDOt4mlN=cSjh^2z<7FRB+2+}(YBQS zGSzoDlq3~L{TEhfZGRx>^l!FHl2N4y*?TP|9> zluG{gw=r50nN>we6_-*BmLE@a;vorf7)j!4fY{)KPqADHGMhzJwb;3d`$~jbluTrU zl04K;(fXK2@H2@EF>a%*+7enSAu{fcc~MtaytJ^U3`ZB)seu^hLyY6M6TKN!l3W8t z82Od(J%wz6CBG0JNW8E{cys)ZS;+I{8lT09I0K(%#)be9!nP)RVh%QutHW9$eoL$% zw{gxa+FWB!mg$asnw1lJv|_P`xmv|Uj4|W>nm@C-fXq}Z@a35rW=Mfr?1!?+6pelH zr$yE=k9dpQ|6sDf=8Z6-*nm59FA5SWbyXKHMptI>LQ-ek_~AtfRs@(qx(=CEVi`&P#n#iS^rx`JuXTeK$G6d64 z>KJ14)eA{^V6#yktAiBTw8FQ=W%o1n$E#WcC#YQq60r*dJgd@m`jwsGeCW9jluuHUob)q3wHCa-; z2hWr?zGf0DhTg>2OCgNaYWw(V@hyaMDx1v16110K=_Ed17oE7M!IVZc15O)P%a+WK zx|8h*Q*-;P8gTI6hjn0ConBUjL;mJQVd507iTHL>sgG|Z(2-Z>!@m0>7wbtZ6D4V! zbhuE*L5Hfc*zZ0ZrbFiA$OOI&Vj+&h&B#U;p3w?u999H+vY4*3NYmVWn3}6Fj~!27 z_WJEh1UBY;_bgP(cU_H`Mg1rFterc_gUh>8G_Y7>r{xIY<_}3^a#CXM=2p4_o2gy@ zHUeywei%tDu7|h9z$&NgxFQ_L(F8@q zAe-B=?^g*|g1|QGX!ydS85?;YFlGAa17I1)BP<*T&J1+G| zJD8{>5Qw3v90B0GQOQ|n5$8#izS??PRD!rVQ7*zuESw5t(HS7x%xTjKCqSH;KJV0N z1qFD{8t-yzc+8pVEp&~aFg`oxU1lula!E;J6S`!QKNn_>&m2F|dY+JJJ-V{2#|eXv zgHI4kJGTPg-8h#it+J&4TzuoIvEtl7&Elp)RwHavoV&20va0mFa~taG&Sh(db5Zg0 z=!>k$_yzhDEeXF7(EJe1vs33MpZ5T1(nN3d(Cb|ijr?r3c|OPR#-CxJd1 zPsC>g{!KxOBk(*F*GTqcWBiWBzek81l$Pf{0vM&HH+I|!CrLOzdZu(lk&NH}_kV*1 z$i7IN-ICf3y_ez8$}uF;W|W_xK3}?N&w)*U|+2Fr$ywP8alXm6iBL_)*$; zL394FYe5HpK;-bzk-|c3@ErJ{?3dh{=SQ8 z4}Kw!UBNrd-#t?BULlV|!Ha}E847O7<5Y0FkjJg!LLObgyM;V{1@95^6exH=$Wx@? zraToYF60R)c+VfW{zD4hE96~dVf_sEKy$bFZ^7JdXJ(d3u(tv`u3wgBt{D&`0J8T#7q$;>6 zk6pq2LY_1Qw+neP6g)%7<5ciAA&*lDY!?-Q=#A? zAx}WTJB2(=3U101Qt*I~XO)7t33*m4c!7|oUBLrFp0x_zBjo8&a8sW33LX;jbSk)C z$kU_X0U^&e1uqiv^eT9-kjGGPQ=WbWZxiwiD7an7lX@W5K355O>;HEsQ6}(f()2`rtAy0>b_Xv5`E4V|*)2ZS@o^BNv?bxH>Jwl#s z3XVUC#U@OKf*V4feg(G+c?J~RDdf=(#@eB&pHv0!74p~>yytE%k3+$=Be{RhP;nuT zQ^7q#9=C#<@^}=yU&x~?c$<)?K*7_5JVgrLe=C>2Lc#q)o`8az@-!*9CVn_Cq~Lxb zPn&`}g*>YjyjRH6uHZ#Np0x^Y%G06Xb|KGt6&Lb!E4VJ?=~3_=AJtmh0dA3v}jqT_og5#hQmcCf+IJu`75;$djhvraTS>cM5qj6ueE$$J`3u zB;@fZc!!WjSMW9=k6*z}c?uNVBjhPkaKDfzpx|vno+brv7xIKuT*$LZ!A*JE6x=W5 zS*_r9A$G8DW=$m3LSQy#a1hlD&H1@{Yi z{0iPLAf}8TBDR`%l$D!bUA&*nVg*eg!w>8BlQTPR>Ug5^JA+A&*_b zJA^!G3f?Q^aVU6^kS9aIO?jLOZWr>nRb0rUD|ok%$FJZ$LY@Kz4+wdR6x@`jLdAtV z0R`_7@r00q_X>GdDR`%lr%k~_LY~zMZpzcH;7%dWS_SV}%iD3if)5CJIu*P_$kVOh zZ9<+N1vll{rr;hSPp^Wv33>Vz+%A5Pc0j?~g*=-0y>)ZEZWr>TD!3_+UBUfAo-`E~ z@?n)6w&;JpIx zQSi_;JiVddj%6Izgg-Fr+bQrg1uwdmr*|s2b{)rc1#cI4k%D_dJiS}_U4E197Jh@g z#Sd`N9x8pz->Fw|^Y3PQ)pgyW~yEqVGA;O5^+BK~rE z^Y4nOxcT??RNVY~Br0z1ud2AY&#B_(exr(;`+h2J?!T$Hxeun|=6;ll+l8M|@r(qx zCjnlN01qUCG z0PjzLrwTu#>cNo!cPGI83Gj*pcqjqBIsx910PjwK_a?vx65!^%Sk;3$KUHyao~YvH zd`-oBCI9nB$zK@~{)mKY;_uuL9~t)~zy~BgRT3Y&ls=i5iBFn@<4@UGp9&jahvHC- zCgT}W`r{=&P6_|1#YNL}37;q76$$W=7FsS4n(&q;lIN{7DJ#knqPPe7%Hkmhc`4|A&Nclkl@7`|OqQFQoJX68@NkYf^o; zNO+or-!0)iBM1A7j603@QB+5`U+J50Ut|CEO>a_el6s3D+e&P2yun z^2@kiN^ky6Gh$aD;hQA>6%u}}ggeDLj*67=fRuhfO5Y^mDA_;Gn@FoerT*BKD;A<1$oeA)5 z3Gn^|c&aoHl;v?Gz}*ROe*(NB0Uk<#uTFq>B*429;Jpd(fdsf+ng_~y$Vh;D65s_1 z@IV55RRX*{0lq!~-je_~65yIN-;?W`mH>Aq!1V-pQ3AXv0p6AXUz-5$On`4ofcGcB zQ>A__>%ox#cPGI83Gj*pc(;WAMe+j!32=wx&*k(5(zq$(JrW;>ApE8*pm zJe?AM88@W#FG=Y&NzPRgo+{xVOL&HakCo(cO8BEvdXI#^BH_A(ACPdrgx@IP1rpvT z;YAYuGYLZ2PFJD32&0{ z6D9s33CEv;v_7jOyv{;3txdxJF5#;se368=OZc%;xoagnMZ&u!{33}@kA$BprQasu z!z8>{!skkS3<C7hSUUwR~5{7FdmLYMIOB|d%$|3JbEBz(Sv7fJYD39pcFd@;xR z1SI@(3&omI!iPzCNW!0!@Kq8{IyFCS68^ji;=WqK{~+P*5^l!I>GfI(#}{?1PltpT zTd1b3m+*Hayi>xDlJIT`A1>iN5{@qrS)XkZKGH%ptyjWr5^hNNdI|5BaC5CduLmT2 zl$2f@HE6fTNO-D*zaZgu2|rfC(oOkCt#< z!jG45zl5J4;RO<2CgDXAo+jZH5`LnD2P7PSTGskBN%%4g)wGa=pDf|4B>WT!Z;ipM>w}hWA;XM+bF5%lGe5{1` zO87RP2yj@D~knpo4JVV0I zmT;$pZ;)`egr6hf9tj^W;ktxxlW@O;pDW=767H1nA_>1u!Yd>^Q^Erheo(@jB)mn! zLlXWYNuR4EJS?Selkf==zFNZnDdFuBK1ssYO88m{?~w2$NuKo*K3PiNDdBDj@0Rd? zNqCQhH%Rz43D1`BUI{->!VL*OU&8w({1{380SPaa(rd>K+W!R-o+{x}B-}3HKbG(` z34cSv9TI+_gl9!=-v{lkkv~zE{Hi5^hNN7>Q56gkK`1ACT~yBwQ2od75MX zTEbH${5=V`OL)G7r%CvJ33o{NPb54;!V4taDd7VW{{7?k8u(rV-)rD|4ScVG?=|qf z2ENz8_Zs+K1K(@l{}m1Fwf*obz4_x*Ju>v=!emWf(HTrW*rPXZO5Mn+cyQu-fcG36 z`#x^AQ#DFoL2vd%cN{!;u#M5Q4%`!c*`kvfy}_a>)UhYJ&Z1KoeUC-cM#i4#%@$1? z6?>w~Et(dId!kobbSk5ZEt)nG_CznSXxb3i6V0(`+KAi}by+lR4D5-Hv1r;5*b^OP z(IXk1WYM$%uqXQI0kggo;79!}ngaW%zeQ6(e@}FSMN_~Y^|xpWr0c4rXcwbj zwrC2>qW%_50aLWUMQ1Vk9*dsH=$kE?0;Z_HMN{At^|xpWcVY{VkdTc&NWc zQ{WEvZ_yN(L;G9w3`Q@vXbRAw{uWIEIMm;wy^OxZqAy}}jzv>I4fVHZAEU=uGzHdB ze~YGo8tQM+6i7q;zcTAj0Wq|{MN_~F{okVJFnWVUQ@{%Kx9GWyzQ>~HG5ThUzLe3+ zEt&#YsJ}&1;0pD(=$|tB5{tf!(K!}9pV2Oh{u!gkSo8u$kFw~?8J%R&g^d36OSAq( zjNW0<#f*O0q8BoHgGHAxdYwg=GWs5iE@Sk~7G2KhBMOQNV5{tfq z(K!~qn9(kau442Ui>_w$D2uLPbdp8aGWyd2v;F}_@381A8U32q8k~#+@hB-`bvv#Vsx=ZFJ<&47JU_?b1eF5M!PKf8b*(?=w*x^Wzp9% zI?1B1WAvx{%=(8Iy~CoHGx}wVzMjz=EV`M|>r5K6i4~X9Yyy6O<`1A-e-cZtQ0Qd} z-JsCb3SFkq3lw^uLgy)Tu0l^y=!pt_jzW)B=u;GWv_g+i=%EU|U(wg+3cX9AcPjLD zg?>$;UsUML3jLHqKcdhNDD)bI{*6N4rqHblt=h*j<$Z%fS1WXxLN8G0c?zAU(76gd zMWH7u^f?MWR-sQ(=+O#2LZOE$^!|CV{o`|m-lfnx6?(fuzoyVHD)eTBeoCPqQRoL0 zdW}N=Mxk$0=vIYZq0q||xq{_ib79R=yMc$tU{ln(4!T4ghCHh=>7Rh`z!P=h2E*q+ZFmX zg?>??H!Jj03jK&eKcLWS6#6#`eVanJD)b74UZ&6uChd(}lkW6p9}JH4My98G^wvO< z(T#~a;dXEK0m6gn4!wCxlF)HF|_icFZ!~50ZeFbH0(#IXpR~!ru zHE8`o+57|-S@2~EN}DQ5yNGdnGChPzkT>#hdK>Om=2H@69x5`AKxSX~BV!)sP9*w% ztVxM}4R@k<(Vb-SW$zA-)S-eRrh?6wyA$s8W$)uSSIq;!DgRD-kNgj?{7D!ev|zu-K7DtOwH!SnnJ(iG+9ujY)cvVCFWKBY)pI<0t@q&6LJrrrC#?c;Dm1h0gfus4C-OCxROMb-bsy zJrKi}{SklLOCr609CHc*gs-=?j))ua}>@KCmnUpI0 zu5q!#Ycti#;zjF^zQ0l{rNO`&F7t)=8T--BB*;a&58NNM7TcJQE41S5)$9YD@MfL-6+#=D;by$t#_ zUZW>Jo>=@ZyipIoX3+Y$?|KPvFe4-(qK$}!8SRwmXkyy4kI&ro@MhyHYCwp*kfq*j zoX15s>X16ubw_&Cp?xPZlsDyx3LV%TUCrTiVH;WRU zfaY>tUl^xT3BRHe{zSS=rzW*LOgZWEPi8d+=w0~TzKvvEWR%nynT66fVpu5?sUCRS zPK`z)Q75#sB6pLLa;c0C@QL1e07XtBavj!RGRJ3FpD;PTkIa4d6V$;B0fyc~&>y%| zn~jCw*4Lg;v%lws8xiP_Y<1&as`m%RStP{EID%)a17e&FABt7H+;$Xr zb`vm7;(3PP$+U8X$o4x*HG*e*;AmvKnSkL^wthTQ6<+0{z|7A<&!f(DJKp%h2aTK9 zeV=i>;5nOEJO(hDgF|W5zqrW5AadVE<6L_EXXw&Y66L27wotza3_)MQh++@kOWhzu zB>DTTVBUALv8NxItI6V^81e)1IJkxh0NM5#H$tPN>t`wbIV8*HIQVClc(N$5m#XnD zlJ-4s@@sIh+U+xR!68H(`cPste^io28OEameQz6!=*4?v3zi2-Cn29POL>&UM7^Fo zg!jkbK+>fXi(e4$Gw^jE;yraY<9#%}SQ*2+o5ZwuUzU(}o8a9IPR0b{ef4P0yAcHB zevn?A8N<6OhW7z_M{*l2l=L8J7238NpYMPdgplWmBFgA>4^l^ecfz1@pAp%6ja+)) zX=Q)(9iIIYRE}!@BYJ%^WpDn$V0O`8`+ALG^nSUOeR3~l-)B51vVZgmUeBTI$@ng! zsq`GH^e%co)yh5~N)L$aYw7i|lzrkUgV~eK>{MfnTf)Aa%m#B&Z&rWCVz*HS7Bs57 zh(e+d{1Ck-pP0S`ZJOfzmCA=M*3HqANkwpG7K}u5aM^O#BbrZ z@mnMKwF-WlcX9d7A%1zMf!{+wnEx^5JLcQ?%@h2l2!0KM-*yNTJtZCdN{HV(P>ad$ zS-fKPCEqlZ<$KqdLaEl`4SbUa=hBU{3HYOwEh4fp-}DkHP4+R2QeDZj{oo8_`}|`7 z0Vx~#TPlrZyF+CAJEc00XWK*Bo+cn$%65vJtw>~BO{s?PY|m#P+YJOHOW8ievsKzS zk*%0gy?{5U+r9Yh3Bymo^U$2Bk_QK~9W)lAPNbTD;ZAit4XLT=-+U1hgcV2`Js)3e zGma)ED&qOP|+_+ve?4BkK2PHaZ#5j+r=r>W59 z&Cb5JjQ9VAaxaIOnaZ9D&d_W03YyxI|Is7&r#n$AU*uj^OJDZR;A!5-1?gVM#~8CU zCg)rV&fext&b~L|E5+9GF_v!3o}=&4Qwf#-4$~7Kjv9)^;25TcWjeYJPvp1uu~vH5 zIG6JL8lpurCk#5CFz+jUy@MLRAU*h0jE**gQ{SHlhnq);{somnBcW}@4K!2XWAlwD zgiPgncHd`QB?|v*6osEch2J)b8tTb(Hk-vFk(6XL{d^|9SV6M&9hNjLd?86qa6XlM zw^2&m-i6Xk^>5_8qVJADA?-*SEk|?ck@J1o2W>0h>mlWC;|ydLP2Fc}!59T`mO1oD zgVPe@Rb~zGY~XQv)4bW;_pDj8F07S)Z-o}U%%n|Ijxa{Tl<|_8o%ymxyu|pzrXCr5 z-p5wWRN2|=JsJMJADNAMCiF`EccB3dya)|Me|R3cTt1O6p~j|Ee4qR9j(=4adZt0NSwwC@ z+n9FnXU2}%!O+>~Cu`-l+Vphr?`mP8BRcD{r6v7V+>-R>PCK4y0f>HrDKOoUZY}Qg zv5mSneKqdJrBE34;1CF!sj$^b8xd5;)l9NJ@_Y1CI^+j6+`ZW!+ggZKeBVm;jDF?l zdz~crhBuK87C;BS@J>=g29KIOkE-#8HD^d%v8VGw>6M4t+z-!fYiSb7_cOr`LhNW6 zAQ}ls|CHLz`MyHrie15{sn~K9()Y0C4`aG*Lnh(9tg^x{Wp_2C(`?E{vY~5seZZTR z{B5*$23lnWO726XMRH_y1Z}0d=FIbk_dgQ=$DFzIaCpk@i;Vr*dC3}$6CQ8t+;n@n zZmW5g{^>5aRc)}1OG!6=i86fQPY^M|QL6*lIBM0pER`Bi59eyeSiH4m9G9*~MxQA- zRiI*eWa2}Vv@6$-(&%0w?nQL(%B_HMP=#FGHfo+`TnAH=8pDr%K$`0oaL!zB_!}no zJijr7$@8He?xG-015@tkzffV3i5Joh`Z+h2pCSqwt^MPixxUu1i%|{<@(LG(&a@ftGP9pz=fe2H z?-?VwAio83AxIO-nk)&Tw+<};3nbRNhMtBe^aKbprJm#o4;@bITLV3KIRcOL!Q)Zl zv1@25dkS3$aOmLsWb+Z{>L^yS5>=u4UJh-o=rqlK=*5#+omX9L)p^SMu*l2M#%86d zd8wMaP+j8i4Qse5=P>C$SpbDEydYVNGJ7)j*D3weBK-?I{eCJb%+hC4`p;1g)2H4# zB^v;=K)9Yg5F8B!{g#rWVU9f5YdbYWmEI`sNPi?tKQWnZaC+f>eZwag>Kg`z=t*1j zmk$Jw1A}FPLF&QYa#oI+pXw8u@*4b{)^HX&)08VFQPelQGdLVdza6--^<8+#Qs`jI z&<8-nRB#Sa&L3UE6vX6WHa;(3k6dWvK*;DPuCBT1m1}}8&;__m$q;GgXMNj%V=)?2`jxWjRh0^rM)v3nocwkw;YbzD$ zu#Gxe&)!_O)p(L#bzw9nrO`tMJy5_5HNg1?VniwL?t7KdIX}s{)EhpKGY_sMUk~qv z9jED$@$huE78Wx{n`iW*U$+(|QQsbpYUtq&B;E>E=E3?NNo}J2aKO%BJAmzR}ZthuD?jq!dIwAOlB6qEnJ3nsj5oYfD zLQs=0av$w1fKQrt+%9-*eZ#?F-te^<4GwSD3L;eJwR&VroFa|$<#_S*1K zin04WC_g*{I@m-CdgY?l>E}QR)6-L<5#+U<>R~~OZSRmunxM~v#*@f`UqSRHh zt?=UovnS-EbE8N!=&y5rLN@Pfz0Tw6%(>Lpy2Oz)&)51OshV{keR&tn4SeC~Ja6m$ z>D$nHrYUZj*`{^fKvN_n(6-F%fcKk;9R@T+*9|mPrjIYYBm|iF;rY6DJg=o^+o+kH zxO#B)0_!JCZ~kaNANS?*4Gv`95FN5tzt7u>;hI@Y)Qias6;B^}?<t<$?||)1h?Z zzGTnq@yz)!vKu647#WrmPDzi+|J=?Ig{G8re)uf|qdU&v!sNZo^zgtttj{aT! zcpCC3y^=?v{+F|!Cf<7Mh*Pdi(xS&${m#ON68;>vK3$!Z9@;*lC!+oDKtgt%JH~*AWmB-O`~&hWR2j zm|ND`!$-jK(cRMMRd^=ZKd2&3rJ^r7ad~k0$+5lg2AokiN>+~^KufSczrzigltOJY zJ>5x-?cmRDdKNbAp=V*!Iz0=UW_Iu4>6zW@^A~Wch`%P7@V@zpSDIrq1 zPUYNza->&Mfvh~n%=EaWdL(6R2IVBDg{|GIUMF{s`jUn{ymnlh8MKm3E!EHB85)k} zgJUmM(irjfv=kq4+ozQ`J8z~O0_r{rE>8R9j4e!%2iIQeg%^z39XtKbC0k8_^BWOPLPx9_qyX8g7o%&3pG+tWRp+RJ}{wd{@5L(yA1R@G|Q*2a<$n}Zb~|4Fdko|Oebq$1?M_p1E9z( z+-QC$o*gg&vSqE|RFWXW(pLlZMmN_Lx|Ro3{m5{_7m5N@`o7v$1EB(=-N}r1JR~>gf%?<_-VTmI+5HV~`RJHQ?>9MLY#w53 z@xT=F!k>7@ZSgiAND8|3*5hEY?-A!?^`z%~@V}93;f5RGW|Q^sRjGQo4v%~E=JyWh zNj=`|f7)8UIe@yfrXXK$GFfXBe$m$4EBO468pc^|G^ZZ`GfwC{8M-u%1Gq@Ua|Zi^_u~7 zb)s;a`Usd#NuYKfK3g8a1BsCT;J_Z1Y9x9ll5j z&|D22Je4=hXI8`H+8fegh;I1SG%q%mR?-~f>QpU=BM0Xj@1Q04dW`hkMh-BXn=0C+ zo9Xxq!}|{;dP0mxG=r)TqKK|-Yx$aW4-dNcHr~De2{F*y(7j)vC#!o$K_%+m=mOKR zruxqP`&tx^*6@z&@-`nl{Qiw^iHiRHx~=6rR)KHr-zOjm>)+fD>52RIJLun0HgRVC zTbd1-^A%VpBCa$o`fOf)!ruKWQe9+$J1^3}r-3<`uh3F24G&rLa55g|=7g5{HKMXW z73A1%gN+gC#)~Fx(3;sBKTMg7(*fRSiG2t_q`)$}xMjgIYrT=`qCE6;O68$ch*Xfp z>ka9q!D2u|m%+T|ZWu!Jr(~y={nGqHhrtj-kJ-WJ`@;XC=p3INM+g2s*k3Vs7-F0t zGGGKJ+qSh(0L+{8PT#mTvg-)O+2)OK_nXtgx&5|Br+v06_p`{ffpG4CFMF@8#f#LY zS^u5P`UuF?x0%_K><* ztiXk{(iyAHqq$U{yf7%$KYHRx_47kmvAGgSRl!(YBUt@jVs%4YRyl{TatNu`t|CQJ z9VZHI^CWKBak=e<7MQwzMk;!k>+JDyKl{bApJ)8+7m?<*tZimzglAx)wM~!Q*+D=g zw4UK9>EvsKTctT>lOCSoA=_E8jS_UtU?Z21@8^Ph^V99;8b^P1y&hTJ&AIi&052`YFq6#+TlP^vwnEls zO&e`1l8K*bLd!RiwVK@yYyJGFRg%&kd?EiP;++rQpF!v!gu*Rg+7IEY3i~BZh@$c#V2Bnqm9a!TcMT z!f7WiHvdxOhZE7U>SIglSw+6c;=n3eKt!fh*v4VLOcoWuvd)*DL8E+2IoOkDyH;Yq zkg;cDF(Rw9dV{+&P9{5u+(&^QmJ>&XSkCEE&J1A*!ahXK_fU0o2#T$~NdJ7PNA6?B z9*Ho`tv~6x#TgIY^W;hdT>q}aJEx)y1PfoZjq{=THteSDCXZ;sk2(=?$T!Qwc0^!14KT8iZ4O?Qn!G!L^X9f zwT^5~$|m&-ONY@*(MUoLJ@Sx;Y$4vFqfZwCqW+(gR(@V}pZ{{RoSGVb2+ zNZ)U3~0wX#BhcOoHymm27| z9mq*X?$N(HsfRkyb9<=gnq4^q54~pRMxUmx)(q<*Cow=|s=gdbEg)T-&t0aiXUcmrKa$7unk z+}N&u6If7JM+f(>;#y(7@d$|X*xbU{sQuXo4&)wKw!1~PB~rHeESohM5S@86 zvQcNY*?->#B~yQX4a9xzg7_3Zg5*VC{-j5*C^DTH%s@f}?1~v3ImQ$7SO5}u$wnv-$g;d)~&X;j01m>U0dchO4 z2Ld7Ag!agMBF!4%<96G|-Id-B09JbE0PfH~`vOOJ0KMWaX25KChQD#xR^U4T;KKF? zBlkJL0TQ!`*fUnx!5p_}U(8%EVJn{&Jk*Ix(TS;AA2Y>b5MgUaTZ@l6_c$!~jZ`c> z9JZBZsK2*)f10gjK8-2Z`oP*~Ko4UA`jXzf;{a_JBz>xH*nyD3m*s6}l#3`Dc%~Up z!1L68+o%de6LdrsJcuZCH{ehlHv;#5l-ABLh8p!`pR)lhXR2F%j?aMjj!5IjeQ}2eVp_ zE5(+hw{^IWgvQ<=4H7T;T7#oC<1>W*F%;ZyjuPWR@ULf~PK`85JZX*+*J6}-zu^yb ze21)>MhhxDdOv&%_79`8;P|cbFbl2Wy=WJrlm3 z9`K2-q{hYOm%~MqAWNN4JT^6+ZRm?mVk0Mxw}cIE(k`(-;vqZ2Dy0*C^4T=os5%2S zFm(WXR}DL&_kM{r%GT%s97M4`%>tQ- za(_*yL9hZ#HO@ggY}wn5bpDWLoWvg-#xeXM!#I*Z6c`$RKoop;CxSQyR97>Ai4YI= zP7vIrvUbpm2m(OV^SVI5C5Z9y;?}cEy&HB8^(O6yz2TJ?2UU$fY=m(~mf4Np&_h?Q z9WUsO2ha4V87(}8Zd{FryhyGeLi__wKM(${GwAhV=}PY3X`N4C_KInXFA2w}w!p-U zPv4)F%w4Ljj5Q0K2%A*l0Ti5J!RPyE4QwDX4y z;|~7dH*Vq&1;z?IpvGH~p$TCcrXSp?Y|S<*&w@D(V1YPY2wK7%+yJDY_KkK%ZFJXF^a(J+Lvf_(i z3f4J((%=nSf!B|`s5RwJdUj86s25{n0SOf#87pXqdt47IJ-mfBlCQ_nQS70AZo8hQ zXl!mk5hXOFn^&c^E`?>n_%V=bTlqVr8jRH43JP->3_%oxAKYK3?RDFz;q0iUj-B=0 zG+K*Tdyp!aa=nhDCvf~LNa+Y2r!4no?{6G~SJN|T?4l6#bXN+h)Hs}h6hNQuN@erA zJGg{8Nm!vP64Jy$w_=IM5EUyS#yQ#^w~ab(P(JpR)8219*b7J`AHFtysC+O|MLyUy z!m840ocuoK0Dr~|!PasoOqrS_z<&448jm0$ouH`D$89k}=q=HDq(i@eZZTF8dIh0b z42AsYvS@YNieUh#4_}M{?nw5sh4hKEw**^C;4p%6gH`{zU*3owI^U#8XRp2pNs8$JGA4=4sx) z9T+dtFl^8A;GcZa2Wgvlnjf9G0y7@;bflrfbaT^TmbbF0Eht~QC<-%#X+@Oo0xy$%R@fdLo=hcEx>dvBU1%Dzx#0uaM2NLWtdGGSJ8**icC$@qbxQLI?q4z8 zbWmIRT5Fxc$A&+rt;Ezp|4I%Mmj{>MhaED6n(Wvw>k@}A=p+U|fdS{52eWN?gsndI z8S~atZ|Ee`$6nf}?9h=5UY1-Hb3aafaOxjvz$}*hDDz&3M#Hh#ZkPjBw(C=95)U_s zFT~_waja``%D9TBa^&G$tv=<&5? z(LN}A^TFUEYWh>r^rSVc26SKeOV<~^BpjLkNviS16QnwzdSqS(Kwr`xJ#uk|FESGb zT!&`q_Kkx!F!G=VJTNeBi{9MjFor|vJky@&#pwUJbWUK(lgpB{zSZQv=$NO@wnHKF zMUiVV93(hvKPLSRQ;{i-p_%i4_`lDo4*Ix$ROi_|j5jTt;D>zSy-Z)2WjW{wDo(%F zJ8_)W_!^c$2Toh@Z4Z36X(l?Coo!Ij$^Cc8Y_hjEY$Z>C!M4E}UBa9n`PoKX%Q649 zwe-PTQIB&+ci@SagFPFnrCKxV)3KLuYnxVyp6&*c1s`VMn3y#Kgk`VY3nX5t$wGm8fM(UzHhoMLLi zu#P^|HPgv7)-{uUHxCvI52kb~q!Du#alDEAub9EO(HV#_Y%*{sbY@6Wn zBDpwfL80Bp%vCn)e&jNpk=&Eno)J!sJ97~G^ouQ}xj5WFFADI&PcNEq8Woz#;N2v} z>L!a>H`zrMppH-9-++p0y{O$=wks~;i0f$V^_$Av7zl6Z`BlM(xSQMX3RUn0jI`ff`N2hZS3t{+a zu^yJ$Awr<2M{-{FC$^RY%-H*^{xE3TL1EfX_T7&mkD^m#?`^y}HQw z5E|;ch0P~Gr(z^9O8!6g-UYtSqT2gUXiFm$cgi&?mjEdw0veD?a{$@th3*YI61f(+ z1u0MzBtSyzL1@|}wA<}Iose2M$$krE*DcNx@er_ksuofj$r6QW0qdZ2sTh zntApuDbRENpa185KmT`s+B|bzGqYyRnl)?I%s`=bme0-io^CHUY_->h3v5_Kbo<74 zpxY0j+a7I{B^zFdb(_3>O+?4nGZz~^^0M_k9bh0bB+<^;LUm#K;F_6e0dp2wbKtVs z>6QJ{D~TJ6vmWZM=G)v489Taeg|5FqaGYSQ1EyC1h;_;ITK_dtCccHRSuSirG=Im& zFX$l1#MAYoMbB>sRlhTO_7b^vrr!g3(oLo^vzTYzWS;R!-~6p3eR{c8KuI=6Q3&A%Rk))F&t5odw3|jOt zu!O88&_5aPk=oLjPpDKO%)RXEzh~u7180P$n<|ty&XzJ$ zH4FaGV%Z5xq>m>!YC`{5(!&vTCKJIr*0oyA$(}%K5&8`7r4DX2aLL?MtMo0vtv9&m zdAQAj3()ee{SjhG&5i9AiDEI2}(-pHSOB zdKVJYhHxLm0`V&tE=I)Xx5IyGf#2iL08I2yClx^bcZ2#~@Q>To668Rj(%UCgL{RI) zIe>8VYj6+PQE=abujT16J&l?aVBE;hk#PF$@G1bf`Cs-hg*`5^rU%*4Oi67T>Lx12 zo%S;03&-Z_6MoAYX5lR^+*%+0)1_y~#=6>Yt6Qst$?(uZ{i01k2b|hQy$8@D_Bek^ z{snpeVe(JU`}dcBocs*h@M0Pq%_s1~a7lrm{){u=W8Hrsbu{S1U8zFwSMVC~`^Aiv z$o)L-9R3s-%>kxPOk{6gL@P7_Oft6RZof*P@)eIRm<+mVYDr-8L6sA2;OQ0c{7HOk z`7%_eC-~0c%i?{(_2f^a7l=IZt#`h)`V26g@atzCfxF(hC&}IH+%x2Eb#9DU26FR7 zzd_~9#x08E#{{y-efr#IH6JnreDv$Lg3W&XoQm|=rtVi_U9JBj`d$~@{-2)BxaCqR z{gPM``iqL7%Shj?GgFxsW)bq$`Fyj7=X)=aX@+OdsSS=Q&dGd2&b1A$#KT9<*-Xwv zP+6R_+H&qX(d8U9a?VzA&I+EkA(g8wo7R;1#KA7-L%;E@blWm&CAGb#2jnEOzgO=B zKeYUAGc%RBLkP_@)+ai@GpQ`?EHN^ji1Ce}Td+=HK%7^Y*8j|AVOdWp{aEE7}IpFXW>b-8ou zjIlVyQZ77Hp|XyN`_JS|8RcjO-Y=r;5;Se7m0()X^d6Qtv2yP`PUPzC?!v%C2`OzxRaUb zlVWvTghxDDl}e;fTDTFPesWi-0J+H#D3z2xYjIc;p00Dv?*FxGHbtnYXrQa|Ox(eB zw`xz~+=S)1#^<>@pT}|4HSvz_ivJJaZ_d_+ux?*2Yf;;^P=(1uH>1%65 z8)oyNuSA^p4p4x&}8s@};>_xZ}f)?OP(rvM)hr!l8O-q?;9&iVi#HHnZBesb(>W zQb^s4_{O@j`;g_{$Up{J$cf#i)JbNJVPewuZNt*og0ZBGbylf}?C6L1-`JEre`Q%y z!>h5btproj_*B8Z!{)7w9o$Sdyw%p^@|G#gVvInE2@=my`tR6x}e&MLJ+*uhNcy@Mu9q3n>n$bvXJIo8wqm){U1n>A4S)W zG>RxIu6+3s^qW1bsAd%<#$iP@?P*2DDXN*G63U_hKvB&$gyJOls3DZde!nJ81{+FD z7y=A&bEM~r7Tbr|Z#OHpRmF0M-WB^5S8NN#wo>dY6)Pwd+iJxg=!q z{1Vw+S1YKjT;LAX$eDrlviL{#IB@)ZcdLp9=>ro8OmixW@bu9(NGp|#qbNpJzqNR0 z*`Yydk&rbJb<~yG_`Oa76nWQhZ%3sq#J5~QVL>muy($&8e8QkL1N6%6D9g0tPT98_? z!nZ>867DRXRtS!M#sPkB^9Q+ml!|hhrhm03&g?b4q*1xoD@J$=YH*}GT@DwW&KQA^ zzgMZ!ot8S4&biw@l_np=Fg88?L6hQf?`S(l% zGSd7z-NnR*&%Xz_nCjv4?+zA|pMNjgMKvm(f8V}Qm3rRIzvvxxMFXm?w^K+1PY@Rg zItef6*bPQwz*~WSC-YMc-RSUY!=T3##y7AxFQ;HNtvQNmwwN(420WFDPJ=%y z>ALr+l674H_v$B+8sEL$kWxFbPa>2tT}N9cL;cjCd9sz%>bR;=uWER?kGDZZe6x%1 z5Opg4G#~GH5O?9a_(@Ux;Xb}h0py1<-r~7#Wt8SKN`s|ryxehFjAJ#*V%;4GLK+K> z*DhRb{kCWoABEbuOpoKV)|>?5SHr#pFUg%)z`BF6Y`wtBTWjr1vYE}8WfJ#$1wzto zE2B0xe=G|(t*8|Yq)+dF*9&qQc&dzk65$|=XldeC*iHhND(egQwB{y~waB9xR37@8 z+Io-4fQ!rn3lmf#4Hc~2oAFzg>YV;5YN_}~3Sr;)50zR2_Vg1sut{{(9JsP(9s*i{ zZw$xP6_8GfRDu>VulJc1z7e;=XQ`=WWYkX))v6>Btt?B`E(^5Tn7}y%N=UQ74om5q z1C&}t@FPvVr6DYl4{MIX7UsixER0fnai}J@E%J*hLVfG+ckYo`K`k?d)}%dMo&kpBAn6g*?lX@p*pa z^JF7rI)OL8@O2lm1!oG5*88mQg*#Tz)dr}>18U9#swQ5`;aa|VK%;AdT>(Nyr?Kj- za<$$ipE2$;CW>fU&FT1jx_9e2~^cA`9Xc2fVW)Z`{1Ck(aw1h@FGis{1)# z>d&upxT4)1#HqRq`R1$ZEJuU$D3~H=SmsAz>q7L*eCA%C`IeH*i|Mdne=-YcEg-Fr z1XkTy(=yXrS=!EV>Wfu`i`=I|KYe6UZN(+@UE{xjzFmJX_I+aqo<#Sb*xwQF%--kE zFTD`_8>J;PCm~<>Fz!+@LAsX333C>FN_VMzR=)JJsCFLP5c6n?;9d(X%enBhv?C4Wqa(!e^WoNDg7F3TX+3;i=hAubNSOIXLiWJF&iAD?` zC~!SaYyXdxI^#6X>DLo=%MzWhV+rzpHTK`JEAL=wl<53`4UYfNA}ReG+hZ8OCm(w~ z(fQh@R5_+lI%xiu)+4k(DSO^Mb;z7k+}I6PHm;A2|Hf)}+I;f*M8nhAg#(0hq(aJ` zh|;keP`OEr9(hW3TCda^)z2u zbZ(oH$-+Re3<@*#2;oX!##m3JX}smTsYaYaYj_LC;(}e|O+i*<+4*2Ec(GMb6`4eG zrbQR(#4&6*h^5534xlqopCmI^-9nDUlAKD#cN$C0gv~mdZGO3t3!E+#%)jyv1MR1cTqj<%S* z9Jk;0s!_2Vw=Zdm^e95J^eCFhpOYJ&B9Q?l3e#e(dP6hmB1NrP$+C7fEQ-aByV=Ste{YA zy)DV=e6bFmQy^>dlANWVifzsp8~4RxetUV`ioMen+pJ=3;ih7biCGbGibb1(c?ZP` z3dJ^Cv6D%l);f6TECiQg1wp@5_D);(`PQ~5FOse8CQs(LE)8XwcBkpzZ4I^3mDoaw zC{o&Isl--SVyl(7*q10!TD2)sCC=ijpIf~&$! zq*wEN`45vwd`}q7y-Z1ok7r>0 zmU;^5l?sx%y5LMzx_OM<>p{8{DT!Ej{6p2u%k$@*TGKp3IY#-j#gT{_DNtsQC(wmNbO9}~uA{`Qvgk>eFv~P?XxLl= z-=+jcXmf$`oOGCsK3DF6Hehe2WP$cwGY_nIORLe`TvtnJHuB|O9 zwpPI^_8?a*L-@EB^fohux_U*Xr?gnH6MV4-h+-|0VgQJQ@t67?@#JrkiJCEVGg*&yTv53M#f4_+GodQV7s6Yo_=1tS4Si~$J zAo{R?Rv*wC1xVmj7MoeA`y5WM!NaM0A4!C&g`_Z4{n#+ksZy!FMW-{1&BxGJs&uD? zs<*!9(WWYis=&#XnDwTGU+dzjfS~E;$)!)xncr6!vEEU|XQ7=u1mbF<>y^`3KrOE3%#S+ zQjPDN#MjqdL0%cAL{VY3h?6qx9YVHD5B9UHI8uT9bTe>^6REtwYI?ALf*s_iEz^Ig zpu&0Z0*In$9z0dWeT@E#B%9HHxft*>O#kI#z|S!KSEi)P9yU5fTr1Rnk*G-jB^t@; zzeGGTra)`>Q-j7Hs(?VSm6BdW$yy9__(%=1T%-n$BG2-Xjz8R2(*b(jT)IdpGzZ3g zk`4th*UoYAJyCqMkB|F!uRLpx;>&z|wU3|W^4CZ4YriK{_-P0DEiS$yitqFBt2MHu zd~@+@S;S06a@w z;wE*s^w0Fzx-+|YEm?wZSx+)4fz$$1ylzwPdRbuJ3Yk(BoD|CIYo%y~zGB^{{1f}$ z>-gjLqqI@XxQuo0jZhJu!5_a*g{)*ti)-E^{7L>q;U3Pm3ZI?}&%t>jAUqZhOaB*w z!aZKkhkN*guMocftNHML%Kclc`z(e3j6a3-d@I&{=sSeZ=1)oZpIEwtyS$VOcT{Y| z+pMO2g(hpC*lk0b_PYvGwr~uQ7H)f1mcCk5{WMioc)@;_+s3+@GQxwYu-liP7s+Ol z*qnUUt16xXYD&uMT}OBp{3x82nGqm*fb5Yi(xP@fvJC{4Vc$rEhgD>?vx-cNQ7|Wz zfIoJN|wI;bDlLMTy3zvD|S`Y~M?hg1}utk*r!Sh!- z(2CYlB$+-T2fBr@@bhf?EZEJ*_HRpW>C;T?Z z0v{<*H!0S2yUD;vejb`fL5neOHd%v%Jv=2aj+(RMbZg5{ZyXtZ*-TcN?6kPm^E?kZ z;UW9>ALi@%e79@AqJbNy{fP**f)gGd)f02hSl2%*)F;=P`dHZ9!(p>Y9FMN53cl~* ztuc6Cp{KmLsgtS0hh9`QSw}huZLOXyWhinLEg;4H9><1sd#Ps12b2}O2Wbv zLm;x>1TOt!viR8^?nd8okJEj9T`PjMU9Rm)0N`o>*G6q$ueL9=%67np%48Z*Y8H{1 zijBE+Oe!Ef5We|>xEXd0r8W&DtdNby9HkcIM4AoeunzDnooP_XQaS>x4$y#Uwk%fK zKzQqVXgzoT7x}4MiQ<9$)hepYS1j|k5aio|9dfD0!0Jc(YF}QxFRwzaT1n=@@{Xjw zC59B4(Ht3ZQ<#cXrzFyc)r5C|kbJo{*tWz(Ia zmz8q&xk@$T1?hL!LIFBEy2(bN)3~a|s|c5{6!1ly?^817GKulwav14|+fw1v6hBjp zlr^2^W|=)ZV>gj^xT`oeQENesi}Hgh{4eorVp?-Cvv7%Rg;U|_ic;6POtmhmI-KXD zge?~(Mk_uV-toT6Rhi3G;c}IQKk>QBa=D^m5>*boSDc3C;ahT zntKLa;lme^OB`~(!iS#oN8~2_h-Xv61DGVtf_uXEW!+9^0_f))1dQ@fuBq_yS z0z8+6V+MV&u0>XGtp)$$S$tWyq-t=NO~kqm)B+G>-$JXzzxHwbYtvW--&xZ^&fuXA zGDc}KzgK4_(${D)7BPoja)SiJ}vKfT^`B- z2qudqx`1!^WU+2*SL+$U8IV9O0hTZ6b$@t#09=n;Udv&xWsu)haGMZm*9JERf+bB0jSh zlsV^h+q$OL7I)YL4|P^B(?hic=yYiNj0j$}I_wg)J&V!^+PcMP>r4-`tQh9D9%f5W z>0w#|)KkE$k6>!>hPOx-_b_`6=0Cg?Q}0{v3F~n$dbI}kUG7?M2})qr3g!+TudRY$ z6bn*c@?hr_gFVfQGtI%-9;_uOp;ti)=L@XoiUk8{r9nU_sPOZPd7 zCLFx+B!C!+?DOlW&EIpg|I6QXB?vR(<<6&S+QZ-^)BXA%* z91dy9!&1vDaxu(V7%4TmCiz?h4uqGNu|!)) zu35?@%FIjf7B~<-WVzgk8ri^!>>LGkN_1uK^;IQsAl&&+MOe|SM;%y5d#tSX?Q;FoAULCELStUt! zlPf!XWqS;?1rCHWt#2IWRnzOOrig2lSLriYr=}G)5I&%KX2+ev|5dimL(#UGdzRQH z;-01JS$S#BWhU}w%hU9X%p2icB1%M}XzMuIO%{#wmddI`!|QGTi#!MZDZG+JrrpCp zxqipnUB9rL`!vdkl#HVmIzeWkn&4*9@UD$6R^9DlUAJ7;8vblVStc6v;np9zoQraB zZ*g!;JN%Y_1sY_ibgvgt@M}uH z+XRP|l%tco5|x_Jz1RVOXno-Hib?k<1Sjr3(*AItITTQ&ceQz;VBdPQQb?~DL6lt` zm#6hmw%6UgG+QeGmi&=j9g0b8tx#+YxR^LG&aRG_W{bJa#Z(hhZx(mN)LYCoE{26I zwUF)fWU6GU&H}ySw@IXyn_XQF6|$>KB7OEzH*fPd&7d)@5tM`{^zv1C<8|kWc~s9m zEEQIBO43o7#5_v!;iYOMCk$kJ%^_WHd(A;DB44z~!ac*)Opng?dfp1rl_gtDvecn9 zoF)76CgD9&)-6(phKnAGx_u|HMY_G0VEufzCm=MnHLkC^eH4z|dVP!f{R`BjsNYu_ zr55!2IHl0O>gM}BWT=eX?~l5e5&Hcu7c)Y?|HQ@Q`~4eKskGlOB~equKlA-=xQP0_ z8GQJDZ!&03MPw>t(S?mvRL1o_(nMS-n-y6_X6ci6@R8zKB8Rnna};A9_4u1@YWsaQ z1eL0jhRcU%@F%a&weV52HavnqUhnLhvjg|M6@)Jq{9lnJId6pu zc4(WYP+bi}sD`o&Z4-&hOt+l?!Pnd^osWN4Ldi+uEnIUui&u<;su}kQ+`0O$v0MBW~ZB1ltXs& zna0Wnw^O;8p^er}CHo$FQ2XDy-%Dl2yiOAG#qM6s1uaWou1nxB%$S{MiQe)sYeeoL$#8fyn)2A+bW>h$ zD`b>@`?PQXho8qCKF9o>Tb+E~FnNg)a~IU5U1u&m%Zz!7_yYLB1kwQNfl88`7EUjQ zC;U9gV*(?+y2A}sUrnU{km&qJdD~gZ^hVy1;YFMb{IuWDwm;XOAIB^fJ^fpi4gNWJk95`&>;xY^ zA4Zj(uobgf@CC-g-~+?^dn{KeJ~7nHP2hb8_DyXV)mCvz^v+~P$9Nnq^W^7ar$Dr# z<3z*c3UlXy<@zsoBV78KB^@A)8{y8U4fEc(1y2F}H~|b2%4!qk;%_krrNM_T8T}dxHkL#9YRf}b>THn7(?5F3lVdpA$|5%aI88<;Z2$S zYX-j;T>Wiz*5S*`!Ry`BF+X1ao9+MRX4(&4{=aEIRQ*xyS3eZC--z|_jmRy%I@f>8I1e*M-a6{(({PlI>o+V#2?U|IgnQa3XG+;A*=) z5=(xvcIAZKAHV%7dErp-f_Q!6&@p>cgZ4eeN|(j%ejJtvN6N9qmtjn;@V|9$c#miM=8O0CO14QqB-zd`QJglayeHH5T+GOT*YGlcF z89LS_L;n-ZQ8Mi)+(W)(`h~#^GA!4koE@bu0Qz=t6scvGcP#IqhhL@^HlD0Z21~G? z2iAR^fJw4f?vATQJ7<|qC&a9&*$J;*i+HKJ%I?s#YL{jQ7*3i6+ywSHD7KilxL|j z-P2}Js=`YPq1^A{?C!_fLIZQxjbLLYnnv+u?L>Rj?ya)XO||HLS53lQVsx zF>Y8I{9}GRGK_erD*wKl;Oi;p6lX3H=?^F*!Ct-2@VfbwGOE>+-1paR_x)`v!1<+d z+d1AG$*uU_oJH?6ywSc23gF7MH``*|6G(5HG0SQqp^kbF?~=II6~A|ZmFRY>CFXyD z4AsI!g1h6wpAzf%edkK<1LVXQG&eN(a|XYwpHdb`U_DWLtSxg}qN2F7b5qa^Fy7XR zQ#ZkC`@H7Cf~N2)x2^P?^syJI)f6tM`W9xCt|rXLDwz3$u`U1R6K{cUfw zUpKhe+OfI%rZT@?XrrDkxV=L|j|v_odnoWfOftHX0t#v#>$+Ka3+?J>l0xM^xcU1D z8V$%uc47Ovm^gjn^b;_uW)onxb1_tDiqlUJjg(igX{$ka{cIN* zSEQlOPNO^g{n1BOE7CAna8!MfMHa4H?cFs^J*=@L7g+&mk_i*tWnd3ObHfY_UFxSk z62Y2vUi}*e?S54|neFc7ql&VocWyQI2upG~>YeV6f_QT)eh(MVGJBe-b~wn7Los7q z43NPcV5cQB?YZvOL+9aNW@-!|q_2$Iy7OBw-0;`@asBNZWbK%ws4vkN5rHxn3}_qA zMf4zb5A@))J^w>`_zIOQ8b^ODrH5UW;^XLH!&x6e56}6?5$WN6A2}jD^jc&gJ+Moi z(`4l6;X5QNqz9+_arEF+KSmG78npYX)j}A03~GrKKZm%eNbjSlDkXzw?&_jeLmp7c z+5##uw`k!X-%#uC7a5YIuONxJmE>tlvUx(g+r^{okyVx57=c4MZ*Vaqkl=h5Lz#{Q zCl4dR7E7qW0ZW~>akD}Y%Ey?ETcGDTE+vOwB?JF*31iF3L#YA))>ymLBj1?Zg zQrb1^v71ShYB-tvw7h=uB~-*V-vM z6S`o+PJY^AdfV2jUr2ACfuxJ+ZQbdjw+}yx-d2zV&f5a*wAzS=I^6vE8y7PIz4f@5 z5$Nqw7X$8$7jU1f5Gi@6fZo1FB90p7=uHTT=&es~C#8O#WYAlWu(p|bbF3xVJXx$e z&B$Ve&i|u}$#;GO74@BOj6(e5783c+KYWvQzN#8^zN+dwe=Z5xo;1NR+j%_^HpN2^ z<`%3|Ey-n^{+}|(;~9ryCc7B!*PFs6sPJ4wxfvEy>tbq&ann9AZrcB}i>V-{iae~1 zRc7WAUdUNdb;@ZH+1(7@ct-g0sm%2~;z6(;?ot0IO!f`_`1wByH$C;tyq?;1nDK=c zWjPKENRB87H$AoP%Vfj|X&zVOmHbhX`+?+xua-vo_tHpP9i#dc>Swi9|C?HJ?~wb= zb!r5A)Rom&`fu6^wKsZDTVzf`pSMznk@x!|Hu{N%Ct_D-4fij^=AR(KYZ%mR!Lcbk zEX}(eUnPi3wDk-Z>V)psNt(EY=j{W7m$bIBmo1-^a{2ygpLRoUGLI^g9{`6r>u@zmo|7&5q;jgjzx&kQGz{?)$&3>*ikIipV2df{);Yg-K>)D}H z`9EpRz%()v4aRkR{PNMdh+mf-bKo|GjwkrpNgB?&_nMFw0Cem-h+qI0ku>+|gBA7Kl`ji5Y(ke%@%wF+t} zRjn+4HmRDFfN^+?=m5QTD*Z5>=<0il`W5Z2Rob0f-{xdG_gE0^*+nL zr>8ID{eW&&PTlvTZqn&ZbxXsQ8m$j7Hual3_!OOd?i+OSktarcBG5D&t7F-Ek}_>gBrFRX)_@BbQ%D7Ts6^;0(A{GMhfmL#vP(Z8)4 zPf9M1-l1tck!LBQhZqw64W2}1wn$pfS0FtgPi#!oeUPYoM^7*=eSZ{l(WqqX_SH>1 z`J1W>6Ae$zKHJ{7T(4f0{F6;o_Q!d&p|fvNBAr+Tjja~uA>07>M!9XKf0~|s(|k;A z+0p&ByEbg**XDPJrD_Oc^Di*EdsDp^oAOku{B{`Wa>59*K0EMn(XTpmbWv8Hnm;EUfkeB+XqQ+lgK1M#Zhb+BzQ9H z!bCM5o^(5^ng9{aB*7#Mdg^aGgek5vmE{H&WvobXgUx+pYPQ_5@yF`L$r&EP*Q<3c zzFXgj%1~P|Nr}$B%Dlw`kD{~wCiaFZkMhXkep%iD>eqa`micxCa+S@u=*n{fV`6dC z+ict?V$*r==dh=Q!(`nvNuIrzhn;4S|g=A+Ym}z+fA; zpPHwydgyC4G_^);8;Gyhr$7EK&ODVNTTJC%SRPJEly4lF#2o&M=uGc2^I~)%ogE_n zB|3YLQl`mRBu{6mR{?#oOFo%;O?LH~?CLd{dKFOFYEjuaqOu0xu3|b9zlW@7;!$fd ziS_BD*2nHnyu0wIcQc8NMrj+{CJd*vB_hvCNV9TqNi=RSawsCLztNRNq-C#c3qz^$ z^`a{-5jua_$aLlT6|X%-bagBQHChC9s=!*mYOl%@)cSCZ5!Bj3e)WaFKv3H?6%&-R zhY&p#$;Yv1{{0yFc$Ds4E|iZ)*@IO7UHLeAAu}f+SLlAFLIL^mRH=ZRfi6ry{vD1| z0r|y`5s>5DB@hY7%)MSf=9Z_NfV@!x@>ojUynt-uBckJC`S^-v^^PQ93H44sew{7| zwY$4H`8bxJmR8axlaJfU)C7zX<>Mm0G5saFd7q<4mX8;o;C^GG{@o(^YSakwm9E@& z@|Eu5lIS7pjo6hpY20Qn*_4;Bc!wGBiovxxx$4VIN56A2)lx50?WxHGR#PcFm^g+| zHI9H<7r<7%6P|cnV3_b!)GsDKon!o^NPe1b`~~^R&10;>3*;x-!6a|($jeWz9l7~q zr1m)ZiT3Qc2-)eGyzI2M+JjJJ?P08Q+Zw{tc7#}i2$|0NQ#@U5GLo~;x)8abwGzgJ z5=GApAH|5fnD|xge<>7^uA$3E5Rq1zh-AEF1QCh#x2{B66<+J*BXlyzM-i3e2~qM9 z6WE5}jiW}Ck5&zrj~;jdLi}lBbNuF)LhNG4poYqkkJ1}tnGvZ}SdXVnB-$gD{(GYC z&zR>U;cxAvd#%UyAcY=UYUrPRRwDflua^0F3S31Zc)A{al|+SjKhjVl-MW^jK%~>p zv}NMZqcGB~nT4C2{sSddZay!epLb&OA2s@U&B@;^_mRIBMf5XMj_4pl!asbO(6CRx zFEvjjZ)CTyema!jWosAAl6VcKa&V55kI?I6ILm_$GcyG)b;^1S&8m5n*G)yh0Eh4NJcRo0G%;i+U*(viO#YBC5!Fnj zKLkZQb`Xq5tR~Rs{4Md|=N|r6?d?Cm@5;pDDc{-d)rr?W1Gj!TQTK2%{gg!ZrgQ+$ z97xsmrP6G;Bv{R`rVtUk8xeW=a(V1Z9ehX}N-44VD^*0paxGtNh1VCG{{TU@!fTNO z3ab_Y%TZ+xc5-;p6gFADTCW;aMDp5jI!m`!)4$D(gE-&T#F*;=(BCl!WB@PTBXIrk z>*ZeL+>146a%+ixM809`i65pKUc%~zCYYg1B6~7o_<8lI>_yGtv5W~X4<*xwBr;!0 z@^UlkU`MW=R0*p{W@n33aYCw>RH9PjC@YzL0NTmfkn1JzM(lT$5V6hk5j7gpHUxf>;2zq!GB?Ec#-^At-()Tk=>B^ ziS12ZKUG5E`RGtb$r6o0P=#>)CR+5Vu#fC$drrmvtQU`|hQ0JtDFk-oE`=3K+#8K! zCRo;RoFpID$2uQ&4BUVG|QVnMw7N?Au>?kzUL zMW3wNJbww{FOQ_^R!N6kWumzFOP}X2CYfDXF2>RDY;67=l$mPyLn=+vq(52}n}4gS zA^lNOjzaEYGK%n32^BpQahUXT1@x3%)B&ZM+955Hf>**;vWt2NO^jKkzXKgox!@`r z=$PSMiS)u2oW#xYA^jcLrVatA0|t{=2%(DT#fWkeE9kl1`McQ(4{E(qY9okAi_yz@3h-3{1zT;(1p5- z6m-K1-K*mg$@IIPAqo`$#PDe^IE89lqi4L&tqga=4?~RTnAsH~?xGZsfL^1RFG9o# zYPqmOv1!<1c44oaM$i2nNEbtewEW*B;*aK{v2M(Y;31=blE`3Lilc-7%SYg(Y9{x_ z(#{Jfg_pC#sQ^a^gxN*F;Xxi9T+~3A8vrXvy4*~g&{D{F)9JsCaGbO%S&msF#=7v) z+r&aUB%vVyj$SUu+K7#cw~x$5BgU3vqoX@uqa8(TwD&8tksVD{!?RIDBt)13UV0hZ zxbV`89*37UA0}R!TkYQSaq?Oh7%x-lT&s$;|F3H4c z#t#u|^%)}*kN%FubQx5pWfh@fqtfWe+265NuKteo`Vdhjg0zSVxd^Gwh1ALw8^3T8 zUp}H;;#+d@v*dyuO9W&TbQqlY!bl42m46_9i#`Z`i}>Hi|J5X2>%&mu=GA_&ZoQhB zIFG?;mMWuk*y702$v<%@T^yT#$rw#W>?TUoWif@Qdn-XU8J!~se5_Wbl| znAZ}_>!~+4j{kpz_d|E(?DGmud0Ax7ELO<~Mb)Y7SuLoxI>LP<2BeW#w0vn|&O3%F zZ!R<5ber(>dTL|l80NdNW$C8sM79Y|*u-pi4BR`6b$!NVS3oTh%yq{w)m_L;=k|68 z(?oX+^BmfYg-r<{K=Vz_B#hf4VU^Z7E&!pksfCC{Q~KOE5E9nW=_U+JvK_dA(X6yg zgvYciN`VL)sJV%8?BjGN)I zSIHL}|AQX;{Ir*k9t-a}%nG~9r`0EszN?Qw%3c;j8NGY{&+wW-}3`KIre*`OwcU&|}U?K1F^;~p7t#>U?)e{B41 z&UaTWJ}M!zLn~5#rGC(?LDTok^v*X(g-pU58-Kr&)3CPBavi^GO$t$5lluEGAHAdu z!P#_VrzJ_Xi0W8^XGy^M2)g-2$pX`z=&;;{LOCWOOow#>>^2u_@`TblapF1`%H+>n zmdx;MIZ5Xeh*gjnQ8Ig#eZ0?|Qst&D7q=i`b=S2y9n^^(Whj@4NF(JKVE!Z4y%LFD zIM@ZvMrF9n`_65SjX%33oakrWGiqbwPn#6J;$!bo17qWjY(h=90&rm~h*l|f#MaO% zY=OYF3LGL?GJ`q;fe4#T>Gx>Qr8YPJ?hxTo;{Pa3Q|I-m_L`)t-P~NhM&H#+Qmd^5 zIIk2aTbC@om`rQ_z+^gVqFW~}iKe?q*>_$bz&cY?Mv0s$s^H~vO^wxw^sXpVcGnBc zwc&-BO(pN|VB+96ckofqhs_@Qc-yka9tv%VUeay*PAao=Q)Y6-;BLu=)wB0YH9U6d zK4ucwcrRs%=eO-GQy<6#2LH7RQeFI_Zp~S(R8?H*L6Y`AH>LkFt?QxoFAB&BDE-^d zRKima>;qsbHhFC-Hib7UFk)g}Po%npD`WD?aB zs(Se{Fa1v$VF7^5fH(yJ@=ZJNPY$30@=I6KZ&`cI{?}5v&o{(@JpQ;{{Q^Us4V4v@J+vw0YVN)o|c5b0R0~mc{0C%#n)rweN&)b|qVE zaKOgc)dL|WBYMwzC}j5om4cI(t-bjZbby;%d}O#vM}~DYOEz#qZTg9#Gkl))<&OM8 zcCoH2MRA!4S1l+PTN_=|#j(2WE$o0V5hh~nq~EkoebpsBm|(Y;368&sE~KI7qkre~FoDB#AN2PK(;EQHEilWE-TQ|L+vu`P$d(7_i z>+e}js#TNDK9p3Lw4Gw*yd8W=U(iH~VA2;$=#%k?v@a8jMhZ9|}$e8%pJ~{ZAcKe9Izt+Sei{ zF6MLKLqQUZKvl7>)fPLWLL)MN%r!Hmg5|ua>-SC+4wJSWrJ#4GUnc-6*8KtijNm!_ zS+Q_x&5k;Uh$c#BWUDwMdzDgkX$vbi9_%dk)bz_ks|Sy^eT2-IeL zc0bNC3{6_RD)s|`J#@@e616W&ERIgcw0*|lhwl8 zh_FVp=r@uC8k5PMzIy5)se`6A%;XxwYx_d1dthZ#sa(Sc_y=LPFAg!uTtOvv8T^%Y z)W4^(o!M;nP)GcBejg51cwSe%s)3h(no9_yW}ZA+vWLLvRtO zVRA+L!8Rp>oY6M2+b3erT}Bsh=$s)1k}+f{#MBULnz94?LfITy^-O;eYy}f>_cx#{ zbem-y%yjy~C!Ch>o5Sh*jd|47)Ay>_&fDEIU-Uh$289gjF?uO@cz@`-VP&lA=SJKU ze@qI7<<`QTZFpr4*n=Q6*I3oO-F5{dkb8L5znPuBD(IPsFYdZ<$fueGnH)2cy`o4mINnisELD^twFNQoAv8iL^$wH>-NbVz^=VGN^h^eTg8%ICR%C zjN@7c(XLkSJ)$Ui4mCoKLyino0eLK}wfM-tI;@hPvWFO*!p$z>cL>oY?yn&w7=40j zh{X4FB(8(!htDJo68|&9J(Ye7`SG-};Cd)T*;@C5$lry?wZ9-~B@?fuHD}IMQ_nJr zxV(b8xcMI|UN`?gl_v$8?1zjx^240@zlR_ybQO?nMiA}oWnmM)M3@TiWQ1w{f7vBH zm|&fY&}hFz2766e^zq#f#bZAh7~Cr_q|M$Y-Zdlv;B^TAE2y$%0l8di(r{G8B`=7} z4e|)MlW#aJh7ho#Z4U_-)lLdndhz(#r&Cb-$=Kapa=g)@lLKA=BZ4d3Eq&q<(q zYQv%G?v!C2z9sag-yT}sh>djmslg{TDX(#;>3T#ABQxG&Ca*Icft{{m+Iw!}X%IIA z4T31*G+GQl&-y5pnV2EJilEyo$VHELk0#CFwZUgVDwF^AWV6YCYx~QH2W!PB3%*L4 zpkZGVDhlLNCVnwSm|}b;8_mXcHMLDbHtn2STh>+w`O>+^*GwvyvzCZ2aO}h^03v&y z;3w23mA#;v+e+xN;Cu>0XIX1c1u$%dvIOG;fVCo#W00?_c#JENW!E9PJBJM^yGUM% z|8pNDRwVK!B*-C|3k{?4Y2Es;&~++7Zk~$HM;u|V+r3|Y>F1W9I-qAKzKR*AAf61w z!G*!g;Ke@zzP>Fx9694uk{;f*< z@6?pEzr_D|iGNv%|92(+UzGT-F7eMV@n2lx|3-=b*b@Kb68}LZ{=G{4+n4yaD)GOw zcS-w8{EwITmzDT`SK|LgiT~;n|NIjF#U=i4l=zP=@lP)CA5`MstHi&3iGQmS|2r(u z3dh^aCH}`t{L4!GzbovM;P29@V zbimWO?W(WsWDdqwOZhhGz(3{X2eyl2jaTM!uH{kaMm9uAJISTBliP}@3|_Q| z?jA+(OSnraT-%K>N=u>9y*g?gTb#J87H8si3 z4`Z{Za(xeu^7b!DIk<3aDpONKP%3>^#pM3c`_fb-#k`uMnR4yQ7ewi}*=#*ExN8pz zjYRsj10N4YF+37!3|LtuN=rHgYc8oof`@n0`oKUB{t|(9^6#LBi3A2^d$1}&wohTq zV|QLvl~z8L-q))BQ>rc$>88oQ2a7+aqTI!Qx?u2VyI^plE*M;R92-4TDzNbxU0VN> zsJ}NnEl~Pvz4^z%n!G+ujiFJ&mqdE$Q6^|d?OA8G+;0UNb|sEhrZRgE_5=;P`(SDRP5>1n_OImeVf1m`u>7x5hfkjWo?-cK-lF`^49kD^ z7UkbCEdPWp%D+orDgIuCo<4c|4;hyK<}J#9(y;tzZ&Ci)!}3qqqWsql%l|5L_sQFT z&#?SAZ&ChdhUGtdi}G(6mVd$)<=OZ7eDd}mGA#eiTa^E#VfoMAqWrTtid)Rz zCu~vv>xSij^|M>j{(FYyzj=%DKQk=<*;|x|X4=m}#qD3` z)^BVcRRyoI5Me2EOBsONjbV1NY~3a;Y{mpT?_kkdNGX8B8F%%{UAHOtV0%Sfj`^0q zx(`ixiz&Kaaxo~Or`7ulrf^6FK0wR^eFA_wdWn*8J??hgR2xcF?)2hr!PLXLC@(#0 zUrP0VhpdH12{HPm4KI`K9>Dp?{Qa-7 zuDQffeO6Q}T}5|S-FW5Y*<^+s{%*R3O6Bef+caJd#Z3zP3QiCES{3eeCcj?z>lmaARzU9G1SE62@$Hcpp!UV5eh(0g{ zytAf5kr2ppK4!$1Jnr|A;A!}MsJ%o!&t$T6;RL!_k%mFM7=DMS>Up#gK33!M_38Qy8d;v3&zIG-w*x}JIUNI%}y1UQ6Gbc z8-?9P;O=1-&W1oJY^|%_nG4*l;7`+G99ML=g8v8mJ58Cd;&~|6HIg2H?I0Fli6b~PU@Z#^Z)>?lys8B0y^X%dBHqQ9oPt=;wDn z?I>WxetybFj@ZwOiL{c{&;N<=Bf-)Od zhAVNYWajRuawk~%S5UVj4N=&Z;t-Awbum=z$Y`MAP|Tjh_~JGMJLZb(3PC|Cb8wB_ zCJNIOO6Y0Bh&W?Z=1bcNt0Qs70T(j@XS|IVD^r}Y7mr;k-w6s*={Lh$|0#&SSAo$D z>lAoLRA8%t<*mo;6xT?MZ(Ji!bTJe-C1S-m)X2l~1-=-3QOOu%mnG7JVGV_Gf1hg~ z#OvAzXN`(0C-un8Zyn>47;sw3{QgLc$LZTaA0E3W{~p@cl>U48?1#$dY^gKu5_dY$ z8C64JJqC6zoj5(}L>y|xr7ng7r$?QLLoui33mgiL!K0}LdyE&uLnurm9wpmihtV0< z#GMnF4r?NZdEKZBc2PcdlS%roV0W^WE=v$AU8#b1x7Bcz2_R3IMAbL|3udc^C3%b_ zzU@Q73Ou<>;VZ)HKTr?5=-?I?$EEO84~Fz{)tjUAumX~Phw3?2K=p8d0xSCjlGs7H z%~$UrlH{s4!BxTCSzj5x{C+|8KIP(u-C19tzdS+qN=3@M+5`JKhqj9?_yMCDbJ*-o zqi%wT&l91YgXA<=$6+SI)T|61S={(|{GJMm|~z~3eR)#m@9{B82jH-A=s zJ#^@$lMaSo8TsZaJf-lHU_iNZDibeK_`wQ4qA2`Kh3~3xcJB+)AFJ@u3Wx6&gillW z8?O_-f$@=te~7}LRQQWU;k63CPvHYa;X5h(CWYT#6uz~>zprrjmclyP^l637QJ8p6 z@a<2*1M=ebYb z3jUcrAJKuEAY}gX1d>rIdsEF?@T|AHH5!BEThqmQ%;hHUA0vO4KhZdV$~P8U@PPF> z_mgyw1E?v0uXSLgd+w^y^rn-psq}k^?5*|?YX9i1=*;`vTjo42x1J>W;oRt4Ouo0~ zqAMstzy2=EjgG&|o=;a;tgg$RIM-K)DfEr=4Hi(>V6U-5L%cfKRntLInUBnpt!9g= z^)4a7<0K-U`w`F4iBYC;3~$^?zjIg(w1U zu$K^H-10Urzu84(oc>C>LAMb?3xtuQf#L$XhEpA(7*2T7nJg z2oNq}5nVR(HBxCc2ZN_Ihg`)Oa_7@((X!BG_<>jq02Ha(=!bKXcZ!IkXpw_{5V61+ z5%>5oQCACYeOvvvgKw~GD-d_uXd_Zp2nyyjS6fZ9dgEZFs2rsl>9n(M`QYi$`)WFX zF6uQ>&yjFTR7h|E@Sva@YOYr`IPRZ*4ew1x(IVVAs=@f1Y8DE9D~KeP3BltkHhw?+ znBhT^B2EtHLy{s+?n#FvMVzOKOi#%Zry6Ydn@rIHZ3gi(q zq@qT0xU_8*P3du#Mk}0T`cqn%O~a`A5w1V(IJB(oqDD@*WSfT?(50mq$EUS_9az=h)#VMBSr_>AWD z#Y~+;>l#)=iO?)8QOT|jg10-O(u$f)pV7$Z#4^Z6Xu7o5dW+y|?O34{-L80Qwou2PD4 zg@wCCYa#i%MdR34|A;~x2aePuz?3Em|)G&Cb;W{U^QRsZq!@q?gENv zJGb#ZHEXcW%Qg4T$7d})uuq4yvO9hLI&1E?_ym`JxM`StXUZDnyQ%K&JiQyjB(lD- z^T>+waHe(1R4sH12(PWNgy_02C0#B_Rd}xJYURr|Rk%KmEiDg^%7qv7`{4b-SA7pN z%(+%NTx|?W9DqARR);U;y3TUIcUOs&y@D-Bz_DL(yA z>swulkbXKi0tD+AR(1x*u;868R1&Pj=Hl@m!CYHX$-H`uaK1g68UCHHU?&d|@@%6FsUti5lXKQ2QC$HB{ zXWYoQKGC^$Q|D{tZPkhF=r|@L+*P&XSPp=bVf)zlO6J4K{23pwj858N`e9bLQZlt~ zg5)w2zWOzcjyup?w9R(p=J=27fk9-Be{-jfn$)?Xc4~TKkpu!0fH@eWa&RtmTq=En zm{j}XTy)jot+w9L`MHaX*xdM!XlB&l0;k6;f8V~eq8b!bB{4d{>V9TTK!f5Nb^8Yw zlE8=B{~TgaH5BEaHg53Wg0~stA`s0Yjm!bgJrEv?+m9QgA8aNVGiEX|r-*iDdD{5_ z&E@=};}@R-YD6`eF;6U46a((gnqE@DW(%n2k;5GI1UrFWk&q7NGe^V9k!6++^{f#d zGZ-E>y0uxgv`_`M$A4zvVgY_P7DJ64mzR~bol(>;d#Yb%<@%+a7X((!$qi_Ddr^NV z{}+q;qpzSp?le3{_!~8NW04J-p$)(J_h06{0gEjb z^cEA~(0r|}XRckw9i|nl+a^jRRb;Mtv2HsrHqV~Rotj=>KzX_OC6OQDLCXgqUBf$G zkwV%+(1M5=;?>K<8Sk_;2}%Pob(p(G`@eC^r}%tuwlPf2bI3muBRDu#k76TGe*vaa z3+Z~BB7*O16g{wb=6X@LjCxV2G%0tl#Dx|Ajx`Ip%En)@qO_AV@?bh|YxyKnP)B0n z^S+x1>Kg&Yac`r9xQSa&ye}5cRWm9Xan_RwawIE}yDf6JtGHa>CXAn{*rEv|)_ofS zeU4Q-G2$whtWb>TzOpb`RXE$00dZuQzV#7^Z| zjoOEUhyJ!n3w_I62{3HMZtl@Csdn|^QPc#-|Kk7hVD)>Opn}>vtv2*UiUm~}t@r zpbIMq(l5mVGK#M9VRW^?Sy#9CFmZ2lpF96NfeX~TDJVpX><8P~(qlU%C-)EYS*!Vk5`!LkA zQFVpN)DDQw4oO~VN_E!|!%H1N3rI26eGw__?j^>(PBOwz-9kq4Wh<^_C zXiOzC2i$lh{7kLbvy_>&?IGcS`v8j9w`#{r){Y#BRT_ZWQbKHhP${@5V;yj`r>RrN ztZRzs?U|_Yf5-_&1K}Fnd6nMA_Wuwvo4pZjt0fK2j!6#t2?!(a;Qhs z37G4i;xTtfqxRjhN1;k$TF`CUUn)M^-F_K-V)#7-OOU;W*2k9aK$iAD>Ol9xsWoh! zuNyknMojE(yln>x`Zy1LoRa>cC~1`?8_eeM<>11{$;XPKiMB_BXfGta2+0TGw(+Kb z{~>-)e)|*PcN`{k!|;2=fy3}y>+m~a^8Xq5ee%7A%9iOo;Q6@f!t%jDjur@J)~0cuqJ*_7@jlg6dPtt`==Ub&$>YIWk5>y`MqK ziH5vHvKG>m@tvh_iu(gv6*@;ghwA4ldJc8VG9BFfjs=}$3H4ZOe*yq<-H5Obtk3A5qd?TWbe=!Gyqi zYg`Teq@0yHa6`F1^&CGar5`Rt_qnfobho1TIA3}SX*TPAn;V17EgGcRF=GzM zV|b`X%MGUp4GsnbB&h}KceU1FQz|xo6d@3FZ2Zw8VMok)jTI{mR1y1;5yXu?-1W@4 z!+K`IVoHH7kFP=3=5KRqKb?J=j%X-JZd>iq{G4OL7US2LFrnx&GOD@W(~# z1+5Dn30@8Jc)p`RYF&|Y4}?$R?onrWtjf;IYn3D9+Wuwd8*< z^OyY!F`F>Vt`y`q+YaW_d3j~oR4z7ZKRtHO330TWO|`7trlF1h(mCnlP@qhyFgyv@ zEI_AJgVRN5r-Y5mhRx}9P_~^Vv)8}X{z&d+*Qz2I%jh1w&giLVdNwVPP2`h?P2{%~ zrSuc|^*y=y`-HN=Zt$M-Yd#-{Nckq@`G|$wi963h6k0H!W8biK$z(Hw0PBIGU7fZs zYgYqitmuMaopH(uLCNVo{VSdw^j~gRBsu5SyiT3Q69Z#*XAjTjCR5vz*VKUDMh(bq ztl2R~Sl?|O+z3~&6%ooh6m9p_w4z@y%8hKC%6a4tr6SbX!Xn&WAizCSEAp92n`PqC zmqf4>c7*W+*AHmt)NRshzx{gEla#%d1Cs@xqdFn*%yPKh zmM4^Mg}6F}!@;=`7TlsjQ zBtd+opQEio#J~Ki&mUn&-)Mii*8)j(pQV!BtWd4cD2aK*k$yxHw$t2(S#a4an})(W zoC<+?Fwuz?rK~+A)h0&T)EFBy3Eh*>YtB{w)9Sd{PDpEur`hCZ)JZ`cP{IMqT(rC4 zlV~=B@2WniE1&a2AwpjGR8G=e8Qzz34}=?X?&aaq0{3kN?zeCk?pGA*(>K!Qf=QvP zM#{L47(a?G7(aH;>bw<#@^RxQnSLhNjwWjep;Gs%aY)K()gQ+2IhoFlp)VmR`z=tf zJdY=olzgTr#^x{nl0z>eL$qjQ$Xg?x40YX1M22(#WQcsEl=Q+|Qk#pMS(m5>D!ke} zM}o{)^!$y`xxKLac5svRmA6(v|G?yaXU$p_#_rc_jR!iuMRhCHt$*Djcl*+py{stv z?#9fKPIY>sG^cIfbhH)TV7l!N5*S@oa_!Tv|dyd)1IB^lxJ;}!u_b0qAEPc=($MG{#C0+ zq5OB`xWN@c|I4b$i(v=R5#Rnv!P4Q*0{0U+_sVb{?gH_U`Cu4-cl4IKWXRc&eFS|< ztS+F>pNK8ZTOlZ)gg!3-XO6`mdJ7q|wtIuLlOxL~Ck!J?BdSVIme5jg%L}ewstV}x zN$XQ>>3lM}WYMj@s~njyx2Bq&Fh3CLVUvqhKt!0U;4br=Dm9AKB1e%+Epjtd zdlQj)rgtTlj^WQBAkIT@(!uk)$wnWZ--#fv^7F!Qf`rY#6*L2-fFQ9}8VbIzN)+(B z@DBwmiDz@}mEo^)?g2})Iew<57x6P8<|F7u(FOFfgOdjY<&)6M;WWw7%eIWv5$NUl zy@%1u43R}4z07%DI1m@)kLP2l+;pn_{g3BkA5AY$Ly?|dF0)SXyGEH?o$&Ax=Tm+n zVlF(S^J7qhP@_XS?tIF3ltSlI5)n9L)E&eh8A33xw|S&JINreASJR=Oh!VB@79}2H zk(;5!&k~uZ#1Fw$IJKBhLXSvH&xMQxOqQsX&ZBV4ONa zsESr1!s(L-!rAmEMYoe`y(r4 zGt~KABJAvx>}L39rJ*7>q5o2J znNsBn>2iu`A{1axwQ21@~*Jg5#5oh)w&_I_G!=w!XRzpXk4g}p0WP=UvVBiu=@=pw5IC8?;ADN0dW8mhPuEZiGTSht4 z6jN&^>Xd)eP?us+k}jh>x1p}2T&cU?=j(mWx%b?&3ob{$`ura5!Qpl8>-~Pe-mmxT z{d&LNulL_$d||t&#|hVP+EZptN?p};tW{l{&UDOAL@PwiJ2B7d++C>^l6j<9$ExuKAMa{NI6GM^{4&PSDSs zo}hIfG;1%{(YmAaL(ca7vT+(xH2atLHi-e1ChqW_?>|1;yF`j>V^zoPOE~@^nm-=$l-vg@7r2cY^B@CZc{TVec6*Vo*knbX-)*lOD^KkAAYO~Pl zDh3?Rog1^)?(?~KybN8Z|0CnKc>hOYU@|c22#!@mHd2rrqdK?@qh%+YV(nrk`xujZ0m2<0#8+s5;_&=VA6o z*X)S~r}uQm*^3PW?ZsVj^VZ5EY*8W3%*2 zj!@TJCGO#65NnaSoper?uzf#ISu|Fytmz#!Jj{|QE7=z5>k{_F31@aE7#>4n(;tdk zulu4xJLhZI7g+|TzGM=p~#rfhbFi+LZMJhpJ}LQEdl+&oYoZ>2cp z(N5!Luj6-^VgDbO$K{T3|0>=GCy&;hPI-K#d7wN#87GepQyz^BnEuD*aWeydTR-mP zeQ@$<+2NGOMe7I36WVcL%iX9!;jWAcoghu>6xaj`xj*WpaNvW+PxCRUPY*rJXl3&-zHt% zR&^IcsM)-JA-WBuL?X#%fNxr{@aJMA*n+A^)L4PNiUsrhPLzaZlQ_?cr4-BNLa_uF z6I7tO)PX&$YN|*4_cE3Fu`2Kq*(xIY@mS(G-JVb8ZuVwAw!_rW5~ihsEHH#rlGmT6 zRIF|Ap>avVtT%fWs%0;VT9Df&!7|S&3)d>SVS2-U zm#GfhI*#Z4u=*$#FN9?Z#2Lw2u*yiX6t>g8Dq?5#c#ay2nUrA1{MSK0G;Y`~q~fQ* zxX-Q8vWUJX5eL_UVEmZE(RZ&U3H$9jNK@2!by}#AkJB~lk9+07mq*%Yg}T*%NqZw* zlvh5*XI=7$8B`{Qt$-*qE=_K(+WE&kF$Sc99e_#=c(0c2fCrUY)|-b3$)0UProl3% zeNSdC*+@tXwa=O*)i#OEw)t!rlfTrX>oeGz1y}Xz?4iWp0$yFgz!-XLkNHK+P+!uwll*rh>l48JzkT zQuIfV;)Cu@rK~xf!;LCbfWgHF)pD&$YW?wWE*}#NsgE?ABwuK#{*K%kx~V6N`^h+B z5QG9BoTjY&#>A$XUo5ymG}`(CgW^MKKW_kTbWr zZ;GDeRQ+MoCUw-cq29bn2nlW{s?<)2RXb8E9D341r*2Xg>!jA7Ntd14FkL!s#NSNL zB3tCQf%5DW=j2lZK>ZrKZ^2(dpp| z3AXaBR|h>|unZ=Q2foWSPh8Gqs~_{P(xY=vg}JS%SgNoj+SQe)e(UqXqTL(Yourgc zy6U4;peYtvvsao(Z+Nynev4bGVI`T?P#{}XvJ~y-AA>eaHEO+IrM1`lO)XPB<2@1y z22`73V`-+ju{2GHi6hWxg)7Tn&-KNLDhUaVS2Hg9U2V)oicS$Wqf>04KB**jKS6cP^K_=S{!jaqq6z_e!UdK81XfGZBsPZ5Re zkEufKdN#y<<6uO>Y(BO3%Q%#3{43+12?q!=zV>qnuYKie-N=Weu~L3EPu1p;$2%+- z8BdV0ZFQ5GAhO>lMc;RYmpIIOM7JRSS=;IUE+tjF(_i8( zemmWLraXzvU6pczU0q3tsQIdowKtt66L*=$I#GN~gyPRO0jdC)faO-8CLEnJCBtPt zW?R7t&Btjf@DO{A(g=*L+y0GWh9@hpob?^qmzk#3RLVlqSeka!9_DA~H>w_@y+1Gd%p$=r%!auuJc%hf ztC7miby3uCb`-ZGd{Xp%TliAP`^k>?G~T2A`GEE<9ZQ^Rwh)Bvn#FGMik*jwdZKW-IQO%DUCSGYQ$+) zBUbg#yK2OHBx1AGvZ*6H5%Yo$9IX+xeOOQEOpEUGbXuM zHcyAf{!JVSn&U5RXHB?@63*#m8))MTp~g`e#vaiW*==u9>JUa@Saoe4?&XksQPx|I zrSkQ%g*D0<0?MCa^LatWvw5n;=0oUXBI1pS*c7577L|#U&C?ugdVVFQ8ppxh1TOsL zO8$*vW5v*PY>_O*f9Jme`*t~7J;2bYIwvy2_=ymSS{mHdLs**8QY;ny$+P3>wsSX;HWx&>x8h;0GI{5M<*gc*C-VY;NM z!tXPQWZ}YRGx21QZLH;*6S9w|i2qVL`_5 z>a%y|XADx+&0Wy{15B)u_mRod@$P_cP>~Z>cBvfBt?y1 zq0gKyougp-TLa=%^}@vOcjSqx(b{E4?OZOOD{Th+Zi!hfd4zZxK8*==*IM#_(f zyV)LBh~El|LcET&rIv#33&-RTCsh;n;&A3?>7@7Z}DFA1G~ls>t&P z2HKni+>sq>oYSi^POX>9FS16~MKZFkLB)Elku?`j`^Y&qvMyEMK0FWKY?vCq_sI}m zgvszdh;(7$RHip~&B%f*)_X=Ub?xcYKKm6hAqBk2{uizTJXRXMU0&G*xb|LDrKqv5 z=O@?${GpIJKL3N_-TXpgzRV<8zkyqeEU=$|lP%e%si2vpLYU z)g24PK$pw14>!=w`-y0&MFizBtE{IwWbbPh!;5;=WQjRHM#a94MyCdksr8ph4Li?a z{1vWcf^EBKq9SGwAWgkcgWKH_iT*^jB-^k{Ly$ZkumZaguyh&L$hTL1%Ei?n zWve?mR?lvd)jF-pCyIjIpyeFHsxiKcl)bEjs_`jBaPv_850OFPLt5)sZX4X5q$6-5UXJl z=Q3PL7iso3yTYTHbur&}ggw#sZQ*Z5-*<&~N8h)FUva$O?07G5yw}CvW#lc28}6`h z(W_8zI3V3ZT*p)k!t9vpJ-ik+zHeS?IZ+B{U4`1D_gAR<&(>#Tsg_roMj7wQv+hL8 z6vJ)amw57D)T`DGN;66jfO!!;9nu!4(}avGek4x#pQVpVQ^eji0?0^??_(Ue43kR& z-{O|Gr{r##mTu7iW3I=IOQcY8!Lms&{+`S&(ctu+%}U?Qecp1P)Gcwp)xB7&;L>t6 z3LE!?moaB*>%!*f`!)%hAOG_temZ8i6P>5rt4ifKPuU5=s?k-v+BGT%-WQ2>+81D& z*WgL#!#a{adL@F6)k#q!b!^t@(}r26#)F%sI;}xdg{nKwt57tlJLOd?H=3jH%D|{z z4SxgM>CZ4fy8=5>SqrHw(_bNu}22SBt;QrIj!i z^OrT%CKC<#mB0Lyh?UY={xUAAn!o0P4qG`r;nRd<^VAb3(>#wRev2K%Hqr@%8M|qo z;yT~s(ANb_(#lV97r2#qlJyfmGW^ZxyHel0_@6TM8>hYl&daSiQ06d4`56mEbN^w^ z?7b-WcaLBSAYxwRU45o?lz}bG(eeLwr8nElT;hz*zq5X=3>xQ^xKY};RAhvy&!Qq| z8oQKp$NdlGd_t;m756>N5^wqE`z3IAsQm)-Pgj@C@R^j<8^1(;*t9FP!l(Y^6J<_P zQfJ4p%=B^PYX?_9i_g_zdZjE%^jDt}>rc2Rh2^0TQ`j9`;rw(uH#Zxh#@R&MS@TEf z2tO-ym;@8ga;&sX`olM9c}&sqt4P#6)LfJioYU;jRBXx0zTD*{*j#BSG&t(KCr8;K zl5rP7p}Hw~2N%C@liQMu4bhY0!p2;jG8Be`8CT5|(x?vUxqT#w%SUFUrH+N9y6&>L zFsGyDAvL!o?TMMRoyKB%&%!k(Q7Ei>Li`*S`=tOx;7~dx6~Ry{b+fecdyW??(ikx} z%^M#;BbTdgXec0zeC*B+M@7ph1%z?ysC8i;;K@{0V2x3h)bvZQ=TqE8l7axV1 z7i5fPGdK!58e;^r?;3YVgKW~LFyo1fGm%b7O*g1CyM z+b`{cjRKiPO~iV$dUWraf&ia}Uz(9AYh6ciuRDVYJ`->;hTJrfw&^sjW0g=s4q5D| zmt@}QTAt|Ijr;7Zx(WDf@R7-~)n=BmOuwArQzCF(+lhy_N z_BxZWB>wCi@n^5(KeL}fH9Ru5mzqKeUd~omre&Q#wxTY4tqOH9&LVR&zI_{n*^|j_ z6z)4CzDH}cW2J|`!7p?CQh@4-KZM+qx%Qxe0IS;zHPO<~Irg5vXWtOTqUZ5 zxZ~a?rE!w-W?W%$8C37BQm&Mg6D6g%$qxUCdjV$#!hXn^{gnJp4qrg&BQJ)Bfd)(? ziE%87ged+2p-}S%YXSRh1X6!CBY4d5@5JbjfFb$Or9^{)eo2WZbu1H_^Qo8c)l3(% zsUbaPcpt;NRj-O%)9^?M zLt`1Y$nWtTF@HdXH_uA9_pzGs^|gPcpldqP>aI_xT839+X|Gp?HZ^=Z%*XnhZL!xV zeFRlNdii3hlLoGlNkccj2n}lciA)4`X}Ii8VYgnYQ}HL6R-ee-SMSAAWRPO#jCAc{ zVV!cg_&v?euemr^@>D$}e0)IrkazBbE`Le_Qjeb17iye^xx79k9S6P1aP^%9eF9PC z)S$!)$$8=dhjlwlt}hM0!HYw_>f0~!;TJO8EL2=f)Fxn}9jU?tU#)cVaR1xF!?oR^ z<|{f(K4x@;nrARUvP-(Ss@~9=ogO1w_*k=@IsdmWzBGIhzhadj_2=YgR04hsa&ZY| zI5G*rPjNgyCz||Z!VkM6L@qP%17buiH=!$RHm_{A znG^l9)#zsgyCgO%aSa}L)2zXw<|T)z!K+|G7`zjBu`~pmyG!G_v+h4!P}F>RpV^$0 zl1&pSxMX-t0u~RU=^@4A;WYi%x2yJP^b17O;5EwPFTlmiPwuZ)I`^a$gL=lzJM9iK zarjBh9^-m9Uyw#^<(i@tLrJrIHgznYQq7Q@nydKg&33BebP~^P#(@x}!=;iv`o?%< zL)cAR2})DQETl^!BlXd!i}7_k@UF#q_57`no;q88Wa5i*VY~6|YossHdsWJ7i_N$U zG$oE$fzVC~`=Frub&{C?^?glJt;4aiY_s zeS6J3YA7?>say8ZEJl?SV@k$=B2;Hc5vK6rS{ZJ`+I^-O9RFrdy)pJXW-r81n4TQf zAMi55emOxr+>?o7>x6O7MpIcHR7(A}@eIFv{!rLt-B`)k@onQa!i0j*oM^wK+AkUM z(sPSjP|>mndmilh8{(*Wv!^YldeQzy=AT%vP)eviL;6b1r^9Qg80Isc2m+<4`GiX@ zS6ETlxG&Und?6JjAO5X=wWw**E~b_8>dxh!)3|Z@wkQKk(wxiQ1i23+ZeafXXpHop z+Y~?1@ou1Jt2tgNkgr{91`le~4rpjb!EG|Faa~toQ_Wt!XJrTy*ey_Q{?EKP z^wghymX{Wu_3dbX6Q!RVz83eS#rEW!{bAk$)px26>1%;d)1+x!u^0cn`WmEReQ%zo zBMrR!Vs(GH)I{%^l>tw+4#`|PaW)P%eqj4$)x$3ZP5-M3Zu(_ML%k*y)nw5+aH(}~ zIX^Fryl&brt0$-QGjgq-gBRuheoLD4wHO5!4NRA|DtNJUD&hGAQGfLJ(XqDhBm9~4 zo~%PwqPyI|@lcDrGm<^QMUbf{o1mAVWi5&5S=KAm^kjv2$reeOyc3eoQdOa*$BN}= z^O_O?a0+4bU5atH871gQon@t7ZwB=Wf6Z%@i*Kb??~;1C8Ku)voV`|Y=Bc1-7a||E z!y6xf7fCdc5WZy1!tfPYg*gYR5mFtAsF&FO?)g*_?#zymO5r61F=c=E6?!tPhRF_hf({iE(?w6bf|f2 zMk^Vx0A#d&lR9lS=Ml^WA>;Q-pRH`ILKd-{s+7f^42ZIE5^SgsG|xl(ikhd+=l)JL zGs$?Qf*B`DttLkC9AzIRLzXfroGl0=!f1>yd3j}D#8dwSB(v$@S()y^Nc1JA-gvT1 z1#kkLaq%l&Ua|4a22_t>PKvn%*$wrmjGg$KW?Ux3n3PBtemlr!Zu69Hg&H>~-t%@1 zS2?fcJBA*J7UsMvV+^w*AQft!xEA5@ygx-XTr> z^8&1l8h>w3)<`1cYUhf^2>F;$e=M4FIH7)DOob`btK`)oR9a`eP>)h-h)`uYtwVBd zUVR&R4V3CjLMrF?F{w&Rx25_IMaB|{wHnVfpNF*(HpiLG!dT3A(qyT=O-e%@wn}#N zF}#`GmZbL-!%o-D07H_^1q z*(&`Zrrken5pRWfi0P6o0_E#@ud$(3R7@eH)%M=Pko+02^fS8(H0EfvFw2Jg%;kfL z!_@CtYQi8FO4dCKZyWcJOewbPoazwjEO}4^U6TGxE4?HB)$aJRN6e0YzuN1Ps~Mk@ z=0dw;iVofG7xgY)FE-wkark#CqGr4&#glj&GU~`s)Pp)1LsVNO!n)$2o=}XN%#anc zu8}&{RLrnX<}(W5$@IRq_nczCl-Mt4+b3I?(TBmCX?U{0T3X2s5sa3W6$gz?#i;(ee%Fa+t6ypR@j1p0d3h9a+5l(Xl#s}5!}Ny3Xj*`iYp zGozWF6Jr#@=UNeZ_-d{=!`pi_(a?TQ*Hl3!3Pni7?Qh%oaSO)*%`l0(9uBX&Xk zo2hGM_A2}`D$w(^oyqZbH8L+`PcZc;@4Kga{F~!n2{Q+*Dz8KG9?-3HqFjZn6NzKW zbwcbtQp5fSMd#4HN6{rFWlVjOoYem6#$St5{+tY|#RXt4=Y|9ALcVEQoFgFCMJ*>* zCmYk6ptwu=T@2!N0X*(WnD2tdgYqS9!+mA7WMN9OncvC~_STJUGvAR7;u?ra140{m8LjjtAs(d&(rK z3(jW9yPWgX|q*sJfvF^Fmi)G0VDSKfMM|XS1BjlcM{2*M^pk}9yj~5CQ||9^H)e)U=2!KvJaG9 z6=#XnFBVm-E>pg=TuBAVQdEWX1!dwSm56xiq;zus7jZ>YV(CQ}qu!SC#ke*uwf<5l z$W(}0&wIXzjXE+*StNbcT7rsXr9dVk*<=uEtdS{%)cR`4wb7E6RU5W|$wk{aoZMr= zJU*)U@W)IUz23i=Syx}_FlCwZLTddmx&+ay3KikELM$G7+Oy3h_q-C*!>Im+KTRO@ zN7dZ}NpV=4Cw(^<(@3RdN-d4L<*-e=!o@eWSiXuIQ>?uD!ztD~H%cfgiIP|@(|i#Q z<#Kkjx#)d8FY*? zB91CBy#lju4oD!{o2E>Hvl4g5Wc)|u(!=-YoWak=g11ZXFXHAi%4DX1{dQHgGEMLU zC08aFEo6xxZ#I!>3FbGd2s*_4hRpQNH>b-_7`IogRaVbxniY`!Mi;6J;77=u&Bd4U zy}9TFl)681eBA8R@2o)PWz;0k#Z1oF^GCA+JwKCv$^OkgqTTlVip=M7-(HYDbTo>%QgEi#qkE3s`{5sp`reY&R17+G74N2jrIDUUd zW6b_R0`2o+A`{D#kpQtbA)2XVrJRI`V!#~25{yZ$EzAjr zQVXOvm436ZIXR6T(pP=DD2Iu3l>=UtqLuSh>bgeB63&W^0Bp&;NUP9MlCZvmYYAhn zVs5=@@{~N*n{g3I2=>fwc)fB+L`3;b=QyO3`m-7S20kvw)Ecp^lbDUKg&J9MxDy@0 zVjss&bT%#J)9-c&4>&h8OZ6wpj#JYuKcxHL0#qNNYp@R6DpfQmltRsBM2Xi$JR_BQ zVKpF^jWBqJ;T-7~DW}k2mqvcmnfa+dyTUIu+H`TiINEG>H!&dvDj$Q~LB#xq`^;0l zFd{?uPH;9O?hKub+e{lbAzhdVsogjw;S8AIXE8rFC@F>?0aMiYjOe8d|Nnwa4pSRz zD@_6mA+X6LPz^>YLHKjL(f%4nQTR`ME!siYI@H6`i|R3=h-oHbThCbJU)1=XQDk-M zQm4^RqG8WaCZ&+` zNaCy#Mh~d>qt478$sPzZGt7&8Q7>0*F*A-^BpZ-0Ut~md=6ux*#~+L*ENSP8?v3T641t0AcTowS`k4~te8XlkdCxG$N#YsPw*e8kAG%g^pC3h69r>2hL&FvdRE zA919k`%7x*+7-guTQZjd)FiP(q~}4??nhqix!X8NWZOI`ZMw9_H;iISV`bc)X;$d2 zo~iO8-3P^T7vq-eMANQSi~PE=c(tiNBb1$g!?*!xY`TX3jzbKVf*9{vTDF8x_*Upg z@BOj)XZJ*OL#(nEHLVI1HJu`7B*u1!p6c_3QXhCK^r*e5P)=*q&lcObw&PI%<^Ma& zWLV(Nkf$^Cn@<&u{d4Nio(ZjeCUx!05-c07BD_4B&VS=y;Unf4f=`5+rl%qIQD?LM zd3IV+&Y!BO+J7;ht>#6EffwoAZ#dg9_Zu4QH*7926l;dBu(@a#4e{^QrxcETOC9oi ztFU>d5qiq-6*W)XAvv-&+qm>vkrC%Z{wkj&RVQ$Mi=L@&Kb^%gdcikR$Fj0G_Rq$6 zoI~Gln z>Uq_Sxf3)?Aq)v$tiBQtl+W&6iI)8%^|p?l(W+injo_1odMM|&m7io!ZK&yd75A7> z(`+o>R|8eAfnxph#hN#%q(EWLj_NUmO&0{IY`EN6Nl-`4BHmVA6v}z2>Vl}h^`ux6 z<0`Z^)VMGp24b7IVA4e$n+8{c=@iSr=2LnA@x#)L3s;G>8B)vbiulLorW>`8({yE~ zk&ZN_5y#fFQ`h#AjZ|l}AzSO(Vm50{Vbt%F-lS>Lqq{{IQ-3U=`PhfBaG~azfn76| zp$hGqDIBBwGpYHRL4J!x)r|R)Da1~#Wo*SFgFPS6blO;oA=SWs|DQpV+F1K#Q41pX zqjE9@B%d|H?43j~%11%oBIg{*xoPT8g%8Xt13Jxk6Y7$12NDRUt5m0bh6on@J1zSC zfxl zVhV<)q!79f}xuB+T7xv7!>niEzFTlfO>cQ$o3S^_}TvX{F z)#E~qf8|_M6Pwq$dFI`B3Y-0VLb4Y$Qq4H+xKJYlG^8$CSwnyfIBU3}{WsMkWM$i^ zR)P`Hl}bG`n2{Zc+ggZ;8wrbgmuH1`&B_w) z=Qh3`HZLyD)k078Os&?9mto$}n5Hnrx67EwyKG6IAIvoFM$VR8jV7haZyWCbEqj~y z#qlo|HvYrem~m^FRKA`uiKF{vEQA>hz58yc$p^j*X^I^cLyCBs(DtrS(^qL+2UF`e z2)=0q##SbxzZ#GuWxXUV%;~Cr^~oTi#%EM3nUAHY!V$d|RRv3`#vN>zG!1xMXFem7 zY7xUp6nxd`N}|7}*|YAOROnIPdJtC1RYerARZ@Dq%+Q({>0A(0kk&Jt@`jr8(!>Z- zGW*+@Wz>iBiF z94Dk_sD=@~ind@pNi9h|U>3MdCtb#N@P0z}Gc+sKK>}>jt-dh+gPw+`CM&wNM8>=l zF}a#AlQ}=wNvx64ShV}M)iw|#f)a?T52qAsI^}cI-kzV>FV9;28RNY#7B#O5sOqQ7 zWUf1)Mty4z1mmioiqhA1B7Nmc4VMAi%@hdi8`K;%m`&$6m)x(Vr@j)ovz4i-lmn7; zz|BDO9hU~l`iczj(qveidgJ~4l%a9zTG1PGcswH;gH%9%a>~*gC11x!019#o*;XZX z7j4s%i6HMxgk4*V9e!G}HSO?>P|SX$Hxr%PpjP)0^0n~~Vy1Xm$G61*_==+DKQ zIGimM0E}@<>5?3<=@NN#pudAX4m>WRn zuFVXeDh9E6YVUBg|Bs9tjQbfXSPe6_;|N3Wu2D^h*t#NU%kRzB?MMdd~r3mZ1B}-~57Oz-x z^{mR}OM_L*uPI%*Z0eY6YJy7_S1(?)a+%}Dl9e?ps%?~uktn@G$$#FY%T}(esy@ps z-}s58m6aJc`oOvZb~mU#Bmrs8IAS z9#b`W%+gCo1`9+HCkG2xs8$oKSX{aI%F^o6$|{iz`J3@1!t(0nG^}e%gHT(3`Qqy3 zD_4xP!=rR&qPC?=gVig8%M&G-92{d3j?q|jC8t{#Rj*vMvT|u@W$^Nqi_3#!mhsP- ze**fFO!865OlD*JCWyau<&rA~##gO!^5muE<&!5@l`XC;U5d@Pta5SX)srV*URs?w zHn>ccXfl>1XoUnRqZYQZw5qi7s#489uxyr7{;#`)LkB#q zJncMPJO+<;X(W=)lf@I_DdDN*Nh7R6!mYp0Uo>HoLuaU(Mdd4(EG}OpoD^J8UOaD- zY82xqDs@uEUs}WXiE?myUQ}ZX3Z%6T{Db~kF8Y_3ue|JtW#FP;S*qAvTz;eq@cE0* zbjrs;zhdQbx~(PUD{GccsQSo~g8oE@d=k_D=;Sr%pP86`IrUe$sA|dL6+<9@82VM! zn6f2{u3TI(MEY3+`FG0xBa2=sEBcdjSo%XFk9;O%52PQH{Se6?f__Z)L#Kb{1PA|O zAEa?PD_|(qEBZ73@brg7Ug$gZ$L2p)0Yf8yDEhGq7&83{XS?L%l>JbvSMeoeo$mHGf7y7ZbHFWZc=>I3{SM+mPY3b6-7B9Kt2yn0J*KL2QS1qrqJ|e{F!Khzn z|04V=e+?~Sm_A|YQ1fy54=(yt#g!GML&?O&e-->I`7Bzz;_{mE#g#+nKdybn=re;+ zI&|{Lf4tK^<4r9oEgve=4ma9GpMgNlm8D0PzSI6!EUu|KlH^4{o%&z760cB3rAMj= z(09sTdH>8?Ena?k8JaZoY@qM-&t{vjN1DG_2k&fP|5g6t3}no}d~}{n%VPe21mqXjLAVX?6eBk>)Qt!1&+cFRpzg7+?*F(<30ixB+sEzt{kCC=4C}{^BO+WBsohz{dvIACbu; zz+asFT>PmChL6aavM@)2Kd1c@{_F{sq3~syzawE^CO9V$g+Ipx(~wy_67q9S(8>f} z+yvVYIs8xX7dOH9Kfs@}e;YW#Iy454g#4WSn~Oi0U>*{KM}j|R`w;#d6YN9c?Fjh? z0~Rp;m)nQfm;dGVA^bTOIEJczN5nom+lPz4|AqDuH-8}f$pUOl2W5frqc@Wy(0&v0 z7t_K2K7Y;$G+TZS9UKyaM?!vsSz!ODHGBm4i}U}i4JtA98K5}f3is|4GTJV36znBjGm-%xpU^(RHsN;{$ z;E|A@bNrE*KefT`Bejwvz+c=383%vX4#|(imOZ>YBL0hW{2~2Y{07VaK7Y>o75?Hj zm=2n~v05;5X}Q|4A2fHx2d^|wGfWP$_xCXSg=I@|bi;9UPc3I0Dob&tRxVkzd<9z} ztD|+|d|`QP{Gz!voR?T6dtVFx-GwZBKfihKVEma?6^pK{iY~Ivo4@Exwn&ek7{5ij zAhsk%{%1Lt=l)$LT=`ETe|x1*NDWwev*PEJ)-EZnkgezgVmPIv+2FnGzyW5eV!x!{6kDEQ6^mF%XXA+`IQxHR9wnR<{_j! zZ+_Myca4ZTiumQcfjs74?=;WxzH{<9lpt3_>>bx1Dd9bHQh%fYmXaZxxilFt-z8T&RS7CfS;Ms9|@kr2Va02fsMdX z9IWU7HUQrR%At(e`Tdb<;MalOz+VF|;o!tk1^tmO;5UHz(=@FI*bbaE6TWjb?FYbC zU<5cS558tW7kC};AaE6MXS|4!JNz~tb&NB16Y(aly3veUw0I=Z# zn7o4GuzZ6WmlEW4yXvXpvWcq#k>_X7KXeZXW6(uBTDKEO8MLSVxp(gQn7 zNDth&7=7RnPVEvdGz7LUMGt@rFDE|^$8?tUM@DnNrh#Mf<-l5Q6lg`C0^BdL9(~$- z9dtO@l70hnMz8j+p`MNhHga1J`V`p3Z}c)9xEq)W>;{$r4+3j}fuoQIa5S(Fm<0@V z_D2=~%YbFT2Hk7buiy{Z_9A-EfuQz1UC-z&7B0z;<8UDYz&79kU^`F_PIUrD0rvtkfd+62un$-a416LI zSqe-C)&etu>w($8R$vJDFmNHT6Ice^2doA50vmvS4r*-#rUTo6~19k$p0`~&DfCg|sun*V=42+__$B;iT2+Rad1ZD$6z!2~f;6h*punbratOafa zHURGfZUlA!+kkt3?LY(A349;87npW3`2#b6eZXvB;8^M#m<}ufW&*2$*}w*12)GHj z5V!?c2HXy;1?~kl01p5+0_DJ88*mh`9heF11Wp0&1r`Gh;8I{8uof8jB=rqU2etw; zfe!<-ft|n*a363XuoqYc^mE{@7MKof0FDE01m*(UfD3@_z%pPba5Zo*umxxUHv{{C z?ZCis)ITsC*bU4C9t36s17pb_I2yPRm<22Y<^yYi3xN&5a^OZ_9k30!0oV>~19k$p z0`~&DfCg|sun*V=3~*skG6xycfk9vTC4n34`;9dy_Y7+2G3sy!Qd z12^WN|M*vX&w+1X_EgG;f7L#Xb__IT&_02o0_qt*D|;sG7nnHhl*ja{rXMq0-b^#dRvylccb0z%d!*2!k2Mm1`ehSE^ zl6nG`Rbdx^wbkS|lXR=d54iDa@|#7x>yZaAdkyJAgf~zQVB5{ew~%x#v|C{It+a3a ztcKetM-lnlj$YAEE(ET|PL}~&fVIGO+DAKZH?R+A05k7^Z)q>U4Z!S8@C)n&9srho zm-3Dy|2xr7U>mR+nE8F|5HR~L>Jh!E1#Y4KJAr$F=|6xE(Fb7qB=X%%IR)N_z5zEr zgq~CXeZUschli;T%G2-&_GvcwN0Bcuu$A@&YPBl;H7x-W%N}cy^;62}k+XOBb z98F36N!l$u<={q|D3x|Q?`7bYI&gb=-wy653l{_);OPK|Drwrxl(hBUSt-F4BU91} zQUZBN=DG7U%6@=2M@t<3B+m?ReGVKhot6#G|5$&7=EsdoDQWAxWho*iiNAobOu~+n zF#aTd8MukyFxyuAg(|+tSpGcM6c?!)7=FhIYdG?5*8^&z&b|x1hk5rOlWXywpOU`L zmzNS;C)Kp!B^7UIaqf<+#;2;Vmg-_8E%DVgi_yp*hU!}3zH*9^~3De>L!P00q6 zmy$`0Xfa5d!ISMn4juiG3whTHr2g|#(%1MXuy?i+Y+g!0@+=|VCdQ=~#^+g(AkSGT z#lE0tWJ-4afJ|p46+vs9@E_`rJP1wy@fc`ie~Y(B$}m&t6-pWANf`>I3_|Y>Z$f$n zNkQnXhF;cbjD30czf4DvQoLsPx?$_}X8+B;TfAB>_*UY)Kc_$Py2N>uII6xWqE+Ad zQr`tBS!;%Q@1w%e$3nZprAcW)p8e3!$VcA&Uz2=@zD{57Z}#0RL76-aer{4^++4-G z**4MDH9i`scb-#IrF}h2+`Yv8l*Hvv+E6FB{owKiQ7HAl2;B`O%|}CozCrjX#^aYt zxN7?r{VZ=T^z)U9!m-rZC>?zv?r*KQ&7vpkrLC=_t!1v!XQgC$uQf|ct()pSC#ftY z6LzBrQ|C&cwGUdkv_-SNLQ-Fe>OViF)#F8F6DVDQs{i|-)x})G&O`AvJEg?C%*9th zQW+R0V+B$#xjgSfulT(F$Zss(ob6cEE9G1>jHckN8L3nf2}Ik$>?BQeGItp5ig^sN zhf=|3C>kNJ>qief1JcC}F&$2xdYZ8X<=YeI3&F?rhutt{qD zu81!mvR^OtxK7$TdTVhPEm?jNeS%0VikX|Fi5(s{oc4h2+QGZ2LsYh9gT1#zwRB$6 zmu%CDjUeh$(q&_JewZj7qP47cancv;S2Nuv(uut~%Hn^$*cdy%Uq{oO=gd#)WDn^Y zp6HJZvt(%1t136v%qq9*^ge^FiKADL zfL;mos-ai=bbsV`=wHG*j@jXP*sxSRTFIEXXr;_b^4^lDt(ZLTC6CR_pFNiRp?SGopmqnS9Zy|Q7skjNZ?T`1v_R*}JZ@p_I-l}+$%vLq0-0r?n(z_y*cpHn_hIW$U` z)B1(f=a=I9pMv=QXLd@i_x6Mw%}iRDsMIqhm*-yyrR2t$aFy?V@@->|>~{(BRsEi7 zQ>x#i&+zUzM82yMD5H_ zH^Z*KIKA;@3U33QpP5M_xW)fj=p~SGkO9P$=ldfw23OX# ziOO1-xU6Z3Wo(u;9T`u&9Q#GS{+sDVKCG<8zULF`Q=Teo8T8K1u*zBpJ(sU1^_rYQ z|MtS5?ZnQ6DpHVCn@9#x`{c3{l5aiv2AFGK6O}PWet=&d)#JwwOFv$ZlM>5#hSKBr z$v5~CYc0`y3!?eD?6C~7Y7U!kT)BK-OPp_BQZeN{h)$)K^+(>N|MTx)fG(l|DCf5hRad^MnCIbk3Zx#lBG2OT{>W!B(*92}p&0Am zTw}_R*}wG;^UWEPkoRakrosDb%(=8d-!GGyDZRs-WuF56VJ0U+`}-q*;9c@mexW&5 zV$dz#WGE#tJ&B=2`YdZ)!(eB&dMzQzLDKh;-mua;$H6hZkNVgb!*O8(&C3TrV*oys zq~$|*FXgCb{cEGxui`Alr*nJ}^FwJ`zRReBEL?Ku0Aogy)Me`@7!R<1_Nuf|C10hp ziTj!gUv=WToR#GJRN@dB3`zgpfJ_$rg*h8(lUta4R(ZwjfvZ2Al~Uo$PfT%El8i?e z9s|8I`y*fF-JBorA}=+~q~=ii$j|?Ff8=8E7v`J!Imb@3Qc9|r(usPKh*kX|R_$SE zhpvgl&4G1-8`aw%ktQd95^o>4G#9QHT(S%2ABFvO;bgtj@4}4(r@3&s;NGVk&U_bu z>vQ2`-SnUfw;Eip3)ceffD5-7oZ-UB`s;odZa27Y7p@!JJ{RsFxVy+v37?fxFLz+YheI zh3f;i*@a8~B<;VA z>j1afh1&zJ)`c^`Rl9KSgR5}i(wMX@ci}R?mAP=);PzY;YNW= zci}R@jdJ0pfJ<}Xioqqja7)1jT)0|reiv>%IL(D?1@}JtB%Jbl7+jwV*9q>R3%3tk zuL~#pQx3RrekKwP7cL##eiv>WxNa9t_D}6|;TC|~>%x_R+vCEm2G`}nwSe30!fgiE z>B6;x+wQ{c2G`-jb%WdL!W{(H?!pDwkMgh!HyYd)7cL9jeJ)%+xHcDVA-K&hTsgQ_ z7p@N6CKql4xQ#Ac8@LTF+*WWcE?gJ5^)B3ga1Ab8AGmrKE}6YJbuL^G+-etYBDh)? zE(EUHg}Ve?g$q{!uH1#I2Uq68Z3MT}g}V=2i3`^O?h+Sn54eRcoB?iu3->;_ViztA z=cUMn%K#U0;j+QyyKqI|a$UF*a8q2kYH-;uTm!g?F5D(?SuWfbaN}IK?cg$9xV_*q zT(|?^f-am~96j2F8wD=ih06pt%7vQ(F3p822AAx@Ed>{F;cCJ8UAXn&G#9QF-1~33 z>>s#37p@cBK^JZxxLy~o7u*3C&VM5I&wiACleI&&zU3Q>-z$R* zR=8#`8Oi)6Q8IXnNF)8hxxB}ZOP1R!EUKTZCSE&x0LDu^{xHex6(K)x*@94OdzY)V zJqg=PSb)6(V=voB`b~PcQd9~=T~#N`OKCrHYc;A4mcw;)JNNb{O*I#t9kc7_Bo4JbMZG5To>WV z@VZXIzejj%ZBW*-Rw_XtIf?r|aVzY&s}3JG6Q1`F_coz>9&zn)-#WEMwkjo^MK9*? z#}U4OxU#3>R*8EGapziN#C5*8A{V%|Vw-Dk^~7%{{i%5bqw-MW;F!Kiy#2)6IEZ-3pE3Kp52H7cc%6fwcM0*j2NAEH zczuJ2cOUUStbIv&_YhC~!4E6P`^3u~L_FE)v2YOaiilS+hHxXUZcawh8JN=P~9y>jK>U!Cy z6!mqoe4q3rBq05D5QaA$WS&yzhgzpN%$h-oZ@QjnW}c)pIjtO89qa*}$Gi3g@hN3z z+03Lbjoc}lwDS31`B)o>XAp0?#KV7D<6TL-q9oiV75*^c=_{P!6@<@G;S))+hwu>L z1ybgipM;3k_EDWbb(zdskl_2ECj(dTRdKHP=2nrPt%#pKy7gFzFB*gVeMz#nn1-ju-UxM_!CC zv#eX)h`Vm?Zz8tLNgX6^;9s#lj>X^_ATkP^?2Yq3$pk^7vd8vi%t~qX`Hq*x)qx8{ zYCmBRn){&nSfD@h72f^z&?J#_Eh87a1~29B_ebVSzA{Ooe8O4rIzBh0%jffwTP7Po zVrni^;-h(FPyOTMQ_j18s^rt*Gy$hiCDdZtGz<)XP;+M}< zp0&hT;E6Pu=#Pv<{>;075#jVb*8Vfrs2M|EpfnmQDmzGMN^AVKTH@NL$hU+z`%)un52*jK56w3}rOqQ8nG@KA>B`Q@ z((v=-sqEYu$+IqQpM~@{dx#@vCRd7`TV>{XsI~7c9`C+^n`=#*Iyy_!ZYQq=QocEo z*A|a+wMNaTx>*EHhbR}ILE)%!KWW-+OC+kkDn1!`}SY#WU2$PPkR&R12E zx6-4?DDrm3F#D~o#OdHHWR8!#-yx2xPGf84R-LvU<(-*Owjm|kWF`bBk!QV>_a(`* z^(d#kmHNyg&U()O$^Kyfu0iC9k9N2`>!4Rao)<};cSxRF2I$ac;>el2v6AO4gUGY> z@Od7D-l^nyiR8INm2*Iz>1^St{!}FLeaZ8}LF7qacDQnuLGKtl&r>DO_`XbJyPh~( zI1AV)d7dEX$uquvN;#{EQ+rY*@-t~O_YOVJ?a;g5&hy%Vd5TRih_jt@jLW2)E6qFy zv{QU(*>7J#3EjJ4#L*8zCu^dn-C*Z6OY*94v|O`YE+CGac~s>)$IQ#Q$H&z!KUAih zp;u(*nIw4*lxZh%MrA}IpOo@NKJnr8*y7;?MFKO+=t0UU=i-hd&tl&FFF-G)CC<)D zJx(D`#~3TmZ9~s@~(_XOiyfAT|8N^BF45`X< z&d~Fe?Zo%mc^*G7&s=1?kvQ8wABhZ;Hj+x5*cyWJ{}}Un7plRSxwImCTtld@@&ZXO z6As*jdO0EZ&k&STcVJTeKdKFun}w2ziYIW%7LS@m&Dd>+dF6COOB zv&GVelL$Y&EuO_N`>^AuebD<6`4v#t{#Vkdf5zpG17?z^mP0$vvhsX^I77^{7<#Mh zJinJX&uZcuL}o}XPwWx0QnA_N;1zWW}L%!mC198$XAuKE>65MUJVgv zLqR0+w8Xi>%rkNSs)pbx3@zMUEFOf}$8LbuSo)rOrHpf=jEuiy?y*)3zMVJ+IdA`< zw3YJ)QN|X(@8QJb8BWrf`6e^4zl3~Wl6+5^n#r>>ee^8qW|BJ>0GL?JC%xRA zRa_H^yeH|8H`CwDnRol_F6#%@SwI;Z_6{6|;&c{6XESutxkKVn(TO)w66=I>ZkY3Q zPsrTxz=?4+Umb*IA2e^lX5J>ey#h@R;W_0e{Ex>!xHBOGvAxNoRhH59xMLE z>0oSIEHm+PIs!RFcMUM3CV4G|PKE7DZZhet_i|2IouxGAr~-M+PcS|*h%Y+7-pe`N zb-sD*+4z<@Varq(^wJ5QNZfYj^`&~vxK~B@suUBpk1(k}Gt4}rKANB9gl!>@QzcC8 z3%N+8lb=F&J>lJ?WjfoMUp_xdw~esPhe#W_<0y}Qj_-Dl$0I4VNW?Sj5pVVnhs&G) zOnNz>{#E_g9)&pGL=0c&>Gi)iOrZ)*RMKSNuRX1vCw&T?ZK8)=^9}uOGh&jJ_HNHU z-vhdeWTB3F#PgK5CrNA3&v-3K1)O8Xoqf0WZSS|P(Y_g&+kdsTVdS)%uF?LMrt|*w z%&+_Ye6{xGsruip)_!=V&U?pfov6=UD3}{A63llllBjQ#3tozIo=G)%tyll6UwhcA zzv~x#a`SNg;0W!>;rd%6lnm}T!E%G36eon&sIC%b&*Y~(zd&BFy*RTE3=c6mX$^RK8;0s02U)AqT)qdmA z?@HAk^Xkp1+VfuhTdCS>e*K0N?SNl@AxV37xZakcy*^yOD@9uu&~M^>g#LJv_RW#{ z&LpjUWctHNTKCA~?@iKnA9XL515Qt?-3sRq`t{~y?J1pXp4asUleM0J-j=N0G(!JY zvi9f*{d>vU9Y+x;DUMyp%LeZy)Y0QU{V`p8&JThB|1`8uB24G^Qhk?SYxG|9m|tu1 z=?^GD`la+lx#Kj?@eB38`h1shP~Y=Sk6!2Zyy5Y_?$dX8iQ&~e*V(eH&XxFMJjYz1 zKkA{SU9VrGw|MjiJSWD-D^l@TfSaQK#P2Ehec$hkR!HQIV?BXmApBYVlo(d%e#(YKA5%cD`+fU;`mbXd zsv%yXT-UrpR2F+I09V! zUOhN|k;$?8?>wITVQ5^uOyn|x6ZOxzgyZQO| z;(yAMJWc<$-}9oU$$zgs zZ@j)$`Y&UNRNbFO9_Q*aVw5boo~%FX^;|IQiCAKwqd?C zUj6&SeC=NSC&PSueBKXq-+j6s)}vWO^DWZ95JlSh`ZfJ^zvtUt?>gQ0bFcm{zwehm zG&mYfrPcLmzDe*MExquJu76GcuHVz*^?pNiU0>_>ZT0E@^u?z=fpll-bE3#tzPIU; zwo#X~AK<0>F|5&OD&Iq={XpeQeA52O@7v_pe><48*ya`bTRzX79^b!wzC9imS))0c z^$^hY(|sB4vZ{37ROx=~^X>EKcg9Njq3NFXd%olGJ@1!vzx#Ld8%BQn|DF7<&~Ng4 zdOf~=pYJB0{ zI+S7Ee&5rca}GEZNVSn5^?8Oq(OsV(E!|vCUakI*k)DU}B$8NG)$dR8z3tUs9O?U= zU;oQU&s)0wtt8*RVfxpTeD4nPzBVv42uqzmBhIgkFL$Mv-9g{p`uhnlH?)i!5q$h{_ZujcH9PYc{r$02@*X{S-J>2)>VfwAZeSO0ODRW+mj`Jlz|9- zWZG{%FkpD}H$7sTFLa0jKE`@JdA7dM=ehRNn|=BZJbI%~f7~P-=yplGh$rb$&!l%e8qTx&Ir5A8_dVJs52F(Tr_Tp_ncf-D zR_p9pI^faY9;U7F>2<@k4}ALF0d1dOe<+~cBp&D=bp6SI_Kjf@_}gI;@tc7QRi856 zb4r$ekI#cuZ}n+!di0bx*t^Wc4Tdnfx{V5x=L)^IPvdj+P~5 zL(fhj@>z^kuSPd* zb3n4xYxwo3=PQ5pYWRsNet~DwnR+9->(PH3(6`aUkI=h4`kw;&>t5dn0sS_g{{0d9 z4~OZ04Inc4e@{UF$p}Z~Tp}#Nzdl>P$K$}~o2(xFCy%emr)~32{<=^5p6^)2f8azt zN1G_4MM-=3F^r1z@qZsn;A07VEP;9p@ycvreDR=RF0U5P7Qnw2iim9EoD*JY)vaHR`c>AHRtU%qZD zU5Ayf&Xq39O4o8%e7Zg>-4-idiz{8OT|QU3w18O;tya2LSGr=me6DnvR=O4|-4<86 z61#ki-<^NCR=PSXU56`OgZx`kG{3M*ZgD_xykK3BR5D_x0|uG^Ka#V(&KU4xab z*h<&yO4n+auQk4Wtya2RE1kw{iu?tw^jqxmxze>;>9VYJX|8k~cKKZCx~z0TD_zi) zuFEc;E1hAbOS95txzcsp<@;WI`Lq$HzGzmuTvxhYyL_&6=~lYl4JN%}S2|5?qM%*6 z(q&ocx~+62u5@Wux-?h1kd>~>N>|}Z7qrrK-5FoL5-VMYm9EZ}F3U>S;%c|GR=O=# zx)xWuT)TX(bS+l8Rx4erD_yZ&K3BRnD_x6~Zi_2jiCw-Q#+R?dN>^v4>u{y3u*>I4 zx7SKnVWsPGrK_{c=StUWr7N-0b-U8F*yVGj3yd`Ntk_D|>q^&Zmv3`?`GQuuTq~Vs z^#{@RXP3{FF560%Wu;4VrR%WE=So*>r3+f=g06I3cKKZC%B*y0R=O-#x^BCCKZq}1 zos~|r(&f6+_1figrQ2ww>%G;aSL{ls-C^Nf>9$zux~+62u5@Wux-?h1PAgrPm9D~- zE@-9ea`h|SR=N%=U7agkmYuFGUM_uBx-C|^7FW7lD_x5#U0RaW|5@o;UFnLgbj7Z8 znO3?ME8P}Xx)Lj0nk!xI|7-7Bz#}WF^yMX}JTeF~C=Ubj6o`F0$t3ZanMpGflgacZ zGcXM5O{cq)G<3R~?#?_=FvvPQge8mO`Xw&-&mKqu%_+gg0LB!3WRv=3D1^<5NC(XVRNL z>w`~>dGi}n-gv_3Gs!twZY;)9huKC?dfMAn<%nDfR9KKNkXn{SPK z<5NC(r+7)k`~uAS;1fH%LGBt9FX?^4fMo4$+9yzr;Y??pIPjlWDs z>366$;yY|j7oR!)-WQ?of8p;f^2Rdn|C9PyVB)}Hjd3Xu;QHRlX_(VNPBWZNa5~B9 z6sOai7C4>Zbe7XOPOV!g+XhY>ISp{y$!VC=K~6KAPH;NO=@h5aoEA8p;dGYMIZiE( zFdH~+G5Ar&F9xb6VhZhSOP2=Qy=)(^foKA5%&1r$t8BS+8o#WK{3g^#hBc}mQJ2?$=I>>2;(+N%|Ii2Ekn$rTO zGn~$HI>)JXC+E*;Bc}mQJ2?$=I>>4JVcvdY{knB;Y&?0((0D38-e}uRElq*b?Q!wd zz9<-M3bZr@PvIXZXYc>N3LV$J*3&QPzhbwsw{@^J^HcieYfLzn@j#e57V%C=z@BA% z@)076_ddd(%|Jsrg#zL2lvw95{v6|tj9mZ5od6GeW#^f4C4n_%0g!L>a$_`JDoV=Yui6&)o#v$oS_Om+MpKGmiGy%R17E z@O`6PEbFT8dE5ElS&p24k#Wf|+;-Gc$Dd_f&htqAr4R@pU&oIC0^)K$N%C*m#@`=Y1u=6)HB$(eVcv zm-EY#KeU4QI*#vOK{=f~&nx*?9qNrA_bS4}8Xq4qf^u}c_b|c-HU2Mr@XX=F&uIL; zR(j*tFg~I2k6T529lw?FNsYhu2;%GbLyS*p{6QFiV0m@?4~$Q1e0;t?@^$=B7`Px_ z(D*m_;J76g`7;_HpZ)HIZ-#*t;-cfU6W+<|jFNx54}Qi8#1Cuy zANb&Bo=E&bjsFWDeE1~dXEgrreenFr#GlakFZtkCoI?CbjepeZz43oxd`jcLqSFYWT?pAz!M@AJWj&hX|x zoBy~Eo`1VH|DYake98xZTdz0&7e2VNk@x}j8xTf{&+i!T z)bOD`;)gZ-&x{Xh_?4T8pV9Ec!-P+0_&+l~so|&p9r33${OgQQYj|Hj@e3OM2;(yv zJ~%-9Sq*=Z@i`68Y$m?N`#PlE_TEBxgN9$rc%z1&ekSn)8eU+$Q^R}CB7RuIA7Ok@ z!xLMHpV9EYFg~H-SDa1!New@08{tzL{&~iyHT;d|5Wk?|-(`G8!_RpK@n<#s_l(bJ z`1{^Te2e!HNc*pP7vT*W{zb+cHN49qen7(?X1r6w&mAOwSi@grd{Dzb7$JT}!%rO| zd_u$TVSG}<-w`GLl!iac__T&!8Y6x|!;gp)KBM6`GCr%}ZyhH7oQ8jkaf|oMN&CNR zg!l~_{x`-OHGFcE_yG++B|&(nhJT&$u!e6rm-vGk{xstm4c~np@h3EVRg&;Y4Zn%; zDGh(q81bhy{9(om8lF!Pe@4SsrU{?b@Y@)l)9?)$;#<65O4@&x@dgdQ;C$jYYWQJU z!UG!qCB{27yeCKeu!jGV@j(s0Fi-r9hOZhYd_u!-WqeY@`?eE*O2dE8__T&!xr6ux z4L@xs;WHZkea2@s{M=o{pVRRDFCg6FeM{2*pJ80?E0cKFyNTbZ@qf*@+!rSKm%WGh zof`kt3kjF|x+MRHj1Ox3b1x#k+?OT!`@NU&35|apw<7pe0EIW zJY<74apvCQJA@75bN>X%4}6jY6bQE7croE~jQ^PND;byjS*858_mTX;caeMuW5j1S z;28JHeY28pUqbwX#-9Kj`Eq}*25?c|m@n=BnyXlUjekGj$e(4tEbkHj!20w4O{xFgfFr*XC+gt? zVYc`*T+RAx{M!LX{uJ{=%zuIT@|=T||Jh06TQRr%X8;%W*W`chABjJ>mH4tgya>3k zKkr+W^8e);;un|?;j#Ga{Xy1Wfd-a$qzGs-wzRgQsdtWIPxc$FYDXB*RuXP zA8_Q)>iiG0{=BbP>i=cHk>9zE^ci4%4*Vz9U*mrRaO6)hAHsX_IrJl}zsA2CaO7L( zkbG&Ml^(_4p-&UP zzHKMlChU*n(kIpTNnLg<++e;RP1zs8T1BAAK*fN-mfk7KmYT@Z)E*t{VV`3^k@DpobP2fvz*UUKllRU{a;}GX2M%2 zv5x#A%lQHU=j=xpJi++QchmQCSBQmt2;^kO*D~J8cst`~ z34EnB$osw|KFjz=G<@@yDBp`Y-|w@WG~jz#E3E@wp!z1|>@`jNzHbvz;wJ!({7#L3 zGxK*dU*5-im_M!YyTRaS|3{e*;l21o07vF){AcaQ@4}+Pip-41CH|B zHU43D5dTNaZ)1I00Y`pDC&@qN0k{6w0gm!VdFP?b_w`>T{ubu{ zJaUah_1`5$-p2ae1~|&UQ{#ueLHyksKLI%M;YoEug4F*7 z=HILFkNqa`Pu|*{SuL2zP`H03px?;}1yi2JTPK z0v!35Y5w5}=8yi6`0r)cUXAF#ktCBHqoESo=f!LH?5(|AT;|{P#RW#2+*N7Us`r{WZ1|&pS!^C;Wu?^4!1V-!AY5OP+(elJ!~lZ^W19{U!f<0*CP?pO2F5 zV&LD2FVFc){=))?@f4qrx|rpk^Dyz{`F_dA=S>LvGyi?ekN=eT@?5{<|5o5&e?Dg= z`RPZ9FVFK!{)++!`|~*~S>CJugZT0szvLhNDCr~5WnIPkd{N-gFYq~h$zS_3;>+_| z*D(KKfkQjh_#;0jzC6bz<^NgWD=m3mU&?>)FNiPCcS-y2{}|#B7x8&*$-hS6V1GW} zbs6h(^epk^`FzR$yuhLUGye+aw>?gLc`je_zb$aEKc5qm{J;B8;>+{+lK+^%!Tx+s z>;o)+?3cus=kO)}1%X5TXTH2&A9#ZJ^8CHzulyC_Q2+V7nJn-10$*v#bN7C4w4snqtf8sa9m*?4J zc@Oz5;xKOFbJ9}&^#TX`^Eo%^ubO{He0iQ;^6wQm*nj8V!t#=T)>Fin=jbK>w*m+I zGk=or*M+|)zC1rK`78c_IM{y|$^S>@e@5Wo|26$v|C{*o9G$HH_X-^RzsBD(M|^p{ zPV#>xaPa>cfBVzKm*?*Of%Vz{e-H=%ukk-BaH#+9cH8IFKN4S_i%k%J(|675B{V#O$-}@)x%X9FOfB3VAgZ-KRLDv6ffrJ0&bLEo1_RqwZ=lwp! z{6_@7(vs)iC4b~O;>&Y@vVQ(a;1HK;@-O)d@#XnI$zSnT#38QJ_@5Fu)c*-8uWT=G zeBOJX_Njnl93r1TbSmpFA&o9#Dd|s3DP9Hg=ET`e8tOzIPGTy1- z7c#CtFX@AT<9y4YwqN}^fgf(k^K>$o`&dpvlmC$5A7;t(d6Ms^|I>nhs1^PZ*+d9OO=L!jhS64BAgT_Bw z@Ly3pmniv;;IFdub6uk>C(LprKQHiC!-J{zk&sBQ0Q^uFLSJqWILy;NK#?lAeet;+ z@MF>6?niQtVEpHdKld`iA#xI*1O5iDmVkO|#YoOwlvqK)4@LYQ0`NV~_~~Q*bR*eW zw(H9QKNi~i#KnYn5Z$_u<;?wtgQ2z|jtq&rrTEqr{3b9(a;~FEjr4 zfD3zGM8M_ylb|0nzwj!Oe+%2;DaIQQBK~o2A^yvu)}oxiX2L(i`g8z}`b{l0Ps%b1CsZ!g2z8kv^S|6S<4884x;35TD0=IV11qe9P=$)On3{4u}<5M_~ASe2f0072)LI%9}|4OSV`{i;XmMm&jOD3>jAAj z9tsr?>tXn>qz}Fu5kIF1oc>n-4>N!AC&b5hBjab(hkq^LXrJkw#P8+y{)7+zIUoGE z{k`q8L*Ov4vKPs3W__;n;ok~4>XSK!3c8Ebx1M5rvWMiXVEmK=`2Kbg|9Se^dh^Q( z?_@c$-Cf7{>}ulovz+fUK6y0Z(9?*|UjWDZQsDY8{k#no3GYi`1M%O?{56bQd|qDW zyAg0KSHm@=;5n>kp7~P{r{m{5#_wl-_-!Pgx7S!3U+HbQfaeT(?H0C3p{ z5kmie6mZcl-cA|D_{S$$&dhTJ{D#C?|1EG;0{wmPfm}adNkn|-B7RO`eBzHJAF7S` z^Z+i}`}>H0EWuVr@cE09d>n95KTjlnh~+%UawgbsoWR%o$w!V2eqYuZMZ7fxxL3RW z0N|Lf#eV)QzUcUaz4=`N2mTYJpzNojfTNrM%!}aXPD-rnm_Lhe)`Cw5+xeS5@*iV4 zdcXXNLr8v@@8f|ie=Xo>=RvmfNsNyHj`eotVaoR!&i7J*Gf7GA^1&ZsITIXctYJA% z`|$T|pmx;2?I_H7oDMj~4-*`x$$n}l;Apo__S^4a{x=xc{rMAs3qSc7mFsfW$6Dci zzYYZ)+jUr*e@h7*=C9ap0nYc+fTKQzILW_<?pXnn$40FV11LJy}kY#+D z@1yiTp9I{iety{p|2g1TuAlOJxb#;IuOj?Otsg%La4-392HdOu|C`E7@L%YVDT;r!sd-_CX(68w@L@~=-Zf8w=7+|57y z7;v;_J3r@R1LJRaHP=JUPYwf)c4+(-<-3IvYZu@s=K%H(QqC8dKlL*rUe9_y!1x5m z13bL2{vB}PH?(%q{2IaoJtXHK)-wXQSG(BF{MpUSXFYHA;eVg`oh<(hmcQST-tu1y zIO;jYdh&3}`WWECo?2Y`O~CQK->BX91CAm&Gn}s+pPmZ1SO0q!;AsE36=cY}xm+26 zv(id(G2p1rwANm32mE^U0~`;$ndST%a4gqMJITlQ3ghR*qbc9QkBFFN{+k$|C=ehY z)78iL^eu!x#QX%~vrmxRhZ(=0af=5=vY%ab4Cz0~^I+0HtOXqH(7dM6yRRKVg`s!O! z7Px-y5AxtZ~~D~Z2~=K=o2xW(h>c2d*oKiS(3+kNmK2pq;ie<1m?-8}_3>OXfZ z73h2}*NLZ4d8htF{FhN;y`AyGuLu}s{<(}d{($h;F@7cB;y!Y}-^}{|P~se4$@qN5 z>%Gf0CUCgFV*cB*E@l644d5uhf$Q@smj88$Yj*yXkNlUIug5bjr+VwN0dUN>Z~_&y zkMqs@@UIp))aU;o`<%k|xygtB9l){PHfrsav~z*x6S8c*E( z1@K0Jxc@t#mE=!wdw(ncd?Dky|M?u_(;UygocXI(qn!DlR=^wQ!`3Fiz1r^`fTNuQ zJnnuKU;CiIVc&u_|L~%a1OEShvd5$6K#mG9R8M>`ibk$@EE`yAk? z&uo}(lq}0p&=F&M&-@SZWk2#(z`gF{AmAupAHQ58vY#jinFcPwuiIv zxD$!y6X{fV^%{{jWg^*pQ^0bz^$q0Po&F&w52WsJPhaa$I-Sd}&h5(O<70M9Jey5t zooFN-(Y&3@cMo(qp}v83#|}b%DAP&CbGgt!MEnwJ zcQ!ilY$O*Ch2g?xskDai_2Eo~M zJ{?Ubow3|Vu8n?nx*?bCpb}Wb?kHZH$!vQqf)Ovk4U4>;j6H*vcj0 zbRv~smrRc%KT!HsB)b)tw7n?UDNA0x$z;V+hKA#+Z4@YAdi83Z@c3ppkRqSe1%GDcQ^wX;K3sg<1wyQ+3s*( ztGLN_%UF7Q+!+Eh11B4|Th_v#eUa^nkqEYf(6(S}s`$+riRYbUIvPnju}D4=+L9b- zc0!J*61{yLjx&-PccMFY+Cir`9X-#I#Oo?@{{L#`IaO*=Hn~9;&N> z3@KC_3uWV?!bOtxtU{`VmY|R^3pJ~4Oi?I<4$6t5hYIHE(%Ot`X3l0UO1ty6S)EK~ zSTS=JYxcHS68l&u2c2x43}#lZn%gUvhpsW|j72hgmc6Rf%bdLe^Xr%D!Rq6{3@T;J zQo)V|Emapo8Pv&~o!aNyspPHdVSQdFTA8zz+QrD4RdQ2xGL=E4YHO*IU)X~zRi#qKEER0i+MnvF>f_Q) zX=Khu>M$PLonoWzL5SoNZKph_+F@Pq04rF{P0?8>tRavr&CaWJn=%7E(PTTBsB^*2h3*^r@YF zs`H4oS$)F$Rh>TO?4$Zb?BR#w@z_u#dR~2Mj|pwe*+w4%<#!}<`T7`U$*P#MiaLRX zHHH`aVO`z{#_D4oV=5W5QnOx@6)QoL$L$MsF_mGxjM*!w-ohNzD$&-QNNQv}8Oheg zROVDNXC*a4D_JR-NX6@ABvTq0vyrW81Z$Gfc(N|lXNh{$#va*tZhS0We|uD}hdFzw zA$%q>4%^)7u?@*yk{ZUW5mW;U*slls51o9vE*4q3D(0-B`Vg_-D3{Mh5+kF~FV)F7 zrZh5VBXyff40r40(@q*U9M-4y7*WTVb!;`BVe8bzE=yI!m_>r>4H9i?m}t7FVM zuG_@w)W5z|p@=byG^;&RX`DHnPUkaO*w$DV>zGukHkQIs0gW~HAWJP#sYNZ-oF8ts z18c{JhvV6GunQe_ovSPTxS2f>NhU53p z>ALJec7fA08=Y7pmw`zyW9I-a|af{9E9IlCW z>PU`_SE_s!M4`*2T&dpGSvjLSW@}-+W;H0mj!oOryGONKsb8t??UCx0YG1w8GqnY> z7Bw%Zhg6C?u5+uMe_pqmJ%2SZnjRsS>RP>RrUogvtD}m2ZZ)x(I%}-$pQ?MWuQmZq zw|d`BwW6VFC7-*eX|`uJ0qm=)*ho~9CZNW?s8^+%cxs}Q8dIWD%I=Vg_R!QqIkQ_t zYN4UAof$PzPW8D<*rQPk-Ilf|q2?M@yept48mawKrTYVFq0o}o#cQIJTFb>UQFSZi zHPKC-)v9)7x)zGL7Wpb)6t0CzMpk8Op_t27m9InALM4NXe>GQcX)A7(s8^Fkuu2qI z#^tcUVv9XBF-F&-*_LYpR}EfT6{(R@rC_+KTB7?gXB{IB+MGHryHwtVnN!4N zmufqCgX$QHhYV_@4t%TH*=epZEhVHjXA#%vr{b|jjr6E|hq|%b;+h~|!bsJeWsD8% z&8cK|HpHA>E>o3yP27=E;V~?8^-1+O)l9V+w2nFm~*@QY$oSRK*IDL2c9ujm>LMs?eyZYxpX!!(4c;)d zLTd`ur$W1iqNow>y(m?qN;VIeGn1!MrMgrnr3~&;F=#5I&z& zjaa-DtLzEs#_GLmE105Lg>b*mSs(XWkxjUwI$nLX6WuB{{ z<&8KjnWlHuWl5nD1?sT4P;>RL7FB;rt`_RKCf2K6?<}uIm3u>7Q|zo##V5RLZmY@{ zA!?zL%U0DcN7O_yb>z3?jT(k4m^xut%@Rc|47Q{PbJRp3^$u3NY*BNKDqg^-g+`{9 zGHRlmIwUPEvsC_Qt6C`M8sLY7@{@t7KWF7FEqPpNhIwKX18g>XqtTC9{MzQBR%Y zk=9cxUfuOGx%^TK1G>UCp4QxRoShnyvvQ@nS8u0cY;mR*_toWA*mO#}sJquwJAP$0 zy;Sw;-g133EY-idYiM>6rzW*fjiZ#?tk%SOo6)t7qUc!R!cHyB=TZ%Ox9XO7YNDEY zXHm7P7JKGt)@!#%lkkviCzFh%Qt-N3ybEEwElgj#;M=D0d}iF#OV8C|+Y$`i0?!%4 z%VY6mJP%J+?e7`LcjR|v;!YwpoQ4;oZywmT$jvPKP3<9vUa;Qgh*z-Nb`aj`4)3Xj zRjNd4q&o_)g14iiU_yKpZa&u?4Z%b7qM;DHtbHTCM;>_N(R_CppzWKTfi`C#9}Enq zvpXW$n3DkQBl$F392tQZ(E~Y?O=p?{)n&OZ*o`la4~2W8V3B5LzC@#Mt;Ydm$vsyR z9+Y+e)EYl8tBP-*@sgLi&YG9pmDTc>TXpN?mviZl z1SzMizPT)ac~$4JluayU6?>7)mcR7sd`#7Qi^?kcLOCsaxm{c0s@UdLPKkZ;SoX5I z_K#J)*RGrrFPPJ^m)jK@DwAZTkzhsBc;y(NyeyaB66y$vEul_lnjUT=NLG2T8t_t^Xl-z;&u5t1F_6!@88tL-bz@6W++rYKG8S6nU1B1mfTmI6k zV{WbV#(RW?^IHD0tBXTe_EPINUi1aCs(;B#ZLsDgcV)HwC0Cb+s@=R=PM;TCAfx3k zwL0yBrCv54=aJd+mtOTdnnK2_`@(rGd)ZxcFg*8Ry39U#EPGj9qi*!)OE`sEP7^O9 zzxvt$*PWKr2HyD@vq4QB09)Pyn>v7Tok|b)&C?3rfJr-Vmb_cUH&@)CQ_gEPmt43q zk2A~dO-nAZ>OGYi%SyNPmD74%Tf64xWvtegX~J#B-)UHm5z5VU`K_U@mMC{e30%yN zPRq-4`E8+wl8P-#!xi(UC@aflx5WJSXI1qc*K%6M!dX>Z@^+_x2sU@d@xm?9KvhaIM{e-vJ}8-^>IrJhbI9g7P_QzDm*$lathE=Mq& zhS!g;OQy$T!T;MOu}dZSKU9zws#>YYcB)q`o1)S2Oe7WEwLY7O)vh3rVPYhbPoz^$ zdUzP)$dPm;2@l(a-N(t88OXp6>+w`R9+O+ayUT9g3%v`y%ouGEMdpQ<3uRgE>(^C-mcm(SM-z4_ zV{BAz5UVQdF{(4Ewa-Yl8clhuT{G<}-?O3Gk*_wXooi})3HyjtHFYsY^;Oqns(C<; zs%Y_glNv7RX*sHPVz-`A&D8nC;+Di}nh9N8GuK=wtMwLHa$6H~RV^U++2BcDNQROptRJFZ@nbh62V?xWWqNnjJptg*t3`IYx4@HeD zJ>Q~hDy}zbQ*|JV8dh=cNLNydsF|Ksu1l$Usv?Z)sV`)@mh-CV0q~}jgw#~m(u7F` zOCYLMQP+|Xqq?dypO}p@(X|9+6)8rwRr`NU+e)5NpxahaH)vAdd83i)pVy+RtVyc5 z)}`uK$)ul`)`}#fTC1xTSU1o@%8gyB+Nvm{dYgRei#e+GMn!Dl#o*yoG)GEEl zJUO-2xKtgNrg2G*QH_l~I7Y1+Y-FWonAF+j_2y?)t)upkWK?T4V&l9tC%d!?q6u3L z-Ai?{TJNVALJ?+E_wrW1stNHF<&~;#)CZQkkZDeR!#-S%{}g*wxbp4cT(G*M6q9wu zwRVnue09&bQMJ{i7}ZuEvsCri8(rCw5Tm-Pk+rs0ty+hzi!rLNYFo@`$$^JkNKJJu zdD1F-6x6jO#Hg;SKhSRA5|mZkuTgE)zJpgjDt?=R+Q3{gYNlzm=Y3U8T{28+>WbcQ z?zQ@ze5$@4l1OW1jWwE$NAhtl%=+F^*Pl{IN&Pti;nTY=t^$WZFg;B`v`Z)WAkd*T{g4 zmbz|l#x4f5Fkp)XpFvRe@W+frjOk*)F0Q*%#qJ3+7BQfO0b984iz|XzevfNR7ZY|- zM_>BplRB8jpgJb3qxROt9WQk-iwQjp*kft?a}4Jj@L(Py~Oz2_49_s2U+Cy$;t3w!PNEH)SF|&$c zsHzxKrWTedEqBzR#^{!*S{W0TQ6ujqtk4*$H0HE1VH-6L)@)PJDyAXh7*xlEbyUZP z)=9_GWAS`;R~=fEAyrIRMQz;JGgY^eY^cTGS{soL!=uuN3$0?GVy(RlLM$-zLg z!s=_nGOE|ZI-{&_SE!8%)0or7gl$x(r`e|7K6r`h7_g3Obsg2wb_Fhf?J=xTt!z~C zJr!+bG~<>%ZB)HRCTwJKx4R)D8CJ=Jm5lDKH)JN$IvKE&Yxab9fz~w)Qs)Jis!xsV zQ)>Rp@3Xz`SJnEMu#dS@3T7JF;!lCEWJiJl3%Pm_uF*=+SckyKutp|qqz(#|z33%R zRFAITxKcH=RLKYIL6)jsDHE1bcL*zWN@;ubFqL_&OxQ}DeDJnaupY*$fm#NvLGln`mbq-)@+L*Qtj&iF!r&9ylEKM5|wo$7yYEwGUQ-?s(kTUMg`WxbrSUgKR z_1(X0PK?3+e%Qeu&E|9Y@!{d7s0GycNFoO>lXmiBPBfWL#dDV9#L~rD1Mis5Ig#<5 zRx~}9!Ix&o@Uehi3}+Y~E9OM9*~l&@o`R`uYd9Mji#xILv9VoniSiAe)|PPnqmfiB8MnI5YRblkn;d6k=T0XR&*svp zNHUS%mu(99HOv?~KMX_@p^YA9JE`ykaB`6aFgEUR_iT9z-X|0dW@`cvpBFsqpB?)!X55j_GW71RmcQ zf%k*A!P6UK@!`mL5^R{s#8a`*n5vGxi4ij#O|=Cb*3J&>$VM`hF+7^mVQoQ8p*1`i zGMmnZLS2E-cD!^z7K#;Y&Sl_1fyL+Mfq>>%GHJKu;9chMJiTOMC>w!dhC-@N>=sMm zqy@(lQ^BG|gH5fN*O@XemjQ#pkvP0#9^RcURCAfK&+eej@$mlhd>Wn{C>~ zx0zc*v5~m6vUs{EzD0h~3hLT@4Eflr;SKiN;dTVqWisN<_4l-j6qWFL_Nmu1L%V`- zm%R&)ae<#GkB4@0eeo%CAKVjJX31Q<*^AbcgQvk_<8rcE#qaA`Ttcu}HhNh%Qn3U& zwNL_W*jlt9bpcSbY<{#Sx*vRGY#aR|mB)%TknaHJKbA;=ciOJiaJ@BZ4#J&jIGu|J zyD(F3k0Wtf;G~@CXEo<4o{8H5j*lH)xFx+J)^h90oM^2PbJPDk2{%s7W}I1c>8}Kw#YjfgGcq| z;gP!?&}`;CytfNFrh#_H4x;`b)4}KXidF`{gzVK|4fbM`Z}$M=@SIWR* z(7(TmdI??}9`GCmzYyJ?@7@u$ZO0iKLN($}UeqdAy>z#ubwRw`#`gz^BYmsYhtpyd z(B0eD0g+B>+==eoX$PI&bo4w}^h_qAyFx+eWy`o}ZFJAdy;YD=#2JFXFoj)8bhLBb z)~##Sb~|=cGsHvqBg9w`a9t30xiRo=$#}{IVUrZyb9*v90=0EG4Q7sBfMxaR>dWZS z+<`l5B&r4)yckfaS}V0g-F!i|Yj(~I+N))VA>BgARnOSJEY><&)y`p&4iU_iie`3c z_JSZK;Bb`(YmPuY6;21trusY5ywZn?3kRCZB}P(M=UQAL4V5rx7rT*6CR}_lcYlu^ zD1EcW;$v`fQ_~z`%=St{ObX1rdq1Dxrfm$oU}Pj7N`#{#)EdaQ!{9&!&rrTdMucqT zKh9bA;T*=I4;StX-5R)Oh|SbTtAn#{kDosd&#TU-yTOw9nDx;}ZdCk@TzJHK8bV8W z++`d_ExFM|Y6NA#&F$jQUHldkKk_f(ECiDfjv`;SaFo^NYyc!58A^gb5l*YScPJ2| ze>U_Sy+iPSVrQs-V6}J;3A-B(*xMmsmruJ+z#z`a#K1g0VLOJSwieFT%SqB5f%3rGNL5yiU9>4Duf`c=bao7CVnTNGVW8pT{zF|E?Co=jR5YD)wN}j^mZph8Wn1=$ z7>&!Y4%Ec>ZoVx!#_?&a3&pHyF+>}N_am?i9~qBiaWs~UL!DT_duPxs;bg_Q>nZWd zK&Mn5i~#11VDbp)j?H4^}0Yvs=VK8!Iq0uWa0I3&SW9#;IK}+SoLn z&y44}`j-@NMLtjswRr)EcZ$&`+;ynGV=$BAGb&XT!&08v2M!8jgvd~0yA4hQOccrG zocyj#oVs4srdVQl5Wehb(j#(o?b*>Zk3}-&gu_b+f!md&HmzKjgr^+FEPTPeUS1`Z3%iG;TI_sox#U_~)XMlz(h0oM1pm$Xwa4~0>itm8I z2nHuwkkkQ9c?U!X5ZAaS%v{rD9uIx6Vo7Mv(CUjbY&gZ@6C=)VH$DTu)_fXDqfEWg zY|Ff1T&NAIw==d=^qtTwD(!us-EEY9i*DIM@xIORzGimCs|7K@QbR*qSfg8xM$Pu? z=>1A{rM$U8zfp&)Yik!>TXX59E(?zkcSLg@!v}ZIEi1qGC_#pXYg=)oWQ%zb?}p}z z)5)963lg-!{_0_v%8kcJC=F^la8M(nb2+M!frdKT6q9@HIQlF#&Elhi*dovuLq`~+?M&!nhuYL^d1K`~7_>Ms8)bXH;-(m^ zQup2-PL9J6xHpXpDjVWSSZ7$r;}x%X&?ERo{~b%hRIldNij%pf8#1ho;Pj%Jo#JJ1 zmFJ|w*xS+OKguod(DRu+-0=#%wJm0xk6^C z9j+-zK!VN10mSAVu&fsB8lcw|DPst>wRnYBtD99aCeE9Pm83G)YM^3@!GUTNIuH+9 z`Oj$WfX>d2!n%zdR_?$#iyfg5P887SDbzpaDkV)5#FF9dV@f#L1Rejj^>Rw5B?aq< z=$+P|Da)a)h?U(l+pQdo%lA8}CB#5Nob7~VoRRTlB#VWW^Utop%C%6z?c5b?!_-|R z!FFYLhy73~oH5<*eS<{rep1GWif`bD?|}h}O{mPO=UV2&_)!UDEFPAZ2I4iDnUId# zGgfd%i?ZmVRoKm0cn28H6o}T1w@DjRdDkXwI+e0V*9sG}uuSY=q)_z17`ka5M>;3h z_|GGFabmk5WPpKLG?L3JtqmpC#E^EIGPf$0CbH;PV2wzQA+dtUX71xZ%`|-S@uh;2 z8*tCu$aaPAR}_=DnkNsbcM~1g{0dJ`tc0Bk@zU6X*P?M1#dbF0+vI3UanX7B;$3k? zb@u8+&XKdr(5;Na)Mcm{fYNQm1@vO2$i_2iSed4{61$(`0=>E@48wG_IC$TLD^x*V zXH_BW|6vjl$3?KMVFXlA`mAs|xoaeyQu-{fwLH7k%`5g={dSA{7m#ju6njK58d!(3 zHcD@(zR*&cI#Xn-^Ug};TWnAZ1#2&6?Rul(mJG}g#!&}e9ue4}QE2M?z7ykNX{ zP(R!bWF0Q$`1iI^y|;~GZySaA485MZmUF|fRz)kTJPWBr5N-5h$y}=mfMGc7!-zUC z^*4f(#$rAq9xFPfW}KV!sjlVs&BZm7V&^PwkU9;8iy67ZA~R7wmUM5q;1!rMe7q}w zL-z%MSaru0I$SFc28%U=W;lyA9;a_}asCQSu_9NQ6HtV^s*_UT@CJ(b&wCsKRj%1} zDONTpMsb+~jOwveAYS~Pio+HhT0sbnX_1@VE(BE+Shi*3V)93fD~q0A9fQKmGt6%l zYZf|B#m(}htP&Go%~QMrZ1~`l(EUB_@=w&$Gmg`HUYa4RX61=CH*q(r`ri)sJ|+iO zOj?pK2&5&T(ke1-+QIb_mU4?&)NY$z<2MRndK+!}09DVdbhwD)t=uZKH{S3@~A40B;=jcH^+pgj+_T zvg3LHukFuU2x(ahYa)Hc*ZYc%E$Xr=yt1~qTnYR_ana z)COzoU2&*rwpV%9T$9uyLU06#g28Q#RPlt5RveI}roMIrW zPQAPOWSCTe+N4jZDBi*B_?S1cLJe+Dq;n#cZ|Q~&$T*wl^17mySGuQyC;v0aNHh*z zF2v>Y=1Y`2Horf_VzcKdv--4(9l~_y_Qh%iRXiWu(a@>$rM|5 ze~+t`dv}YjF}!qAddD-rSKRM<^Ef76#%iIm90YYV=+gDcJ_$>{?3Hgj~J)oe~@16Eb7xCPi8JJ_fT#gvXfcNfEZT7^S7|5(84W-Si018FetsY#_jEx}(g!;ad^UBh z@!!=Ye)1Trk+?CoA{4K^NWzgQ4YxE|4hIk7Vy+zG7S~uEwyka- zTV$}_y%`Um$WudbdIrx7ZN{x>*pZcD^B6>NG2hsHOKL{~98G^bH$Da)Yu!3HQ~^hK zN|VphS?Du*qPO&})1o8h&4I;^L+#(Wl6x7@W9mr;H?Xr;?5eSCI|zGyU^bQ_q;B8o zoW&MTmN>u)Ex0D4KY0xeCi{|7%q+&Pu`{8k6g!WFC#3CHkti&FWnJ5^T=%uuv%1z- z?B;c8IP#TFib*~hABqNtJJ!5UgcLPfZ1~*{t21fX!!A}MaUZ5ukPr@lW8?D%z@^$a z47G810X<~6;3|}R(TIIhF;vrZz&+(zSTS^PYdQ9&((KyGmv$JTRZa}cZO7=!;bauV zlGq%IE5%;Z-^fr}oafl3bsZd;OU09Cz!XKk?5!9@3pOUg zPxEgJ%!AnTCP!ep6Q@Tw#K(9fF$_Bx`8Z(rW+z0GD7beq;9BKXM>}Fnid#iXQ&DPD zFT*m_0XGCmu^Y<%GK29(bKwHDY4}j7w_CH zRd)|ualIU?9E0f-28er$DRbm5W+JR6*bxMq`610`DIJx6fgg-F!BL+k=!jsyQGS=z zRD_1c6Uo@=iI^n_ct*f#f(2p_Mrl4vzraDt93FKRH2&hiZ!pq};063wCYiUIaHo#d z1b;S-r1_6rJZd$;nkRe}C#jpVX|bceDL%?axJP3kO@4!HqHr!(9I$r85e1S;r@Kjp z{2S$<8X!`rjBDF)eL9aC0D`A9n}%{Z3tmGvhC83FrGAbC0sF(hy;;zW{D({%8JEcc z@()g%AXTq_fUlS7@fv?$?fV+|{_IOHrDgte{D(~QC>%cWIz9g$`0bVOPhLOz3S?VW zF+GaD?+3|V@K0WU1i{t-r2D`>dHu|Z^n*+uHSG8T`$uvzW?$vJ^y>P>nE?K>t*`wHS$VI zHFH`&)UMzBBf4Iu&uIqG^M4$!KN$XD8^%xIdAdIEJ4#O`xcI-!U#7o@Uk-tP^7^fO z{Z_tS@}vOyU8aA5AMpOk>j(MzLB76mg}jndS??tJGPoGqj=X;2K>BOq1=3&MPHg}9 zk@-qF*8-;IKXWi$Kl38n0e@6J@_Lyb0>7)*TMbeZO4~Jq%j;x%gm!)VVe&dkrJs>1 z$m`_yW8iwkWUhmJeRy9&!l<0`X;l8kKVQfB2Q*w>f4YWCacWAIauX*w|0g#Rehefl qfGG8s<$5{bQg>CtXg{GpYnM6EukLRk)93oG50K$c(=O1yTK@~V@(57? literal 144464 zcmeFad0-Sp7C+vB08zmi6foX{22DIOA;FBoIw2X-0~3fO!6@P+HqjqkP3*7n`~Ef#>%Ve2R$raq()7Fhcf6ebE0-IHq~sO6T=J&y z-zCTJa!UsDU%ec)n#vWv#Pb)v7XDi##G?PkiQ>c~-OEI}`PU}$>A&W3n?yPDZ-OYN z|56=v<$dWZ{deli73CH;@^+bj2UFv5>A!k?m!X_U;u(JBujqDVs;DnoKXeV^g!Es| z6_+K^Af3scT%bTa9mI>Sxb!?sz?44twkd@#DOaYqy_UNo%ir-8Kodb@GZ6Bg6PP7d@igjd+Ev5-&3e&Ak&84;& z=ifcbMSwTo8k-e+lzAat)5sVX7KABO#^A%j&Dj)+DH~NaC>pn1;&&T+!eU5!95#y4enap4Y+-{>7rph zk3A{Mb>eqEeiw*m`neG|$(ycb+%4=$QEtZXV%!1TLENplZ^cd5ZMg5meHQ_^?#BHW z-1p%AD{i{($K8(m0o)Jb{s-1K9$v)nOSoUgy$W{+ z?lrhK;HIk+_a@x0;C>bN>$vsn4Loedy+t72#_v|#@8kYZ;64(+pW^p3+}m+$xI?(V z!My|bZrpoue~)`VZu9j65C;YFXZ*&*K;Ll>#2trQzm5{m$KZFcgdc)u3vMg!<8Y6_ zO;!%KVuJn(O&v|j~BKIQ~pLy)Z9y#^t*RDSA(Bc=q%zfjrjYC^93QM2qJv3_Uwd++a>#~+&eK<0%{_M3eF`3L8}c*4oOvqsI`Hgf*_ zna{hvsebR-c>~rp{uDa!)1l?LIg?MHxqI1PPwr?v_nm#Kt1jJgWzj>&&ARip^}D-2 zoR!*AbLEG<@2GzFuG{m^spmdibz%B*wVs+kW&Hfj_KO}pW!UyBPd+2}tNE`T z|4L{3#b@?=|Jzr)=ly=xd#Aj3=Yok%i1-;e3uRZ_9epjsBzP0$1r-o(s zrj9@Jz08wdwq1DrtZz@N8mHy{C3{ZnWl6Ve>(S1i`qfF*f8H?s+F<3F%d$^hU2vQ0 z_$9mU`(0=IjMG=Y^;G*?8|sh!;j1?%tiDpcV#@V@UHj})y@{_r(Q{$x4~O!fDRkU3 zX6$u0?dpZ^e3@CW`{Oewl}-F;>vv~teRO#D((^W*`t7+_o|XI6 zhU!|+-)CmzzVz3>U$XFr)Kk9BjUP49wxnQB^ z39~0Ief{fyKINY?>(XiG96M=s;lGOJjD4goYu&V>Q)YKPIpBh2hbC5U@65dB*dLxh z`k4==r{-PP{(1W1x!>nM^GaoROK-wS&+?HH;qQ4EOA*l7XGO-Zw?xLzIWsa|!weLO z{$3y>;WrJ7jK4ZDGQJ)YR3tjjVFHbWpMbWa$YIx<0g>}J!hecH=dAN1WH^qh>ZVhl>Fz6ikyEr=;%I-ER33=k@If_Gm-c?kQ5nTFkDeU>ac55 z6ge-5BG2?F{OpLL4_ik@=I6XqBIDOY;b(IcIt!xUhewffaTNYPxF9k=GcSycuZc3g z>{04{Jc>RX6=l3sMH%mN`bXyHv123SZ-|orFH!8k<59*-8}O0z=awk?|8^Apv7lX% z=xmFkpJQ#2+tqPYWc;I1?A5F&{Z(~xWIE?X8Amx$`s-s1-bmx|gedYE7=_P|21TZG ze-yo{h@ziMqv+eF*vRy^M!`2k>Bk{a@;{yuxqr#Fp(_u&M#V+ur}c!$`0JzChp(d8 zft)DgeQA_>ABsZf($vW9IwOkRh(SwjhqYEwhNE4P<~vMB5%V96LVsfveg1GnWIj)e zqHhDyF=r^pD#>qhW|zP&&~+E+jKqwz{90;>qMXM5D$1GYU&7nwa{So>p9*`<=v>V4 zc7Y$v@{dp&U*&jxj#~$Ulz-P)j=xFde~k6Zu}ah5IpRct&&4>UdaZMK{&<0(4tb7L zMkqzEaJ(w;U%>7Wewo1M3jBMg^LmRe<@pC9A6+l7_Kr|ih`5bTe3k5#&V>KglE#gTwf!_szJfNJcIN#>^lLh{7%>Ep! zw9(=T*Oj7Q-nDW1)=Z8c&r+c~up7i@8!a|*UCw_L8~i20Ti0{MN?uG^fbq-ZFWU8t z$iIo{8}g^{jHwuJbbX0m%CD^CuxPe2RPecL0MCCv_@L`{*bT~GH;v;fQ6F6eP#BV1 z(LN3fa?0X!IKDlR=O4#^lth7Feg;SUP2j(Wf{{FT34Ory#IEB7{q9{H-Xideg}xP; z^zCS&=T4#LXg<4&5;#9?pL2M+kbe`i!{~R78m$g=BE&?C9oM8Cj%u`5lNzX&ks@}DpCN)dE09kAapG=bkE_;i~1oXGrh=+6^8JT1U84n`-6lT27X98W`hBTMT#{yTv$fL$kihZrxq9u9&W2=5g9>-HxV0wnuy z{TnCnnV_@aM4o@S(DS1NezNc%7MtXGv9M!p6oka}3gk#vgV48Jlm7fPir4Gx;rVp= z=cWpKQq1X$|4YUA8iVmm_HDUIKi3L-Yde<{*5}hdK+j43 z?I!!VL-gZvF(5KTy|JQS+C{$%68Iw6XX@V~FQ@;E;Qu|=Usk1E_=kle|17~z+gUtg zKf!+%+DrW89^@Ib!3SNbNN0K_^ao;OmpzJo9x2+@eLknJ&kN&VpQ&GpM87l&I?sV1 z)oXj2BVH8zTm!{p{5;6rDDnf_g{})<<~8*@DlrqxAdO5uE?UCjWWZF`^$$_PIukyLK_|AYOL800m?6|0{=| zLVwd$fSdH8`&kAm`n>tF=$BomaR$c=IShe4Bs$&69504}a{5smzw7Uu&J6ye`~*HJ zf73rW;$~5Auh83elV05-=HEIoKc)*hv&48@VUp(o=ppfMUhg~yIWzn!PC&Qk`KX`t z_JHskp-$}DE9^k-P7eP;^vjE)-;0D^`9=QYML*U(#1Xd&`RqG`*ISpv2@HbV>AEw4 z%fb2;hdV|7r9uwfNj$%9PtL_SApYBh{l{=%*R4=svP+7PgU){&I*#&piH5Ed^yiCu zcYVprLoL}gU?k6PUhixcdX>DE=lej&p;gGC-K75oqF=PPc*ZzEzfeAyIsFd>KeNCm>07&L z{622u`OWK^LV>Rn>z&Dhz9!n$F4~nP@ZSmh&?V@r0)I6GMRF(-c3`Z)*NAc5L7R!V zw(uY2{&>+|(J$y$cBw+&5>4aoiQ&9mb*6TmCFI%h9WTE|(7#s97gjM}*m$yXv6wHq zh2TdE`(T5eA-UPaxeeV8_=JAeb#MYJ1^v&2{WZ>e?D_gRi*W=k+RHcYcnyw5+~tR%L^)tUf;{v#PeHEWdbWRT)oBM zEcTXH))ZG&UWbfVPM(xDv$nRuH>P18$W)IhE%OyuR;8rODyu20uPpJF)z{b7drOLa zWwUDQ=cU#ZSC_fwL@#IOWRA3$JvE%ZJ-O^!f90Iws5Q}8KU%7)K;PA+;dzhX?kH@!rsan^l(i{nd?YNqzaKorVz1v?I{hu(j)HF z@Z1+wmo+pL&ng?AR?^sLx07&4Crf5~ODbl2N%)8N(Ju>Lr|7E8EvZ%-eD%D;>!Z_& zqMhM{%sqf+Fcs(Bo0Y=UIDcArH-%@SJ}9lqol{M$-{yp%IdN(62C+s%gzT& zL{$juged$7$%S_TQ;j^|Xz#QXv-S%z;XFko@*7x7jx^%UHHQlyy;xIHHxIf&_GTuG zR1N7{Nrk|a)iC{I@T|s0Z(Uh^LoFF4-#qV}{0hmsAwjdwuoAmA(ddi7T%de_Rf4j<=+~ z45rgHwYplVE~{n|xf1yuUazmBzILv+s&dIfXgct~; ztBdR0xs$@R%-lsTv@_XXQ+chw%vhXFD0|CZc1rw zb(ybz-qcCy`s2T+UiM_@SAAJsRWT-u>T0MD%v3?m+)50ssbvlRYMS0tQlzSLYa7b) ze8tsuWu;Rm+3gZYIg^c?QUXSn-BDjVv(|@cjx+A9ZkW|zPmwZt-RPD%=m;|2rT#il zv!{hoOUa5x=pv$%lGR5dtgKpAjER!UrgA1|-lF<1;wf^ml+i|&`E%hDrDPdxoov5| z7q2Ov<4xfd>}lb-!btT=ZAwXrzYg>9yzKf)NX=k`C^12#hUGA*Otp^=OHitdtEy^g zgxk{%K0U<^W%YR_#Wh4&u-H&m(*Tc)(M~op^wD(~IHJ|3S-G{9H9q)&e#T`4V(4!@ zM;|qNav#>L1bXh2DBM8>j4TpGsh$*&jMy+Kk6uzA#F69>DZ7|iBV~`o*e{Wfm|xJ( z^NpEPR^qFzcUM5kD_k!7MHqIvwJNTw!%XI4#=W7;M>f8?b`A}^nz`gOW~8#Tb}X=A*?5rZulV%hxuzree-! zif=dCg3$sus2WSUTvXGCuEC1V3s;JU3nXtO{J-6H`xvx6tldmnuDmp;gb$i6deB>m zRe3QP4ie@}*OYuNmlQ`|4lcc7OxX2hGmEQe**7zajIa`(nnx5OFWAEu2*2#WWlxK; zHcj)@!l~_B80sr<(<0sFN`?PNy!@ZixrpdgH+q#ic*B_Lz14nSStFd{lG*YU4P z(*7DwoGnXc`HSmIy~QO6In+Z584lihW2H|3JVQN(1=rI5bIIAq=nH;*o&O(bXhfp_ z0bbL@g84tmY6P0{Bq=Y^DcFzc@G7VwsjB~8RdxC`o2IuD^TEbU5)8=y@`G-|{6#+nw4n^&|%36`?5 zwt*SWRCi6KFVpZ;!tr8(PkJ!gS3Wv5AAyVXe0Pq^o0C!KLhQ*$9!j=%BE`OT;|W zCS&HKg?;hN$~kt}Z*NIeaYKXGH?OXYyE{b5RGS_~h_WmvF`6qsk|N=}kn7k``CHhD z9N7FNZhoOeJFw49`dbIJPQ3JVa{Dlsvv}Piv_2meeAeSXRnL@i!#AAmV@j1=75${>xfy*B2h5{|@gZ7N>X6;HfU2UFH=k;47Y$ z=FRi^DlzY+VL@Izi|j-mj83Gi6vis4rZCojr_V(!qR&qKXI09jlvb5gVZ{9R8X!je ze`PRc2_5~f5WndEf-$kvh>ohDeXfM)b(2MkX1x$yh00dcq+x@gzKNj+#Ed|zh}cKZC_2ha(H=RX9*u6a>|ts^{($i5?2d4^ zz)OBZIzm*nh}K}I4u=kK=AayB2>4nQQ8r%%6QS5jKP)kwG9Pv@;;?LXdmY@78f<0} z!8yv@k_NW5=$*@V25JACdW!ePVZ}?QQGeN4hecgL%|$nd?M2Qd?{My6dy&zrxu^tD zxso|z8Fx4@)Cxqg`Bo<~vwbZ@gy+JOnhGuZtt&e=gX@*L=a$&*UT^hG94PRWd3{Xm zoQGfQ!Bm8VOX1>0=sWWJ9F**>M%z-!({h1T!(d9$0h&5%5z@yG3mHW24_%W|Sj?(n zUUgMv&1|MQ$zHB5YJyViisxJml0%0fHpSesYQsv)&W<;II`PO#|4=$hKO7+4LJH6^tTRD(S|Y}?Q8tE|G# zB`qdWvhr(*6Ov8D4bH>%C8M+yvZDResI@t$wmhDRSDBI(wbCfNs=jipVH$jNA`9jn zol;8&E3mA=83goC75a*WPr-(qm3o7^5>Y4&*P4d%+In%2WlRYUAJo*c^{ehKV7@4s zEhGrhQ4C04Wn#@<=F8W~FqQJ!9R@N@)Qcj$*kWzv9YS6+Tm1eXY=XnMPf)A8xe7Mm^tHmoV79Eh(##^N`$q zv;&PUswpe3kDOLqGs|BUp4it$QX1+zrdm>ErW_b%rbJe+8ChOdRyq?Wv}Am7X{oLp z2*N{sF>_!z@>3}@VJq2oHCu;9+G0I?XWE#bBkY?+$W7C;#5=Krup1P-0Rm<0q@;(j zQ(s>^&s$T5BQtCt^{~=)v>AeHlrhDeRx1u?$as>u7)B&1iD^oa zm^P(I^_UAuNntDyLC9UIj?5T(#@HJ}80c5_(o9?sxjr5n%$!iN#Lcgzr-Ws|rXCHu z$ifkceRa!Ki$tmBUrr5^k|=Bzl+JsSpTKwOGG3-BT9hClotDnX%;|ljgez_ zw882A<=7kxo%DIxQbQ^c->(!$b-m2!l`}U{HuL<58LG~o_Fo9=P?OAa0d^eV-=t^x z;W3Lfq3~LDpGTimm=ncU8r*XgOAYQ?hsS|I)o^fy-g=Ma5&eTb?!^$`D z=%@|(Fh)rdkG>mlgbOV=pNtqcBK(>8KQd9#ZrMA9S6EkzO>x$CK2hpokzGOK2uP%L z*}vmR3*`&nb%E?fMM9)@DJ(|fj2J1M7-ba%ui1W>D~m2!8LHx;6fY62Xc`OhYJwNfdIqgM~;TsCnEOQf&L@Zay~dm zOL2fBEy#u|FKDcck0>H_k(iiSK!F8e?cN-=@qYxik_}{9<^HxJ6zcu0W?8T5-zjJ8 ziAlpk-tVE6tznSlv2$1^osuaAryDa9TF-6&)rgQ$`w?U{s2*9!ku}b=mx*Qloa$fM z%oMWv4{hp16!CwOHgxJgzTqWw?!QI1Pg|4IKP^diZi1yPOmvYJi{6AV}~`C3SUSFx4m!ID=Gj?zu#H z3=yq1o@SL`#Y>Q#@FfNw9up5x0Hb7QW_m}1ootVLe5N<$f|Lu=l+41yjPY)-{esaK zICw%~Au^?2kP;5s?NZk8jCL86pD(Zr8mMb@3QxhKJ=J&|-S^n{gxZx0D{!RxLMD&O zl7BSfN8|p7)`;!-<;TO{@XZ`TyTzm``-_wm% z2C}C?cpd=EAiz3)0KRBT82SY6Aa)N>j>dO$2P;EZUdk5(XfXZ~ofy247K{ImVN?bH zV-ZiW$TI-Xyadln&x6>116hg{`H2Q`qSK=i^dC{>PyhS-|E~o23yX}0H2zIQ8&J7t z&BGg&RwW5Ix+r}=-W|2#*@oMHQYGHxjZ^GK`XEIU=}kzVxA{qwKUNthXwExu7v8)) zTA3)|Vg?_jTp{4Mo}4{Efu>Xl_*#}eP-zgbFX>ji=^Cpn6mTU=@2~s`FkLaEn<9_X*tX3?8B^6Yw((9;`eq;9-@Oc;oaKWtD(=`v$P~QJZ}yQhWO;n~+YI z{=IQ9#V)6Ke?DEj_p0NQ?-l7L{BnU;P59*dc>Y`yzDVE;O?dqy%F9jfS?h3-$WL#T z;uUMwe^i71UaA?de>cXA*S{xb#_Qh+G2<(wdat1h#ihR&O!>#)CfU-Zf5*U#*WZse zc1gb^G+r)d*W_)54yfX@3 zje^%Ce%d5{k|lm*{4Oc~Mk#-~lwZa-MZw!7dCK_@Nc7t!`c8?Cj9(lD-!9RY^T$c~ z>m>SC3IB|QFN}h3kAjbr>Xqr(r2H1HO01Sd3ICZ?uPWi|C48=gUo7D(B>X25zD~m5 zA>kV({7n+RO~Nmf@Jl58jS_yjgs1PT>(`1mI4}^1Fuat1kDeL7%OttU_zo%m(^CFT z68<#_-zDKa5`L?Me@w!6OZe#$zDwd?#%of3F;%cqyCi(CL`M#=DHSN%!mheRq{v?Ud zItl->l)p*BH%j<62|q)^FOl$W;C24nCA|2w1}n8(!i!J2Fnqcs9~s{z<@ZYU#!LBS zye8#;MasWR!rw396^Tx)B%e45|ALgiLc;6cMxo>-QScp6@TxSvWIAjB|5nhzEZ+>NbS8Almhg!Z{znO)Ea4xP@aYnMm4tUn_?Z$uSHd4J^-H0I zKUTt5NccZVbm}Dh2q}M~gcnOGR;o$D|5>85Si%pL=(I`rbyEH%68=pIze2*_E#W&P z{6q=ANy0Ce@LdxA5edIl!sko)ZV5k4!fO)#LkYi2!e1%jSBUi;iC}_+?~w3QC477r z7uF!)B(W(r36BN2aV1LlS_6x9u7nq#U}CB15;iP1s{ zPrjOd6-oGF9f{ux2~Ta-uR00;ppL|EqlDMj>Xh6h;g6E?FP896Ncc7hf3$>OBH;&1 z_;v}8fP--@lkmqHSVdVb;Vlw=g@o6A4NC5i@I$5inVvhAC&OQW?mWf;cy8bC*em(c&mgzLBhvNcznXlxNH(0pW-sE zL>vSX=E34f}TUzPBuN%&j|f4YP(l<+nQUnJollJFH0ex!u2 zlkne5_(lnThJ&25`LM4KU=~tm+*-aeuacj zlJFf8{u~LvNy2|A;kzXKCJDb)!k;VQyCwX25?+(==S%oq68-`Se?Y=tDB<-F?~(o| zOZYhPK|aFc({jdTmGG|{Sp1t{5`MIVw@G-Pgin<4J0*Owgin?5=@R}T3GbBf&q{bz z!f%%Fxe{LgmLDY-O89M3{vrv#OTt%3c!z|qlkgWy_(lnTiG**G@WZ$%VyTNId_cw=N%#y2zg5DI zm+;*Z{woQuNqG7ewSMiA@R>RizXv3ImV{S?KSO#RC(((M@GdF8Rl;XW_;?AwTEg2T zyei=nCH&tde6oa}B;nH~e2#>7O8ChVUX}1uBz&%fe^0^}O88s}UnJoVNcai~f0=}@ zlkmeOc{WP;0TRAR!f%!6ESB(j6248s%j>}<68>^2f4hV)l<>Fu{Mc$)!_WbkGKrT`JzZ@_0V_$dRXfDqbmz!V5V`wf@^J!roHpUvQF4LFg( z#Rg0P9kkzoDUgHyH((0Lp#26+ff%&kfGGfj_8Tw-UeJC6rhp6De^76KGK04nu${r1 z4LF6tYYdnID(HU$roaf=Z@_5`zQusYF!)9Trhp0b-+PE z(?T5WH(*+Tqy0bX?WYAc?4JSC0vh(ufN5g|?Kfar0Hgm6IG@1}7;piDZ!zF$48GBT zX+ezk8!#<|(S8HIg29&=Fl_{({|$ILgY5=<6@y0_@C*hIHQ=im9Am&<27mX1-u@y6 zZ!_Rx25&atnG9ZIz$FZR%79B5{D1+MG58h(E@$wK20V+w*BUTQKxn@KS2FlA1HOjA z83sI?!FB_#V(>@f=@NU!%grI6Fk5KADR$8e!e%s zJ52DGCir6${Ei8J-2`ti!7rQOXHD?qCU~g{zRv{TVS-ytaEl4P-URziaJ308Gr==V zaDfTVF~KeqJkA8Cnc(wH@F){}stF!$f`^#k0Vep6%hdlSc!vr8(gc5Og5NR0ubbcv zCirC&{HzIn+ypN*!S|WqJ4|q^32rgL*PCF!39dH5WhQur2`(_fIVRX;g2$QQG!uNj z2_9vFPc^~AP4EyMb_ai-knDE!`i^x6vl5(YYh8?12*;H0R=4A4!ut|z>Y|M?niF_6 zuv6QFg$qkp9f#y}8`3>UUnbHksA!{_^t0O1>l>hvze~k(30-8MzlIwaskSY^Q*Bo= zYA+=;VbXU8pGj!LuTl8%D24A3g-=CcPhh*2248`Q4H5A7SbGqBh<<}FCD0b1>e%TU zss@`AtOSrCrVtgRcA-j-V>?gnrur$B{9S793%^L?7?x|hcALoMlxxb>Yx)wtpk9;O z+MK{Dadet$Tur$GpK94AS{sOKgVx>1)$>o4B&|9=^&P9CV~ZHa3q&pLXoAPFpHpA1 zw}<>);-QHUx-6KqskNC%g(O|}UTyLI-d?Q%y&0;3XRFNv=(2yI6%%Fwd|9g6s!X8hd$FuRM9fv1-N9Y4M@7f4L(0U07 zH@tT)7c`Lxmy6_YP;u5lUufIENA@k~u+T|x?X|4{sbL+2*$WI6hmf;KK|}}TGO=FG zhRLVeLqy;fWJi5Vu^Q0sAVd>Ks8|k=RQQ&U$5MdO zlBqu!v%GV^XE}FjIr!~aOJX$!-3NqvEK;EkQD_Co^9rA!Csep^FDl#yfrnnMu( zBz9A!m!QXdeH*Pz=~|7@w=rn2HWEPS=igJK4mVxvSx+-LIgqbswI}!>@7&;HOagAlH$L=Bq;6PD zWfyHo?)i`;b2!VE=E>CNVq^T?ASv zv!Clv&li!3T_=jIrDBg$v9uOmtcaQ&_%w2{Oi?UI#pY14@i+5g@9+ABVuMAod@6P+ z6}v3Ji`4@#YO`wnvkCpFb}nTkdlx!82n^b7)wXByGz+EuNZs`JKk8{g7f;&*F0@Z5 z?fyUMX|)Il5Y6`}Z7rqU27@0-C}m7yJGI9t=Mv zh`a@q*U9s)ye)FxZA|<-wIs?rjOTshb{OAyLiFc|YwiGI5FvI$vf%P=LhRcK#5RuD zM2Ig5(Zvz_3Gp@|R&&HzcX9^exL?-b#cbDc;dZ3QqbI^mVkE@eh?xc?fDEFD|KlxO1Y)> zlmt&;e`hmu5RU>Y8=Dg#Jtan6)M3T*ay+AGb3zmSBHuFn(q$QXZvv<)ngW`{5Xh+D zb2IS*{Y&Ixs;905SY!;tIyMZ?VO+8q&>3k?JsRm`tm4Q-!dQ`2`~_q6AvJ8#1{*h4 z%!G!SasTyX#Ja&R`H9SQJ39UKe#7W2DY@ZL}=H2 zd^XD{a0d>pu0vZg@(Zv_X7vO|m88ZfZn!2+cWZuvwOqB-tffDj$}LrEEJ?8m+QlH_ z34DtlLe~#LZngFLIO=0H;8L`YzUl3?Bo0eZgTw8DQUw~I2GdrKM%GSOE^4D+it*BK z5&d?$D!@Bh>QXI33l!}k^ciV_JFuQw{~oJyyY?o~Y)395-yJx}7%#}x9zm6!z*jU5 zpfx?vbyWTg)fP;1(GMsTgcH0;5WE5C@(V1dHxm;GU^-d+vOsM;e<}XmVJi4X5?*lJ zQby-nwRQM(BxnO!CFfrVtmQeE(%eC=%zzasF-lXq;_tsviJ7sEN>2uT>^Odo#7mF} zzW(?wS`B#wtlApz-Lpxq9_Er-P4&lOMkfk`dHth|`ac|l`iXxvc>a^1Db=5TSp9FH zw5b0@JF9Y4-n)tHUy=AV7TK;V079SFqrc9*ky^ztOnrg86}tl~cxIzDa-^GE|<`Uay}e*#`} zgfK2jp?(_&JPsILhkanVd}uBb1wTYDdjZc{pMt^+-4TZR70;C2)ZkSoB zo-TUc)sH_@sSL|d*kH<+^aFcdfoD#Kftvtwg290qf!KuLgxHKgRzhrWLLB{KM}4wY zZF$Ag>;O?U@TMB@B*bY4HPRP6VFGfu!g;W7vG3ESF@2pB&+>mqb?W(Flk*4j6Vjy^5;f+&ZTED0kv%L z`PJa0BCzDET9E*`sE#$NC3}kMt?{RY*H6>|Ga2hjL(9aW7o2p*Ha#1q(TkeMpGW=hdP0< zbsStSz(x*Iw;**G%{uP6_x9WT{|JSy6Ftr`rr153>;GdELpXqIMpa#AyI zASlq+OtfzpweR*=(xYE%AJ4Ns-V^AaJ|p9*j2RhMXLvD`oJz(DatYMdAuqq7D52vJ z2IAw?z%M2Is|c*vJ19R|qYF(`9E1MxYd!6Ur3=CH^w51EO@kD3LFj&1opRa0a{Hb! zjgQ~C){Fu+U}3;&Z8StX2Z2cpOPdlL=n2+fN~^U720`;N+~O%IkWKmzsE9*x7>mic zow#$!2RQg8O-3QodW-;gGTN12b44bhP~T)FBvWVG_;Wfv3!Qe-v(Rako`p^`y?65b zOz+j{GdNVlWxS3W>_&sX>M=E#%_ppl!UAE~4pga0ae?eODD$El;uZgR^zx)+{K<2= zF|@7V6u@*=ZxLAbB*f!yenKMtUX>uYQt1%Q^o*;}y=2#eH(0d=Xll>XlEll|YzF&I6XA7!O0Ut+IUuOWban`8rPLhSkzUB4_oSfO@1Pslq<*K)1N1YA|+GB9$bg z1!wrx&q?>tUXrkrw~k9Qks@_*rRur7fd3e@kf}1N*Yn0Gz(l$LBAvU(Vs)jXo}5B8kZHw3fr_|9@*d03OdB9qg(vtpwa61($`m+|8ISa_ zmZ5pjr%BG7q;HUudcf5P0&uNM>eSB63Y5Uu7@Aq+AI0R)M%lVP9an=3tlEAU)zExQ zGUaU)*w_lzK?8nRC(V(aRL~Rn3{D&$q@nF!gwJoL_5}w~>tX{FV#C^KX}KTFP&<=7 z!RlOUrYBfdNG*&fY%1D7z$r`=&?(H>{aE`6F_xYZ=*dY>iS(33Pv_B-3TjTYH=Wu` z*kr#Sl|X1^BnFkxATNj_bM7z6WBYcCX7)OK^;-#@g7t}*msiO5}rSu=eIOB zfD)IV_Be8eN&xFVB=-+8rg6)%m#YcRpGB1+7THkh84#gAo$0i?;?aNb%iWokz^slt z>64tm_wF@^`gtr*YB_+C0Gq zn^;dj z8C8HzoamQLOvd{(_h-oTqR}7wwK%~_yc$>oRxHgw!RD*2&I{F~t(dH@w%i0KS8*@; zd9bDVHF|1wiAg*Jh^<|=s)1MGM)p%%Q|HiZuEu=A=Oc?%wOXuB8)mv>bE?P5K4P@-4j0cf1~Pn%gvE>xM6(+1k;iT&!D-dJd6C)X@!X01C|U6g~7PPv?v%bm1?G+y;5=q?53 z4h$LCISUjabOp81Si{jAtZn0}>eL7LbOcgp^}=V%(1B0;#w*k9=S2Y<*KJEPIsA}x zPf{D{br4SIqIFP*4elM*^7pvFwDL;2OHEqm;=#glzfUB=u(q5aZD;#fsN#-F1(Q(P{;R{g@t$^X1M-7Z!sJ1r7U@*6kGGd6u-spB5nllnf zSt+qF6Evk|*<;b9IfEIJ25goc!98%uwikkVDsfn$nmD8-n`+pm;|L1p5PlW8-p0M@%Ta=nzFJ2 ze`p0SRsje#OjwJry5+QD!`&A}h-($o>gtS`Dj3@8bi9dN_43Zm~nTtVOp-_(|BToW|)8R8rAXo++hugYK%r>*XBl8m+172$U*R- z2~W@yHR%nggm&~+>iHeMk+g85;aXvNZg4-Uner_j4C~T?wc7^&7u+wEHL|>o$Zt2u zf2{ZEY-YltUw!sfZOyh(UwT?=lZA~9e4j@>*JoeJVB+ShHO$CrAlr)hSzp}IN{_Gb zU|e%r_F0;rMQno>aih=(J)+?We8++sH-1FwM{&%!UHjqgyt4k{}JtJZQm9g&60sEb5|8n|2F6LgZoC+vDVVu18YSiEZ5lkTuCLMPX8*KQTEE{Yq-sWn9 z_QFx-gKZd0UeFbvgPjeh`hq^x*zhBhnH+ST!hBGK!;>=>BUUI})cl|;oe~O>kV^@T z9t4ns{zN`ZVvS)kn+=m4)Bqazv~vX|x7j~*+4uEaW5}D-I$rPx1y1+0o_BJXYR_i+ zP+%iPOn2khmKyVcCq@fSj0>nX%X3o)FV4m0hzsek96?XK6@x2&e86)~qQ~J+oO3!& zk|$v_#bXrtZ{h0wOb67vjp+du!?`Xx84&cVWRS@izhSu`kjum5tvX(nj70BVF z!F&`3jEceRp9#e`#pakEuIY|NJNo4~zO^*}%#^*ynD6^k+b2|;?|wi14hDr|j~^@3 zssv}x#lmjUs4mj(goJu-rSV9cT5*^Ow5MSULf2sML6orD7|*1)_c3Yt6GJz%VWI2o zy4BFzgV5VHa=fvmv9!>lyXQ7tk5@n)7#)Ua9WZ<)+cI#n`2d>>L&D(T)DJb|@(+>2$oG(KXMJ_h*s#99aZ z59+lEe%R=w?K&(f5v~f4Zy}i0n+q$7LD(0`IvO31q0~)*taH#eeKnB$XuI`YEQmw* zcgaacdyVu3k<+pNQhh>%ZWD z9F!MjL$5%yjPWMY&kv`sEkz!XT4jaS&j_6zPLJgCb^%TO$zjq-MAu`+ova4_r7rp* z##iDF90=@I1KZs}*qx0`Z09AYt$DEa+u^`y)LzqQ()jy9N?J!! zXL+tb$ykg%W17k0Ah7{tK{?Gn@f4~zuBU1tA+a0>_hb)X_BQs6Y@@p4>{}=t^c{MY zoM!~KnK~0)C^1%O=(BIN6zUvO^PJ#$2|a(%rf#Or8aMhm$n{R}*f(DP&Gx?@rTvi0 z|DE{VcT~JodF8%DQjJj^zos{pYbmgZr|gUB)pd|_#(l^D6# zfROYmgZu#C6r9ayU5!E?`j74z%jdUKFrCkj*Ye1(q*I2$`Cu~^!=~4V$loB0;#K4y z(K)CEq&kbnCMf<*?%)lH?%>tnh-EvWcPh_Di;Y6c*7BjH872ZNwoiJ3I9CVfxJ=J2 z5#6bsgWO=;H`NpPGBgHUu<^v|q#5=YL9B&B5i~m|QsWMzL<6pyG#)FpTXF*5gnqz; ztOl=LI}n;P^4O3>P_P=ym<4L!sr@g`J+nyyFtW>6_hb1jpN!8NhvVu@!JwV<~`_B`US3fscD$?;DS(CvN(TaQfJt%JT$g<*I?7 zP>D*1owEA;Ih>y5>lb!}R_pl}s~Fyli!ETv;he%J#bS>ZFs~yO<^7wW0dOIA_$*lW zptpCbN#APAS<(5)f?k7@)zKz#v?qiX>-V=U-S zZP`PiD)vUhg?Q!AB*+q-sOK_AG;|F!l*dm5**D z`}0Q6Nxnsp)^xg15Xq3Z zZzPV{#Zq7cQ4~693B2Wley<+(@o#x0Pa!=#I6VpSz+{xpRt+DqT zC;8CNn4+*6H1}U8CQ#JaIuW7mo*3V0SRdYv!#ehT)SlMDgWyVAwt-~5MvZyjO`Mp> za5?IF0KeK3com(<`28z7lHNYTMlqPCC19s^4OAgC6@q5tiKixQ#35+Cr%z`+oxK+E zm=HWqfWx!Y_=^chP6k_#O z%UGp{h`5=Z0yBH52Q!;H@TNNn>zae^H3w-o{VPvWw;EXM33T+uYq8z5&T=y%2p9*M zV(0h^s;;#G`j3;aJ3H)K*gg`bG-F;^8)F$tYgh!Bo!z(*h9$6MGa?xX~z(-}uU>Y|@xd5L9-m4}<) z@-}_ksg4gU3vU9`9>?ccYtiK4iRrd1oJH**`*0Q>n624Cjm$w{mK}2&MCoX!Cvf1% z+qt_Q?Ti)e9JVW`b?jZR6Evea*7+`R^SEI~XSQhQk1@Wp+@0C1p}>lk{@BlF1y<~l z;0P8jwr_iY;x!|6IR}#|5{|8T?0eVm4b~kD)E~r#mH#Vp4UU1iAL5OTtfs4I0X>oY`7_nNQzks-QjEiwdipkCKw z2|@a`0k$Z_Ucp>-fY~8oh~B^~ppEO07gHjh!~BI8(Oh+<`_=^dzhB1X?iG4mKVurg ze8x#Nv8SO1=aNcOI1t_g{0W=_{Yg#Q4Od!S^J9O^Mg85DC%bZRUc{3WQXTKiEmH&7 z1m8uyN`6wSYTf9xlES8|0d*5}tcxTL#nte9fS%b*|1Nr#qN0e{Q73O?t9z5$fnJ~9 zg)9rJncnT7-m_%B-yc=>*CH0slg8Fzfuq%+3QI%KL5je!jQ|zCNeTV9B zC%xy!ksI=S4y`6bJ2ru8#5}h#CkC;)1MJgF?$u%HRl5}G^~uZO>Luc#G~Z!|1o|a3 zndz5qA*1Nm0xYFcQ881npP)Hv(jGMl#|%lqVxeHMCIzcv3I?;m;yHc#l??srf=+Fv zwsksp(a%QbUOe_`8GMVZY6Zt%a1!T0WEJD`57V=0hwB-Rp`(wmZ?zA#;AL4^&xkKc zzYwJx55C58zQ%LD#)B_O%eqL*%1O%{qFv#7M)sbL2|=y42Gw1Gi8vmj?p-`_Z%{qJ zH0^+I)Zv=8lGM`*)wK4k)a@FAtYNB^x5}hilXMArVs?>UbwRJzM%F96m;3Z;DikV~ z6zVZ(2=7-~D+`6%71~OU(`sy#)5RU9(-8C1T3-xTD8ApC&z3M)zhISVrP#wfPjEy| zaC}_PscuKt+;csS&2!G8O$uwq3+NA;aC~QyYYuM>qwzAFukNb<6y4r(>e=Wc;j;% zANkIprNg;speJ}dNMYDxnX;bv5_Qgblom}OfWj1_1p$IwcAV%Uf`sTZMNE%rJs>aM zRAGO$<5$D*qXjjHHRrVY<1*N5SJYD9>SVN88~+k5XK>2$yDS`x2&}_m6ssuAF0@IH znY)u8mIIuq?ZZODy~ve_#nt*aHL!IhNlkLi`xRux9?^(NXE{uu+5I( zG(_aqS{BxzM@hQF0nN2yNMO4t!*Xk9r~nCVfI}WU==P1Zmz{`UQ3{Y-5Wd9_kN7Z) zYSEgNL>qW3Eaqovj-eQw5hBE#X=!GTNN`jq-g?NeJQthLj4&LXcku+zpAH-agx-R< z$S?Vlp+{W>G`BsF@ikbD$I-D21o>z$rU#pq!XD2(s=32IKy|!dKLRU3v1{!+o=L)S zZCt~rKp|eY#lHjdKQtbu{}gv{8rnrW=^N`UB${b>ntK_XvIfs;EA`z?SbL~}!Z;dd zc#-ETC>Gw`S@tvjZ7^WiyHFVQR(c;(lmWPOv;tOGg+0D?V7# zcm%?SvD3(wmi;s^7Ol5INA&K*egUa)JX9E>X#RwpkYL(ns21<4jHjR0;UD&+ou>(L zofEh>)j5GUhGtEO$8A$PCnOTAm7r20HXW)Jx*e3b{er|dgAjVb1?i8PfoF6^%M7Rh z^}>C;7n*ZOX@UcuJ49SpFp2V?tu(KBT&q!=P4A{ z>lr8N`5uh)+^Ic866w_5L>W@Jo?&d+O%ebuV5koRv z4PG6ts)=a9-h-T1>bDNdhygEwDb{Ux`MVjPd{ngSK%KT299u2Tcn1Me8TTaTd=RNA zol;YSr=|i09}hZ%a%??Mfz5AO;_xX{Nib~}e(bMk??WQcDoZna2P0S*Mgxd34_90e;;muka98WF$7zwP=mX`1{Lbx<+zS&q^KE7%R? z1K|FL<0rs9Ft?xfVv?Z)xF_^HL^I$@=pH(>1eB?h&SpcSb}*v&q1#;m<<5X+yeCLa-vxhVgO!RN9A-b^z1du=MBxY*JxhK*(@BWl7OHZa841_#HwO%*rZDy7T8qI)nhz)ck|8ce`jHE#rAQDy%y3}dP5H5kRM4HjBQ-fT@?v!q)`N1!W7vFb;Lj=BAY{rEM14Y zLh%HnZhX#`3{&s~_F6D4O5Y=`4W8cwHb}+kL#XWiY^eC{z(K|Dz;MTrO}LLU6fGSZ zLdBB*2_@ePC8zZvc`HF@B3cu`Ol4^)VH$CL9C+dLKca4Y{(q-W71(6o%e13!m}CBL z0}2dvf)bk%NcYZQxU}iOQRQzv!{`6cc-E_c#yG-WNQbY17T7thWf=OeI4yT{_MFk@ z56wN=+1yJGz}MsetOv`i2!xA65RQp)4W9rN-(#F-akskQ1g!U+PR@mmdjV_eht9nk z75T5gI=LaA=^^(6Zh%D8;E*S=m{0>1%pE{HVJ8hXtXRm3w_b*Kd#cvL3$P@id$q@& zq!P4)1ZHGcU_WiUC2HqAMQ2()j@faRmdhdPEXO6(-5w@&*iRB~U}tYv24b3lcY5BY zsWlG9hx+ji^dm-N25}P{mqI_S!~aEN_XzxfJXB1?CM`!NH-nO}0WIVoQV0BvcR=&i zfHC@B{GO)%YRvrf)^i!N5aXs%oowdEtKKn;;(qN1G?^(U)5Xv^2)}xQX-}g(Re^4I zq7-_(B^H!>Zr2_`IZXarP>fCf1KD4Uc)qvW=RNJ$enuWG=?Ugk@bw?%rHP*(Hu=U` zs!GRS(&3vfT9BytlAylmT%53y=B$-u7Z$B|vKd^76SR9C=L&!vI4X=CFn#-zAlg#9 zjym8fh=R{kdX)Q83YRVVF=o!mD=7qzk2k#Z7aAEmH8&{sEY|8#4)N@d?to3-2NZLe z0-Zf)(*sj|;z(2~fA#4o6eLgZGU5#Mejq|Hsd3tctYLTxbwMFGKV<#Mzu}Qownv5S zNxT6CvAn#DI14_zg8g8lQva~cLrm*mZX-OD9-D;Ew$n30Bte#ReJ=JFi}d3_MNAOa z@)StLm8_>Evy{t`vZ{j!fZd6dt>r`b3fLvCHRQI4?R&PZYm<&MlHDTyccZ9y#f7b6 z5XnFr@CpQ$zAv{U(H(qtIev0zOI>fnXXtr%@V+LNT-}IGH{bc#Z|eJ^jiq@3(x6$i zJ<1tv_ z@znP@0BC_7pCdN+S=IOyQcDS1)3dk@3N2zv>!8w3-u`o7L-l|mI{{7m{_z?u@7=+B z*#|n~m_8eus+ML1<#i$OJ>o|w2ZVs-y&wlc z98aH$Thb?qW@I8R>WH|N5}POy3UZA|fsp7_BYhByCSfk79FYh)(ZO{+kxMEWiL2T{ zhP3-k0CafaEIPa}7%dg;S+oJimLab*feKwI;rrw!X;6goCYVp4QpxNz6D&MUCw=El zu8SpMU<4WGO$8yJ4$i}=&A|zp7wmh2gAtU=fCyjc010>SsV@Am6DM4gEwnutuO0sw z<;4?9L$n!8vL~Qq5ld&|Z0DnWc_85o60oKD1x7Xl;W2Vj$YgaQA2ZLZwjxbiK_DGt zu{0y-h*k_v2o-`gckrcz4pvwkFk$`7*h4=rL@CmMi2Yn95+nAribPg3_46fEISG(9 zfZXWoM_{`8`~i{gP3?Hf$CzxH&Ul*uZrHfePv=;t1 z1j{&1uahieG5~f+X%Q;&1a^cTqXkOvMXFFgu>({5v|gdY$j;Bjh7WHf7H7PXUp+zv zak9NNQ#ZwUphmui6xK+rRJ7HU4P$IRzL*wDKxO4^FsM`=l$W;;_D`?w7UYKEg-p%H zZ!S-POwGn`hF%>Rzgz(9+|~1FA6|eBy${#I9b8Qn>W6~RiO=g5+8Egds4!daL_AO{ z2J#eCn5}mr9w_CTWkx6VYM%n2sRsV~r=g2b85`Paa0S(<1`AmeN2tLjRwK5W0$A2S zwI8N(@HXItUkB-qXSLHxfz?jAv}R<{&nFpdoR5QPj188hSY{<(up*8sE%}LhnD>O1 z&eQP13@i@}(yy@>NBLn2kaZr|UhP=`Bq_$8 zHT2n!5_@;^H1#=8N%YDw%$ydV2J zS}r@W)tl*YWlVKX&c+*9@vwJjIm445rVV789Uh|4U>|77hiMUh^$HjM;Dsw|Zbm^~ zIZ6)$oe@gUY7sxpSmB;5GPovZm1pTWsJau_yCvNJTs_Ujvt?xFyUVfV!|8;4urZv^ z+^H^CPHw)7_{?H_;^{<%9dUp^u_CV^Gt=eDIxIZ}6w2FH?n;w#4t>K#ch1>0$No!}^R55~*iOUOq|>Q$jUVM$pamWK5nM)?cA1GJlyP)24?AS@`D`S^8mrk%-N!)GEPccZeGQ7@&Ha~ z4F*v5zH-a3SIR9{tSPq?;)PMY8Z8+VvD0-s`_dbI1C7ik z5cejmzyrQ1!#-O{pMO)q#o-_L%|&^>&VrDs-P)=L8O2gEWn3hdp=W|3K8Ro$I-B-v z@!>Pe&;}cx@QGE+(D`_+xi&u`7oXK|8sF3cDu}^js;AcA8 z&)WDH^T$Y6slUS%d&c`JbU}%U98=;p zfg|lI!ZZEWiQgvtYP$9;G@2B*kvJIoV8UXeZ-XF_idQCttSm#%1&qJ| zRFaev-w=gLl5*ygn&SOjOYK#prqlbBlN!wAHx*SAAuhXUpQJvI<-%JQC zJfDtz@i&Cc3FUq)IN+RX+Me@=lCGqVZ&0pKNzDc(ncJ5Gf9$;rcvRKZ|9>WvfygBjywrHBqsAJ%21N}h8X$oQ2AdWy zs91s!go+R$DBfx`0f`|RD_Rt4(^gyCT1D#x??FJYT1%}}yho)v(_oFQ7p&I#f4+O~ zGc%b?)c4o-eSg3I^LUY^LQDgexEZCEc>>%ZFz79)nBh!qU0=+RjXF@4?VVs$vJH& z7>f7ALWsIUWQWZf5iWn6rPI94?7xLdwDEZQ|EJ-mSM@*-X@l>T%^%7-tmanzsUAqA z+lv|mk$lY)=t9aB9c^(>Q9Dp!nrPAGpPL!G=O$@{_o9zZI(=pPVQb2v#H8ce=-R*S zpm&Wwew(|2(b_d|D*EJArJ@;wd2F^&}Bxd!$Ym{qYZAc`u%fqkRu&sx;k{%RpJ-DRrQ8io!v`- zjMZqT9nq@U!NgP_(VWi09j!^uyGR^06Vz%>HU5_+z@M1X2tA zP8W}35E3bOF9fpRVrv&C_^>r;tKRHpBmTeq@fHf7t%W-PnaHZOdm z>xZ%VkhW@iV>}O4ZtEosONF*Y7WHV6S*}HzRmzsb%=-fG)}LD0yp{EfAFPwlfDN=d zM}7ZVQ}zna7)NuUs#)k*muTB!C@|0(&7uYqaS<~Sr+UI9dd4qrw6&g~c9@whSqFtG>ll2f`g=5^7C^40P|U(2XxL=A@^}y>(qJWw_+w%L zN~SQj`wdit*A0Y`S2z$eAQ%y5SO6 z3b+@HI4J(aF1!wz*t*;~-Vr(8>C5tActieuecJtz+`D-e1v~9qmnO&@*<+-*{(_X| zorva@zG)*nqk*M#_cR4VptaM?RyNUQicR0|OWz(1i_|I8^zF{*+dALMIQ#NS>^Q?q zdl>4U)9lAP+^6h^OxWIRCKue*zAw=~(I%e;qIq`y%p5U_f`~t=q$w3`*poHAWF<>_ zX`8QyNO{+tv%%-)`4?n|?#zO0-Fc6~+ry@Z z2(!EhH{HoSlV^1x`WsEr{7>mp=7u%Pg%+wNf@Vu63u>Z=*L!OeFiQw2lGVE5Qnzhz zP=c-Jc3`^hJi*#A{|ht>&C!|*uC-ls4Uv;tb74)U3G?-({D#f)mox_dd+c7r5q52{ zNy`kU8DdY;5POP-*k@>nJwrq6=^A2lJ_)l!W$}eFUI&q{gjO>tN1|TS+A=vmXuYYj zG*)8AZGS%1$tI<#{w~ZZp(+&c@7f6xNwi5lS>th_d@uSd<4oTxJQ7JO%j~bV zeloiittzFhpHddEO}L=zq1d@?DM`EqGu)=C;Ei9tZK>g7au|#0b7u4kQz++<*Iy=Q zoHAqV-%GTWNd)a#7eBwoFiM2khpn<)Dc$0?hxF1s$M>_1 zvejFc|70*`ioZb>eYbu%pz+cIue_z!TUZjl|Ku`^&ctHOWkqRu^L{g%C*jzP%`-(+ zg-`=yBO0-p{;n-lvB!`NcW&dbj-g^_(Ze79gC^ zj?sDR!;2rsg{f*e+AL@-9%#$`ty}4_JP&hj&ZIX8u@jS;2#^-mM1Wk0@X%Fe4lq3Q7Ble|9(uc(>#I8G zb~|CvG>X!Nm!8VM76@No22TPTWIH>=%gqpHQbYw-DN^N})8QAQa_EQk`R28E54J`TUA;-eY9lYrNk zczO9_Gk;Gr;1d1KIs0>5sxJ5%x$wfLJtfmt6ZR`XW#WK(1ZHkp)S-^ z+)vsmE|7GH3G8D8@d1M72C(=^lG&tg3NL1Jpj=mpcAD#|J-%lA`Um>6MpEY0a|9&* z(!Z1bki5ELd7LURGvo(fE%rH2EIGcgvIWE7QEWCGmLwPZ9$WmXG|ZntaXioMNYIj-p&4lA7ZfvbLJzla%JLeZXjr80n4HQ0unw5H7Y#gJTWt;XqOv z>ixPTDn`1Ae4C!>3hh8?3P}_vAT|{hn%X^Xh6Cu9h#rUjgI%T+AiCx=nuzN%womH) zmn+AJH_b1^?p`W!;obb6lv$gr9Qv_T$lA()&Oqpp`cQeKG7Rkz-E`OHKeCz=d(~(I zL!`qMPYz6#rkPmkKywt6va)%D%2}#{QMo8k7rKYDs)bAyQU~F#cIvDkRl*LUtL`G6 zy@Vo~%p|RZ$Ji25qFzd^j3id6owC?=9Xd)=X-pjb{W(8t+&aD#j$0JU9Bzw~KeAMUP*=)>HU{$i29s+pS%j`#_5&S5Ac zH!tJs%T9`kzWmI{fLrZJ)0bj^)0ZP?yrjNVviNdj-e31O`ZBzjxpT#jA8*uN8QLMK zH*Yjq^r<&rYU-gkKey#(5#+)>Vm~0vmK*(l;ZCu+$>-R*E6lu7>>*(63CUOMs9$~` z{W*X*80)T^r9Vf|GoAjFZj@NXe`S1KyB2k8@_u9d7s>l|@pF^+8{$7syT60`G>_3= z|4E)1b@?_UbL^w&IZ-IFl#`|@Lc|CkE$?w`csRWHQz?;_y*2L_F?;yl6Qb3uC_1xj z6>Dj%jjEbgcI=vtJ~AeU7j?>O#_$&vT8BPUC1r@}rQD=Wqpi+6BNBTF{l<>*f<9+8 zYH!=O`>FM2a6oV4XLx6)hNNRZ!{4}`!syj4yfaCJ$QEkLfwZji&mv%#8Rc1ps%s}e zip-N}qKl+lb=f0U!%eTC&7%))H!71eRd`L2_w}w^Nru>u*bf?2!=BW@OIvP?20oRx zl=~^7ekQz0T|ZpMDKs;&T}D}WKeqS#`fGV#zcD_jYACbxagNHrb73*xi5g5uhK@U_7)+B47qZ{59O8eE+K+T zUY(qoCofFU1C%P-;!1$97D{f|K^?X7p0R(r#l%kAKmA=rx0}fA{^{9@q5ad6d_zq8 zr)B-EM4AJt$_*LoRwwPA`kn0F0WPppj7@#;)o2KnxW5Ts)uVf<#N))zjPkyGnQE6- zURjbJ<$XXg{PJ2m?8o&g$I0@p7Ae_(7ucX@lwq47d%JtitAB@B630M% zj*%wCrZG3z{oG>{OyFL`pR=Pr{$dPX{BHg%e%Q-;!THmaePo`dScKx+K~jIop~fqn z;IO}!Tu$YZD%qDvt8Lwq$OD%qrP8YI_(lIp(O=wg;Tf@0h~mn{x(jOcrT8_b4k@{R zFE+$vrLlfe^0n=~1zSZEF3Chynz zJth8J3K!o6hssa)r!@TTN&LF_?MeKG_?6t7`KliCCrgpx9`h%QK$!UxW12O6;+y6+ zY20+v{2d`=nk((DtPb@D-#R}oF6f2YUe>uR-8!32g5ELKSsUFXkfvk#*4da&r-6-i zUXFE4)4(@U3afz==s9Pd4F$*=>zqfq(p0gv%2jl5(;+C}ZORuLC$~hGBv1+tr73e{ z^uW+qxapg!@PnN+7#Ct?t@TU7Vy!nzG_1MHAWcDvGtHgya^_C-D|Ps@-e(j;QNra# z1zksQ{-&vqH1c!~8`^*|Usu9#?f|c)qPux4r(INhuypJPEfw3F$8Lv2{qfP(i;d}u zHXqZ=bd^!iG{(hH$zojnP1;fPBBhwrm%Z`S&+N)E)TRa%tp7D|ZoQ)V8Uiq@;Z zFs_DYU1FSGnTQ^=%BXfuu2q{Xy_i2TgRUKD0xcue7mO^S`;T<`DP7fHV^~m8U>aJ6 ztNqjNgU`Trm>QC$7(!h-(EaF zSqd93UTSI~{xRm}MNadEH8#z@wQBLoNA-IzHo|0)Y1bgG$-YYRZN$m7x#`I#NL^2$IcP=0=+F zFqYQ0YI`T|*TqXy_p)&6dhy+pxQ+3x_)WfrJ3EPA7yk+P^!4BL{V9_fy7jPUQ4rbI z11_TP`}23lN+Y0maRto$T^nhfAuEgxrIP9Mclxq@a{kVI^WJ|UB{hXtEq#Xsn#uo@ zlpuWyZ^7RtO zo|9Hq?`z)bNq^pIyy7x%6-JFXxi<6rIwMd`V~)df_QK6C=@>W7FRe34%BX^y_g8Jv zGsD%-5eZXyt7ll9vSQ`tvCgA`lJi)K=X~S_IFX{)VCii&ecFncdIqv>=ovAz>j?X# z(^IneuZ(s{(X;CYHyToX&wrh#Ruy@(Ztx~FVjoGCT1UNbD%xeF!dpi@rTas<*WA|Q z+Rd3JX^VD=$FOCPZ-R5^pd=tGSk1%?PlbR$!*!WNc_vQqvuF+^ju$T}i%*)uVlFWzyDvHc^*s?W>o(t6FUP>M+|r+ZUrkYoyxuv~z9sQtiuG z8?Jp{=>baO%p!s95c7DX&f`!cY)!feSau9LC~&(5O|6S?4$7|pQ*+d zds2-*hsGBNGaKK`)0_Pxe!K6}yWJa9UU-$Y-Peh0sdj%;?f&}QjCMZ&*T--71C#d~ z;ve9GTK(Cpy~U3n{rNT!+1mSxwDy+5?z6ooXSDb1b28ieY?9mld?a`OS$qEmzPR>| zh#id4j4AV71AO`MrhjG3Gc7RuOq*vi$8)65i+x8i{CTFOa7DX3%)HnNJy_@dU_-_~ zS0LJwI#*yv7m`(hd^jgvhQ#uu422griTvg3)iPP-=xL`vW=MZLX$&Cjbn{X>Ay@GU z{5gA>rdJ}8>b~S_x>_kEU-Knjmr=?ddgXImL0`TC3|Q>_hWJz9`;ag7h92^z4wFs3 zzMm#vTJh>rzG^b$YX_!vg-YoYovUW$Yp?%UzD|HIPQI#@5%!{;8YET5YCWJb_A~rj zRn(wob`#hGJT$+FFTs#jCmWb?OZxcn@85?aBF6YpDSY`d!e-^`X~kfKkyVQBB(M)n z6kfbU1<9_B81Kjq{XSie-bl)k9q*a>&DyMTw8e-;A9`vJ!=$8rx=M0%v?`HqpWej< z^5v*IdA}k4Aop49lg``G8z0d$LBda#6n$iOC1?fB>~hklRMKAmc+0d#^7W5dlJH_J zOy@=Q!DzT%ST*9^`lFau$_wwUaUk>2v7=SN)=SImahcm={Z08fp}MOf_M5X+mks(t z=%1CIck(nAYeF??YxC|W+@>G%0G{Q`EKj2{DBa6 z{K0#%4n^ao_ksEbN2~2`e)GeQqy|~YCffDUr0OvH%hcX4#_oWE<@?6?5AojpHn@=c zwEp{F;J3a&md5XadQ81;Q)l?CF}r&&$LG0PK#XS_q8MLh`Tm&roW}Qgrf0+Vbv3ag zW)hac_i)qorhVc4`uoK~cR-N+go$`B9je~BSgqEQ^?;!7o1kpXQf))BG|N$hJlY}J zxRFB@jb`CMaQ}dyGB0bp9MDI*s2^^ufE*5agf18B2T8x3`r!@w?es1l>wnfi^v+OP zdoAeMUW-!gwbDdPZ!a~K+H6>|y~1jrwD$UmX(P2)S*&peVXnP2r=)XwIoFRW+QWeU zOoR1#7|=w7QC5($X08uCgiZpPQmW6ISZ31Du4mn-7)%;|;dwyyM9O|1(CIED>p3x_ zTp(vuXaLvqJfH}HkG4}BZLHsk2L`eA`8p41hom0ODi_E1mW!l4{}crpKRlUU{djG< zytGTfJ`b~fdHDx}0bgET&wo5! z!95PqD)IGtN@TSE+$||uH9M1JW}dl^bF{`8ve}N3IoO6E4`^(}fwS42>fVtMqa(Ca zG{l1z847fc))BO(FW>(Kd9Zd_iOtc;R$up$hfR`)Ct#v451-=|`||Kt?z6~4YW}cs z+vFZLHflUIe2#gRY4hP4 zSL;2qu3IdB+?ZPL*X?68Qg64#yk|C}hQg-YUz#fsYcOJ&HmrUXm)^2+L;ODO>4$nu z8$bUy@lPX|*s$;YH}bEE`u&Ied&Bh1KKNIp#?IhhnRuG+_C1MT<@4`n-2ad9Z-w~x z{*?bl{*_a||B!z_HR99<|F)jln}43^Kxy)KFJla!e^+z=KgPdy@$arP{u}w1i!lF( z{F`RPsSp0$qQ=gUzYg&1=DdYg^ZU3LEkZiJ$`|-ZH-_@pLuy^7O#@8*|p0ckejLpEJ%wos7d9 zN5IfLz++4$sysU8%dJnZ_2(GCdX%Nqf}8F*B7Uc(X7Awz`X?dmB#qC9{KE8 zGg^=Bdnzvn-Pf)1#!tXJS^M_D?MV{GX$9#km3u3=NO}%bIRclQi zyuO6PzaAZ+>b5c}YpxKk^kyELU)MptV7%B;YG%(nzM8aZGCo$Z*GOXN-h3BOZ85~F z--m;kTfY-%o|wl;we^R3-8uF0b-)>@SS#XN! zU3weI?0amTqGQLj%Q1Ze(&s_R=!~KgcF(Y?Sz?w|wLMtto7%9W<#R5 z9wbwBS@zbJsMqdkT3)3tmZLqmdvqiHneKR?iKrz^))gO*H{)X~N3Y+w8l+lgjjFg3&E`j>!mT~s54wSGL8fr!~Zf;-Rtk&JPRi6iH{#4d_9DO*||K{IP!C3>iHJ3=;|MA>NB z60xl&{_B6MGg*C!I+@Km)uthlkqyV2_l8UMqDX9w8IEd4nqnWFtd`aM9%4b$nf5hr zLY_n^L9co(xrZpez@=)rF_R!qyah%Zu>DtamP`OM563zRHqn|viJ^D1RJ9;-foiF`mgb| z+_=2>W#CiKgSyryo{i^#16&G-t%i;8YXqf_wk|hYp2!ofefv18YM<3h#5+(L>D^6+ zf7s678y@PT4zVB5L*iFy9MC#y!U($KZ@;T%d)>D9iB#3PSL@>Mrj0<)VKn8tUFRk5 z*Trv2yT2@Xzaf4@GVO-=#oVV^oXq{3joUbbAh}q{HflRwZMs*IsPB3Flg}eDMZ7_W zXlaUgLwNCxl41Aa+Dye;bF#fcX49W|pcXN2QJXio<||U`@b5-j5B^Xgn)KUB#jC86 z;*-|mR|os)?dJQ(tLR<(S$X5N<=vw#^V!n&IA`muUhRDL&7I zu3>T4yeyZeObe=pJ`sBk`bHPw#$bWSzBt-i#f#)bhzxr-${Qsm;(8>?d761$$N2q$ zG&=jmoaD2C&pGKdpX!*_imRi|)!WsS#e6^b{P7}NZ1Uzc<$=7VJeDIc$iSTHQOyU5 z7T%*&yaJGjon1{UH(GKez98)*RYq?0o@s)fVaDz_!{$7q63xd9rW&Ikfq%x>GPiJ0 zu00`6=kv@eZ;tiQB=k>SR^oh~eiEzJQKyVV#IE{wN_uX_XGz(5e}96$2T{Yj2I}9E zv$WzFdC})^t6KR;OR6lI+jK#b_z6* z;zx@q&6)98G_Nl|s;@Gw|0bThK6!0&4aJH`@x<6d?+mdDKKuNU??11=tzSzTS7>WUEc*FRJzVG)Q}Q7umSGm2ObI|tT59cZ;P z<1cWNCNDVNe(^&^DtbmCyex%SgH%xl20ZCy_?9Wd#huaCb2@A}#&$$o$99{UO+JFw zt&gb3N>vV+Kv~W>Gd_W*lsrm)(#x}46f??G2QGstoTo58N_nb#mgffEDdJpkRNp!5 z{uW>O{!;5$-oRj{mGYFD^4unQN|xtAo_dxC=`>wo8{IM#%u=A&$>ysSC}awBDNBg8 zu0V`vS}XhlaU)-9SBjuDfWuM+d$dknjl(EEt)h56%SWlsg|3R(I&Y27Ej6mOD=nep zdKYkf_7Q*s`aCu|>6ymN{xF|T~|>YNg6_#`$LVCN-+m6%XOpEu%Se7=a| zd)zyd=37?m{4B61T8zbJ*iEfNxsz@i&47r3%^{a7Z*37}9CVl(i-ABzS+wjkqdinb;I^ zM$u&c36w7OmuRV7L86-BlU-w}-(3^>s7hzZS=UZ#2FQ<{Og85835Gi4^v+r;iYipC zb%C8zWRiCgIUDKoA9tEoS|3RpN;tb>R(oE+k?blqc1A;l0<{E(F2ttNK&n%*8zh*v zV7n~e75;ndXHj`%;y-Rkar`>t(|eXV3snC>MZFkPUo_d&$#4|kmga|}5uA|tL3(N( zHRDh$#m4b?&+{vjen{5{<0dq3=-R=**$W|x%_9ok%7|)gM;;`@Hz9Zm+?VIBH|*zf zy7&UCITb5KU>`AWK#FCRE$)EYLJ>I`B3*B~MDnvt^Dt+n;&wgRvp;k<&dUqbhs?VF z$wWck^25R4F~&eh9q`0Q@M=sNSIvMi(jhr)9NxPTBwdFBPW1(7qPl;b-Q#QYQf!lAxz;rnZr-eV0C0wjo4p5LK7%sTz6 za5|kJ-mg#aae`*HWuxcMr&Kn7hLZYFrx9riF--+Dnb&1BorcF8yKS5@!H;ezox^wi z=tvEV0<*)5HS^N2DCey3;(YEVs)O(fQQpL6PMqZ%ig?0$tu+t}U;ntqj>|hDdS{yt zX+6g#XKnf}v|Ya~r-W+` zUEp~SGW9`QyoOZi9ia}MW-_@W3QXuc3z(-j7c^I^nBe{l@M71GRTbR}&HmnRr83QS zfnk7UO479oFV3SHjPb?0w2sNKE3{*@)$Mztr_HKBqTT$IbFcQ7u>1MBD(VF@6)ZW) zl+6EQ2$<2gKI2%`Ix2C93dVS!A!i-uO;BCBj#0Ck5nx0%3p((@av=6XIk~Ov%+YZg zYd-<-^~7d|sa5QXZQMR6eB~0&81eBHCST6cEqzJO;{KfSN>^kGD*AxN&0NRFMv53C zokEqjgKYSG{0-6O<)-YT?YHAKdR%-tjafP3HTmMU1t7(^Y%yprR%CcmuieP_n;*w^ zR?k3p)#%}VBh{KzV-~MXHDM;KTIVe{=wZZP!cFy1kxVj5v+oNsfu#NFijAb{*rjNc zqle4HjVMT(=wUOt2hXa#CvuAd@omDkj=Jk$0hbY9%XX81HVq9*)UeUp1gW7ea48fWe8Pd z*lz0#b6^S|Z}&xl<3{L}et(<-%_=M?{V~D(WCNw;j>(v3kT)~TunlNtCm%Kqjp3t9 zmQpMKa{=~skJt%is)?%vP!eragHa2TQ+Wc;2Y7*;(iFi)va>#bzDV!;09Fw=1<+R) zeaJW`G|EIKiAGC-|6nFq0-UR@Wfsu*ClbQYs2v#k{!$cP|NU zR@r=sx;Joyt5^RmS(J8?rirM1$7!`eBA&k`vM5C)<+7sTrneM^CbgDy84=>*Y|=XQ zvWByXVhR?%Z%_PZ+vk*Jo03_t#yzMnqS!#dKBb ztw!b%>K85$pYeIBaC|qy`lrXG7p^z`LkxYjBro3P)!!|~TNSetAzeR9HhTO|De7?+ zuzfa&=W%_{$ItK@UuI@IWc(pS-w)kUH}U0Zm{5Eq)KrW3&mG$MgU!$D3ygm-V}D#i zCP*&drr9^v`Fl8qw}CU0#`S|@#~%x~Hex)8)_7?ii#$VfEQECqQ$u8e(sAyp86 zzK}8P0}a#@yP<`KXe+XVZW{eWMQ)(dE)W0eGP{ODFdl=(%xey-n?G;*VJvi_CdI}z zi)yVSyvoo}XK#L-E;bxJXWbi|o)v`6nR!^P?*QfVjMt-O8|a#2To{$c29&Cfw&8kr zkt#TVHX4pBTuA%$&|2Z9&ymrT$VZ49?*fIt9Q!$9NoAj=GNy8VjaA9`6GXD}sA6p0 z0}L`PD`+7rFVx#QzGRBaUsY5WJCD}!tEWuqbI4G*N#ASK=hf=u#m*9o8bl6lf!JN@ z4%+2-03(0cPf4cM4^obG)rjtJ(>0LM+{5GgCt-5n3wxTDWFJJd zd7{Rj;ij7vfFEiyg!3kgG=KAn{*(X(X^POCORaEGN5glVjEcP2Hz0<94`Y9Wn#Fdc#X1FzCK~#K zlsqQoFuo&!8uzq>c7x=D5;$L2XTn4yc-e(hO)7cx7n1%D<#Ij-rf=rmW29D1Ob^oB z$PrYGcVmkK=ktNUbYA@nsch<~AJ;>4(-*>L8&YY@)8VGYpW3FVHr`^C2=V!#EI&TE zMAAjSP0Dp1%r>1@!fY(zMn}Nfg8m|K4=A1YgN&L*Dm&Oq4eqq1$4RHoV>P10hMnE@ zQQ~?AOCQBLj#8I%RE=vpdK3$-IM!uE3-hP(LN5A;ScAv7IQ@1D=zLdJ>rr)G!cJ-jSQZ)HkNZR?HZvJMLCR_Nf?t(>HnOk6($=stu z)(H7Mi!8`8pEH&q+CSU;i)M!PdJgJlucnL4Hi`Q2E#vdFC+bvfiC}cpXg-&$_LmBA zyQk`hrSF_t_)>D6oKM`E1c}ZySV8Sy8lTs7pX`|bTro(;9+uhtx4f@X+5CZb+yO`+IVElJ!pW4rz<%aidr`J@eJj}>`Y=K3r4c&p z4<$Lx9Eqk?NsUHC&m23pg}oA0Ek*dEmqxl);)K7x8@6~1*x0+nt#oP@8&P5~kFA*avng#(rw5hufn`9$52lh??y*!_nk=FNzL(W@$%~HoQ@H5~*-oq&h$uW} zotc|FKA$9E&~qtYu#avmlP2+EM?=|skVlQrb$s+7xdA#&b*?N9Dh~zw_t>2!DwvLj zCqJDLU+w4jvda&I&9BDKPl}*{OqA(lGSxSV`9ze>Su6UJ*Mt>}o?28nA|4G_$kJkX z>MC0Y6+}m@J%9JA5lo|-9O%r3)`+#?CC$o`l2ykFDVFBP+C|53!hVp;FWf{^IA@Se z`IgzZup~}<4*X#vAIkq;)rQ2%$;1rZo1~&qS&Eo&X&+r4)bPQ zR*q%vP#LhuTY$PG>^MQ4<{npJH8*Bo?oxM{krBlvJ8{?Q9}Q_o6|vW;7;6D#EJff= z#z)A0Qcuy~(-Gw@N0f)}J118y+Hyu->^sJQg`Qog!n{|GBJLn!`O@3%sYw(>vp1W- z$!8IfXhw$V1@{XbJKYu{YPKuX#EU2dWvb?Jx#42`UNBY7Pf9K|?Qpt!Q{SzzSZ}&Q zU@`<&SpxN7j1a{4KRUkx>$HI!=xnQc#BxqH5C4HQeeh{=}J+0Osu5mZ#+C{ zXBE%t8xC`@YS{l8fPCz{$BHRVn&vUz1&I9uUTQX{wG3a<>_Z7n+RKOu@4*$20+C>0 zwI%c+bKp-@um{3OONAF3L=kYT%50oxlzn*dgWPEjU%PFp>3B7m2Q`yft@{IN zf|FyIhGC5jQxGvO^hTou=%U`{M4#p$rn2xk!tW)+W4I%K#=Jqpqdgr;WPWus`9&_komAQw`#bQ`6Q@ z4Nj9=rFI|QXGUl=#FJT))`e448N>8Z?D#`ye|X3$CNbh4sdpspx!dnH74k_tZg$JX z&(r#n_Pa6h<@;ysNi!2Dt)p^wqI17m?J;@?+tH2;()QD&c{I(F>GMlj!t{S|2D z4Naa8_#cMzEh}b~&Fobz7vxp793U|o+8JHDUEhpb8-2jvO~wb^!c7wpD-5jrbl``T zY{>~%Y>Eapg@3rVYUtbH`<{$0elooHFKR*^ZI$5W0l44|dzqt}(Mx;-a7+QKmpdQL zJmt{^RU`gd&x+^2SfFv`n@2yv#+e;okF(orLM+-mf&FE%g>OY$D`KW-v0!Cu)kYM{ zhxG?k4rPe>N6aT*l#fN%#)4I?!#5~LHW0_oWZ9Eh!>Yjh`dE@VXqRs|&RHinhB?uN zSam)P#meN+w`0TLBciH(JZGweIChQedwy`c%!@v>otERe427r#43;y87sYN>K6Fkq zR6+T0CSJ5PS{NTl%pM$!hQ~99xhEgOdn?%WFuM3;>EiQ;_V9O3scO9-&wPc37NQ=bE*C&cNj$-4no#MQ@}X zg8Px-*96ZF8ll2TDm{B*` zR>SGXRXY2jps`x$cN>HXTPn23f2J1si;9|Aif4JBQbqaL%w)bntPYu4U|%?smWcRj zfTTA#SCe|)k zFzxKxNPX?1`pCk$)6SnZd(O18=G1y;O}>BPQP&c{Mj=i3ua$bJ8$Nw{V!S=nK7+?+SGY7(;lYJTR6AgM`aWwNw2R4J@Lr1 z=FMABf0V7?u;I1y=g*rzb^5gW+Oy})zp%pV8Gh7^+Inhy_~DK%Q!}IY$|igIVK%!j zPo6#hVTUiMpOIF`v|M}UM?}+ZHlvEPcd`FOMV?a1#mewy$;fS;zUhw$5jUlG4=@ni1P(}CnC@naRxV{eVe44B7kxfkNM z3%}F(sm*8dtK~PFpN?D@%Wr>vqxc=jZwkLX`SFgl$5wBTiC2&1G_!MjXW(Rh$MQRe zpQccz^1GBDGv_+4-#iZlUcqlLKaO_r&g93sptm1Ch3(6)k>7FrcIP*j->&>L!MHcS zVtyy^JAN^QRv+y1H_~Vbkemv(2Lk)6=2T>*`Fo&)9##$o*%W zIUq8o_UzenM@EP-`|N4;v**p7I(zQy`q^~hi)v|{WV{M>n30jnxu$nV>ZZ+~c5ZEb z?feC5!L)dl$<%2y2lPqjq;Zisvr#!{A44cJzj1WK+8L4hd687H(g?#XTY&#!I$LPU zZM4G3ku&DZ898#ntZDOWXP~prnm=v+g(F9vU0Yu~G&0ka01nNgq$VVC!L$XDIrFB? zs71?3t9*s@-cT=6JY$a0BN5wKBj;KrJ2LVmQOlH0teN z`5mSFOdp^TzEUCm#K4UFk0k$eQptS$Brg7l+C|fA>m>HQV)!vFbn%BBl}te8WFv-^ zOrJAvZmlWs7trw2ms~J^TAigKuK4z-r~Xc{C0U4!`jw{DZ}z!$bEekKsI2ab%O_4M znVQ)mla81Q_frL)RDbvp>HM#&pO0A8&6zg6w(`Uy>SrD?tYpl>nKNtWk442)Oqy0X zY1q_?FB76<>3iQKHB#|{deqKz>234dBY4aplzs=#EBd25x!;nXNK^x3z*)e;ZzK}S zFj&>V4}f)gM$tbCT!bz#HvqeVKLs9)fw~X7vz!|`kFkCBv zYk>Cy3o&Sa1}*}=2iyW2xHqc=80Mpa4+EzIOZNA?Yk;=^9|FdJPXi<0^1N4plYxH+ zwgD5sO~8T(cntn;fUAKo0t<%X6R>~$IpDRxJPhT8{jhHbdfsioHk^gxBFe32UX4WfCX1bGEwDR2|; z7GNps%A0_Byoaze^`8ZNg?g0o+SSfDGBIEk@V3fCVm)x_3DBp0(TRz~D(Y8QO+4zi z=(~x8oUu)uld=uC=49j%hwQdf;6E^W8gc+Eo&vvUkLWb$0B<{sdf`UU)> z|E#$UIRO^`8h!z{{02Qh|H-?9bil&Z)CV~EF8n_F(TckhiB8~_-@y<1PvO1r2{;)j zCwK+06u1Rg2Q0h~c>_)c79IdxLw>-e4^WR{>hUP`r5|nSKz?YS81OUTc3=TcZ{gZR zq6kjf4S z1ojxzKd&ud#~p}>H!DEi^gS=XU|fFTgz&%%@*2G3cOQA+5k<79OT|V*lNOPGIJ^ZEN~Blqd8ps$tJ#dtSfL)b(Lv>{wrH2 zVN(5e7)}^n!qB@1n0p2t#+1E0zi>%#bbh2Uw>-Z(=Yj$G5iq0k3&!Nmn`WwI={5Be|dgQ@Q$4PQZS?Q zi;0n}21&)=ydd&%Y$991bpI;q(Cucx@X?d@VD+WfC7iDi6g#O_+_eSXDS2y-w(r;O) zHTUWuuVR#eFCxz7y%UKy73Wf$=hZ5fYol_tQAK`9W51l+X(VK#(r+Y*wHM(x3mPYp zkM48FC?BFP2`$TQ4PH%<8IKRjMTbNZi3L0-WvnqsC&@W+fG=C)1{xVJCGPFS#T=PS za$E&&4Y+b)D%ECFk+8 z{=+kWjOLg1=n}?$tRdf5=&vtj$=CFK(*~yR(+6`l^pWoc+43Fl=i9R@n)X@^fA68a zu2lce!5EwN$27YBpUS(!_W!crMPJzeRS#_BiLfvC5uC4#%8JLgU_UqGOaE{3Sp8-d zWt)6JBJmVF&K*kGOkGdxsbf!&te>E|R^^ukX9lwBy0J#rZG+x%)OD2V8bf1bscTyv z^*t_;Sf{>~Ax|ltZu-`Dkf+zOwcl93{d&kvuvlg5-4VwP6p!)pc>Ux>jbdYrz-QbsJ?XJTQ@Xh#+Y28Z=*d>*QpSP`#zf-F zzp<#4Gu;QnV_70!rV*P`9Xtz`lwcY#g-PWexI%E-!1+TL#XA_B=plCI60QuK=rMhe#GT~m`8d^a7Pwc4 zSE*PNC5LrXaqK{(&y;m3;e!|#4>MHgC%{$UO2NrDv}263jeo4LHH1weY^cIckZ*qq zj`2F<&p$_49bu0t49}=F2m9oFCuFG+eNvWlc80%$-&I8P0t)>BX^&Id9{pvu@1dEs zqc!kx(4C3Ig+$A3VWeaHcRL6w2DgRqIfO4!_&mZ>KDGMZJVOkhT5-ky&3@eRUmUlD zxWkz@xL)b@Cazz`CB{#^AiogDlOfQ-gij&vZN$A!aX)4tZTN2dp@|wdQCb(<`oEgv zEMzX?2E~7h_=YxeZE4s0u~p_$=82|EWE{Y~hq_pjgW9m;g`138p>AW=Jz>_-W)QcV zxSuI5ZPlv2(z88Euw?4dNf{n3{YrG{uvr6O_t=k_ACj0BaoFq zlq6e~{@e5)3H_Iklg5OyDSVPjSFJ`qmc!G2vRA)h5(6G(*Hu;Ms$XQ$RpSRr&NP2G zg?XM6x%a*!{%KS?Zs2JHp3nfVoPQ%{?Zj&$-WbJ0h8lC`5wB_>>^0#Xgx}L6T>a_< z6JAY{8tT54@B(yJ?kHQ{bRC}Z*NhGypT9aMc&wBaHW%Kej}*&MD`vg+9%!~df^I-L zp(qV1&9@q}}}Z`fpX#>0fI zI5{;}q54+Ck6XZLOq}}`8jbepRbOn08yAnyZx06dkVBEpF@j!%-!0I5>+{bO-{(H} zHfWM4-6JmpuXTXWPE8~xDc@x_->aEl=^^J6^EU^BIpkK%SUDx?s;hM5AnNm1@|nYZ z?lk4IIoKFllDmu{>TqzkYwqr}MB)za)enp3?sUdBD%(!BY}ZI9UL8D9{@sM6kLAV-9XY`X*`2K9czO*qZ>6lWp`ZKmPL#DH zC!HrM?-bCtP5G~tSLt@9o{=*XiK}Gqu7iltLq>0*A~fGq7CZnqcUQ{|7_=aHT&&^xFe| z`-sm^6TSudYgO(8Ed4K(y@s6N-r4(_rg)6(MIiIHe}A5spgjLc2Kfy=_$-}NPMngO zMB+&!^Tg>>Cymc<2WCO5Mdz7qsal zzXyX~LAueBuTP;$JE!}B#ukp3wtXT)exs%h=8*oG&py{FgI?|*Nl#QOzpX~UEF*mB zj6~v2g|D;pd$w&&&a|FQYwEL}bU%kKCG~Pwif)bHw4VHst%P?%_fHCMAw1m|qPJad z2O^lX@_}mWopC*Cp*KwN7laRVdoGwDX9mI{E7b>)!+hE46M%2 z!;Kj0t3%0I9qC6xo0MMeF{0g5SGEy;3-rh55`GZj@c0Y+FdF^k?7EU!XxoRkLh}W_ zUpP&9@+l3Y*V86BRj#E}dP?x=m{ntr*Xa~rRxa$?O~JnH zcm$f4K(n24UBZ3t%h1fQYs$BsIK$^A5E#$kq z+ny8LKfC>?^kuIGnop7M(aLwa@@?;7Ka_71alFeDiN7h|Nnat~ww&OY?D-lWF#p})+^u7h_mR5MB;SWt?*aKw>uEL=}YphW*Vf@&-Y~nq*0jX0%C5($0bJNHVPZ!i5_&TPTK zWY)v~s4)8bV5W>-rr$_k>3_^XDx>^btDkjMB9SBdU4&<|Z{p9Y?DVThANJ|5 z>q)-~p3Nb=a2fNj;?Mn-{uk)>a$J<0-}RwVBG7vndJp~U^Mq_%Zlm(Ur%97F<=IJ` zt$v;h{_{NbMV|$Jp2ug;^CaS|ZgurM>_5-*7U&)1=lLm<1X=32i8z;B?egp*&KLKo zFXe3^OABv(;&^+1?>u3vcPw$D*CecN`z3L#&&ULbU6-Fi_*2j@w*E(=QDFR)dhpeL zx@&r;(;kCc{B)P4(%lYzvY+mP-szqu{8B&N*{O6}!PofdruI&k%bt=`{B++=rP~wy zEI-}&-sz4d{4hV=(Kg+1`0`u?KZiXcmvC==Eq`w2dr41g?7%t6`!Oi7V~M&9dZpy| z5A~0a2f6;odWV~9VQsgOwcYAqIJ5s%8vXP%=~r+6Jh2j5x$lsk`DinIZOt+BnA-^7 z(w0aZB>G*J{xXe2lk4~;nibTr%X)CE5vrYwBD`}B-9tt9kD^708J3w^T z`Q=W{5oxLPN=w(Sle)RSOGtM)a=nJ+xi|UgCM3t~Oo5nk-$K0A(4Q@TpuxuLr8mqP zg&99jU?^<-h*wA#V=vCj(r4H7PG{yxQt8I$cLahL!rCmplbXoTEZK+k5_qUJ*xY%& zX{P3$-B`XehlT&FzKNM*p@?1+G}nIodEy%JZ#p#dJA2q_&7l?V$N21_MB*mmXg@-) z`lsepwAWyl?A5>}C;12DS9ZPA^8>%M4&}K2!G}Z;US-TlCGBr?U*s1V(R^_3Q*cXT zhBRGV4t@@Cwmi(9F4gm8;-I6hmi)PW1khd*Vhq>VPfMo%W|$0yQAKi@fpJ{~I`=>) zhkmEJ&b`1MPvw zf(Bh`(fuyEHuOsyaiB~4%fKtaJ3}UtLk(J-6M8?_yC)~~ajx)#TKk7suzRe3=vDqs2o!Qpc`r3ZR*JrU#}$)lLrxI7kLCpL4tdY!1m6vLi-W;6 zA@9auaBaxj77Vc=;o02a-*eeDq5GePNc6V;pgdbVxFIKW_Ym)&Iic5f@|FZc@9gCL zC=|Lr?A;OyZQ0T5%nLP#y3X>bJ*+7SLkQ?p$GUMVl&%!(Zf{huH4YWgT0qRq2CSmp4=hy%ph<3 z4xtT$yf+6EOZ-R#cA3oW@|-E~@sVKYp^*27TuUMMcqBv-bR_PK(8gS^Ip_3;a=n&d z=uX4i-0_A;V&!gu!T04X3$%uUjk%#4L%}qDt{f7`ol3M6iZ(z_Mg>1Nd{-HG&dsxN zOqo{F{U?N;2n440doYk9mJls9LJwNB=l4VR1_PaeoL>clZ{~!4984vFHfhRwr`hTD zz>>ftq2RN@lWq+KH|O@dD^+TfMpf&P4`n(&bVndCso&4Ov`o~eu-{PE4Q&1TrPuGj zA$&x?**)k>#3--jAN0R4d`O_*ioo=Hb5e{lb>1(t&a_kIn4c&6bGCLmw%-GRKs2-= zlPCN4rj@%#zl-}W^O1p}-`o3|yudPKFfaH>ZqCZQ;GO+Kjd{VV`iFj*7wqgW{QLcd zNi~cq_r95VO$>}aD|Fp}Ks+aO|A1giZs<1yf{%xCelj3Lht1V{*V*;G(Wg3H}qD1@crD-y8OW1{X_SI z&&#sf2 z1}MN4IiVk=7@b&2J!gk*9UOQ$C-lhR;ODua#e;*7_RINXQ1FTYp}PhL5(7f-4hp_G zFm&zU;BN;h^!`Buh`DKyLf;ysP(K&-!(9S9Od*@=b3&uS>~-2c13@agpEds2BR6twmV?D!Bfj7)n4LJtOmD8f5}DFtlW zGm<^cYbMRJ!5|8VQ{9qOzbMT!xytA9+~B9d(0#dIEzNx<&98HVZv{g)e`y-prYiVs zPM|3edM(wVOqq7ftcQtvRWPtD5V|3yrM@U`u8EtQ`~58#SRM#{^ksB+!z2cR zcjW~C76{#%Zu)2&+N&4u|6hx5bmu{R>yiK4;fDlvKc03!FZ5P0xG9kHTrh+d!t4iy z$ngS~`mTnz?OWJdlpxyZhThv2OQ#)huU2~G_CBty=2 z$&j-|VBiCRBmWuj@P$a82n;wS^wWU1DuDOL)0jzM&kFIX$0ecH`gz*|q4)ZEjlobu zfA8NxRIT?)Zs>P;-c>RwZ-hdR<$2fkQ{apJ6!C|7r($FhD~AORI5M;?H}K1x(1*F+ zAA_OKbNM#K&ipS6g*Jw~$Cdo~G_ypSiolUagtqMvygU%X4Y@Wav}!=;x?r$rKxkz! z)HWc5X6f7^^kKiyZ39B+W&WSpL6{Uv*6gpmcM0T92z@&Z?X?^HkU;+{1MlTz_JO@A zNm5%F7JQ+X2#bdNHGr1!*5!=6BIx}j$N~h&yl;j^c*EsEs|;W4N18}q|9#DYuQ~8F z2fpUO*Btno17CCCYYzOcaiHvGn^nmw3s*Z>@?-m4x}+KIWYM3AMAh8 zkl)1J*52gB(oK&mhdpQMSB@4({(pFFPuT0riq0$L=K8XD z|F^;!7MklKEd+A;&rKQ6Kg)RDnDP8V#&ay=`Qwb|z>z)k3%h4l+s(C`d)~`EKj)rV z?Xp*c1+3bd>*;B>Kuwo{p_P>U&CYzsHn;G-;LHOpLDN79w-3!80u{Qj`XCEVFY^ZfTj*xF^Tyg3&7 z_*rKI#CyXTKivxraJv!9QP_BHr_j02OO z_IqZ}f|WGb=Q0PY9jtM%&cOx;+Z=3naJ7RS4sLd^)4^^By~{mw6*w4iu*AVK2df>d zaj?$81_#?5Y z9qe#$vxA)ub~|Veqf`EkE`JA094vFN+QAwJ>l|!wu+71C2Uk1T;oxQmJ00wH&}*{! z7C0Dju*AVK2df>daj?$81_#?5YM{9bEnAfK8C`KYr}kk&)t)&*G@eg^^*y4jq1I$sxlQn#W;3 zIQ;NKONJkM_)r&+k+jF-hmg?K6m&n z4j*y&Lm(tN+a3Nyhdwbs7E(8Cz!~5eCv(JMRtPoI# zJ-jy@|A=O;&#;%->boQHm2c-r8}l(2e}==CylfG7IsCyv8^7D(KXdqxeEcgGG2CVR zSgws<@|wk)ci^~P8nXDGyLx@!(fOsr4{oyvXU4s!9R5OwpY7tm@9=LryxsxOwM#!s z{{x3V+@ifh9R6F^TEqm0uWM`+eCc%-af!okaQJ&1UVGbh{ng>0S#F(g5`Sf>{vo-^Nl*6B& zfuHK|S7zWZbNCewztYv~CWk-bMvFMq;qP|%X%4SFeY&1>`0ktR^H2-CcOBlJUpdd= zeLdFkoF(AMc)QWjvgxq_;8kCLe&(PII%_lN9MK0Iz3VPI{`^iRoyRli9NPySy?7xy zZT>uwE011`5WXV=KgHoQ=ac4xmp<>z(C0re_+31I9OlS-w>moh{FRTt%f#O~Ij-~Z zeU*Q+iNB-Q{&S!6ev0h;e~tJ$HF@cxp7Gx|bawYL?fK^pzsA*z?q;st2Z=vn&mWIE zv=;#%;3VUYjQFJ{K0?&s$F%S`6Mt7P)9)Ye=xlOy{8W<+p3$!xXZrZr;C)@{;{>lT z_--ChqzQ{?zdK>XU$JPq) zJ4&wpY4LjJQr9#WKXROn=+v<}!(boYRxUf=;z!u%-iwY-d$GmqodjJUfmitppR|am zMSDZ@vzPM%@IOU9*SLP+x8w3Y;%^0S%5$Ejf2gDXjj*j(-~FhKxWUDL&Zl#~MfiH@bBC`RVe`e!FxP1`T(oA8uc&F@q_>RFte;>=Y z?_1#2^%4IA@G4Ku$u{Bu7k`hPZN3doZil;kCxOq(fAIUdIO(9JkN7wDfxi#D_+OJD z|FJ&e58B1@t!8IiaJ{pqtH|K(efs}*`oJ&j1K->S{&#)g^S)ur(|M=ORp%P%(tE4f z_%;^2_}_VqE&qi!x_7C=M_j)^Y|Qnb!>@MZ1;4-SwX3DmaFV6-T^IiZhp%z{2u*FS z3&4xd)lNQrJ$Wa1$zjbjOXpY@|1pPecX&VFryTvwFWQJYqej<1T>OGx*=KXU8@GJJ z*3`G-OdIhg7yl{n+4%FC!P9@u$k6lj{;RI-;OP~;u3hPv;%D`JHfV;6aTfS&^e;E~ ze6KrX3!?X2b**yvP7NfvjyC_C0S#XLvBUL?eH{L+KIrE&VXOEJuK(`u;vWq@TX`0M z_uJY>f7e@>ZvZ#%ssNxUt;k2-s*jAz4Wf2u3N#2&TcF1^BN1hH(dPc z_blQn7hf;tWGnwZ;Eg=rYvW^H&2<`h@gZ`7C2)YlHyJwUy+VuEyRW)#?t}iH`oPB> zo$8}40pCvL?Pbf;;N%}QVy<(+XDfdLc=e0hoc^hH@qYzgcGh!t*0&2EIQpHg-(BM3 zkNBp|H&d_HfLH(O{-!PG7-y$0F?g3a4O|Ug<>}1mr++i}0 z1U_3iH@oSf49$fSl~5+mpqp_dG_V!DL?+lHsX;k{;S|6hqE%;F{jA#VYO2jqh0(7;Ip;k z6oW_pe{Lh5>Ei#=(W#kg3x1C)f2WIIlTn_3^g+J@1)DA32m8SP5xn>i%NWhMnlIvq=9XH;QQlXi2i0rXPirOJos$wdzy>i z?(}?r7hm5I5T7?Yy%cqLoeQpZFV1M+F$dawYwouZN4WU4;IrA~Yr$tL&(FXs-?EJU z@)v_g3%YUoNsj&@2dO*-UZ?XDoVa*X!HW-dr&#{$9aLRE0dLyfDeOO4w0Dogcf4;A zYi%-bqrFb9x!Dp-QgW$9A=N0hcbA#hE zH8x#Wu@yb@J3D?1I*cv;6HNl3w~yzd2d?aMHuj^A9XIY zjPb`)lfa9g)wkG)FS_`@0&nO$xypC=d=d)(tn-(u9ex6MwflD0?tVZ0nZp-2enwpU z2f$~m@6!g4AAYdwcdi|~9KQNyOF-vv>*7^qQ=Set|8Rpvdr^n4n`jAW{#w@q;IrlX zW*_(u3?BP3v}bvS49iZZ2)y`{sn5@Jc<&)w4!>VFfR~)C&ghRnaq;UqYqFi3)L}MZH(m|KgI&boZN?vU_@( zuFlvQKiqj;^}4&5s(Q8cUiF_Ky8A^J_lpWj%m*2XAEF{mP~wn?ED5p;nkXs}{NgUC z1R<~@#8E-~oqNxD@7`Chx~pe;Bq`{=Rp;G*@80wOp7)NaxWxPZhkz4a-Q|g&a6kXV z^`G@k#m4uU3;)jWxj#0)zs&UVp@vz9x4&uN^u9X!e2n4m{)_qjL#C)~0Z#k)#)|2G zh2f_RfAIGPK<`4O&sP~fWW7t|$Q%?2qOU7Vzq}2VhK8HA_W#`}@CV$_+(%tJ{5s$o z-#%gB^nOPAym!Hjd$-_^`YD0`xT*hPUWW~azx~LxziKX527pull`k4NmREniAn-4l z-@MJ0|Iqz#C9Al#Xy)_E9~dCLN1Hw=;MD(p#@pu%xbiy;FRuTafd4Spv5R-Oeo^1} z05+nm{)>RqxI^CGP44XWl=?4gIOh4hS(tz0^?4m|l3%+l*Pi8mzBr|yZvamC{IFmz zJOW(rV?kf4Ll6`E%AcB!1wUT~yzIVSO1Hg$Mf9-u{1L-|m^!Zf{H{b35e>4UDSAgsNVtb0-GfJQ5peUBz?~8y_{n}Sd zy?KzyFr`t>0&kuP=SvjQjQ-Ga$W_ zgFbHpPV4aOzw&Q}zXN#L{{AobQ~66%f0gTRf1><;e*y40Q`PzZI^eVp_u210_*8qPng;vwL3)^GXxw}6-J%hFY|p6{}K@gv;emWF?z@>+pT zUj>}j?e4ct#V>LF-vXTE_^=?yzoqMQC9C-Oin-r63i{P=GkojIrt*N>d;@T;zYV`f zL*^L%-(3G~?x)G{;i{SE{emCwHvli&-#-Cd<1?>6*ogl8gYJiaSjGQv{a5$^=Qrh= zy0h6zm(=#2N=H%D+*+3mds8yRw$jEh?2el2t%gFf!rFMyeR&)yr3*r+Kzi2)bo9- z-d3DMtu*Wpqt5oa=UG3q>NTtS24*s9wZp;o`f~O?tJ$z>I@x{Eb=?7EO+2f~R@@z= ztG#$kQ>o>@vdZqnrLu1(p8e$N*EephT-{JA8Skkkm?bu1#+s-k`fqnfRGd`I#| zfX=}x)X1^w>EDN+@3jBjeJ3Cv!NTvN8)+ESqO_bEuNn@eKD`^U@(v@}cM_%QAP#%V z?-&A}av3>8iCLLC={{2SniRqyjTFhrDWND)>LA&VNBxPOkk?n4+ue_25G%H8qE0_P zic}g;z&n&Ew*;>1BN2MoJvdCuDvV-GtF+5uI8LI8fspZe+@A!4>$aobL~Xk7^cbr^ z8P}r_435~mwR(AlaBwj0mDI>rWe9OVtC%mb)ls2Z$wpBPJ(r0|(3Slt>gdlMJK zPDi$`9}Y_|_ek)l1Zz-wc}Rq(kdAjei9OW2b=@!RAZB?vagip@XjL{hx_rjm=(06- zY46tc$+=Mn$ng4m7UdI}2rb_-~tx?1nuW7ApQMg7vpNT1R4QnDdn>rG!N>X5R$Puyaj*W6sP*;yMIzS9yWP!1*Rf9my zO6P_*YLI*JMufcP;2+2nUud?|C4K~Wdz2cr}x z7w>_Qcc5)0Djc6w+A;D}BO7>!G^4T_Y9C9X!qF%^RnY)yYh|Bu%&X40-#zB?^ z?^IRl+V<6(0o3RKRHoDB(aYN zNO+%~sw2#erjgaSv(s#)d6w{s9=W2{KDBxEW^)z2?%af?WCrDhymjqbE7(yxS64Oy znu-=M`>K6-@5=R!=E|z_tMzKbX3kmyKmpxpx<*L3QDIj+FCV#9Y8Tpvqj(V4)oma4 z$tiYAZ&mwVeGnf9M`l*Ly9%?addo%;UO0kj5|5f8w8bz8yhWHGV#$QVVKnFjeft)Q z@{cBYI9T-6#S)*d)q*2x+LDF1a6WlUR58WpjGKDCEj^os;3f9`gJ=+qpf*IK5p-*q zVbMW6It|bSsnM4+ldC&`2m&`AbkryUf|EGjBdaVkCAC$i)4?O{`gTj%%ZuLb=xpi@ zBBrICCNTEq)!pDI@Y6mnW{b7og}SbHD#HQLJOt!v9|Hbfu)R)6-C-M~hXLh%mpt+{ zo<0mintHyDPG`QySWvv#$;B)3L3__dELnWg2ld}6HB`zJ{52oDT|OqU%4uDdXxM9> z#+%Z;b!~FSFjzE~p735z0y>~s)lo+6es!L1Wqp1xj+2zPD%;m2?PTcY*M-Jp9|xUo z+KmT67d?3MnVoF7G_b&9#hrQ~#eLCS$rN@p1OEbl?so^kX@}Txtw;_Tf6(I-3R#1a zU%+3ZL6?n~If#sZ!HjG!+ao`Q#k&kX!~6v{b>AkQs_$Vw5d#@_B-0|<1Jt{nBy3KllJxmu;L+=z?0c~rMlSQkk~ajlJN15hSs(^eDa6PzI9rF5(YWp6l10YC7jnj{xq?A|ua@&R&m#uLK9xmA z?qF;E7@m~v$3PyiMYwR zTIY*UEud$7I&xS8I7TlTI3PlEO=3&E_y8ENA7fS8pQfozkzdm|x|x*TFIt7@q#X^3 zq1bb?wvNmsv4RY#XsuyEq@xg2L2EFSSx@)tUTp=MVKiEeN71SWr>0t38v{3qhgEQ= zgZA*WaI4_NH9;;c!~rlub06HXP5x#&7j@8Sev%~Jg8}Vs!;!{@q4=|SA1nQ+5B0h* zb7(kA7gcarP?ZXUDROzS57%9I5Cz?>wxuPA1X)UQJncfqR>Mw{bb@-P*Yg?)yp}-V zUUzR4LZ2bE!K0+`Z$jVLD$cOBKclvwY5GCjgI&zbNJ@ZY7a7Y;>hMRK==WiSTLnBCclL)$dMbY z4T;ky-9bA}9MmjObZ+lr%APPvH{e0!7`Y2cAm&LaXRN~sGs`j76C7PF z0_Qec3uVVt**OJH$F34P(Yn2nKKnReJ6H0E(vah-tu)M=HORyl&fGGaM@(9yvVwfJ z*C+L#f?uREM2BQh^~Y&+qQ(QzVQ!wErLa%46dMEhDh!7c{pvj9+@eyt`&?ZzsEea1 zOXJdRkwJ8flR*-Zj`CIYVYRDc$S6ox$I;-p8A|*^7zujTrCUP4a<~gJdD7{@GX(O_ z+#d}?430J?;>@=VBBHn{D*t>MW65&ciEU!9!KRZ~@WCPoL-kKcvBSMySoY|9VFqSo zqcd$+*g)kmN@QVgE%xS!3iKRr)d#6`hh*!zY35Pp=7Q`NRVwEtv@ z?2(*!vwd*nz%;7Y%PUQCHz9$a23;AZD#n3!A{if^8Sgty1ZZ7Xs{tU!lWtLc7E}Im zF<#b4JR_jQvyL>i3Ong@rv)`8n76dm8^fmCh{-X2BkI9l3^FG-WvSvgc_Oc8)ak)@ zSKxpmD1`N08qLGUO>RtkIEx{{S5W+9PZ)OqL;_rqY&I74Z660GM(mGy2Rp` z!(fAB$4>=_U(Y;sJ9H}RueBt+Cc_zCYkk^2@7Bu91$33UglA&iM*h^DMKLGI)Ya7Z zmSf08C%)yFqC}2^fXruOW(C`Q=PubBSG4&d4x@BOj^4FQR^D<3;AOa(9Oz3Ij<*T+ za37uC)=}S5l&f@Raa|lEjR6j!hyh=}EsIxhxz4OPc=K#Ei2KmSHPWWckvpHLV@NDx zehvUH)J2Sgi)}P>x!fIexlB|Gu!~4%Tm98mOA9(zcn@&|#{ZCp2lnD|cmM>AJg?XujN}($+`tQj$kRGbcg~*YG6M z0=`O|Kq}IRGO<94XMqWdC~~y$64gs}PN6fwAZ>?9YSAL~_*53hTcpi~nMQ}o#&Jce zHqJK0cGkNugB!w~Fa}WP$=QMtJNb?BSdJYk^a(XP)h^V6Y8u!(M7)%Yw(su>r1!huxQz)I$E)}&`p%Mh9M}&a8U+z0CFrKCuD|Gqi7i8@Kj@| z5e;SMmhf)atb(>(^I03nVOTbZ%x8+P0i|=*Vqvwh!cnD}1h(TSEx%qSeYfrS zQ}mZH+w%;2Eouu?sgD&yrW1&w$d}tz@cJF}aKGA(5p^=usgqlB%682z_%ero zCi(R3+ukd3+9%y}99WawNw0%@Do*HcYv3eVX*ca4KEkLO_@bk;5qki@9O&avh|W-F zjC-ctrbD2MQJ>Yh6Fy10^NazO+l3fuy6>kT(Tsz}a?8u&&M1W!N0IhT8HX2K`cH%D zWb|em*6@Tw2xRC^oYGPV*k5ev1Vj!NBPb1ur`>*xqZ|{f!V<@3bj$9ci=tDRMi2HQ zorg_0AoQ8cGF?`EU}=K{T7cuv%)x1)u+hsVL_VK!*fOQk>BR`s$rh^axB_{da%U@@ z&9(c(tU9|ujVPZHDGX`D_&7&2nsp;NOxDA+P?%OK4yZ%eB8SsrkTBMaw8bJ>FkM7X zK`@K1BZqDN?9VgsAup3gsZunt`EptfLULWwB@Sul{K|S_quem}5OI@L%4C{NEF@Wt z-A|T24F7!N?0K^IULck{PPE*fA@s;w%1q!5s4o1xNU7|d^q2(O`GX}yoMPL}h=_rG zmzmOTac4UC%8v00Pmb7a?T~cbJ_SV6WfMJ#fJB^YWZK3A8ZOH-H?sKU2$*R{wlmw2 zWAHtI;)|Y#e6ys7OL8CS2q{}0=MEc)qNQ$}=x~Te6Hm>MBh2ACT3lW9ke*%a!(NC8 zoqit*;DoE;Odd~5!OFpm7&5OzRY7!<{X)LjHYFn3tU5$8g$fhbEs_$P$>%qP>p1(( zt|pu9lj)Za4LZJd4n8c}(vyKf3tfnt>(t`CP6#XE%T zT4#c{j;$$1aFMA`QWN`N*-Km7gzUVb!+WKR3kfgX2q!b-nJAZnGTn*r2J3K0JG&7a zI~f2}Kq5=>lDZE1UZ!}Xg3DxHwN%s)BGGbVzvz)+ z-)UKlO=*}ct-ggnx#!V5>_eOCMi%%8d^_r&W1Ka2D5P8B_YucV(fd2$9^&KGCkMw} zVDYwo2C9)@*?N1P#joi=y)@(wab2ZmA51(?741Gm z&XDD2omzGG5da3Qg91RBJ1Q{tA%%X`oMRqaSJkS#`{asEXogdrxiW^}u63hcN7PsS z;BXeaHgUqQx!L}K6($(mYpPM}Fpf~x2VcK}qO+O3(!-7;k-K&u@rVsh9 zA6msU(50R-o%*4DoYD*cp$E09dr4Bk^Q1n_hdQ*@k#j-Uy%7r zJt+^-o^t)kZ^`#3CBKHZLi49SWd8W7Kgak_aA`031*N=?SGCbc#+UYT|F8136pE3}uqTvGnz-|>K4$hZRi{X%=m zHzMWxq#~Ea^`rMF()?xrEBrnvDPNNfa9|d@|mB|KeK;@=~Vt2SM2eB?{lWTl+O@xTc6_i@8BAtIMFbDBwuLlq3P!%2Autu z@umD0uDl~edlCmqOdDgO%>Xe4QWm){R|m)k#*3*13*|GyuNX^Ewp}>Lq1)Q+rJwiXI#rSJv%E8Jn1&Cs3#JQc zC*=nV?Ipj?gRh$Q!e?Xx(oTNAfNBJjVI;rMTipJeQjyEK!tWpB@oNRRw8!gjosXH7 zxo5gSzcbpgYChueAAQ&WUqG>oKbgPW7rlQ@=5A`5|L;;_JbY?v=C}Np@#QzYd#$Yf T+(*r<__eoEUC%fBfiM3DeB{HR diff --git a/examples/NavigationExample/msgs/CommonMsgs.cs b/examples/NavigationExample/msgs/CommonMsgs.cs new file mode 100644 index 0000000..00c1311 --- /dev/null +++ b/examples/NavigationExample/msgs/CommonMsgs.cs @@ -0,0 +1,274 @@ +using System; +using System.Runtime.InteropServices; + +namespace NavigationExample +{ + // ============================================================================ + // Structures + // ============================================================================ + [StructLayout(LayoutKind.Sequential)] + public struct Point + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose2D + { + public double x; + public double y; + public double theta; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist2D + { + public double x; + public double y; + public double theta; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public double x; + public double y; + public double z; + public double w; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Position + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose + { + public Point position; + public Quaternion orientation; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist + { + public Vector3 linear; + public Vector3 angular; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Header + { + public uint seq; + public uint sec; + public uint nsec; + public IntPtr frame_id; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct PoseStamped + { + public Header header; + public Pose pose; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist2DStamped + { + public Header header; + public Twist2D velocity; + } + + [StructLayout(LayoutKind.Sequential)] + public struct LaserScan + { + public Header header; + public float angle_min; + public float angle_max; + public float angle_increment; + public float time_increment; + public float scan_time; + public float range_min; + public float range_max; + public IntPtr ranges; + public UIntPtr ranges_count; + public IntPtr intensities; + public UIntPtr intensities_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PointCloud + { + public Header header; + public IntPtr points; + public UIntPtr points_count; + public IntPtr channels; + public UIntPtr channels_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PointCloud2 + { + public Header header; + public uint height; + public uint width; + public IntPtr fields; + public UIntPtr fields_count; + [MarshalAs(UnmanagedType.I1)] + public bool is_bigendian; + public uint point_step; + public uint row_step; + public IntPtr data; + public UIntPtr data_count; + [MarshalAs(UnmanagedType.I1)] + public bool is_dense; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PoseWithCovariance + { + public Pose pose; + public IntPtr covariance; + public UIntPtr covariance_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct TwistWithCovariance { + public Twist twist; + public IntPtr covariance; + public UIntPtr covariance_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Odometry + { + public Header header; + public IntPtr child_frame_id; + public PoseWithCovariance pose; + public TwistWithCovariance twist; + } + + [StructLayout(LayoutKind.Sequential)] + public struct OccupancyGrid + { + public Header header; + public MapMetaData info; + public IntPtr data; + public UIntPtr data_count; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct MapMetaData + { + public Time map_load_time; + public float resolution; + public uint width; + public uint height; + public Pose origin; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Time + { + public uint sec; + public uint nsec; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Point32 + { + public float x; + public float y; + public float z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose2DStamped + { + public Header header; + public Pose2D pose; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Polygon + { + public IntPtr points; + public UIntPtr points_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct PolygonStamped + { + public Header header; + public Polygon polygon; + } + + [StructLayout(LayoutKind.Sequential)] + public struct OccupancyGridUpdate + { + public Header header; + public int x; + public int y; + public uint width; + public uint height; + public IntPtr data; + public UIntPtr data_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Path2D + { + public Header header; + public IntPtr poses; + public UIntPtr poses_count; + } + + /// Name (char*) + OccupancyGrid for static map list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedOccupancyGrid + { + public IntPtr name; + public OccupancyGrid grid; + } + + /// Name (char*) + LaserScan for laser scan list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedLaserScan + { + public IntPtr name; + public LaserScan scan; + } + + /// Name (char*) + PointCloud for point cloud list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedPointCloud + { + public IntPtr name; + public PointCloud cloud; + } + + /// Name (char*) + PointCloud2 for point cloud2 list API. + [StructLayout(LayoutKind.Sequential)] + public struct NamedPointCloud2 + { + public IntPtr name; + public PointCloud2 cloud; + } + + +} \ No newline at end of file diff --git a/examples/NavigationExample/msgs/ProtocolMsgsTypes.cs b/examples/NavigationExample/msgs/ProtocolMsgsTypes.cs new file mode 100644 index 0000000..4ff3bec --- /dev/null +++ b/examples/NavigationExample/msgs/ProtocolMsgsTypes.cs @@ -0,0 +1,109 @@ +using System; +using System.Runtime.InteropServices; + +namespace NavigationExample +{ + /// + /// C# struct layout cho protocol_msgs (robot_protocol_msgs C API). + /// Khớp với pnkx_nav_core/src/APIs/c_api/include/protocol_msgs/*.h + /// + [StructLayout(LayoutKind.Sequential)] + public struct ControlPoint + { + public double x; + public double y; + public double weight; + } + + [StructLayout(LayoutKind.Sequential)] + public struct ActionParameter + { + public IntPtr key; // char* + public IntPtr value; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct NodePosition + { + public double x; + public double y; + public double theta; + public float allowedDeviationXY; + public float allowedDeviationTheta; + public IntPtr mapId; // char* + public IntPtr mapDescription; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct Trajectory + { + public uint degree; + public IntPtr knotVector; // double* + public UIntPtr knotVector_count; + public IntPtr controlPoints; // ControlPoint* + public UIntPtr controlPoints_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Action + { + public IntPtr actionType; // char* + public IntPtr actionId; // char* + public IntPtr actionDescription;// char* + public IntPtr blockingType; // char* + public IntPtr actionParameters; // ActionParameter* + public UIntPtr actionParameters_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Node + { + public IntPtr nodeId; // char* + public int sequenceId; + public IntPtr nodeDescription; // char* + public byte released; + public NodePosition nodePosition; + public IntPtr actions; // Action* + public UIntPtr actions_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Edge + { + public IntPtr edgeId; // char* + public int sequenceId; + public IntPtr edgeDescription; // char* + public byte released; + public IntPtr startNodeId; // char* + public IntPtr endNodeId; // char* + public double maxSpeed; + public double maxHeight; + public double minHeight; + public double orientation; + public IntPtr orientationType; // char* + public IntPtr direction; // char* + public byte rotationAllowed; + public double maxRotationSpeed; + public Trajectory trajectory; + public double length; + public IntPtr actions; // Action* + public UIntPtr actions_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Order + { + public int headerId; + public IntPtr timestamp; // char* + public IntPtr version; // char* + public IntPtr manufacturer; // char* + public IntPtr serialNumber; // char* + public IntPtr orderId; // char* + public uint orderUpdateId; + public IntPtr nodes; // Node* + public UIntPtr nodes_count; + public IntPtr edges; // Edge* + public UIntPtr edges_count; + public IntPtr zoneSetId; // char* + } +} diff --git a/examples/NavigationExample/msgs/SensorMsgs.cs b/examples/NavigationExample/msgs/SensorMsgs.cs new file mode 100644 index 0000000..e69de29 diff --git a/pnkx_nav_core.sln b/pnkx_nav_core.sln new file mode 100644 index 0000000..76b51c8 --- /dev/null +++ b/pnkx_nav_core.sln @@ -0,0 +1,29 @@ +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.5.2.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "examples", "examples", "{B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "NavigationExample", "examples\NavigationExample\NavigationExample.csproj", "{995839D6-1E72-F444-6587-97EF24F93814}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.Build.0 = Debug|Any CPU + {995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.ActiveCfg = Release|Any CPU + {995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(NestedProjects) = preSolution + {995839D6-1E72-F444-6587-97EF24F93814} = {B36A84DF-456D-A817-6EDD-3EC3E7F6E11F} + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {1A56EED1-5E6A-48A6-9AEE-F39361B34305} + EndGlobalSection +EndGlobal diff --git a/robotapp.db b/robotapp.db new file mode 100644 index 0000000..e69de29 diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index efeb0da..a83d80c 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/src/APIs/c_api/CMakeLists.txt @@ -21,6 +21,9 @@ set(PACKAGES_DIR robot_time robot_cpp geometry_msgs + angles + data_convert + robot_protocol_msgs ) # Thư mục include diff --git a/src/APIs/c_api/include/convertor.h b/src/APIs/c_api/include/convertor.h index 1be3c7c..6322a2c 100644 --- a/src/APIs/c_api/include/convertor.h +++ b/src/APIs/c_api/include/convertor.h @@ -13,6 +13,10 @@ #include "geometry_msgs/Pose2D.h" #include "nav_2d_msgs/Twist2D.h" #include "nav_2d_msgs/Twist2DStamped.h" +#include "nav_2d_msgs/Path2D.h" +#include "map_msgs/OccupancyGridUpdate.h" +#include "geometry_msgs/PolygonStamped.h" +#include "protocol_msgs/Order.h" // C++ #include @@ -24,6 +28,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -49,6 +57,21 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu */ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out); +/** + * @brief Convert C++ Path2D to C Path2D + */ +void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out); + +/** + * @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate + */ +void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out); + +/** + * @brief Convert C++ PolygonStamped to C PolygonStamped + */ +void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out); + /** * @brief Convert C++ LaserScan to C LaserScan * @param cpp C++ LaserScan @@ -150,4 +173,27 @@ robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped */ Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped); +/** + * @brief Convert C Order to C++ Order + * @param order C Order + * @return C++ Order + */ +robot_protocol_msgs::Order convert2CppOrder(const Order& order); + +/** + * @brief Convert C++ Order to C Order + * @param cpp_order C++ Order + * @return C Order (caller must call order_free when done to release memory) + */ +Order convert2COrder(const robot_protocol_msgs::Order& cpp_order); + +/** + * @brief Free dynamic memory held by an Order returned from convert2COrder + * @param order Order to free (pointers and counts are zeroed) + */ +#ifdef __cplusplus +extern "C" +#endif +void order_free(Order* order); + #endif // C_API_CONVERTOR_H \ No newline at end of file diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 72f8548..1df99b7 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -24,22 +24,13 @@ extern "C" #include "sensor_msgs/PointCloud.h" #include "sensor_msgs/PointCloud2.h" #include "geometry_msgs/PolygonStamped.h" + #include "protocol_msgs/Order.h" typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid; typedef struct { char *name; LaserScan scan; } NamedLaserScan; typedef struct { char *name; PointCloud cloud; } NamedPointCloud; typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2; - - typedef struct - { - void *ptr; - } NavigationHandle; - - // typedef struct - // { - // void *ptr; - // } TFListenerHandle; - + typedef void* NavigationHandle; typedef void* TFListenerHandle; typedef enum @@ -100,53 +91,18 @@ extern "C" */ void navigation_destroy(NavigationHandle handle); - /** - * @brief Create a TF listener instance - * @return TF listener handle, or NULL on failure - */ - TFListenerHandle tf_listener_create(void); - - /** - * @brief Destroy a TF listener instance - * @param handle TF listener handle to destroy - */ - void tf_listener_destroy(TFListenerHandle handle); - - /** - * @brief Inject a static transform into the TF buffer. - * - * This is a convenience for standalone usage where no external TF publisher exists yet. - * It will create/ensure the frames exist and become transformable. - * - * @param tf_handle TF listener handle - * @param parent_frame Parent frame id (e.g. "map") - * @param child_frame Child frame id (e.g. "base_link") - * @param x Translation x (meters) - * @param y Translation y (meters) - * @param z Translation z (meters) - * @param qx Rotation quaternion x - * @param qy Rotation quaternion y - * @param qz Rotation quaternion z - * @param qw Rotation quaternion w - * @return true on success, false on failure - */ - bool tf_listener_set_static_transform(TFListenerHandle tf_handle, - const char *parent_frame, - const char *child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw); - // ============================================================================ // Navigation Interface Methods // ============================================================================ /** - * @brief Initialize the navigation system + * @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create). + * Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy). * @param handle Navigation handle - * @param tf_handle TF listener handle + * @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3) * @return true on success, false on failure */ - bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); + bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer); /** * @brief Set the robot's footprint (outline shape) @@ -171,51 +127,37 @@ extern "C" * @brief Send a goal for the robot to navigate to * @param handle Navigation handle * @param goal Target pose in the global frame - * @param xy_goal_tolerance Acceptable error in X/Y (meters) - * @param yaw_goal_tolerance Acceptable angular error (radians) * @return true if goal was accepted and sent successfully */ - bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + bool navigation_move_to(NavigationHandle handle, const PoseStamped goal); - // /** - // * @brief Send a goal for the robot to navigate to - // * @param handle Navigation handle - // * @param order Order message - // * @param goal Target pose in the global frame - // * @param xy_goal_tolerance Acceptable error in X/Y (meters) - // * @param yaw_goal_tolerance Acceptable angular error (radians) - // * @return true if goal was accepted and sent successfully - // */ - // bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, - // const PoseStamped &goal, - // double xy_goal_tolerance, double yaw_goal_tolerance); + /** + * @brief Send a goal for the robot to navigate to + * @param handle Navigation handle + * @param order Order message + * @param goal Target pose in the global frame + * @return true if goal was accepted and sent successfully + * @note If order was obtained from convert2COrder(), call order_free(&order) when done + */ + bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal); /** * @brief Send a docking goal to a predefined marker * @param handle Navigation handle * @param marker Marker name or ID (null-terminated string) * @param goal Target pose for docking - * @param xy_goal_tolerance Acceptable XY error (meters) - * @param yaw_goal_tolerance Acceptable heading error (radians) * @return true if docking command succeeded */ - bool navigation_dock_to(NavigationHandle handle, const char *marker, - const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal); - // /** - // * @brief Send a docking goal to a predefined marker - // * @param handle Navigation handle - // * @param order Order message - // * @param goal Target pose for docking - // * @param xy_goal_tolerance Acceptable XY error (meters) - // * @param yaw_goal_tolerance Acceptable heading error (radians) - // * @return true if docking command succeeded - // */ - // bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, - // const PoseStamped &goal, - // double xy_goal_tolerance, double yaw_goal_tolerance); + /** + * @brief Send a docking goal to a predefined marker + * @param handle Navigation handle + * @param order Order message + * @param goal Target pose for docking + * @return true if docking command succeeded + */ + bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal); /** * @brief Move straight toward the target position @@ -228,12 +170,10 @@ extern "C" /** * @brief Rotate in place to align with target orientation * @param handle Navigation handle - * @param goal Pose containing desired heading (only Z-axis used) - * @param yaw_goal_tolerance Acceptable angular error (radians) + * @param goal_yaw Desired heading (radians) * @return true if rotation command was sent successfully */ - bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, - double yaw_goal_tolerance); + bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal); /** * @brief Pause the robot's movement diff --git a/src/APIs/c_api/include/protocol_msgs/Action.h b/src/APIs/c_api/include/protocol_msgs/Action.h new file mode 100644 index 0000000..08d2650 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Action.h @@ -0,0 +1,30 @@ +#ifndef C_API_PROTOCOL_MSGS_ACTION_H +#define C_API_PROTOCOL_MSGS_ACTION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/ActionParameter.h" + +#define PROTOCOL_MSGS_ACTION_BLOCKING_NONE "NONE" +#define PROTOCOL_MSGS_ACTION_BLOCKING_SOFT "SOFT" +#define PROTOCOL_MSGS_ACTION_BLOCKING_HARD "HARD" + +typedef struct +{ + char *actionType; + char *actionId; + char *actionDescription; + char *blockingType; + ActionParameter *actionParameters; + size_t actionParameters_count; +} Action; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ACTION_H diff --git a/src/APIs/c_api/include/protocol_msgs/ActionParameter.h b/src/APIs/c_api/include/protocol_msgs/ActionParameter.h new file mode 100644 index 0000000..423ed79 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/ActionParameter.h @@ -0,0 +1,21 @@ +#ifndef C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H +#define C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *key; + char *value; +} ActionParameter; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H diff --git a/src/APIs/c_api/include/protocol_msgs/ControlPoint.h b/src/APIs/c_api/include/protocol_msgs/ControlPoint.h new file mode 100644 index 0000000..17ec8dd --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/ControlPoint.h @@ -0,0 +1,22 @@ +#ifndef C_API_PROTOCOL_MSGS_CONTROLPOINT_H +#define C_API_PROTOCOL_MSGS_CONTROLPOINT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double weight; +} ControlPoint; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_CONTROLPOINT_H diff --git a/src/APIs/c_api/include/protocol_msgs/Edge.h b/src/APIs/c_api/include/protocol_msgs/Edge.h new file mode 100644 index 0000000..aeb791c --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Edge.h @@ -0,0 +1,42 @@ +#ifndef C_API_PROTOCOL_MSGS_EDGE_H +#define C_API_PROTOCOL_MSGS_EDGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/Trajectory.h" +#include "protocol_msgs/Action.h" + +#define PROTOCOL_MSGS_EDGE_ORIENTATION_GLOBAL "GLOBAL" +#define PROTOCOL_MSGS_EDGE_ORIENTATION_TANGENTIAL "TANGENTIAL" + +typedef struct +{ + char *edgeId; + int32_t sequenceId; + char *edgeDescription; + uint8_t released; + char *startNodeId; + char *endNodeId; + double maxSpeed; + double maxHeight; + double minHeight; + double orientation; + char *orientationType; + char *direction; + uint8_t rotationAllowed; + double maxRotationSpeed; + Trajectory trajectory; + double length; + Action *actions; + size_t actions_count; +} Edge; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_EDGE_H diff --git a/src/APIs/c_api/include/protocol_msgs/Error.h b/src/APIs/c_api/include/protocol_msgs/Error.h new file mode 100644 index 0000000..2a4b8f5 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Error.h @@ -0,0 +1,28 @@ +#ifndef C_API_PROTOCOL_MSGS_ERROR_H +#define C_API_PROTOCOL_MSGS_ERROR_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/ErrorReference.h" + +#define PROTOCOL_MSGS_ERROR_LEVEL_WARNING "WARNING" +#define PROTOCOL_MSGS_ERROR_LEVEL_FATAL "FATAL" + +typedef struct +{ + char *errorType; + ErrorReference *errorReferences; + size_t errorReferences_count; + char *errorDescription; + char *errorLevel; +} Error; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ERROR_H diff --git a/src/APIs/c_api/include/protocol_msgs/ErrorReference.h b/src/APIs/c_api/include/protocol_msgs/ErrorReference.h new file mode 100644 index 0000000..5bb3d78 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/ErrorReference.h @@ -0,0 +1,21 @@ +#ifndef C_API_PROTOCOL_MSGS_ERRORREFERENCE_H +#define C_API_PROTOCOL_MSGS_ERRORREFERENCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *referenceKey; + char *referenceValue; +} ErrorReference; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ERRORREFERENCE_H diff --git a/src/APIs/c_api/include/protocol_msgs/Info.h b/src/APIs/c_api/include/protocol_msgs/Info.h new file mode 100644 index 0000000..2657bd3 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Info.h @@ -0,0 +1,28 @@ +#ifndef C_API_PROTOCOL_MSGS_INFO_H +#define C_API_PROTOCOL_MSGS_INFO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/InfoReference.h" + +#define PROTOCOL_MSGS_INFO_LEVEL_DEBUG "DEBUG" +#define PROTOCOL_MSGS_INFO_LEVEL_INFO "INFO" + +typedef struct +{ + char *infoType; + InfoReference *infoReferences; + size_t infoReferences_count; + char *infoDescription; + char *infoLevel; +} Info; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_INFO_H diff --git a/src/APIs/c_api/include/protocol_msgs/InfoReference.h b/src/APIs/c_api/include/protocol_msgs/InfoReference.h new file mode 100644 index 0000000..d6cd08b --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/InfoReference.h @@ -0,0 +1,21 @@ +#ifndef C_API_PROTOCOL_MSGS_INFOREFERENCE_H +#define C_API_PROTOCOL_MSGS_INFOREFERENCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *referenceKey; + char *referenceValue; +} InfoReference; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_INFOREFERENCE_H diff --git a/src/APIs/c_api/include/protocol_msgs/Information.h b/src/APIs/c_api/include/protocol_msgs/Information.h new file mode 100644 index 0000000..d0e9f4f --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Information.h @@ -0,0 +1,22 @@ +#ifndef C_API_PROTOCOL_MSGS_INFORMATION_H +#define C_API_PROTOCOL_MSGS_INFORMATION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/Info.h" + +typedef struct +{ + Info *information; + size_t information_count; +} Information; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_INFORMATION_H diff --git a/src/APIs/c_api/include/protocol_msgs/Node.h b/src/APIs/c_api/include/protocol_msgs/Node.h new file mode 100644 index 0000000..cede8ba --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Node.h @@ -0,0 +1,28 @@ +#ifndef C_API_PROTOCOL_MSGS_NODE_H +#define C_API_PROTOCOL_MSGS_NODE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/NodePosition.h" +#include "protocol_msgs/Action.h" + +typedef struct +{ + char *nodeId; + int32_t sequenceId; + char *nodeDescription; + uint8_t released; + NodePosition nodePosition; + Action *actions; + size_t actions_count; +} Node; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_NODE_H diff --git a/src/APIs/c_api/include/protocol_msgs/NodePosition.h b/src/APIs/c_api/include/protocol_msgs/NodePosition.h new file mode 100644 index 0000000..986c8b2 --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/NodePosition.h @@ -0,0 +1,26 @@ +#ifndef C_API_PROTOCOL_MSGS_NODEPOSITION_H +#define C_API_PROTOCOL_MSGS_NODEPOSITION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double theta; + float allowedDeviationXY; + float allowedDeviationTheta; + char *mapId; + char *mapDescription; +} NodePosition; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_NODEPOSITION_H diff --git a/src/APIs/c_api/include/protocol_msgs/Order.h b/src/APIs/c_api/include/protocol_msgs/Order.h new file mode 100644 index 0000000..af45fbb --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Order.h @@ -0,0 +1,33 @@ +#ifndef C_API_PROTOCOL_MSGS_ORDER_H +#define C_API_PROTOCOL_MSGS_ORDER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/Node.h" +#include "protocol_msgs/Edge.h" + +typedef struct +{ + int32_t headerId; + char *timestamp; + char *version; + char *manufacturer; + char *serialNumber; + char *orderId; + uint32_t orderUpdateId; + Node *nodes; + size_t nodes_count; + Edge *edges; + size_t edges_count; + char *zoneSetId; +} Order; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_ORDER_H diff --git a/src/APIs/c_api/include/protocol_msgs/Trajectory.h b/src/APIs/c_api/include/protocol_msgs/Trajectory.h new file mode 100644 index 0000000..bc1ec5b --- /dev/null +++ b/src/APIs/c_api/include/protocol_msgs/Trajectory.h @@ -0,0 +1,25 @@ +#ifndef C_API_PROTOCOL_MSGS_TRAJECTORY_H +#define C_API_PROTOCOL_MSGS_TRAJECTORY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "protocol_msgs/ControlPoint.h" + +typedef struct +{ + uint32_t degree; + double *knotVector; + size_t knotVector_count; + ControlPoint *controlPoints; + size_t controlPoints_count; +} Trajectory; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_PROTOCOL_MSGS_TRAJECTORY_H diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp index 216f950..4e3e76d 100644 --- a/src/APIs/c_api/src/convertor.cpp +++ b/src/APIs/c_api/src/convertor.cpp @@ -201,6 +201,70 @@ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyG } } +static void convert2CPose2DStamped(const robot_nav_2d_msgs::Pose2DStamped& cpp, Pose2DStamped& out) +{ + out.header = convert2CHeader(cpp.header); + out.pose = convert2CPose2D(cpp.pose); +} + +void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out) +{ + out.header = convert2CHeader(cpp.header); + out.poses_count = cpp.poses.size(); + out.poses = nullptr; + if (out.poses_count > 0) + { + out.poses = static_cast(malloc(out.poses_count * sizeof(Pose2DStamped))); + if (out.poses) + { + for (size_t i = 0; i < out.poses_count; i++) + convert2CPose2DStamped(cpp.poses[i], out.poses[i]); + } + } +} + +void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out) +{ + out.header = convert2CHeader(cpp.header); + out.x = cpp.x; + out.y = cpp.y; + out.width = cpp.width; + out.height = cpp.height; + out.data_count = cpp.data.size(); + out.data = nullptr; + if (out.data_count > 0) + { + out.data = static_cast(malloc(out.data_count * sizeof(int8_t))); + if (out.data) + memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t)); + } +} + +static void convert2CPolygon(const robot_geometry_msgs::Polygon& cpp, Polygon& out) +{ + out.points_count = cpp.points.size(); + out.points = nullptr; + if (out.points_count > 0) + { + out.points = static_cast(malloc(out.points_count * sizeof(Point32))); + if (out.points) + { + for (size_t i = 0; i < out.points_count; i++) + { + out.points[i].x = cpp.points[i].x; + out.points[i].y = cpp.points[i].y; + out.points[i].z = cpp.points[i].z; + } + } + } +} + +void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out) +{ + out.header = convert2CHeader(cpp.header); + convert2CPolygon(cpp.polygon, out.polygon); +} + void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out) { out.header.seq = cpp.header.seq; @@ -361,3 +425,374 @@ robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c) memcpy(cpp.data.data(), c.data, c.data_count); return cpp; } + +// --- protocol_msgs helpers (C <-> C++ robot_protocol_msgs) --- + +static inline char* dup_cstr(const char* s) +{ + if (!s) return nullptr; + const size_t n = std::strlen(s) + 1; + char* p = static_cast(std::malloc(n)); + if (p) std::memcpy(p, s, n); + return p; +} + +static robot_protocol_msgs::ActionParameter convert2CppActionParameter(const ActionParameter& c) +{ + robot_protocol_msgs::ActionParameter cpp; + cpp.key = c.key ? c.key : ""; + cpp.value = c.value ? c.value : ""; + return cpp; +} + +static ActionParameter convert2CActionParameter(const robot_protocol_msgs::ActionParameter& cpp) +{ + ActionParameter c; + c.key = dup_cstr(cpp.key.c_str()); + c.value = dup_cstr(cpp.value.c_str()); + return c; +} + +static robot_protocol_msgs::ControlPoint convert2CppControlPoint(const ControlPoint& c) +{ + robot_protocol_msgs::ControlPoint cpp; + cpp.x = c.x; + cpp.y = c.y; + cpp.weight = c.weight; + return cpp; +} + +static ControlPoint convert2CControlPoint(const robot_protocol_msgs::ControlPoint& cpp) +{ + ControlPoint c; + c.x = cpp.x; + c.y = cpp.y; + c.weight = cpp.weight; + return c; +} + +static robot_protocol_msgs::NodePosition convert2CppNodePosition(const NodePosition& c) +{ + robot_protocol_msgs::NodePosition cpp; + cpp.x = c.x; + cpp.y = c.y; + cpp.theta = c.theta; + cpp.allowedDeviationXY = c.allowedDeviationXY; + cpp.allowedDeviationTheta = c.allowedDeviationTheta; + cpp.mapId = c.mapId ? c.mapId : ""; + cpp.mapDescription = c.mapDescription ? c.mapDescription : ""; + return cpp; +} + +static NodePosition convert2CNodePosition(const robot_protocol_msgs::NodePosition& cpp) +{ + NodePosition c; + c.x = cpp.x; + c.y = cpp.y; + c.theta = cpp.theta; + c.allowedDeviationXY = cpp.allowedDeviationXY; + c.allowedDeviationTheta = cpp.allowedDeviationTheta; + c.mapId = dup_cstr(cpp.mapId.c_str()); + c.mapDescription = dup_cstr(cpp.mapDescription.c_str()); + return c; +} + +static robot_protocol_msgs::Trajectory convert2CppTrajectory(const Trajectory& c) +{ + robot_protocol_msgs::Trajectory cpp; + cpp.degree = c.degree; + const size_t kv = c.knotVector ? c.knotVector_count : 0; + cpp.knotVector.resize(kv); + for (size_t i = 0; i < kv; i++) + cpp.knotVector[i] = c.knotVector[i]; + const size_t ncp = c.controlPoints ? c.controlPoints_count : 0; + cpp.controlPoints.resize(ncp); + for (size_t i = 0; i < ncp; i++) + cpp.controlPoints[i] = convert2CppControlPoint(c.controlPoints[i]); + return cpp; +} + +static Trajectory convert2CTrajectory(const robot_protocol_msgs::Trajectory& cpp) +{ + Trajectory c; + c.degree = cpp.degree; + c.knotVector_count = cpp.knotVector.size(); + c.knotVector = c.knotVector_count ? static_cast(std::malloc(c.knotVector_count * sizeof(double))) : nullptr; + if (c.knotVector) + for (size_t i = 0; i < c.knotVector_count; i++) + c.knotVector[i] = cpp.knotVector[i]; + c.controlPoints_count = cpp.controlPoints.size(); + c.controlPoints = c.controlPoints_count ? static_cast(std::malloc(c.controlPoints_count * sizeof(ControlPoint))) : nullptr; + if (c.controlPoints) + for (size_t i = 0; i < c.controlPoints_count; i++) + c.controlPoints[i] = convert2CControlPoint(cpp.controlPoints[i]); + return c; +} + +static robot_protocol_msgs::Action convert2CppAction(const Action& c) +{ + robot_protocol_msgs::Action cpp; + cpp.actionType = c.actionType ? c.actionType : ""; + cpp.actionId = c.actionId ? c.actionId : ""; + cpp.actionDescription = c.actionDescription ? c.actionDescription : ""; + cpp.blockingType = c.blockingType ? c.blockingType : ""; + const size_t n = c.actionParameters ? c.actionParameters_count : 0; + cpp.actionParameters.resize(n); + for (size_t i = 0; i < n; i++) + cpp.actionParameters[i] = convert2CppActionParameter(c.actionParameters[i]); + return cpp; +} + +static Action convert2CAction(const robot_protocol_msgs::Action& cpp) +{ + Action c; + c.actionType = dup_cstr(cpp.actionType.c_str()); + c.actionId = dup_cstr(cpp.actionId.c_str()); + c.actionDescription = dup_cstr(cpp.actionDescription.c_str()); + c.blockingType = dup_cstr(cpp.blockingType.c_str()); + c.actionParameters_count = cpp.actionParameters.size(); + c.actionParameters = c.actionParameters_count ? static_cast(std::malloc(c.actionParameters_count * sizeof(ActionParameter))) : nullptr; + if (c.actionParameters) + for (size_t i = 0; i < c.actionParameters_count; i++) + c.actionParameters[i] = convert2CActionParameter(cpp.actionParameters[i]); + return c; +} + +static robot_protocol_msgs::Node convert2CppNode(const Node& c) +{ + robot_protocol_msgs::Node cpp; + cpp.nodeId = c.nodeId ? c.nodeId : ""; + cpp.sequenceId = c.sequenceId; + cpp.nodeDescription = c.nodeDescription ? c.nodeDescription : ""; + cpp.released = c.released; + cpp.nodePosition = convert2CppNodePosition(c.nodePosition); + const size_t n = c.actions ? c.actions_count : 0; + cpp.actions.resize(n); + for (size_t i = 0; i < n; i++) + cpp.actions[i] = convert2CppAction(c.actions[i]); + return cpp; +} + +static Node convert2CNode(const robot_protocol_msgs::Node& cpp) +{ + Node c; + c.nodeId = dup_cstr(cpp.nodeId.c_str()); + c.sequenceId = cpp.sequenceId; + c.nodeDescription = dup_cstr(cpp.nodeDescription.c_str()); + c.released = cpp.released; + c.nodePosition = convert2CNodePosition(cpp.nodePosition); + c.actions_count = cpp.actions.size(); + c.actions = c.actions_count ? static_cast(std::malloc(c.actions_count * sizeof(Action))) : nullptr; + if (c.actions) + for (size_t i = 0; i < c.actions_count; i++) + c.actions[i] = convert2CAction(cpp.actions[i]); + return c; +} + +static robot_protocol_msgs::Edge convert2CppEdge(const Edge& c) +{ + robot_protocol_msgs::Edge cpp; + cpp.edgeId = c.edgeId ? c.edgeId : ""; + cpp.sequenceId = c.sequenceId; + cpp.edgeDescription = c.edgeDescription ? c.edgeDescription : ""; + cpp.released = c.released; + cpp.startNodeId = c.startNodeId ? c.startNodeId : ""; + cpp.endNodeId = c.endNodeId ? c.endNodeId : ""; + cpp.maxSpeed = c.maxSpeed; + cpp.maxHeight = c.maxHeight; + cpp.minHeight = c.minHeight; + cpp.orientation = c.orientation; + cpp.orientationType = c.orientationType ? c.orientationType : ""; + cpp.direction = c.direction ? c.direction : ""; + cpp.rotationAllowed = c.rotationAllowed; + cpp.maxRotationSpeed = c.maxRotationSpeed; + cpp.trajectory = convert2CppTrajectory(c.trajectory); + cpp.length = c.length; + const size_t n = c.actions ? c.actions_count : 0; + cpp.actions.resize(n); + for (size_t i = 0; i < n; i++) + cpp.actions[i] = convert2CppAction(c.actions[i]); + return cpp; +} + +static Edge convert2CEdge(const robot_protocol_msgs::Edge& cpp) +{ + Edge c; + c.edgeId = dup_cstr(cpp.edgeId.c_str()); + c.sequenceId = cpp.sequenceId; + c.edgeDescription = dup_cstr(cpp.edgeDescription.c_str()); + c.released = cpp.released; + c.startNodeId = dup_cstr(cpp.startNodeId.c_str()); + c.endNodeId = dup_cstr(cpp.endNodeId.c_str()); + c.maxSpeed = cpp.maxSpeed; + c.maxHeight = cpp.maxHeight; + c.minHeight = cpp.minHeight; + c.orientation = cpp.orientation; + c.orientationType = dup_cstr(cpp.orientationType.c_str()); + c.direction = dup_cstr(cpp.direction.c_str()); + c.rotationAllowed = cpp.rotationAllowed; + c.maxRotationSpeed = cpp.maxRotationSpeed; + c.trajectory = convert2CTrajectory(cpp.trajectory); + c.length = cpp.length; + c.actions_count = cpp.actions.size(); + c.actions = c.actions_count ? static_cast(std::malloc(c.actions_count * sizeof(Action))) : nullptr; + if (c.actions) + for (size_t i = 0; i < c.actions_count; i++) + c.actions[i] = convert2CAction(cpp.actions[i]); + return c; +} + +static void free_action_parameter(ActionParameter* p) +{ + if (!p) return; + std::free(p->key); + std::free(p->value); + p->key = nullptr; + p->value = nullptr; +} + +static void free_action(Action* p) +{ + if (!p) return; + std::free(p->actionType); + std::free(p->actionId); + std::free(p->actionDescription); + std::free(p->blockingType); + p->actionType = p->actionId = p->actionDescription = p->blockingType = nullptr; + if (p->actionParameters) { + for (size_t i = 0; i < p->actionParameters_count; i++) + free_action_parameter(&p->actionParameters[i]); + std::free(p->actionParameters); + p->actionParameters = nullptr; + } + p->actionParameters_count = 0; +} + +static void free_node_position(NodePosition* p) +{ + if (!p) return; + std::free(p->mapId); + std::free(p->mapDescription); + p->mapId = p->mapDescription = nullptr; +} + +static void free_trajectory(Trajectory* p) +{ + if (!p) return; + std::free(p->knotVector); + p->knotVector = nullptr; + p->knotVector_count = 0; + std::free(p->controlPoints); + p->controlPoints = nullptr; + p->controlPoints_count = 0; +} + +static void free_node(Node* p) +{ + if (!p) return; + std::free(p->nodeId); + std::free(p->nodeDescription); + p->nodeId = p->nodeDescription = nullptr; + free_node_position(&p->nodePosition); + if (p->actions) { + for (size_t i = 0; i < p->actions_count; i++) + free_action(&p->actions[i]); + std::free(p->actions); + p->actions = nullptr; + } + p->actions_count = 0; +} + +static void free_edge(Edge* p) +{ + if (!p) return; + std::free(p->edgeId); + std::free(p->edgeDescription); + std::free(p->startNodeId); + std::free(p->endNodeId); + std::free(p->orientationType); + std::free(p->direction); + p->edgeId = p->edgeDescription = p->startNodeId = p->endNodeId = nullptr; + p->orientationType = p->direction = nullptr; + free_trajectory(&p->trajectory); + if (p->actions) { + for (size_t i = 0; i < p->actions_count; i++) + free_action(&p->actions[i]); + std::free(p->actions); + p->actions = nullptr; + } + p->actions_count = 0; +} + +robot_protocol_msgs::Order convert2CppOrder(const Order& order) +{ + robot_protocol_msgs::Order cpp_order; + cpp_order.headerId = order.headerId; + cpp_order.timestamp = order.timestamp ? order.timestamp : ""; + cpp_order.version = order.version ? order.version : ""; + cpp_order.manufacturer = order.manufacturer ? order.manufacturer : ""; + cpp_order.serialNumber = order.serialNumber ? order.serialNumber : ""; + cpp_order.orderId = order.orderId ? order.orderId : ""; + cpp_order.orderUpdateId = order.orderUpdateId; + const size_t nn = order.nodes ? order.nodes_count : 0; + cpp_order.nodes.resize(nn); + for (size_t i = 0; i < nn; i++) + cpp_order.nodes[i] = convert2CppNode(order.nodes[i]); + const size_t ne = order.edges ? order.edges_count : 0; + cpp_order.edges.resize(ne); + for (size_t i = 0; i < ne; i++) + cpp_order.edges[i] = convert2CppEdge(order.edges[i]); + cpp_order.zoneSetId = order.zoneSetId ? order.zoneSetId : ""; + return cpp_order; +} + +Order convert2COrder(const robot_protocol_msgs::Order& cpp_order) +{ + Order order; + order.headerId = cpp_order.headerId; + order.timestamp = dup_cstr(cpp_order.timestamp.c_str()); + order.version = dup_cstr(cpp_order.version.c_str()); + order.manufacturer = dup_cstr(cpp_order.manufacturer.c_str()); + order.serialNumber = dup_cstr(cpp_order.serialNumber.c_str()); + order.orderId = dup_cstr(cpp_order.orderId.c_str()); + order.orderUpdateId = cpp_order.orderUpdateId; + order.nodes_count = cpp_order.nodes.size(); + order.nodes = order.nodes_count ? static_cast(std::malloc(order.nodes_count * sizeof(Node))) : nullptr; + if (order.nodes) + for (size_t i = 0; i < order.nodes_count; i++) + order.nodes[i] = convert2CNode(cpp_order.nodes[i]); + order.edges_count = cpp_order.edges.size(); + order.edges = order.edges_count ? static_cast(std::malloc(order.edges_count * sizeof(Edge))) : nullptr; + if (order.edges) + for (size_t i = 0; i < order.edges_count; i++) + order.edges[i] = convert2CEdge(cpp_order.edges[i]); + order.zoneSetId = dup_cstr(cpp_order.zoneSetId.c_str()); + return order; +} + +extern "C" void order_free(Order* order) +{ + if (!order) return; + std::free(order->timestamp); + std::free(order->version); + std::free(order->manufacturer); + std::free(order->serialNumber); + std::free(order->orderId); + std::free(order->zoneSetId); + order->timestamp = order->version = order->manufacturer = nullptr; + order->serialNumber = order->orderId = order->zoneSetId = nullptr; + if (order->nodes) { + for (size_t i = 0; i < order->nodes_count; i++) + free_node(&order->nodes[i]); + std::free(order->nodes); + order->nodes = nullptr; + } + order->nodes_count = 0; + if (order->edges) { + for (size_t i = 0; i < order->edges_count; i++) + free_edge(&order->edges[i]); + std::free(order->edges); + order->edges = nullptr; + } + order->edges_count = 0; +} \ No newline at end of file diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 9611e7b..fa212cf 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -1,6 +1,8 @@ #include #include "nav_c_api.h" #include "convertor.h" +#include +#include #include #include #include @@ -11,6 +13,14 @@ #include #include #include +#include +#include + +// Keep navigation instances alive even when returning raw pointers as C handles. +// Key is the raw BaseNavigation* (used as NavigationHandle), value owns the instance. +static std::mutex g_nav_instances_mutex; +static std::unordered_map>g_nav_instances; static NavFeedback convert2CNavFeedback(const robot::move_base_core::NavFeedback &cpp) { @@ -38,10 +48,6 @@ extern "C" void nav_c_api_free_string(char *str) free(str); } -// ============================================================================ -// Helper Functions -// ============================================================================ - extern "C" bool navigation_offset_goal_2d(const Pose2D &in_pose, const char *frame_id, double offset_distance, PoseStamped &out_goal) @@ -66,75 +72,9 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped &in_pose, doubl return true; } -// ============================================================================ -// TF Listener Management -// ============================================================================ - -extern "C" TFListenerHandle tf_listener_create(void) -{ - try - { - auto tf = std::make_shared(); - return TFListenerHandle{new std::shared_ptr(tf)}; - } - catch (...) - { - return TFListenerHandle{nullptr}; - } -} - -extern "C" void tf_listener_destroy(TFListenerHandle handle) -{ - if (handle) - { - auto *tf_ptr = static_cast *>(handle); - delete tf_ptr; - } -} - -extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle, - const char *parent_frame, - const char *child_frame, - double x, double y, double z, - double qx, double qy, double qz, double qw) -{ - if (!tf_handle || !parent_frame || !child_frame) - { - return false; - } - - try - { - auto *tf_ptr = static_cast *>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) - { - return false; - } - - tf3::TransformStampedMsg st; - st.header.stamp = tf3::Time::now(); - st.header.frame_id = parent_frame; - st.child_frame_id = child_frame; - st.transform.translation.x = x; - st.transform.translation.y = y; - st.transform.translation.z = z; - st.transform.rotation.x = qx; - st.transform.rotation.y = qy; - st.transform.rotation.z = qz; - st.transform.rotation.w = qw; - - return (*tf_ptr)->setTransform(st, "nav_c_api(static)", true /*is_static*/); - } - catch (...) - { - return false; - } -} - // ============================================================================ // Navigation Handle Management // ============================================================================ - extern "C" NavigationHandle navigation_create(void) { try @@ -146,27 +86,37 @@ extern "C" NavigationHandle navigation_create(void) auto move_base_loader = boost::dll::import_alias( path_file_so, "MoveBase", boost::dll::load_mode::append_decorations); auto move_base_ptr = move_base_loader(); - // Store shared_ptr in handle (same pattern as TFListenerHandle) - return NavigationHandle{new std::shared_ptr(move_base_ptr)}; + if (!move_base_ptr) + return nullptr; + + auto *raw = move_base_ptr.get(); + { + std::lock_guard lock(g_nav_instances_mutex); + g_nav_instances[raw] = std::move(move_base_ptr); + } + + // Return raw pointer as opaque C handle. + return static_cast(raw); } catch (const std::exception &e) { // Log error if possible (in production, use proper logging) - return NavigationHandle{nullptr}; + return nullptr; } catch (...) { - return NavigationHandle{nullptr}; + return nullptr; } } extern "C" void navigation_destroy(NavigationHandle handle) { - if (handle.ptr) - { - auto *nav_ptr = static_cast *>(handle.ptr); - delete nav_ptr; - } + if (!handle) + return; + + auto *raw = static_cast(handle); + std::lock_guard lock(g_nav_instances_mutex); + g_nav_instances.erase(raw); } // ============================================================================ @@ -175,26 +125,29 @@ extern "C" void navigation_destroy(NavigationHandle handle) extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { - printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); - if (!handle.ptr || !tf_handle) + if (!handle || !tf_handle) { printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); return false; } try { - printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + printf("[%s:%d]: Initialize navigation\n", __FILE__, __LINE__); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto *tf_ptr = static_cast *>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) + + std::shared_ptr tf_ptr = + std::shared_ptr(reinterpret_cast(tf_handle), [](::tf3::BufferCore *) {}); + if (!tf_ptr) { - printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); + printf("[%s:%d]\n Error: Invalid TF listener", __FILE__, __LINE__); return false; } - nav->initialize(*tf_ptr); + nav_ptr->initialize(tf_ptr); return true; } @@ -205,24 +158,25 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle } catch (...) { - printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__); return false; } } extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count) { - if (!handle.ptr || !points || point_count == 0) + if (!handle || !points || point_count == 0) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + std::vector footprint; footprint.reserve(point_count); for (size_t i = 0; i < point_count; ++i) @@ -233,7 +187,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po pt.z = points[i].z; footprint.push_back(pt); } - nav->setRobotFootprint(footprint); + nav_ptr->setRobotFootprint(footprint); return true; } catch (...) @@ -244,18 +198,19 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - std::vector footprint = nav->getRobotFootprint(); + std::vector footprint = nav_ptr->getRobotFootprint(); if (footprint.empty()) { return false; @@ -276,22 +231,23 @@ extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *o } } -extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance) +extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); - return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + return nav_ptr->moveTo(cpp_goal); } catch (...) { @@ -299,39 +255,52 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped go } } -// extern "C" bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, -// const PoseStamped& goal, -// double xy_goal_tolerance, double yaw_goal_tolerance) { -// if (!handle.ptr || !order.ptr || !goal) { -// return false; -// } -// try { -// auto* nav = static_cast(handle.ptr); -// auto* order_ptr = static_cast(order.ptr); -// robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); -// return nav->moveTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); -// } catch (...) { -// return false; -// } -// } - -extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, - const PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance) +extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal) { - if (!handle.ptr || !marker) + if (!handle) + { + return false; + } + try + { + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return false; + robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y); + robot_protocol_msgs::Order cpp_order = convert2CppOrder(order); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav_ptr->moveTo(cpp_order, cpp_goal); + } + catch (const std::exception &e) + { + return false; + } + catch (...) + { + printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__); + return false; + } +} + +extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal) +{ + if (!handle || !marker) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); - return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + return nav_ptr->dockTo(std::string(marker), cpp_goal); } catch (...) { @@ -339,39 +308,59 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, } } -// extern "C" bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, -// const PoseStamped& goal, -// double xy_goal_tolerance, double yaw_goal_tolerance) { -// if (!handle.ptr || !order.ptr || !goal) { -// return false; -// } -// try { -// auto* nav = static_cast(handle.ptr); -// auto* order_ptr = static_cast(order.ptr); -// robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); -// return nav->dockTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); -// } catch (...) { -// return false; -// } +extern "C" bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal) +{ + if (!handle) + { + return false; + } + try + { + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return false; + robot_protocol_msgs::Order cpp_order = convert2CppOrder(order); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav_ptr->dockTo(cpp_order, std::string(marker), cpp_goal); + } + catch (const std::exception &e) + { + return false; + } + catch (...) + { + printf("[%s:%d]\n Error: Failed to dock to order\n", __FILE__, __LINE__); + return false; + } +} extern "C" bool navigation_move_straight_to(NavigationHandle handle, const double distance) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped pose; - if (!nav->getRobotPose(pose)) + if (!nav_ptr->getRobotPose(pose)) return false; + robot_geometry_msgs::Pose2D pose2d; + pose2d.x = pose.pose.position.x; + pose2d.y = pose.pose.position.y; + pose2d.theta = data_convert::getYaw(pose.pose.orientation); + robot::log_error("pose2d: %f %f %f", pose2d.x, pose2d.y, pose2d.theta); robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance); - return nav->moveStraightTo(goal); + return nav_ptr->moveStraightTo(goal); } catch (...) { @@ -379,21 +368,22 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const doubl } } -extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, double yaw_goal_tolerance) +extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); - return nav->rotateTo(cpp_goal, yaw_goal_tolerance); + return nav_ptr->rotateTo(cpp_goal); } catch (...) { @@ -403,13 +393,16 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped extern "C" void navigation_pause(NavigationHandle handle) { - if (handle.ptr) + if (handle) { try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (nav_ptr && *nav_ptr) - nav_ptr->get()->pause(); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return; + nav_ptr->pause(); } catch (...) { @@ -420,13 +413,15 @@ extern "C" void navigation_pause(NavigationHandle handle) extern "C" void navigation_resume(NavigationHandle handle) { - if (handle.ptr) + if (handle) { try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (nav_ptr && *nav_ptr) - nav_ptr->get()->resume(); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (nav_ptr) + nav_ptr->resume(); } catch (...) { @@ -437,13 +432,15 @@ extern "C" void navigation_resume(NavigationHandle handle) extern "C" void navigation_cancel(NavigationHandle handle) { - if (handle.ptr) + if (handle) { try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (nav_ptr && *nav_ptr) - nav_ptr->get()->cancel(); + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (nav_ptr) + nav_ptr->cancel(); } catch (...) { @@ -455,22 +452,24 @@ extern "C" void navigation_cancel(NavigationHandle handle) extern "C" bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::Vector3 linear; linear.x = linear_x; linear.y = linear_y; linear.z = linear_z; - return nav->setTwistLinear(linear); + return nav_ptr->setTwistLinear(linear); } catch (...) { @@ -481,22 +480,24 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle, extern "C" bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::Vector3 angular; angular.x = angular_x; angular.y = angular_y; angular.z = angular_z; - return nav->setTwistAngular(angular); + return nav_ptr->setTwistAngular(angular); } catch (...) { @@ -506,19 +507,21 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle, extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::PoseStamped cpp_pose; - if (nav->getRobotPose(cpp_pose)) + if (nav_ptr->getRobotPose(cpp_pose)) { out_pose = convert2CPoseStamped(cpp_pose); return true; @@ -533,19 +536,21 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_geometry_msgs::Pose2D cpp_pose; - if (nav->getRobotPose(cpp_pose)) + if (nav_ptr->getRobotPose(cpp_pose)) { out_pose = convert2CPose2D(cpp_pose); return true; @@ -560,18 +565,20 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &ou extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); + + robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav_ptr->getTwist(); out_twist = convert2CTwist2DStamped(cpp_twist); return true; } @@ -583,18 +590,20 @@ extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &ou extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot::move_base_core::NavFeedback *fb = nav->getFeedback(); + + robot::move_base_core::NavFeedback *fb = nav_ptr->getFeedback(); if (fb == nullptr) return false; out_feedback = convert2CNavFeedback(*fb); @@ -612,20 +621,22 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &ou extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid occupancy_grid) { - if (!handle.ptr || !map_name) + if (!handle || !map_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot::log_info("occupancy_grid.data_count: %d", occupancy_grid.data_count); robot_nav_msgs::OccupancyGrid robot_occupancy_grid = convert2CppOccupancyGrid(occupancy_grid); - nav->addStaticMap(std::string(map_name), robot_occupancy_grid); + nav_ptr->addStaticMap(std::string(map_name), robot_occupancy_grid); return true; } catch (...) @@ -636,19 +647,20 @@ extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *m extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan laser_scan) { - if (!handle.ptr || !laser_scan_name) + if (!handle || !laser_scan_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); robot_sensor_msgs::LaserScan robot_laser_scan = convert2CppLaserScan(laser_scan); - nav->addLaserScan(std::string(laser_scan_name), robot_laser_scan); + nav_ptr->addLaserScan(std::string(laser_scan_name), robot_laser_scan); return true; } catch (...) @@ -659,18 +671,20 @@ extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *l extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud) { - if (!handle.ptr || !point_cloud_name) + if (!handle || !point_cloud_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud cpp_cloud = convert2CppPointCloud(point_cloud); - nav->addPointCloud(std::string(point_cloud_name), cpp_cloud); + nav_ptr->addPointCloud(std::string(point_cloud_name), cpp_cloud); return true; } catch (...) @@ -681,18 +695,20 @@ extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char * extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2) { - if (!handle.ptr || !point_cloud2_name) + if (!handle || !point_cloud2_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud2 cpp_cloud = convert2CppPointCloud2(point_cloud2); - nav->addPointCloud2(std::string(point_cloud2_name), cpp_cloud); + nav_ptr->addPointCloud2(std::string(point_cloud2_name), cpp_cloud); return true; } catch (...) @@ -703,20 +719,22 @@ extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry) { - if (!handle.ptr || !odometry_name) + if (!handle || !odometry_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) { return false; } - auto *nav = nav_ptr->get(); + robot_nav_msgs::Odometry robot_odometry = convert2CppOdometry(odometry); - nav->addOdometry(std::string(odometry_name), robot_odometry); + nav_ptr->addOdometry(std::string(odometry_name), robot_odometry); return true; } catch (...) @@ -727,17 +745,19 @@ extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odo extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map) { - if (!handle.ptr || !map_name) + if (!handle || !map_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_nav_msgs::OccupancyGrid robot_map = nav->getStaticMap(std::string(map_name)); + + robot_nav_msgs::OccupancyGrid robot_map = nav_ptr->getStaticMap(std::string(map_name)); convert2COccupancyGrid(robot_map, out_map); return true; } @@ -749,17 +769,19 @@ extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *m extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan) { - if (!handle.ptr || !laser_scan_name) + if (!handle || !laser_scan_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_sensor_msgs::LaserScan robot_scan = nav->getLaserScan(std::string(laser_scan_name)); + + robot_sensor_msgs::LaserScan robot_scan = nav_ptr->getLaserScan(std::string(laser_scan_name)); convert2CLaserScan(robot_scan, out_scan); return true; } @@ -771,17 +793,19 @@ extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *l extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud) { - if (!handle.ptr || !point_cloud_name) + if (!handle || !point_cloud_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_sensor_msgs::PointCloud robot_cloud = nav->getPointCloud(std::string(point_cloud_name)); + + robot_sensor_msgs::PointCloud robot_cloud = nav_ptr->getPointCloud(std::string(point_cloud_name)); convert2CPointCloud(robot_cloud, out_cloud); return true; } @@ -793,17 +817,19 @@ extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char * extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud) { - if (!handle.ptr || !point_cloud2_name) + if (!handle || !point_cloud2_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot_sensor_msgs::PointCloud2 robot_cloud = nav->getPointCloud2(std::string(point_cloud2_name)); + + robot_sensor_msgs::PointCloud2 robot_cloud = nav_ptr->getPointCloud2(std::string(point_cloud2_name)); convert2CPointCloud2(robot_cloud, out_cloud); return true; } @@ -815,17 +841,19 @@ extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count) { - if (!handle.ptr || !out_maps) + if (!handle || !out_maps) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto maps = nav->getAllStaticMaps(); + + auto maps = nav_ptr->getAllStaticMaps(); out_count = maps.size(); size_t i = 0; for (const auto &p : maps) @@ -844,17 +872,19 @@ extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOcc extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count) { - if (!handle.ptr || !out_scans) + if (!handle || !out_scans) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto scans = nav->getAllLaserScans(); + + auto scans = nav_ptr->getAllLaserScans(); if (scans.empty()) { out_count = 0; @@ -878,17 +908,19 @@ extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLas extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count) { - if (!handle.ptr || !out_clouds) + if (!handle || !out_clouds) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto clouds = nav->getAllPointClouds(); + + auto clouds = nav_ptr->getAllPointClouds(); if (clouds.empty()) { out_count = 0; @@ -912,17 +944,19 @@ extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPo extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count) { - if (!handle.ptr || !out_clouds) + if (!handle || !out_clouds) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - auto clouds = nav->getAllPointCloud2s(); + + auto clouds = nav_ptr->getAllPointCloud2s(); if (clouds.empty()) { out_count = 0; @@ -946,16 +980,18 @@ extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedP extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char *map_name) { - if (!handle.ptr || !map_name) + if (!handle || !map_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeStaticMap(std::string(map_name)); + return nav_ptr->removeStaticMap(std::string(map_name)); } catch (...) { @@ -965,16 +1001,18 @@ extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name) { - if (!handle.ptr || !laser_scan_name) + if (!handle || !laser_scan_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeLaserScan(std::string(laser_scan_name)); + return nav_ptr->removeLaserScan(std::string(laser_scan_name)); } catch (...) { @@ -984,16 +1022,18 @@ extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name) { - if (!handle.ptr || !point_cloud_name) + if (!handle || !point_cloud_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removePointCloud(std::string(point_cloud_name)); + return nav_ptr->removePointCloud(std::string(point_cloud_name)); } catch (...) { @@ -1003,16 +1043,18 @@ extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const cha extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name) { - if (!handle.ptr || !point_cloud2_name) + if (!handle || !point_cloud2_name) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removePointCloud2(std::string(point_cloud2_name)); + return nav_ptr->removePointCloud2(std::string(point_cloud2_name)); } catch (...) { @@ -1022,16 +1064,18 @@ extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const ch extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllStaticMaps(); + return nav_ptr->removeAllStaticMaps(); } catch (...) { @@ -1041,16 +1085,18 @@ extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllLaserScans(); + return nav_ptr->removeAllLaserScans(); } catch (...) { @@ -1060,16 +1106,18 @@ extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllPointClouds(); + return nav_ptr->removeAllPointClouds(); } catch (...) { @@ -1079,16 +1127,18 @@ extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllPointCloud2s(); + return nav_ptr->removeAllPointCloud2s(); } catch (...) { @@ -1098,16 +1148,18 @@ extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) extern "C" bool navigation_remove_all_data(NavigationHandle handle) { - if (!handle.ptr) + if (!handle) { return false; } try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - return nav_ptr->get()->removeAllData(); + return nav_ptr->removeAllData(); } catch (...) { @@ -1116,35 +1168,85 @@ extern "C" bool navigation_remove_all_data(NavigationHandle handle) } // ============================================================================ -// Planner Data (disabled - PlannerDataOutput and related types not in public API) +// Planner Data // ============================================================================ -#if 0 +static void clear_planner_data(PlannerDataOutput *out) +{ + if (!out) + return; + // Free Path2D plan + if (out->plan.poses) + { + for (size_t i = 0; i < out->plan.poses_count; i++) + { + if (out->plan.poses[i].header.frame_id) + free(out->plan.poses[i].header.frame_id); + } + free(out->plan.poses); + out->plan.poses = nullptr; + out->plan.poses_count = 0; + } + if (out->plan.header.frame_id) + { + free(out->plan.header.frame_id); + out->plan.header.frame_id = nullptr; + } + // Free OccupancyGrid costmap + if (out->costmap.header.frame_id) + { + free(out->costmap.header.frame_id); + out->costmap.header.frame_id = nullptr; + } + if (out->costmap.data) + { + free(out->costmap.data); + out->costmap.data = nullptr; + } + // Free OccupancyGridUpdate + if (out->costmap_update.header.frame_id) + { + free(out->costmap_update.header.frame_id); + out->costmap_update.header.frame_id = nullptr; + } + if (out->costmap_update.data) + { + free(out->costmap_update.data); + out->costmap_update.data = nullptr; + } + // Free PolygonStamped footprint + if (out->footprint.header.frame_id) + { + free(out->footprint.header.frame_id); + out->footprint.header.frame_id = nullptr; + } + if (out->footprint.polygon.points) + { + free(out->footprint.polygon.points); + out->footprint.polygon.points = nullptr; + out->footprint.polygon.points_count = 0; + } +} + extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data) { - if (!handle.ptr || !out_data) - { + if (!handle || !out_data) return false; - } clear_planner_data(out_data); try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), + [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot::move_base_core::PlannerDataOutput data = nav->getGlobalData(); - out_data->plan = Path2DHandle{clone_message(data.plan)}; - out_data->costmap = OccupancyGridHandle{clone_message(data.costmap)}; - out_data->costmap_update = OccupancyGridUpdateHandle{clone_message(data.costmap_update)}; - out_data->footprint = PolygonStampedHandle{clone_message(data.footprint)}; - out_data->is_costmap_updated = data.is_costmap_updated; - if (!out_data->plan.ptr || !out_data->costmap.ptr || !out_data->costmap_update.ptr || - !out_data->footprint.ptr) - { - clear_planner_data(out_data); - return false; - } + robot::move_base_core::PlannerDataOutput data = nav_ptr->getGlobalData(); + convert2CPath2D(data.plan, out_data->plan); + convert2COccupancyGrid(data.costmap, out_data->costmap); + convert2COccupancyGridUpdate(data.costmap_update, out_data->costmap_update); + convert2CPolygonStamped(data.footprint, out_data->footprint); + out_data->is_costmap_updated = data.is_costmap_updated; return true; } catch (...) @@ -1156,30 +1258,24 @@ extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataO extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data) { - if (!handle.ptr || !out_data) - { + if (!handle || !out_data) return false; - } clear_planner_data(out_data); try { - auto *nav_ptr = static_cast *>(handle.ptr); - if (!nav_ptr || !*nav_ptr) + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), + [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) return false; - auto *nav = nav_ptr->get(); - robot::move_base_core::PlannerDataOutput data = nav->getLocalData(); - out_data->plan = Path2DHandle{clone_message(data.plan)}; - out_data->costmap = convert2CppOccupancyGrid(data.costmap); - out_data->costmap_update = OccupancyGridUpdateHandle{clone_message(data.costmap_update)}; - out_data->footprint = convert2CppPolygonStamped(data.footprint); - out_data->is_costmap_updated = data.is_costmap_updated; - if (!out_data->plan.ptr || !out_data->costmap.ptr || !out_data->costmap_update.ptr || - !out_data->footprint.ptr) - { - clear_planner_data(out_data); - return false; - } + robot::move_base_core::PlannerDataOutput data = nav_ptr->getLocalData(); + convert2CPath2D(data.plan, out_data->plan); + convert2COccupancyGrid(data.costmap, out_data->costmap); + convert2COccupancyGridUpdate(data.costmap_update, out_data->costmap_update); + convert2CPolygonStamped(data.footprint, out_data->footprint); + out_data->is_costmap_updated = data.is_costmap_updated; return true; } catch (...) @@ -1188,4 +1284,4 @@ extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOu return false; } } -#endif + diff --git a/src/Algorithms/Libraries/kalman/src/kalman.cpp b/src/Algorithms/Libraries/kalman/src/kalman.cpp index 30c5345..4c44312 100755 --- a/src/Algorithms/Libraries/kalman/src/kalman.cpp +++ b/src/Algorithms/Libraries/kalman/src/kalman.cpp @@ -24,7 +24,15 @@ KalmanFilter::KalmanFilter( I.setIdentity(); } -KalmanFilter::KalmanFilter() {} +KalmanFilter::KalmanFilter() + : m(0), + n(0), + t0(0.0), + t(0.0), + dt(0.0), + initialized(false) +{ +} void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) { x_hat = x0; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h index 423a082..88d3dba 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h @@ -15,9 +15,9 @@ namespace mkt_algorithm class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory { public: - GoStraight() {}; + GoStraight(); - virtual ~GoStraight() {}; + virtual ~GoStraight(); /** * @brief Initialize parameters as needed diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index b7951bc..bab5fc9 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -30,9 +30,14 @@ namespace mkt_algorithm class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm { public: - PredictiveTrajectory() : initialized_(false), nav_stop_(false), - near_goal_heading_integral_(0.0), near_goal_heading_last_error_(0.0), near_goal_heading_was_active_(false) {}; + /** + * @brief Constructor + */ + PredictiveTrajectory(); + /** + * @brief Destructor + */ virtual ~PredictiveTrajectory(); // Standard ScoreAlgorithm Interface diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h index 4af845f..e888cfc 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h @@ -14,9 +14,9 @@ namespace mkt_algorithm class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory { public: - RotateToGoal() {}; + RotateToGoal(); - virtual ~RotateToGoal() {}; + virtual ~RotateToGoal(); /** * @brief Initialize parameters as needed diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 029309f..d3351eb 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -2,6 +2,13 @@ #include #include +mkt_algorithm::diff::GoStraight::GoStraight() + : PredictiveTrajectory() +{ +} + +mkt_algorithm::diff::GoStraight::~GoStraight() {} + void mkt_algorithm::diff::GoStraight::initialize( robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 70b0ed5..7d57a13 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -1,6 +1,48 @@ #include #include + +mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory() + : initialized_(false), + nav_stop_(false), + x_direction_(0.0), + y_direction_(0.0), + theta_direction_(0.0), + use_velocity_scaled_lookahead_dist_(false), + lookahead_time_(0.0), + lookahead_dist_(0.0), + min_lookahead_dist_(0.0), + max_lookahead_dist_(0.0), + max_lateral_accel_(0.0), + use_rotate_to_heading_(false), + rotate_to_heading_min_angle_(0.0), + min_path_distance_(0.0), + max_path_distance_(0.0), + use_final_heading_alignment_(false), + final_heading_xy_tolerance_(0.0), + final_heading_angle_tolerance_(0.0), + final_heading_min_velocity_(0.0), + final_heading_kp_angular_(0.0), + final_heading_ki_angular_(0.0), + final_heading_kd_angular_(0.0), + near_goal_heading_integral_(0.0), + near_goal_heading_last_error_(0.0), + near_goal_heading_was_active_(false), + use_regulated_linear_velocity_scaling_(false), + min_approach_linear_velocity_(0.0), + regulated_linear_scaling_min_radius_(0.0), + regulated_linear_scaling_min_speed_(0.0), + use_cost_regulated_linear_velocity_scaling_(false), + inflation_cost_scaling_factor_(0.0), + cost_scaling_dist_(0.0), + cost_scaling_gain_(0.0), + control_duration_(0.0), + kf_(nullptr), + m_(0), + n_(0) +{ +} + mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} void mkt_algorithm::diff::PredictiveTrajectory::initialize( diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index c621c02..51670e9 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -4,6 +4,14 @@ #include #include + +mkt_algorithm::diff::RotateToGoal::RotateToGoal() + : PredictiveTrajectory() +{ +} + +mkt_algorithm::diff::RotateToGoal::~RotateToGoal() {} + void mkt_algorithm::diff::RotateToGoal::initialize( robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h index 253f43c..56aac68 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h @@ -15,6 +15,7 @@ namespace mkt_plugins { public: KinematicParameters(); + virtual ~KinematicParameters(); void initialize(const robot::NodeHandle &nh); /** diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h index 99ee362..5c0d783 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h @@ -12,6 +12,15 @@ namespace mkt_plugins class LimitedAccelGenerator : public StandardTrajectoryGenerator { public: + /** + * @brief Constructor + */ + LimitedAccelGenerator(); + /** + * @brief Destructor + */ + virtual ~LimitedAccelGenerator(); + /** * @brief Initialize the generator with parameters from NodeHandle * @param nh NodeHandle for loading acceleration_time parameter diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h index abe6e52..b7480d0 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h @@ -17,6 +17,16 @@ namespace mkt_plugins class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator { public: + + /** + * @brief Constructor + */ + StandardTrajectoryGenerator(); + + /** + * @brief Destructor + */ + virtual ~StandardTrajectoryGenerator(); /** * @brief Initialize the trajectory generator with parameters from NodeHandle * @param nh NodeHandle for loading configuration parameters diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h index d650f7d..37ecab8 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h @@ -21,7 +21,12 @@ namespace mkt_plugins * @brief Default constructor * Initializes all iterators to nullptr */ - XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} + XYThetaIterator(); + + /** + * @brief Destructor + */ + virtual ~XYThetaIterator(); /** * @brief Initialize the iterator with parameters and kinematics diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp index 6fa90d2..ae8b0c1 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -7,7 +7,12 @@ #include #include -mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25) +mkt_plugins::GoalChecker::GoalChecker() + : nh_(), + line_generator_(nullptr), + yaw_goal_tolerance_(0.25), + xy_goal_tolerance_(0.25), + old_xy_goal_tolerance_(0.0) { } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp index f11251d..dfd45dd 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp @@ -32,12 +32,42 @@ namespace mkt_plugins nh.setParam(decel_param, -accel); } - KinematicParameters::KinematicParameters() : xytheta_direct_(), - min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0), - min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0), - acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0), - decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0), - min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0) + KinematicParameters::KinematicParameters() + : xytheta_direct_(), + min_vel_x_(0.0), + min_vel_y_(0.0), + max_vel_x_(0.0), + max_vel_y_(0.0), + max_vel_theta_(0.0), + min_speed_xy_(0.0), + max_speed_xy_(0.0), + min_speed_theta_(0.0), + acc_lim_x_(0.0), + acc_lim_y_(0.0), + acc_lim_theta_(0.0), + decel_lim_x_(0.0), + decel_lim_y_(0.0), + decel_lim_theta_(0.0), + min_speed_xy_sq_(0.0), + max_speed_xy_sq_(0.0), + original_min_vel_x_(0.0), + original_min_vel_y_(0.0), + original_max_vel_x_(0.0), + original_max_vel_y_(0.0), + original_max_vel_theta_(0.0), + original_min_speed_xy_(0.0), + original_max_speed_xy_(0.0), + original_min_speed_theta_(0.0), + original_acc_lim_x_(0.0), + original_acc_lim_y_(0.0), + original_acc_lim_theta_(0.0), + original_decel_lim_x_(0.0), + original_decel_lim_y_(0.0), + original_decel_lim_theta_(0.0) + { + } + + KinematicParameters::~KinematicParameters() { } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp index af19d51..1b6e8c6 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -8,6 +8,16 @@ namespace mkt_plugins { +LimitedAccelGenerator::LimitedAccelGenerator() + : StandardTrajectoryGenerator(), + acceleration_time_(0.0) +{ +} + +LimitedAccelGenerator::~LimitedAccelGenerator() +{ +} + void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) { StandardTrajectoryGenerator::initialize(nh); diff --git a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp index c08fe66..fa26177 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp @@ -7,8 +7,13 @@ namespace mkt_plugins { -SimpleGoalChecker::SimpleGoalChecker() : - xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625) +SimpleGoalChecker::SimpleGoalChecker() + : nh_(), + xy_goal_tolerance_(0.25), + yaw_goal_tolerance_(0.25), + stateful_(true), + check_xy_(true), + xy_goal_tolerance_sq_(0.0625) { } diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp index c077009..581a835 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp @@ -13,6 +13,24 @@ using robot_nav_2d_utils::loadParameterWithDeprecation; namespace mkt_plugins { + + StandardTrajectoryGenerator::StandardTrajectoryGenerator() + : nh_kinematics_(), + kinematics_(nullptr), + velocity_iterator_(nullptr), + sim_time_(0.0), + discretize_by_time_(false), + time_granularity_(0.0), + linear_granularity_(0.0), + angular_granularity_(0.0), + include_last_point_(false) + { + } + + StandardTrajectoryGenerator::~StandardTrajectoryGenerator() + { + } + void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh) { nh_kinematics_ = nh; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp index ab7a690..c4de3c0 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -4,6 +4,21 @@ namespace mkt_plugins { +XYThetaIterator::XYThetaIterator() + : vx_samples_(0), + vy_samples_(0), + vtheta_samples_(0), + kinematics_(nullptr), + x_it_(nullptr), + y_it_(nullptr), + th_it_(nullptr) +{ +} + +XYThetaIterator::~XYThetaIterator() +{ +} + void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) { kinematics_ = kinematics; diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index f3829bf..eac6f13 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -13,7 +13,17 @@ namespace two_points_planner { - TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {} + TwoPointsPlanner::TwoPointsPlanner() + : initialized_(false), + lethal_obstacle_(0), + inscribed_inflated_obstacle_(0), + circumscribed_cost_(0), + allow_unknown_(false), + costmap_robot_(nullptr), + current_env_width_(0), + current_env_height_(0) + { + } TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) : initialized_(false), costmap_robot_(NULL) diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h index 6ac81c1..f05ccfa 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h @@ -79,7 +79,6 @@ namespace pnkx_local_planner */ robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; - bool is_ready_; }; } // namespace pnkx_local_planner diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 74375cf..d030d04 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -14,7 +14,10 @@ #include pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner() - : start_docking_(false) + : PNKXLocalPlanner(), + start_docking_(false), + original_xy_goal_tolerance_(0.0), + original_yaw_goal_tolerance_(0.0) { } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp index 401e9b6..ddbc668 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp @@ -15,6 +15,7 @@ #include pnkx_local_planner::PNKXGoStraightLocalPlanner::PNKXGoStraightLocalPlanner() + : PNKXLocalPlanner() { } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index accc3b9..04358f5 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -17,7 +17,21 @@ #include pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner() - : initialized_(false) + : initialized_(false), + traj_generator_(nullptr), + nav_algorithm_(nullptr), + rotate_algorithm_(nullptr), + goal_checker_(nullptr), + tf_(nullptr), + costmap_(nullptr), + costmap_robot_(nullptr), + info_(), + update_costmap_before_planning_(false), + ret_angle_(false), + ret_nav_(false), + yaw_goal_tolerance_(0.0), + xy_goal_tolerance_(0.0), + lock_(false) { } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp index 429d802..0d4c5a5 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp @@ -14,6 +14,7 @@ #include pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner() + : PNKXLocalPlanner() { } diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index 49dbcb8..264f603 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -406,7 +406,8 @@ namespace robot * @param yaw_goal_tolerance Acceptable heading error (radians). * @return True if docking command succeeded. */ - virtual bool dockTo(const robot_protocol_msgs::Order &msg, + virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const std::string &marker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) = 0; diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 66a860f..52c46fa 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -303,6 +303,7 @@ namespace move_base * @return True if docking command succeeded. */ virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const std::string &marker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) override; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index bff0768..57aed24 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -33,20 +33,50 @@ #include move_base::MoveBase::MoveBase() - : initialized_(false), - planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), - planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), - runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), - pause_ctr_(false), paused_(false) + : initialized_(false), + tf_(), + as_(NULL), + planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), + planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), + recovery_index_(0), + recovery_behavior_enabled_(false), + planner_frequency_(0.0), controller_frequency_(0.0), + inscribed_radius_(0.0), circumscribed_radius_(0.0), + planner_patience_(0.0), controller_patience_(0.0), + max_planning_retries_(0), planning_retries_(0), + conservative_reset_dist_(0.0), clearing_radius_(0.0), + shutdown_costmaps_(false), clearing_rotation_allowed_(false), + make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false), + oscillation_timeout_(0.0), oscillation_distance_(0.0), + state_(PLANNING), recovery_trigger_(PLANNING_R), + runPlanner_(false), planner_thread_(NULL), + setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), + pause_ctr_(false), paused_(false), cancel_ctr_(false), + original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0) { } move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) - : initialized_(false), - planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), - planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), - runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), - pause_ctr_(false), paused_(false) + : initialized_(false), + tf_(), + as_(NULL), + planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), + planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), + recovery_index_(0), + recovery_behavior_enabled_(false), + planner_frequency_(0.0), controller_frequency_(0.0), + inscribed_radius_(0.0), circumscribed_radius_(0.0), + planner_patience_(0.0), controller_patience_(0.0), + max_planning_retries_(0), planning_retries_(0), + conservative_reset_dist_(0.0), clearing_radius_(0.0), + shutdown_costmaps_(false), clearing_rotation_allowed_(false), + make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false), + oscillation_timeout_(0.0), oscillation_distance_(0.0), + state_(PLANNING), recovery_trigger_(PLANNING_R), + runPlanner_(false), planner_thread_(NULL), + setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), + pause_ctr_(false), paused_(false), cancel_ctr_(false), + original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0) { initialize(tf); } @@ -134,6 +164,11 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__); } tf_ = tf; + if(tf_) + { + std::string all_frames_string = tf_->allFramesAsString(); + robot::log_info("[%s:%d]\n INFO: tf_ allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str()); + } as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true); setupActionServerCallbacks(); @@ -341,8 +376,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) } initialized_ = true; setup_ = true; - if(tf_) - robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); robot::log_info("========== End: initialize() - SUCCESS =========="); } else @@ -717,37 +750,37 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na // robot::log_info("addOdometry: %s", odometry_name.c_str()); // robot::log_info("odometry header: %s", odometry.header.frame_id.c_str()); // robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str()); - if(tf_) - { - // try - // { - // robot_geometry_msgs::PoseStamped global_pose_stamped; - // tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); - // robot_geometry_msgs::PoseStamped robot_pose; - // tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); - // robot_pose.header.frame_id = robot_base_frame_; - // robot_pose.header.stamp = robot::Time(); - // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + // if(tf_) + // { + // try + // { + // robot_geometry_msgs::PoseStamped global_pose_stamped; + // tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); + // robot_geometry_msgs::PoseStamped robot_pose; + // tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); + // robot_pose.header.frame_id = robot_base_frame_; + // robot_pose.header.stamp = robot::Time(); + // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later - // // Convert robot::Time to tf3::Time - // tf3::Time tf3_current_time = data_convert::convertTime(current_time); - // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); + // // Convert robot::Time to tf3::Time + // tf3::Time tf3_current_time = data_convert::convertTime(current_time); + // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); - // std::string error_msg; - // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) - // { - // // Transform is available at current time - // tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time); - // tf3::doTransform(robot_pose, global_pose_stamped, transform); - // robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation)); - // } - // } - // catch (const tf3::TransformException &ex) - // { - // robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what()); - // } - robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); - } + // std::string error_msg; + // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) + // { + // // Transform is available at current time + // tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time); + // tf3::doTransform(robot_pose, global_pose_stamped, transform); + // robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation)); + // } + // } + // catch (const tf3::TransformException &ex) + // { + // robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what()); + // } + // // robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); + // } // robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation)); // std::stringstream pose_covariance_str; // for(int i = 0; i < 36; i++) { @@ -770,7 +803,8 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na odometry_ = odometry; } -bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) +bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) { if (setup_) { @@ -816,12 +850,25 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d // Check pointers if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } - if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); @@ -829,7 +876,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d // Swap planner try { - if(!tc_->swapPlanner(position_planner_name_)) + if (!tc_->swapPlanner(position_planner_name_)) { robot::log_error("[MoveBase::moveTo] Failed to swapPlanner"); return false; @@ -837,8 +884,9 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d } catch (const std::exception &e) { - std::cerr << e.what() << "\n"; - throw std::exception(e); + robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; } // Update navigation feedback @@ -935,20 +983,33 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); robot::Duration(0.01).sleep(); - robot::log_info("[MoveBase] In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server."); if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); try { - if(!tc_->swapPlanner(position_planner_name_)) + if (!tc_->swapPlanner(position_planner_name_)) { robot::log_error("[MoveBase::moveTo] Failed to swapPlanner"); return false; @@ -967,49 +1028,76 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->goal_checked = false; } + else + { + robot::log_error("[MoveBase::moveTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } if (cancel_ctr_) cancel_ctr_ = false; - robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); - action_goal->header.stamp = robot::Time::now(); - action_goal->goal.target_pose = goal; - - // Generate unique goal ID using timestamp - robot::Time now = robot::Time::now(); - action_goal->goal_id.stamp = now; - std::ostringstream goal_id_stream; - goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; - action_goal->goal_id.id = goal_id_stream.str(); - - robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); - - // Store Order message for use in planThread + if (!as_) { - boost::unique_lock planner_lock(planner_mutex_); - planner_order_ = boost::make_shared(msg); - robot::log_info("[MoveBase::moveTo] Stored Order message for planning"); + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + + // Store Order message for use in planThread + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_ = boost::make_shared(msg); + robot::log_info("[MoveBase::moveTo] Stored Order message for planning"); + } + if (!planner_order_) + { + robot::log_error("[MoveBase::moveTo] Failed to store Order message for planning"); + lock.unlock(); + return false; + } + + as_->processGoal(action_goal); + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what()); + lock.unlock(); + return false; } - - as_->processGoal(action_goal); lock.unlock(); return true; } -bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal, +bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) { std::string maker_sources; - // private_nh_.param("maker_sources", maker_sources, std::string("")); + private_nh_.param("maker_sources", maker_sources, std::string("")); std::stringstream ss(maker_sources); std::string source; bool has_maker = false; while (ss >> source) { - if (maker == source) + if (marker == source) { - private_nh_.setParam("maker_name", maker); + private_nh_.setParam("maker_name", marker); has_maker = true; } } @@ -1019,7 +1107,120 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_ { nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; std::stringstream fb_ss; - fb_ss << "The system has not been '" << maker << "'"; + fb_ss << "The system has not been '" << marker << "'"; + nav_feedback_->feed_back_str = fb_ss.str(); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (setup_) + { + swapPlanner(default_config_.base_global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (fabs(xy_goal_tolerance) > 0.001) + this->setXyGoalTolerance(fabs(xy_goal_tolerance)); + else + this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + + if (fabs(yaw_goal_tolerance) > 0.001) + this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); + else + this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); + + if (!tc_) + { + robot::log_error("[MoveBase::dockTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; + } + if (!controller_costmap_robot_) + { + robot::log_error("[MoveBase::dockTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + try + { + if (!tc_->swapPlanner(docking_planner_name_)) + { + robot::log_error("[MoveBase::dockTo] Failed to swapPlanner"); + return false; + } + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; + } + + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + else + { + robot::log_error("[MoveBase::dockTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } + if (cancel_ctr_) + cancel_ctr_ = false; + + lock.unlock(); + return true; +} + +bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, + const std::string &marker, + const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + std::string maker_sources; + private_nh_.param("maker_sources", maker_sources, std::string("")); + std::stringstream ss(maker_sources); + std::string source; + bool has_maker = false; + while (ss >> source) + { + if (marker == source) + { + private_nh_.setParam("maker_name", marker); + has_maker = true; + } + } + if (!has_maker) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + std::stringstream fb_ss; + fb_ss << "The system has not been '" << marker << "'"; nav_feedback_->feed_back_str = fb_ss.str(); nav_feedback_->goal_checked = false; } @@ -1084,17 +1285,53 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_ if (cancel_ctr_) cancel_ctr_ = false; + if (!as_) + { + robot::log_error("[MoveBase::dockTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_dock_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::dockTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + + // Store Order message for use in planThread (same as moveTo with Order) + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_ = boost::make_shared(msg); + robot::log_info("[MoveBase::dockTo] Stored Order message for planning"); + } + if (!planner_order_) + { + robot::log_error("[MoveBase::dockTo] Failed to store Order message for planning"); + lock.unlock(); + return false; + } + + as_->processGoal(action_goal); + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::dockTo] Exception during processGoal: %s", e.what()); + lock.unlock(); + return false; + } + lock.unlock(); return true; } -bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, - const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance, double yaw_goal_tolerance) -{ - return false; -} - bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance) { robot::log_info("[MoveBase::moveStraightTo] Entry"); @@ -1117,13 +1354,28 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped this->setXyGoalTolerance(fabs(xy_goal_tolerance)); else this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::moveStraightTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); @@ -1131,17 +1383,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped try { robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner"); - if(!tc_->swapPlanner(go_straight_planner_name_)) + if (!tc_->swapPlanner(go_straight_planner_name_)) { - if(tf_ == nullptr) - { - robot::log_error("[MoveBase::moveTo] tf_ pointer is null!"); - throw std::runtime_error("Null 'tf_' pointer encountered"); - } - else - { - robot::log_info("[MoveBase::moveTo] tf_ pointer is not null!"); - } + robot::log_error("[MoveBase::moveStraightTo] Failed to swapPlanner"); + lock.unlock(); return false; } } @@ -1158,13 +1403,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->goal_checked = false; } + else + { + robot::log_error("[MoveBase::moveStraightTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } if (cancel_ctr_) cancel_ctr_ = false; - // Check if action server exists if (!as_) { - robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + robot::log_error("[MoveBase::moveStraightTo] as_ pointer is null!"); lock.unlock(); return false; } @@ -1174,31 +1424,24 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); action_goal->header.stamp = robot::Time::now(); action_goal->goal.target_pose = goal; - - // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); action_goal->goal_id.stamp = now; std::ostringstream goal_id_stream; goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; action_goal->goal_id.id = goal_id_stream.str(); - + robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); - robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld", + robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld", action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); - - // Clear Order message since this is a non-Order moveTo call + + // Clear Order message since this is a non-Order moveStraightTo call { boost::unique_lock planner_lock(planner_mutex_); planner_order_.reset(); } - - robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server..."); - if(controller_costmap_robot_ == nullptr) - { - robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ is null!"); - return false; - } + robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server..."); as_->processGoal(action_goal); robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server"); } @@ -1240,19 +1483,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, if (!tc_) { - throw std::runtime_error("Null 'tc_' pointer encountered"); + robot::log_error("[MoveBase::rotateTo] tc_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null tc_"); + nav_feedback_->goal_checked = false; + } + return false; } if (!controller_costmap_robot_) { - throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + robot::log_error("[MoveBase::rotateTo] controller_costmap_robot_ pointer is null"); + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_"); + nav_feedback_->goal_checked = false; + } + return false; } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); try { - if(!tc_->swapPlanner(rotate_planner_name_)) + if (!tc_->swapPlanner(rotate_planner_name_)) { robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner"); + lock.unlock(); return false; } } @@ -1269,29 +1527,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->goal_checked = false; } + else + { + robot::log_error("[MoveBase::rotateTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } if (cancel_ctr_) cancel_ctr_ = false; - // Check if action server exists if (!as_) { - robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + robot::log_error("[MoveBase::rotateTo] as_ pointer is null!"); lock.unlock(); return false; } try { - robot_geometry_msgs::Pose2D pose; if (!this->getRobotPose(pose)) { if (nav_feedback_) { nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; - nav_feedback_->feed_back_str = std::string("Coudn't get robot pose"); + nav_feedback_->feed_back_str = std::string("Couldn't get robot pose"); nav_feedback_->goal_checked = false; } + lock.unlock(); return false; } @@ -1302,23 +1565,22 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta); action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta); - // Generate unique goal ID using timestamp robot::Time now = robot::Time::now(); action_goal->goal_id.stamp = now; std::ostringstream goal_id_stream; goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; action_goal->goal_id.id = goal_id_stream.str(); - + robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); - robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld", + robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld", action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); - - // Clear Order message since this is a non-Order moveTo call + + // Clear Order message since this is a non-Order rotateTo call { boost::unique_lock planner_lock(planner_mutex_); planner_order_.reset(); } - + robot::log_info("[MoveBase::rotateTo] Processing goal through action server..."); as_->processGoal(action_goal); robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server"); @@ -1326,7 +1588,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, catch (const std::exception &e) { lock.unlock(); - robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what()); + robot::log_error("[MoveBase::rotateTo] Exception during processGoal: %s", e.what()); return false; } @@ -1459,7 +1721,6 @@ bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &lin { if (tc_->islock()) return true; - return tc_->setTwistLinear(linear); } else if (tc_ && cancel_ctr_) @@ -1878,7 +2139,7 @@ void move_base::MoveBase::planThread() while (wait_for_wake || !runPlanner_) { // if we should not be running the planner then suspend this thread - std::cout << "Planner thread is suspending" << std::endl; + robot::log_info("Planner thread is suspending"); planner_cond_.wait(lock); wait_for_wake = false; } @@ -1888,7 +2149,7 @@ void move_base::MoveBase::planThread() robot_geometry_msgs::PoseStamped temp_goal = planner_goal_; boost::shared_ptr temp_order = planner_order_; lock.unlock(); - std::cout << "Planning..." << std::endl; + robot::log_info("Planning..."); // run planner planner_plan_->clear(); // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y); @@ -1910,7 +2171,7 @@ void move_base::MoveBase::planThread() if (gotPlan) { - std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl; + robot::log_info("Got Plan with %d points!", planner_plan_->size()); // pointer swap the plans under mutex (the controller will pull from latest_plan_) std::vector *temp_plan = planner_plan_; @@ -1921,7 +2182,7 @@ void move_base::MoveBase::planThread() planning_retries_ = 0; new_global_plan_ = true; - std::cout << "Generated a plan from the base_global_planner" << std::endl; + robot::log_info("Generated a plan from the base_global_planner"); // make sure we only start the controller if we still haven't reached the goal if (runPlanner_) @@ -1937,7 +2198,7 @@ void move_base::MoveBase::planThread() // if we didn't get a plan and we are in the planning state (the robot isn't moving) else if (state_ == move_base::PLANNING) { - std::cout << "No Plan..." << std::endl; + robot::log_info("No Plan..."); robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_); // check if we've tried to make a plan for over our time limit or our maximum number of retries