From 10379d5b163573e1e9856c242825448508cd6436 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 24 Dec 2025 14:29:58 +0700 Subject: [PATCH] update init --- .gitmodules | 6 + .vscode/settings.json | 71 -- CMAKE_BUILD_README.md | 3 +- CMakeLists.txt | 16 + config/costmap_global_params.yaml | 23 +- ...lobal_params_plugins_no_virtual_walls.yaml | 8 +- config/costmap_local_params.yaml | 31 +- ...local_params_plugins_no_virtual_walls.yaml | 14 +- config/pnkx_local_planner_params.yaml | 28 +- examples/CSharpExample.cs | 29 + examples/NavigationExample/Program.cs | 29 + examples/NavigationExample/libnav_c_api.so | Bin 161112 -> 165560 bytes src/APIs/c_api/include/nav_c_api.h | 24 + src/APIs/c_api/src/nav_c_api.cpp | 48 +- .../Cores/score_algorithm/CMakeLists.txt | 3 +- .../include/score_algorithm/goal_checker.h | 5 +- .../include/score_algorithm/score_algorithm.h | 12 +- .../score_algorithm/trajectory_generator.h | 2 +- .../score_algorithm/src/score_algorithm.cpp | 2 +- .../Libraries/mkt_algorithm/CMakeLists.txt | 3 +- .../include/mkt_algorithm/bicycle/bicycle.h | 10 +- .../mkt_algorithm/bicycle/go_straight.h | 8 +- .../mkt_algorithm/bicycle/goal_checker.h | 2 +- .../include/mkt_algorithm/bicycle/pta.h | 2 +- .../mkt_algorithm/bicycle/rotate_to_goal.h | 8 +- .../mkt_algorithm/diff/diff_go_straight.h | 2 +- .../diff/diff_predictive_trajectory.h | 7 +- .../mkt_algorithm/diff/diff_rotate_to_goal.h | 2 +- .../src/bicycle/bicycel_go_straight.cpp | 10 +- .../src/bicycle/bicycle_pure_pursuit.cpp | 34 +- .../src/bicycle/bicycle_rotate_to_goal.cpp | 14 +- .../src/diff/diff_go_straight.cpp | 2 +- .../src/diff/diff_predictive_trajectory.cpp | 125 +-- .../src/diff/diff_rotate_to_goal.cpp | 6 +- .../mkt_msgs/include/mkt_msgs/Trajectory2D.h | 2 +- .../Libraries/mkt_plugins/CMakeLists.txt | 120 +++ .../mkt_plugins/doc/VelocitySpace.png | Bin 0 -> 34570 bytes .../Libraries/mkt_plugins/doc/lim_pv.png | Bin 0 -> 29306 bytes .../Libraries/mkt_plugins/doc/std_pv.png | Bin 0 -> 31906 bytes .../include/mkt_plugins/equation_line.h | 74 ++ .../include/mkt_plugins/goal_checker.h | 57 ++ .../mkt_plugins/kinematic_parameters.h | 300 +++++++ .../mkt_plugins/limited_accel_generator.h | 42 + .../mkt_plugins/one_d_velocity_iterator.h | 221 +++++ .../include/mkt_plugins/simple_goal_checker.h | 77 ++ .../mkt_plugins/standard_traj_generator.h | 189 +++++ .../include/mkt_plugins/velocity_iterator.h | 61 ++ .../include/mkt_plugins/xy_theta_iterator.h | 79 ++ .../mkt_plugins/src/equation_line.cpp | 118 +++ .../mkt_plugins/src/goal_checker.cpp | 106 +++ .../mkt_plugins/src/kinematic_parameters.cpp | 178 ++++ .../src/limited_accel_generator.cpp | 47 ++ .../mkt_plugins/src/simple_goal_checker.cpp | 76 ++ .../src/standard_traj_generator.cpp | 264 ++++++ .../mkt_plugins/src/xy_theta_iterator.cpp | 73 ++ .../Packages/global_planners/dock_planner | 1 + .../two_points_planner/CMakeLists.txt | 2 +- .../src/two_points_planner.cpp | 31 +- .../pnkx_local_planner/CMakeLists.txt | 134 +++ .../pnkx_docking_local_planner.h | 181 ++++ .../pnkx_go_straight_local_planner.h | 86 ++ .../pnkx_local_planner/pnkx_local_planner.h | 200 +++++ .../pnkx_rotate_local_planner.h | 85 ++ .../include/pnkx_local_planner/transforms.h | 50 ++ .../src/pnkx_docking_local_planner.cpp | 772 ++++++++++++++++++ .../src/pnkx_go_straight_local_planner.cpp | 238 ++++++ .../src/pnkx_local_planner.cpp | 444 ++++++++++ .../src/pnkx_rotate_local_planner.cpp | 234 ++++++ .../pnkx_local_planner/src/transforms.cpp | 206 +++++ src/Libraries/common_msgs | 2 +- src/Libraries/costmap_2d | 2 +- src/Libraries/data_convert | 2 +- src/Libraries/nav_2d_utils/CMakeLists.txt | 18 +- src/Libraries/nav_2d_utils/doc/Conversions.md | 4 +- .../include/nav_2d_utils/parameters.h | 20 +- .../include/nav_2d_utils/plugin_mux.h | 4 +- .../nav_2d_utils/src/conversions.cpp | 4 +- .../nav_2d_utils/src/nav_2d_utils/__init__.py | 0 .../src/nav_2d_utils/conversions.py | 131 --- src/Libraries/nav_2d_utils/src/polygons.cpp | 1 + .../nav_2d_utils/test/param_tests.cpp | 2 +- src/Libraries/robot_cpp | 2 +- src/Libraries/tf3/CHANGELOG.rst | 2 +- src/Libraries/tf3/include/tf3/buffer_core.h | 4 +- src/Libraries/tf3/include/tf3/compat.h | 4 +- src/Libraries/tf3/include/tf3/exceptions.h | 2 +- src/Libraries/tf3/src/buffer_core.cpp | 8 +- src/Libraries/tf3/test/cache_unittest.cpp | 52 +- src/Libraries/tf3/test/simple_tf3_core.cpp | 58 +- src/Libraries/tf3/test/speed_test.cpp | 92 +-- src/Libraries/tf3/test/static_cache_test.cpp | 10 +- .../tf3/test/test_transform_datatypes.cpp | 14 +- .../include/nav_core/base_local_planner.h | 2 +- .../Cores/nav_core2/CMakeLists.txt | 3 +- src/Navigations/Cores/nav_core2/README.md | 14 +- .../nav_core2/include/nav_core2/exceptions.h | 2 +- .../Cores/nav_core_adapter/CMakeLists.txt | 28 + .../Cores/nav_core_adapter/README.md | 6 +- .../nav_core_adapter/costmap_adapter.h | 8 +- .../nav_core_adapter/local_planner_adapter.h | 3 +- .../src/local_planner_adapter.cpp | 55 +- .../nav_core_adapter/test/unload_test.cpp | 12 +- src/Navigations/Libraries/nav_grid/README.md | 2 +- .../Packages/move_base/CMakeLists.txt | 20 +- .../move_base/include/move_base/move_base.h | 12 +- .../Packages/move_base/src/move_base.cpp | 288 +++---- .../Packages/move_base/src/move_base_main.cpp | 69 +- 107 files changed, 5532 insertions(+), 767 deletions(-) delete mode 100644 .vscode/settings.json create mode 100644 src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt create mode 100644 src/Algorithms/Libraries/mkt_plugins/doc/VelocitySpace.png create mode 100644 src/Algorithms/Libraries/mkt_plugins/doc/lim_pv.png create mode 100644 src/Algorithms/Libraries/mkt_plugins/doc/std_pv.png create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp create mode 100644 src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp create mode 160000 src/Algorithms/Packages/global_planners/dock_planner create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp create mode 100644 src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp delete mode 100755 src/Libraries/nav_2d_utils/src/nav_2d_utils/__init__.py delete mode 100755 src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py diff --git a/.gitmodules b/.gitmodules index 81897f5..6ffd934 100644 --- a/.gitmodules +++ b/.gitmodules @@ -26,3 +26,9 @@ [submodule "src/Libraries/robot_cpp"] path = src/Libraries/robot_cpp url = http://git.pnkx/HiepLM/robot_cpp.git +[submodule "src/Algorithms/Packages/global_planners/custom_planner"] + path = src/Algorithms/Packages/global_planners/custom_planner + url = https://git.pnkr.asia/DuongTD/custom_planner.git +[submodule "src/Algorithms/Packages/global_planners/dock_planner"] + path = src/Algorithms/Packages/global_planners/dock_planner + url = https://git.pnkr.asia/DuongTD/dock_planner.git diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 8fba0f3..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "files.associations": { - "stdexcept": "cpp", - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "bit": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "string": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "ostream": "cpp", - "ranges": "cpp", - "sstream": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "variant": "cpp", - "codecvt": "cpp" - } -} \ No newline at end of file diff --git a/CMAKE_BUILD_README.md b/CMAKE_BUILD_README.md index 657a0f7..dcadfe1 100644 --- a/CMAKE_BUILD_README.md +++ b/CMAKE_BUILD_README.md @@ -214,8 +214,7 @@ find_package(navigation_c_api REQUIRED) add_executable(my_app main.cpp) target_link_libraries(my_app PRIVATE - robot::node_handle - robot::console + robot_cpp move_base navigation_c_api ) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c1e008..2668be0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -114,6 +114,10 @@ if (NOT TARGET score_algorithm) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm) endif() +if (NOT TARGET mkt_plugins) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins) +endif() + if (NOT TARGET mkt_algorithm) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm) endif() @@ -122,6 +126,18 @@ if (NOT TARGET two_points_planner) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner) endif() +if (NOT TARGET custom_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner) +endif() + +if (NOT TARGET dock_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner) +endif() + +if (NOT TARGET pnkx_local_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner) +endif() + # 2. Main packages (phụ thuộc vào cores) message(STATUS "[move_base] Shared library configured") diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml index ae6d769..c6dea24 100755 --- a/config/costmap_global_params.yaml +++ b/config/costmap_global_params.yaml @@ -1,13 +1,14 @@ global_costmap: - global_frame: map - update_frequency: 1.0 - publish_frequency: 1.0 - raytrace_range: 2.0 - resolution: 0.05 - z_resolution: 0.2 - rolling_window: false - z_voxels: 10 - inflation: - cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. - inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so + global_frame: map + update_frequency: 1.0 + publish_frequency: 1.0 + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.2 + rolling_window: false + z_voxels: 10 + inflation: + cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. + inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. diff --git a/config/costmap_global_params_plugins_no_virtual_walls.yaml b/config/costmap_global_params_plugins_no_virtual_walls.yaml index 06d0037..d79c6a7 100755 --- a/config/costmap_global_params_plugins_no_virtual_walls.yaml +++ b/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -1,10 +1,10 @@ global_costmap: frame_id: map plugins: - - {name: navigation_map, type: "costmap_2d::StaticLayer" } - - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } + - {name: navigation_map, type: "StaticLayer" } + - {name: virtual_walls_map, type: "StaticLayer" } + - {name: obstacles, type: "VoxelLayer" } + - {name: inflation, type: "InflationLayer" } obstacles: enabled: false footprint_clearing_enabled: false \ No newline at end of file diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index 144ee12..48600c5 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -1,16 +1,17 @@ local_costmap: - global_frame: odom - update_frequency: 6.0 - publish_frequency: 6.0 - rolling_window: true - raytrace_range: 2.0 - resolution: 0.05 - z_resolution: 0.15 - z_voxels: 8 - inflation: - cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. - inflation_radius: 0.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. - width: 8.0 - height: 8.0 - origin_x: 0.0 - origin_y: 0.0 \ No newline at end of file + path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so + global_frame: odom + update_frequency: 6.0 + publish_frequency: 6.0 + rolling_window: true + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.15 + z_voxels: 8 + inflation: + cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. + inflation_radius: 0.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + width: 8.0 + height: 8.0 + origin_x: 0.0 + origin_y: 0.0 \ No newline at end of file diff --git a/config/costmap_local_params_plugins_no_virtual_walls.yaml b/config/costmap_local_params_plugins_no_virtual_walls.yaml index a45b181..84936bb 100755 --- a/config/costmap_local_params_plugins_no_virtual_walls.yaml +++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -1,8 +1,8 @@ local_costmap: - # frame_id: odom - # plugins: - # - {name: obstacles, type: "costmap_2d::VoxelLayer" } - # - {name: inflation, type: "costmap_2d::InflationLayer" } - obstacles: - enabled: true - footprint_clearing_enabled: true + frame_id: odom + plugins: + - {name: obstacles, type: "VoxelLayer" } + - {name: inflation, type: "InflationLayer" } + obstacles: + enabled: true + footprint_clearing_enabled: true diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index f7e6e6d..547ecd0 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -14,33 +14,33 @@ publish_topic: true LocalPlannerAdapter: PNKXLocalPlanner: # Algorithm - trajectory_generator_name: mkt_plugins::LimitedAccelGenerator - algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory - algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + trajectory_generator_name: LimitedAccelGenerator + algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory + algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal # Goal checking - goal_checker_name: mkt_plugins::GoalChecker + goal_checker_name: GoalChecker PNKXDockingLocalPlanner: # Algorithm - trajectory_generator_name: mkt_plugins::LimitedAccelGenerator - algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory - algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + trajectory_generator_name: LimitedAccelGenerator + algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory + algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal # Goal checking - goal_checker_name: mkt_plugins::GoalChecker + goal_checker_name: GoalChecker PNKXGoStraightLocalPlanner: # Algorithm - trajectory_generator_name: mkt_plugins::LimitedAccelGenerator - algorithm_nav_name: mkt_algorithm::diff::GoStraight + trajectory_generator_name: LimitedAccelGenerator + algorithm_nav_name: MKTAlgorithmDiffGoStraight # Goal checking - goal_checker_name: mkt_plugins::GoalChecker + goal_checker_name: GoalChecker PNKXRotateLocalPlanner: # Algorithm - algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal - trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal + trajectory_generator_name: LimitedAccelGenerator # Goal checking - goal_checker_name: mkt_plugins::SimpleGoalChecker + goal_checker_name: SimpleGoalChecker stateful: true LimitedAccelGenerator: diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs index f3975af..8da2079 100644 --- a/examples/CSharpExample.cs +++ b/examples/CSharpExample.cs @@ -159,6 +159,15 @@ namespace NavigationExample [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] public static extern void tf_listener_destroy(IntPtr handle); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf_listener_set_static_transform( + IntPtr tf_handle, + string parent_frame, + string child_frame, + double x, double y, double z, + double qx, double qy, double qz, double qw); + // ============================================================================ // Navigation Interface Methods // ============================================================================ @@ -267,6 +276,26 @@ namespace NavigationExample return; } + // 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)) + { + LogError("Failed to inject static TF map -> odom"); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "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 IntPtr navHandle = NavigationAPI.navigation_create(); if (navHandle == IntPtr.Zero) diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index f3975af..8da2079 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -159,6 +159,15 @@ namespace NavigationExample [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] public static extern void tf_listener_destroy(IntPtr handle); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool tf_listener_set_static_transform( + IntPtr tf_handle, + string parent_frame, + string child_frame, + double x, double y, double z, + double qx, double qy, double qz, double qw); + // ============================================================================ // Navigation Interface Methods // ============================================================================ @@ -267,6 +276,26 @@ namespace NavigationExample return; } + // 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)) + { + LogError("Failed to inject static TF map -> odom"); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "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 IntPtr navHandle = NavigationAPI.navigation_create(); if (navHandle == IntPtr.Zero) diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 4c1e0d17128b5f31995297a3f870f924f65c649a..f06356da27bf2c6e769f68023701b0bffe8007d7 100755 GIT binary patch literal 165560 zcmeFa33wD$);?Z=1R4dqQNSpmjYJKKkc2I9?Eozm2^dL06tNSskZ86^IuKmOU=pQ` z`6fE3I5;zLMjf1S6nAjJuruQpaAREKUTr`kD&T_ozvtdtmD81O7RB%P{hw!wr&4v^ zI(6>3=bn4+TJ#Ox^b31LMfKFw(NjBLi^6^O%M$P?eARDzs5?#bXvx~q`0LhA7QE5c zlkPX=t0iV$HJ79*jTpLZp+7iU#9!;W^|2Y&G#tR^j=203GB!kqMGJU27^R$pIrsL8*6&=y^eCXrZ|EMc}d;PK*@0{?YzPRedlaCP=?G1tX z;Bn(T8mHD{*t#A!9_#wU4U3~&J_;a9O`4|HG<#x>eZ4tm$&&aUQGJr4-O;X7PZ@U0 zfT-U);$2NO9`|DHf^_e3T6Fr>bpxZPr@D`QXx_~=vBMn+weF3cz`1{ka+iDDn!8Rb z9~Lt#`l9F_vsY@V4(;DBX{obZ$1c%49!=}h15lL9F?RAqE!z>D7wuUQbs}U>y`y=e zHqle_;4Z)D&e$CDk}|Ez0Zt&xa>Wa=Tut%hUQ!%cya!giMcPjH8Kc zF&}3k&LW&w;Vi~kf|HI?oMkx6aaQ82!f750%o|)6;;d%$HMqYX=M6X)c?$@j9 zMqD@H+>BF&I`QP+;;;Pk5-t>mU%|OmLe}5c)!iGoZpZmH&UbLWi<6EIaJJ$62;^Kjw5Kh6O-2QvJ4+z-Jylwl|1ewd<9Vfb*| z$1$2@pJt=uL7%~}k+?rg(W4k{>NQ$n3AiTW9D{Q_&a-izi}M1UbWBoLNom?-MNh$1 zSNCb^dJ(Qu)jjd2lPC6!zex%27PmkID!t58bFFoU!sTXffU%uz6_b;7t z^+oqDeARv5@4d$@>zCeH{oRw_H~-H|@BXRy^-)pRUK@A*jo*zOGU>$U-OG=k)o*KQ z%y%i@)?R!~=hxf%|HTnsU3l?t-%sv0XpzTV_@~F_B?tHYu>axVAJ>dN`=s7?`#lfk z6xEdvJ8{ISlIzOO`fh*L6K9V;>hHfhcl@)t1+&iUbG75fp8xss-`dm8HwK@Q@!1t0 zmg&x<<@E)1(=Pk!@srQErD*&mm)2CDRGriL-hYP8z2F&p7oVonZ8y=W)?0}PEE*Wv#-%ovIcx$Wc(GTC5KX%QW z563_l~VEJ?1;-l!c8~PUv;o)88zZkovdRAHDjaGydnc%3JzuuU)MF{`CK}-+k9D zfBXCG#}B^i_ZxPPn>hTf z=@*@{bML16w~UTSzw_^nk8E6faaHYxietX}$EuhA{OFp?dhFe|U_$X*PcGWF@RmL= zjQY<1;a4}UnR-Iw>RF%MKXT%-yQceYZ*DHR@%EK>q@45MydO4}tsMG?{r7&HyyWA( zSG~1q!;Jpd54kaFQ}fxwpE~M^w^zRWc+ZKCmpoRv>61}|zZt#VT|fPvF|`HXf3`j6 z{YTH9dEwA|9=fBp-{1%SbxG^fGnRg{_sQQ(Nt(ES;k~at+WeZM$35%sefOl@JFYrq z&WI&f_qn#9X6CRxH?6tLxPRpRog2%>oc`wGzsyU`$h_d0oYzb4ym?}4RPI~jXPkb` z!kQm;Z+*OHulvs)zVO+FwbTE6){ntW=Y6qqz?9D94~nOJn(@j*&y>!My|u?lpT{nq zxcuVTSH3dmy|1ozRR8Uy?=R~WT=;F)6EW+nfB4epV?}!Vpk=dP`r@%i z@A+=-w0`T;m&fjFdi>r&&U5~@chq;2*U!qnsO0m_FHY&KzUi6oGpD+j%}lEq{y_5W z$)|s|6v!nUOKJ30dYMKDe{J-0;2SH|h# z`C}r;Z;6oK6C&7oeS~reCWlY=Ib=3m`Mwc>|I!HgI>sAbe%VFg?Tw+Dxpb8!TGO9Xq)$OvzrXQ974j33_9H+;I+ zMJUIwBk*@d$X82*`0zl#@aZl&KRmuSLflvy!Tx(O9}un_kLej6A2}&JK0bo~7!hzM zv-H zf}cDWp}u??q23jr6keaR^ze96gnXSGA#U6Q4 z3tN91Bc$tzAb)&>{JtBZTxLWl$Il|z|EcrB=Qk>X{Cz0jaP?(pgnAJb!4CJ$44>}E z2>gR1=szivA5IIe=l2ofWH3Vg@<#B_YRDgkNz1l7DTkVNs`#sE;}WRdIYx7NB>t7c z{|81JrVY}nzm~XL;W}=P1%J~=5}4FOUiXiYaI>^y6=|!2uI>LH%sE4U%vZvPlFpQ`X_pL>cy_zQL5kx&x5R4^K@^cEm z3NI2~(_ac+6(b)!)KlTg4hq&f&y;w^2+42G8#KpC{Q6hr`>U1wBD7OfUQ2BG-GvS! zrThMClCehdf3C{6ZI6_{Lg8Z&2=0abJ!-y};sPCa!O=-hGfl4Gpm_#5)`5ZWw!M-z z>)L7$$r-G9qNRf*$j4ee+AqO>y%cc1l5;caDe3R}P6BSlzi+HcS51nT>E1D2@)v$3 z`C%4u6o~c+<@=tbA5r*F!EJFigkYt)2_DgWznGTnr+QhtKcbGows2AXukVcLHa z9F_dG<4we^>GU z$)WH)66me)0jjHkw4j z@m#bNRFJI7#U?*BUEv+k(QcaGcIK!ZY$D|L^2Q>p4-{ zr&-lI(>||Gm-1Wgl8iKNq+<}uk>Yvt4oQEkL07(gDK~Gx8(0IB>#BDKR!*S zyGo@Cw-m?uUa6;Rv1F`M?Zuf%64z*Q6GxVkvkLwo@|z?9nh&KTWtdF2clo#!BRlm$hbGWNG9#;O=Ve_}BRB6w0n>}Z$cD5}; z3ZAayKdtKN7FAEpdi|yHw+`iR82*T3g3?nvNeVFSvn@rY+hMC;uPJ*boGkgx_UPe( zGQTbRq+Ey<$A_xE@3YybHyoSnBGku|Rs1YhtN}fXfyB5*&u?w$kQ5UkN#b zv@JF}L^~ya!d@xBtQWZ|p14&!IbF%GH_K(BOgB~O?;0TGXv+V=CXPo{+&=adNk69W zd}W`e1S!C@&)W)j4VSnXXEM{I|2Ny};~W*wYg9ZRujDUP{=YI&%2CN^Z^BQ>K5k_n zGyWH5C_AhCqFE6~E!q*{Z>y4WUQmAib|&!;(h_X_flCHT{u*V6WF=>>@)LW#o{8r| z{@0T6D8-+v%4>zKyrw{5;!jv2^|@VMv}&Ob#x;MCfLTwUMf*$qSH3O<->l>Z6D00Z z1!&fFyfjr$QIAplePIyNCqu1;L@N-IcrLA9gZG?JPda9IfucxmaE%7Fsp2LM74$@jx zf8O+;d#6#lF3qL<{3NNa_80;J*~eb*o>%p{T-9s1fjI8CNXl`mc!FkK9F?XVwWw?M zuO5IsNdC%iB;PE>Ul^f&4R=U>mn~mYm7Ux6hssGCFZnxE$LG(={@=jP!Vhin96!TL+SsZ(kG)+0%p0t1O`gCWtdbXRq1m-8UiYJkF6hj zuCV7It;SYQPf&W+*v4h&43cu1RfCV>7RQq)Q0jlSzA0(5-FXQ7q>oGW+eavV;}Thp zZK_>PQ1}q#ht0P5f1-#FgS1$ieSSAs%I{DHNK_5jBW~4Re=Y^1`4dO8YIj_*(lFO5 z{D-5YoN{Gnvz`{9p504!`$6)X_2>uX&mQH^2>ar=P~aH<+U%C6>PwAH{vu>t*oQV1 z;84kEx4NbNH8%ZMgNOLV<_+A#J;d=20x^}ZOO=ZmPgX#GAz#_EpW;79jo+KAWx8j> z?sN=S`Ax9pcY-J{%p=+SXNd4ym*!S+J6(#=c8*r*s)>QG6h10KJsK0KzMLuLw4N#T zH0OmL9xw5xSrRwv-5ZDlWQPuA2QzN(NRs^7Y6C{5d`%lA+9`}XR6I#kdS0da&uzB; zHp7E*^V^X z`m4{Wa<5V4ez}sLtLkI3s*lqZeiihja*6#`3ic>`*Ev%EYhIH;uEIaMK;rI7$^W?0 z^MMSRuQt`sIbZQV2osPU+{!*6#qmeC%-2d=J?g+y;&-Wb0ofEsACx=c9@=b!!yG?; zhl)n}&sO>;h^N>i=9T=-wUYm4)qjZdNV~<`?Dp|2DW`RhrF$g=68}oYAFKHHA?}g>`&7Gcw#&J2B*K@exN7FB zR<$D;gQfgbrO)yUm7cc#+ti^7KUy+Q>m?K3d#sf2u*HqfCQ3Zs=I3X^VT7F(ZrX5PzHM*R4=^yNWZ}n$%~E(*O7He=+a_3s}cKI}pR zN^;88hBtHkb(-oQ*1RXvLwFZQZ-u+nMB`=34#y}zoNa3_n#M`_EwniehdFOqjq;>) z?enfwO!rcpxkvJ=W=wk)^_t2hLG^FVeBG(yaKd!SuZCgTr|?fvF6|;&YmeZW=%14u zL-~(s|Fv$(?=mERzPxBVRX-%-KN8re;)dq)%`Yh{^;PCdmonl`DptTaC>XI^o>d=~zRFSk0!S5Q=%Q(SZn7%!hbZeCegrGI>0zCWj^IB{HM zxi7b@s??vK=bM*PnLi;fztUe(w#d7Hc&94n%0)=BM7@)gG(W#IzoIDDmtRp)R^iLd z@#oJkt5`I)G^Zs0K<_0=F^BqE;@D96i3$0?sVZ8KQ=DJw_ZH2VIz2PNmpD%Gl$GWc z`HRX*z1`J5pd2M8!4#91?=3pGi17umU4C9TR?|q~Gn+IfFTWtCs@QKD!P1*jwF)Q1 z-ihOjDt)HOk^L&D=p6%q^bA)(i3unYUtw7>yd`acH))*tvRP8TqEdKOX>Pu-)}e?P ztBNkXgs^Usm{jS{J7nhw<(^QIUs;(mKY!x5-0JGYM6xs0gWRiqxrJBz$oQ;`636FP z=jNAF>7#y-S0^d2C@L)|gLjt#gTwlYO3I5T;DayZB$GJ$F0`r@z?ZVFW4- zGyO?pe3@O*yW9^CFD)<5$<0rjKE_`#CNW`RRY5_1#iX)|d~aq>T4tipn?5}S%}Qw% zT7~MwBwuxmyLwOUpnN z8k1LCoH(|!FsA}-UvbgAikyl?UeTzG!5bx&2qwk(Xlc_j$B^eGPMDwX_rVPj64Vp# zjF4Es;_(qqI)#!(BaNt&XC)pct4B0eBqma-hnm$RoAl4H^^sNJ*SEMW@ceIC+n((I zs-^9TA8{*p50}4^UBe{wYg#f)a*=KKD}-+uW7*IkuQ-E`5Pne?YMkh}h|pcSumn*w zYnJNQ{vxrwbF1R<5seX(6NJiS2~yhZiXU!L-HTQU{<#9X^#g6SE>!$4GO;P@SMWGf z^sm#FAGm4D%Cy0S!rgh ziYS3bZ5WY-(^5z{xDE%A@Jlq9EIQ`GLc(6kS_KvPA(on1y0EA;&o?8#vZ^GXrlzW^ zedYNTm1Q)sI^9e`Ik+Zf-hB>Nm`SdR;zVCaO&0g^LY z^w74&o^og-fYzimw|tQbO7k%HT}o4mxrGYJM@*75I59CStIq_A_?S0LOhWL+JT;yb zmCjGgMa4?Y#dzQ6ugEF#SEl9Cv`DVkn}TLFw;~^ngLg&=_2op%=o9e}BmVj3RCmVI z-0m)V$bHK09lZk-t}S*b7dx=&4m`P>wIIP)Iwr|yw(W-!qz~AtUjB0=hD6Pfuy5)W zHZG1`SeWCNlRKDlv4uzWFw=jyWiLiH75QS`%v(}|A{L$b!xuzOlv7>~kHi$Q5P}|H z0(x823-Q4>=lcs~I9G0@3bT7T9ANqy)Bb+UI*HCif;9>46E#^)W{% zrh8rPwi;G}DL|UWWpiybZD>Zql+f8jG54OFm!Df!Az}&2$(NW=fTbcC7RQ$7_zOi@ znPqQ(m`u#w*3r7jNK7optYxX+Cj>6g7Uou};AgIA*b<$PTL!fjs4CQ*i3GwuWn2Vv z>4N+UKm2$h=00bjLvpyaO3aL!iKZ>gO-%IpO6Fk+2mNn9_g1?B@@CPiiCFHWeo%yD z7uo~)LJH&yy9M%vCHW)v9hwN1;jTu;Q(5QaE{hl~hIwDJ^rX z?`H`dPo-8>d1#uI<}C2#A`BM!!~jAzyc*D=`8i^NPPssts-HAm%&RKr9AyOsm6&&) zUxs=1q`XK^Ag2VA;ry{|!O7SuC1nfp#d3D?G~7)@4W2}yj_IM6xixM``NAU{8E9+r-In!-7)2k;m(9(NnVAD}HxO3EwBnO|B~NzOrQ!4)OK$Eo?3 z;VZeQtWvn)B)LeOH496}vP+65;rgQ#8nd!^?oHW~6`}Q@iHt2O zEkblh6dh-EkRf(nRRJVpm5`K}^~;o?tcH``tp~0&9xoQwWHq&?-lZfgAeHPUrYrkM zcf8L>_Cl6urvqBDa0c_v0OuH)f5n}a<;zO&c{9iRu*g5gCpH;GY=k0K!?BeMbIKuo ztZKwm*32V06OUmJS8Ow^PcUs0&a{w&+I`(m$H`9-HsHh^1Qb*1bs!EG~QJtQPh_k>@Zaig+B+ zK!+*J>}N%kXLp3qy{O${xjl>?R6aAl*%^tb3+FwEqa-EdmX$B^iH+!8%duXXo?wfs z=m5)kpoq;|cZWMn1ADz}?+FGxwuxm7+t692Zv25fq?^kgwh{+!#JlIxR_vjtwdRXa2_V1tP3z6j=G3%O19AZ90jq|IN`ma;DLKFG@m!uGbrO z{rYhCKXyx`TQm-zQ1{+nlGuT(X6w6FLCfY*KQPh;Ec7{StD<7DZ^|rl&FEK5$D5Q- zTLu5uQWCqo=2zuZVEmekZ|N%XdDrHbyGNLkL)S*`zVqi-7x@(-RZtTUAz045vI>(+ zZQ!FpcccdN5K27MWq+O0K6n~OqNEQw6|1xQ%S$vgP3&()VPyKpFHi%kMC?Z>E6Mj) zEW-CP^DD<^loer7It`QY3uuZ9-yG4WGl>&0{fgOm@eP-nkiQ5WYw=ml0{%r3=A%n# z8Ku})gb#0WXz-Q+w#;#mGP(TXEFWLQp+zORHdIhn<}b(c2s|gUuOud7VI3dKV%Xs? zJV4h?F8_w~@)CSW<1r~ui;CgKmsviqTo3lraAADtc;V1m7-3lD<>Taz5i5WlNY@23 zrot!~pICF1hn*f@6USO~IVzycrC5ZtM$UYF+gjiXH(r8&Sj!5Bxc^GIRd;DBA!V3E zwx^kl67~ZwI2KiM`@RA%*6I_xE#SjR!E9C&+Wk>f01x#0aw;lv7GXI^6)gE)LWuLx z3R;o+rrIZbuYkTy#gac(@?|aQnn(UWy*7ezp4?O}YbHK3lj|~NSn`+7jB&A*7c7#S z>r)}HZF$P#Ka}N^|99&`SiYF1mIzh8#cGfj0VfUFn_#X4bcG{YXqxAxNJP5?lTx}k z#6qJl2OAPA^5^9g(;E0ZT5YmS0mo8HcvvKuA*L0~nR~U%!*s-u9i0c>)z{TevB?-;03Y8aV(TcS!)_yqkA1oUZpenknsd-vz_@4IYH5#pfOv)A?i zGN1v(%B=D+_`CSe6tI9vdyK8Ju$+&Qu$ry|r9mwO7?mKq52Fj2w+OrDun!|Qr&9C+ z)GXM6WQORC|4#ynt1Y_N|0e%838acNtr~1KvSCJm|62=!2MpKdmV^IW8q?kb+KHnY zm_q6!vQ|bc_L?j5Fg6<$#LPBZRE{PD>im+605eE%?BX;Qj4lxFOO4;qP zE#e|@^0(9?ZiJ1v5ocfzu$hR@hMM04na#l=kJrPfcpyV09_I9noDP>h7Rk7WEi;k& z%u;4VyLF!dKTORrgTOuq>k2vD$M#1cDG{q^qVO;`+yAmWQJgZjn~hCF?^Nt~l<_MB zS2M=cUeSJtv03=cC^;)_n%6fibv8Z_O!8Czb&BbP-Ay!Rf;BV|J8cd);Sw@{Ocej& z%#jk^ZMH#*KVY^Z28F}QRD_g|jPNiP6Q61(`leN35=HC*JIE#*lwTJ0Uk~$@S<|sQ zhrXQ2nOC$R5xrkuZgEa!rO#^GglE%dqldfe$5iO&ycen!eNj0rwbvuJ0O!KDZ<7* zu9)gibsI$xk4=tBGt&PS7OJ02`cPhpkV^PZlMj^00p19oM(BT0B}Cg8LqFOImv}f1 z)ThwTJ-IQ%ctW>Uo)^%e@|&7>=RZIvB_v0O|E85DtBy2}b6Ef1R;UiYSdyhvm@Ylr zd2$dF^O#MnwwQ&FUlx>v4_XM*UHi!nQt=O9XjWhZB&d|6HSLDCFeow4u738@6# z9m0%n2iOpFFbRj-DLRNvevY^UxyUbGzYop-p-Dq3H7+{i;f`y;Mb9U!LE8DtUG$ghA1MWU9*)rW=wY=#lCsgg{5AG|Q-AnZ zkSo?U%d0Bz)oD?Mzbc158R=SnJ3xoZ9A1S^P#v(d6ycS~{g~;P(hB!+zu1G{{eRfR zK62g|v9GFWYAm$X=z55+l_K?^yN7??PRfdKIB$1%b+Y#1NI2Z^Z`1LR!!yRkh2pc$ ztMeDB|5-pYp%O8Qt16{W1#O^!kR|FAbT!cUuL;G{BaIS9RhDU><<7~jZ+rodN*pyI zaa005%_}B4iEc5#SFd0#Aot8O>A`q+f(x{|Slqbn6PF(9|R{D>$grO|)Lb40{d zmlRi&lWz1se)rINYSH+oX)#(aaTg<~-k^GEy@6^1kI@{UqH*>Sf1~lFH!ynVDE!qx z(Y=O~B+z?DtNUpD>PBy^pP+h)r&2DZOYhKchma)sZg0U$9R0+TV{k{(NWMey5Jd;Q zO{qoWf35%TzyDewoCSoV2=ACPTciJZUvuNkIBgMrVdfaE78o7$<^=Y<>ck@aa*0E0 zu%7qU($(`C@CR-ff&VGLpLUnR7Hxe5@{ZQ-S9H$4yYRe^_L!nyd*o_3ac^y{qJJZv z$7nAp+CSoUICr%6j-rdi^Pbx0py`Ohvm*73=mAAV_-UHH+8%{1z3om^Ap9uLWipRa z|6W3W%Cr9j%1;mNXgsH*Q2l-p)hIez-jvsJ8{YA{#H($1jpC10zgJ+&Y2GJ)r@(E) zI~1O6`+XRbu2J&o?IAeL-?1=%56zC7zej4v&ELDTE5ZWB5vjo4-*)ue31yEmH`ttqcz^yp7@J zZ*b7#4u-$T_%&8u*-Re?!(V0m?g)4*vm57+W&FQmauOJx&+uf1Co_ECPf|H*O*yVJ z{vJ&K42I`0IoS+%GyZahf6Vx+8NP$zOBjAX!BPDUdHeY&}JNHjewiKlS3+U ze)D(R?6~=RTz1_2y<$7w%+h_A6jaAbhX2t7aWx{~&Fc49NIvJUVgA7J7A6Nj4PzbK z8UC<^;^zPv9?x(WOZPK|yBXe(lu}2N`n@KCIKDChp1|b1Z3@A)Fao|k0^ZE*z~$(S z-^=uz&G0COZ)bXPd|w2-hRG>r`ZqB=g~{Kie*cFcj;~_;o0*&~3|Bt^E1ou~-(Mm* z9A6ay?_hFbxjqd4GfTIF$>DgsYKKV#$Fn2g%Oc=L1bpRwsT{>0F5kh*SN$}d(ALH9 zJDC3Q5pYige0Buf&2SI1TQbAdPg{x9GZ~o1XrG$#*b0w=g^-0=_H)KAYu>%ZX+DFEIIThLaej-t(!rGVjS-K8}Kf`br!~ezb6)bLZ z+_6t8L4P^EG6FtZjlb;tZkBE>OE;e3RSZvHcmu3%j-O(*jvDnlilidP+Zg|!EN=8~8NQI=8k3`@ z48&6h!yjS%E{0#s@MPxy9FJxExllG%z2mmwOBCM3>`eUTk<9onWq4r(d_@GjjrlW| z<6->0nEZIg&vBjce`qR+>k`J#@eIaa!1!k~`~!xUGu-?wV0v85@cvBCl??A-a#k_C zn&B-Be}my$82$vqTN!>WaI+k@+i+2OdpPGO#bzZ-^1i{+`;%qGW}f)zntOm3@>H!6BwSw_zM~S65}stxM^d0T+Q%& zCZ~qs=P-O3!yjVun;Cu+!&?}BGsCwqyoAYbW%wN6mY>*gGYe{&`H#WQHd(ypZ9;8E($Y z62$RUEM1P5Gda7M{`(@}EldvQuV(zOFgZ00{|CceOinSBHT@yphMWH2VfB>w&BMd^ z%NSlA0bdybZ(;t!<>-vx{LN)byO8O_@eIa)tEnWeH4HCe_!5Szp9B_9w=n!Y#^1{D zRSa)q_%X~52E)})0*kjg82%KKvyb7;4A<1}W|Qc4hU*dVrU>}<2)K*cpUZbJ{*zd~ zTns;j;js+IB8zpzGyFt`Coud3h9@(8k0nLZJPcPq^(~%eFnk=7Gn?UinVf8ff5-50 zh8qm8X82l$*D(AZhA&}w0@G(1!}~D2nc@G#9tsfU(MvW8NQz3@eGe< z@)H<-8gQ3-$HVYKhA(5|nv0p7W`@%$y?Lx)_=6^h>lTJ5GC8dbKbhg%8NQOqX=C_U z#&0nE7{=eh@Fs@uWB9iW*H|35li{%p-^6e?!E(c7~TTd<4U*8Ga_iYZ!h5!U9`vFMwT*iG&jS)5*+eRJj3Oi5>H@w zPsX3jaBSGH4iCd=ZNofthLcX_k->0>3F11N;e8mM&2a4Eu#Q58A8nzUR?hH^46kN* zUxwE({85H4VR%1=H!lAII=j3?IPo7KRUG_!fr$ zp5d(w-^uXp3?IbsHijS1aD(9|Fua4|Co+5=!v`~5i|X>*p$vC0{3M3E7=ALtV;Syd zxSQdVSjC8E_%Ozw!0@XWp3Lx5817;CP$pkz_;ALb!SFbS&t~|k49{lxX$&u9_~{HU zXZRTmuV(m2hSxCsEQT*(_$Y=qF?=+`moYqn;mr)+#_$ykPh$8=hL2(RDu$0`cnibF zF?7U z`x)NC@aYWS!tlQ`yp`d9V)%B3&tP~P!_!#442EYh{tkxEWcWUY&tkZy`ZLr=7|i55 z7=9_^cQJf6!($nK8N=NSKaa_eXZRe(pTO`Z8J^5=AHzKi&t|yJ@b?*>!SGy$&t~{1 z4FB)#e=YD|3;fpt|Fyt>E%1NA0$)2%`d+Wy;m{jm9=#w+)9YLOJvv+U+6|8NLcPv$ z9|7LodFscwI8WAyzmT5n4u06#+1Vs$YKM0Rw^}rHfp-VjT69lAKWfp@g1*2@X0P=6qG!cOOTQp4oApaIk1OMH@M=hEL{JVqq zS~LyxcL#5?Xd191{}xRHb>!coV+EaK(KKL3{wd3!E(*Pa$x9A~)9%j)rP)GhP zng-~|zeUr)9QogG=HD&o4=s9_pto8y4XBZSiykiMM=d%|(Dzz24Umz4i>84w@^8^J zAV&TzI$qE@7EKe%$iGF?KpFYBXqs3?{wJ(4tcXz15;=po#oj z^dvz)YSA=MME)(B28hVNMbp3#`L}2q_#yumO#?gBe~YF89rAC{G%!Q{Et)1)QT`U4 zF6dzvO#?IJ-=b;Yh5TFe#e)9odo%wTg8tB=FA?-si>3h=@^8_Zf_~JZX@G_NTQm)< zkbjG&0TuFZ(KKK}{w3w$ z`8R2_PD3Y9>jb}|_6YQODWUW@8-13IKGjAKwb27@bUzy%W25&cho=9njoxXaKef^C z+2}WH^vgDSqm6#vMn7qz|7oM|x6yyJ(RbMBMjKskqp!EoRW`cBM(5k;xi)&Hjh<$s zy*Bzh8$HfOpJk&@wb4Uu^gtWk&ql}C=>0Z-{MJVAw9%j1==W^&8#el78@cIuD8+G+vqAAU1Fp2ZS-6lJ<~={v(a7~eV&aTXQR)u(Wlzz zp*DJ;jqYcoV{G((%wh6)^sSBFX`?^2(eK&lH*ECFHhQCte%?kuX`}yXqwlxTf3(qe z*yu(ZU2mhWx6xHLy2M83+vvGAdZvw@W~03}`aByw&PJbQqffQbLv8dx8{N-F$Jpro z*z07Ke`?muOg-?!3)F2h=Ac#88?JTefsGh*HDb?+9=P72*S5IyfHwiqEd)st!?Dh) zjD|^Wr)w%6@6sE_Wnt4&!0QIzHQ;mhpXSmVGh+3?OigcWT7m@ds$QTwOSjRV7QJEM zJn$7bi`O~(d*h9M=+o;By*o2v+gDIinHJapxyDmaDn0PIaXuas9HQ}qXlJ`Leg_KJ zG~P~7M2)YpG^410nYv>iSNxRjil2Ny z#e<_vQ#-+TD5l=a75@vm&tVjw3Tu+7U&G@_icbhpya-Zl7B3(uqQxC}Dt+W(+#QPI zCn2kr#e?YghEe=>C`F2&DDrh(oGU%$bAOC+4tWP$s3%}NUb8P6PF^6u4~*%1J3GCJ zpBM`uGMI>Aw%%}ET!xnzyVx)-M zT)G`4ZfDc&aC&39eB&RO05Fav;5-Q&g&6>miZsh+*aEgD%kKCVT*ree_zZZA_lR`J zmzHCifJ)|WaFc=7(z6|~I@CS^Qv~#EsC?G`5lMF;+`#BX0^4QWM*Po#Kln6pH^eN% z2d(LWy{=!LKo1zf z3``@Ll1i1NH*uAfKP~X%3*}H{CMMxx^Yn)3Q{zc7SkRH!VvK^y>4DEkYv@r!8E6>z zD2f3sK@Ykhsk4lbSPg%n%qRt}QVQ%=Fxrv~gcH4Ib(^V(F^j7a zp!ATCjAw<$gLnw-E|O{-eGoM&O*N82)%aK>@wss|;DS-klj~qsO%oQGsW+ZikBA48 zySkaYft;-2yw@r9#)0P%TjRJ(aWPVbdCsDIJD>7ubuIq!P;)RCYmzl*-OW^P4KeHS z6(wl@`FaD&Fi#&NOVnAvl%CgaaLrv$r8WR+VXk!z-WY~ADF2%^cuJ@7_HOvz1Dmuc zz2U4YW#)}NUy)K=UX>0d_tKjXsRzD^@ z->3v%Fr5z1Br0&#nWzNk&7iMe z^}wh8zR2Jj;Km6ZP%;miZi;ysbktnD8}fc|As#AyL`_2&>m9ru87yd8U(i+f&XYZ2 zegNg398&J_i7mCA{hT*DF>wtOWv|Bnd(^4tMn9uRwVqE|_GtR5N$K9y8EMm}Al$`5 z!O3whfj1&<8U-R0q{qdhy!(n8?5sB^AGI5jp)K;e%8NgUn68Y}nVAK6djRAN(%N58 zdZ(^R!>p5F@J^pxFxljw#v%|pmVYxhMt>mS1_&ZlN-OR~^s3G61e*#*vBEqdcaq8Z*4-2?H6x?9kk zniz7MYP#xyhvI7J%35Ws?FQD0XLLA6)WtQc>q>QP!8JW)ud{9{k~hxZ)!B)#RIb-P zC)@Y*1}S>C)3sE*i<|r6R^X*TPu$)bM{@9nAH+j(t3cr9p12ly^F-Vh+~|Qx4xmyG z@Ky&Vxtw)Qq=%Mux3mA`xMckG$0gwJ6>;(STO8-cUz8jE=Eb@2H$Bc_Y(>z79Ngls zfL(&c1$LwXW|w%~B}BO(w!k^yb(}Bbd>=w4WjOn1x>KJ+%IHN;O>p*~<4H@|=|6^A z_(>?5_UI<`!Sn`MtSr_)O4(Svp$(VGfwnnwQ?E#!n|dY1Eksdg{awIOWI?^lA2;^| zhQ86RmbAc5ee)!TB;z{@oblUn8k;A%6x0e5Q8a!F*wDq(wBY2fdI{BZ_>rEt&A1Id zNnVj5Iz4bABH$_?QTQLbQ?@8*MzZ2Ef@{c$HDn2WvG#Xa%oM#8>CLq}s+lWKjw>WT zhVN1bOb>i3e0nGD8+xZVl*Lk3(i`*8OAo;I82$#LEm$ z8D7-@I-DaW$0g7o5B*1v)9C0Apo0 z{$wWL&m7NYgfko{O{5^I$7Zjv?PjmAr@q-Mw81rl7@Z^1De){y{E9d@fY*%^Mep^< z@y^s8ot>-5BiegW{D%!-z(SP$01@}A1|jBG%Mz#ZMmIf3ZI|&^yeAr`z{cS7XyRFW zm74VejhZrP9oG1Pav0c*3U9cmjA}QaL)kVb^@{cK?ajdl@V2~PAG`(kth5S5|AxA! z7zy|8%yxut|L}YY0+b%upPAk;-!ZNJ6aP@XVQ4n~11!D%9e?gBDBRhR>RgM`K&l9izK~G0#(Eyx{utRU6&2gjn9-Sv zaO-IQgC&pfz6$T#zCre2FHIE@H7)QuCQ=sfaKQYcKvA(#)2Y78x_^a;19>VYP<-&m z@s_?WZXZ;Z;dkjeWZSv4rO!HY;-xJZ2s+j|>tBLwz4ih)^be_lH=T7<`OS!$7Wg*3 z@i`H~$YK4I{{zL+^ne`QQkAI{N7Y(>jiZ5(Zow=WgpmMXy>R017Z)sWElQ8Xw8<1+{+ zCQ3Cu@S(-jOiZgq`MIu(b5Q-vNN+eZaU1Fis-@m|1r6K~J|M(7;yE)eUl*55Z9yyE z7L4(F0Cj_Gpkvf5+KDz?VejYSI&fnQg0oY-&xpVP35bp`sm18wzEH) zD8!#l9@hx!---}5n_4b}DZ>>cDQLk`1DoVP{uPmVK4$NQ`O~Dqb)j&6;_C5$;(HBiL%)mq0Kw9UX^1Xl6p9 zW3`NqOV?6dT)NJ&6;?%75kgg@aMpiDIxpTpQdGbwXlm${-cX=|XK`FYdO$Q*`_U~3 zpr@lZTqpu)JYJz#IiTS}3ZMxhfR-yTwkvkhT{czkA>87%$iAD;Y6hCu4dV?2Pl#M7tXt@ZD0HWh$d>oM%m))7+rebnqcEwqDJVM8k=s06I zT+sPcbX>3(MZfz&nK8?Qcx~Q~#XYmGj9;m$zSFGfYpCyAkTF;o0YliCkJ;)(AHuBB z2tD>1?H@%RunH0^4{+8$M3s8+29g`<1+ah5^oB~y{^exS*<2tQ=50%AUWAZ1mEV9!BUzug$CeKC7Ma|b7 z6#eJaBb3D;iioPGC=jZCXrz|L)j(u=Lv_4!#62pR`{J5`!A>&H81H_Jbf6KYh$mxO z1%1ece(FC~U76~9YCs%1j~+enHa$eA3dM|ubq^eXsvu~XTyI4EOznJ&N;tu)h+c9f zAT(R%th*ci7$XT#@OPp|L;d>N>(ImVUD_IEuBV$65F5gJqU$kjor zDuPjHRWw)*-bEFqi$J~EZpSJmH<^_6O>EI#Vbpeutir22}g ziY11SstRKm3*jWrr*IdHrpmexq*+1tf6;aSmAbdbzyH73v$d_eJs*O%{UUoNhq34B zC^%-%6(CJ}wyI_Z6Q02h<~{mj$5ZD|%({E1%ZIL`>R-thHwT{qGwP8bL-_UZ{C7m+ z&j`<7&iHG0U?PJ}MaGJ@%s&SDVQ91#(ItzTiO-xPqT@O20GoryAstpjMSc!1z3hXgH-V-11O?Tu{s5&X#^$vfYC`g8w%6zmqP<3U z@DsL&`Tk-NZaIy1_V_d&K&91O2<&sh((`=UFAmdEAne>5rU zYJbBhQ2L<72*!5PM|R*xln zi7*zs#;*@=4Xx>&t-ZLG8ZWrp5Ru}=^DW}lw&R@KMT zFtvVcIxX_RtVZx1awyzUxwYtxW8?KkOiCo~M7gz)&$-iS21LhjP|TIM+Ry9K9|JD* zn&1scX(LwU)+4nHq$c`#8AuW_19Mmlrceqf6i9>nqoTPm$%eiH4VtMpma?hp33>~Y z51mHu_o0yzhnE*Jd!?ZR6!;0yPemTbnf>Kxurd1^dw`Cs9ecmKyM z?~TEFbTwFwG5i0k-$MytCjLwC%dtOAR*2O()T6JlA~W(nFf867JN8ynCCAxVIQ#Z3 zq*uRFItC32;1ACFr!jfD`eP~j_Ye&^V<9I|uKO7+c!?%oJ)+;8xQ%=vp-`}k8QDEF zfJLnL7~`o{QumbX=}E`(%WCn9vG_S38u9ji7_tL;HkFf`2D%xrB5E8x`{AE>M#bUm z?{N|EJpkOJ7|LRPlK1HIm&Npe5a1rV?!$S-SwHD7D2(UewKOMLw;oPSIYKq6Z^eCjLos#1 zF=E;2AKZ~jae&HKtjr$WQCsGSa@LK8JqZ-+v;Fh{hJ7PZxOxN41$B0|uBPcg6yxbg z3ATRf-Y#2by-9pz9@1#M5Y!u$JCQ%D$Q31_4=r{l0Fv=N!Y5?dMmcGP+tQ5d9j zZquP5GPKh_6|wC;aM19t?i}#Q*(c{(FU?+6jif#?1uHQ(<*cJ0w6Hj6cFI}T3lOHD z5bvbZl4#^~r(cKYG*fgda$~(HL}Q!~BKl6AxD>sdG(TCGu`kS6LUwza%z|O*3S3!i zS_xJRGRUVx!xP?L{Q*$5JWXM(ap1OQVbrPjq7-|*fywB9QhS3|;X~yCL-7WA02To; zx`X{!fRzSdOSV&2vJQ_?TZ6+aQat^# zi&+Qr@~)>umUbt*vZmS{>|qd7!P37)L)#b=bB~z1RsEC3nElH+-O!t6UQl>6!QL`C zrKjIPJu}(wnTirfMc1mO>S#S>>m0;Z^b|1nHqjyO!=^vodgpjOMP*`7d~`Rh6rbq8=lr?J;X_{QY(AZzR8V%Iqj8A@WT$YfeRf=aQ8<*$^#HL-B8;5P8e{7ZVBjee|)rtWj_FI_Zs$Ah<9bZVC9%!>j0Z{aE9 zF2?_KtEcSs_eW%|x!??cKjRL>9@LQN^u}Ct#H=M8)xYU_|D17O<9{pnW3(g$4FVA= zD7{M%jjPSIppf#T?Ne{+fqw^muX0zj>c92;g7DALW;`Dgg)K;d{c;$A#iJYCdZQQH z=Qd+Q*zB#aa$wio;H$XlawbbY-k(Vpzy_Y08)z>;Z$J8ikbrp#XYi_4{u1+T{{BF` zZ>Dd`IL4F_W1I#V&|nP>u;&IHP=os~z2HAdl!;0;#`p$V-Gt>)Gj4JJhQ8B~UzOM3 z2{3VpJ@NdkPdl^d#+~RN?19DFpQpH&Iy1eo%q3?) zx^Fm34~!PexD*>`yh@()t+5P}s3lgL5Eh;%dowCJ$vc7tG+s0E*+V;rr zh=g@W#RRDM1(K|Cql+rrBs37RUD_1bK1c7CAyj0&-KgGU-)`K!^6Y@|1qpg|OC%+I|@t~E2>TqPvi zFB9(OMF+U0YKq=a727ZqyUb~9r`kHq)YHfj#kI>AECkR)IIbH31Kspal-6(*Suh`W zC<^qebz@{g#q4Ohm}Jjm60 zLyz>v1+Hn>$IwvdXdht4m$blpdcz#CsC_DiB`|WegF^hJR9_!~s%wpt(V5u}CK6Qj z&a{*-{X@|ocGq05ouN0BJN$X1Rh`9p7R@ZA(yHMc)L&6JVrz(Ok1-?YyuF3qt@>m8 z0|iZxl<;+`vus*CUg;Z0RcMPC0?nB#`WM!|l9X@hz4Ongx(%!8R`*u;>r6El=I`A> z;}8GQbGzDqtH0~Qv%XPPp9nzq<2yRBDeo9)4-at@M{$AHtI|cdIiL1A zrDDCxl|~O!H%0ftV5csQM0SP6VvL_CRc2Y`e=yvp_F+wwwu9E8J~w`YB1scrZIc*y zkX*R*-t@p1RJg~IKOyKkrZtR>YrjR+A9&_hP4LV-Xlc9(mM&9R?V06Yhh>kyYz}Xa z6Je+S6?^2MA|1dUJ*Zs5+2h0C!5&%A(s%(Z;p}1U-}q6w1~rvv%PhyhOcZsbv9{fK z1=a2JxW2R~iHevgssaX6^vS+h<)#)`!$=`X#-eo**^C&TYEM{B$F-YEnMhgu}U_Xt& zslNJqt9e>hZBu?kwRQ4pOYv9MTM-ZB`Z{*Ah=C1);Nl(oU{qEhb#Vp&s;QWvG2)f( z?waIcXFY~hT1|CSk+YuuW6GN99`l^_w3$8)GiS*4$hc_@Rg~Pch7$BvJJO>J=N;H< zLyIczb*R5iXZ@|csD>6}CbD)v_B1xf;O*M|81|nZjTso{Q@v7`kfp!@kGMzdJDIpC zdNk@gO&Fn_Q61A4P{^xBRchKZn9)L-R1kVVZYL}V$PFvlaI*~$%V-nM7Tgdgu-9^f5wgU zV&lkqVo3{pnYQ+)9<*r`HvOrm^X6jY6L+xbN3f|L^?B&-ET-CRD}#{?^OLx9i@e^N)?+X9(9^D^^gU0fH3(( zV!sm%kfCjgj>C*r(6mE(R1p4Ude%jd%{=QT*i?Gf2Hc4btMm#zu$_dGL!g-8T=7sz z?w8V$-)7)O>N?82Do#~qmXjl|>4AN4ZhxO?G=sSj_GSt96XqwE&PBp->E3CW>FRQ8 z<<8vmcEi@>i#13aJ8aRjX#-MoZk*nfh}a&D3SummsCVHTFPCD`;h7p1Vatp~zv+S>i_Hd%w= zZ5U%HJ7*!g@Je**-#duAt)sd}?k2af2#?!k{MbT9e<-enB&NJ4zovN$B|){O1|W(b zbJNlvsNO7X34SRi1MBdRHrSvlt;1Jf!K>ig*gp=(jHULz(|DAmz|UI1rna77nMV2p zwfXqWBYY z{QEx{e++qlt4O>CdLX+WP|?O1mGDYp#oPYW@?c?l&V#tnMNPyh};rB{zvv z&E6KF&Mu<~j%KCV{-@A}?62y@Z2g>b;~?nwpsDT%1%suXGAF3Y?S0G)Ak6T9l!G^@ z8098~Oi*IHC+Z^SOCiaWZRb-%;>-v}xuAe9jQ`caq(>uY2`k8H_}{%P3C z;lPU4r>MyRgE|d0V8r&f+AUbz>h$c{*uyy)xNJj>)5uAcL8xj{sHQdoy*wJ|K23rV zO2n*u-@P006st^+LN?vkXjPXi{uBoJiAoc-eD(`q5A+l<3>^*4nDQLG-)%i=J2ljD zJ&Ns=RpTkRqX+tu;2W?EjA+2Z*-L2h{}^Dw>}K`O0Kbh2X&v;w^ofVuPDnMU71fY zRJfj~F@bgP&9RUndmw0(XvA1tFgm{=aBCcvK}27O40;Ssw0VER1tMvZ0dixFFR(qr zw7qPXb?B^?O6%x1^+GDOv+gl?k`V`<;0oNC^DZA_0tA_xyc?OL%$PZ(`~+_RW0zga zQ&){59VolvV@)bq=#nkEgh)^m--neOFTlcP#;E*J*TISj7+9NTgUV?dsgUeZL-H_X z;~*D+L8u>jtF%%;?j@&<1u_A!!nrz}bu%F*4ego>Q_{}58$r>Igmv`iUuh7A-hOl( zZ6?9ggm6CegZz{<_Py6Yhz^^Jf`HM=WEFEAoigzaiM~s1wE2ofJ~9sz$t&G-%;QD0RDE( z({_K`E<6y0M~Yw*t+5#uiS~j;Es(pY)MOm~{{{pC%toCZ;*l`&ok9ZZLptj^sPX$P z@Ze{-v&OT?^1ZO65McSK1Njy84mGM=^eM;vh_^lT_L;b&{ttN?8riJ4a_L^?=tZ<3 z%ExHSvT%s{ZD{W>3FwD^?=enMN3fI{igBvwXQ~ zTl*WP9IJW48iwiGRh!!X#azGY>Go$}H+qBZcVE0CR#YGqZb&oY07}gWJ{#OZ3>Zq% z=o_>1)OHA)Vhe zw8K0*X+c4Q26VGY1ERK2@1PYaQ1DP~2R13ok|L$)z8TNyx>8+Ra5Wlb6di?)CA8@c z?IT5V(is;QPD%nnyaTu3kini(BYNgouwI?Cp{3<+z1H1S3uB3ru>kLG{IH z0(2=#H5~xuR&Tgfe6f(8(&D^{#*g}lE#yb;#BJhB9%HYi7@>M&d9v{dX%4EK!l(;g z`d8@&OTVE16YaP7B!PUdjiiT`(8nktvs_wH9_!3fiz(=tSCd?i`vhI*2n z^>>p&7jK~YzyleyaFNfh)I{5 zU@e-oJ0CDDDWsr0opl)q{nV~OfnM18gf*Kx;UUiYEg}aO;&UlKT2stU=F#`Ie0DMq z9HMj!>4wiv{`fd~6wPYZx6rkm%~5*D0&o=v{VAkbO6N^O&`(okO`-8+vM-*(%Qlm3 z3dwvmNQ2@n-uu$Hni3FCfRqQn5zUteMM@pasw*&$8f+7o93KnWSYINQg4@Ld5+{n* zoi<_%xtOqDa5=@?Cc#hjtPM=YmyaUi--rjs>mpwti0{s2)LU;3B-XdjLojKe^6qju z7b#A4)pV<#LhqP|K9^~Sx>bSjX5m&?(+c&dTTPGp5Z_Npj}ui=#vEh$O0w7oMkR{Y z49ES=8+>4b{|^BvlTQp2oV$!P%Qr;T`NSwhxwOA$zGKDm^^_PnP}O_%*}k-=;KD~e zm&&k_;5uE~6KfJeHjX>4Ep!U#0OWp&hU; zts#9O7IkI(2)AA?R&N{1AatjHve;H!g^7$043|nUy&+#DIJhgpJ0P0A0~?qg_>>Y9 z>D5TEs-2}r`JuH&nxc1L^IJL@8JZrCFfTR;(o9}qVH=r-6r>NdyPsAK7DPg~v+LzV-t~I_wGv~h{b!J*%zsRXrV1+J~v1x&K zsQ`5pTpDI65Oio|Zl}4fHJGga4=Pm{@l~mzSa=rw6EEf2oHws}-BLzuS&R7;bLM?f zI_=LYyLWDDU#Z&X+OpYE{$sq@2^FBdPBrjiI9|-%FbysBP(YLmv=|4Xn{fm7+Ie+3 z@EuCpG|Y`nZ>+-L>>YoS%*J~(k4pYAts!6Jg36{3cGO|UJIX(XGWjtTCi3wLv|f{q z6pj9nl`6J?&zTENtZ&L|@DTkb^ZQ#N1uJ>aC`fI}8~zD$*oSh6&#^ojvUfLKsk~YL zLwZx!r3n*}>WVp5P^D4pVKD(a#D zs}kAF)J8Jl51Usb2f}1ld!rW+Kk%&Lu!zsyJoYa~)Puan-X7EY9P43cZ?I!;@e4X; z%YK7Dz5zGA>_!}TAl5r@-_ZEg$bKF!F&Eh6x^{fM(>bd$H6oXPyG z79P9v^B}4razu(`!DPWcE|^yn?`xAGo)$jzHoDG5J}(6y_R)J48Bz)6{)%;HPnix6hc`@#0Wz9eWD#g`9~giL?}OeH9;ud`?ubcvpL6> z$AB=!C`y}O8z>ll{etb=BMII9Wc9VNhuIdo18t#8V%s?IDSx5EyAOF%$=wg`6z-&w zyHUiZl)H0@Oq9DDfb`co6!zw>A4ybddoz}-4tsMPkK18y!mrr!xC8d4 z8{XUUXdCXE1?H|FUx#uSIYv^9U~d|UPwLu9hI<)^;$6E*p>ylnX(V&CH_LuFV%L6} zXh+xnl*b+E+WWq2vvNo6O&N`l%+FGRx%2ZX2!o#wQC_lOI_=HDVAw8uGah_mZw?|u zD#8584@TwOCFoQ9O7z=>{Wje z7Vlotc|p!Fce5}jdgAwW|Cdx9%Zb%n)B%crT48XL z)H5ieR7Q23VLYRc#JOClhIqfZT^M>Xo>5(bb9pERe7>xwl5ybKg%rE!Hx(%Nc&YtG z#3Z|zzfEv48=2D5F6O=XML#v;4yV1geM`nh-fKv0^Uh}6LgA;oZ0-r+vnjX0jsf^G0z33+Dw`X z;|wDi=6KiBO2RXVe##!wynUPE)*widM2sPJ+z~$$Oun`b5Qs=ccC}?p?c0LcHPS?` zPR_>)zxyb05pk}g#OX3h{I%qiQKCAb$suC>ZxSQpUXl1XC9!1nX#6nPfzg9Gyd;KW;=ODbBkInUv>hL=GWh z1ply=XonH`1&=#oME>}qEoRDyY@dHPNTy_dZMaqO>sq)zl9Bj=`0er!-vOWT4?iJK zDm&3evef?J+qaBhCr%WKWr9U%G{zy()j+%XdfCbpIyaUDHhj<;a@u6t*?)fPH|^ zw(QxeuS;O=_Ti5Z2Gjmc86$P$C1BVF*P5gdSFM*u8LnJgcJs(wdy05>uDwUh$Xwg+ z4+&h0+XpJ@ZXcEtvn~7ZBZ`Gvz2Hs~F{gdV1Jg+MA*h(`CSQNPN$|VdhhvFLZ6CfX zIbH3;CStyfeR%mfyTcB%4^DqXc4*o1W{-utZ&Yl$Q=&$&4dY?sHiYT`#c4NI-Ax&; zLbW>)Be`l##JdaCcZeBTs5bvTL8u%vShjq9NQy-H8j>k9vvS#Y6n8q{?+8Nl0nxs; zD{LntJ8s&C=B3jVf8CgKr7FT*j{bE+3OPDn{EX8ir>h)YK+JGCiobv|+~3%k=x=m9 zYs=I1_#5AL`WsGpdJzUD%Tqve>`aLoL7pBZejD=idxgS{WoN?{S9#iW{mAlk2pQew zDM-x7^0exiZOPLUaBUm%v>U~sl#V6k2=erOqPHPWKT{aom~#R=NTru8feKxO=139?jv*}K<@Y^0jEJBiqo_9l(SC&B$l zz8;OU&ZP2Q&{xvAyr*RCfGjl8CWXCvt^P09xG}Scx~bv8Bb4L=ox6|mT{7cWn>CHM zBDu1@uNE~t#LQ<|g0VY0*^*X<-k#t7L9~(-1HV;XE|f;E=COYP3{393i7P|DVz+=} zWyq<|WV5JQ4nltFI4{8Q29$bP_+4Ks$6QbsktW&N(f(enj0I#l1QZs|mWa}hqYqRJ zE1OW7_jh00Sspp1X<=GW-qYLlMHR&gTRSfc<~`+WJCbNtycV({)Jhk@W2=Ll@+AlD z`r8Zr0?01ysA7eq5H`Fe?~;125G=C97GDQ~oZEv?N0p<0KL1;XjFKq!kapI${&dz9Ce zvgda+1Y~|k4r-~BgIazHy_}*DJzcVMnAq>)W1dzZYR~YH-ECJaaMP1}lh~;z`EG

u>Tgo%;nVAzg-@(f)6IhIqk^4t%_?exB4rhesB_^r`%;7#Gi0tyt=~m^yM$B! znBTE7JJ_)%AbT3@lYl-{v_2O-T2QooWM~!kE&{Smq4Tv}{RNdop89{u5888>-{5N- zqymY)!%0twP%&+%4w~sDp7{;Vqmn5O4y2MJYU;+Nl_JExw&^N2`+C%R|DG!66^d$7 zW=fj=-ADq-@NZsRPl}Fuz7##-SFic}`g7I)`>!U{{FOs$*l|_LXq!(ngN6{r!qz3d=|+CAy+kwyUku|cZ}bO*m&y8iv<7Jz_{dH_UTq5T54>yx(g%9! z#Ld4OctPe31ZvLr?j^_y8h&ZTEvfPF?9J ztbmGnWhI?W`H+?0UR(jbK1nlvK2T&seMv3hQ#b8=<7T?lSGek-dbAe0+q`GbCw`h& zptbBn4van!?bNEYlRp2ALk}pB9emMGg*)^n^aF%(F28s~<@{6G1M=<$nt$OHL(Q*VLy_iJFE_}KuZyv2CJEthqyeK_1TTV6 zp-O%~8s&N5bxHPb`Ss`I@rr4Gmv&ctT^N-wtSQV3$R8JjM;}WRg94$U!@Ky=9Tfx4 zucCczF~Ey3Vz4n$3@RyaL@{8Oge?Z|LIf}AfgslFE(9-2ijjpNAhk^?1WO!3Ff<>i z#y7ezSg@%0pTeSXJh=9nldh^Wg#3{s}j{{B`0ZSMwbySh83eA-TkE=Y3GD*7XC><%4?&G z&;fbR$>w|$QOz|%u<|(MECY4QG#|BkUw@?TXQUY6BXvD8UBv-`KYm0WK##bCG@(^2_omC98~E(aT) ze`FXNo)eIR4Z97op_ZsfDAt>b33n5}{Y>~Fq_{9axIFM(yZUP5qG$x#?fvK>!Z)+Y zD*7Z=P8hHNt_;cQQ2p&XoAxKQqv9|9W?04lBp^q{o$XK5$n8IRhpK)T#JE&_TkSth zIA&LW^Lo?%@MH(tpIi<$Jpb!qYI_A3lm0a|Ea<^n*mQ= zZrUHNxU~P!`TOy1u!S#z^w95`pIQ(Qd&jz<5D^aRPGZz50lwdN)I7o1SxGL+99RC;BiN! z^pB`QJbr@a32`>vEINp^o3- zJ&L+3UOE{$86-oDEQqspxEA;cahwM|qy=Wu4iBJW-1&Jrbsnw-dIU=U2UjXS?oX1` z^SPH?GE)C1GO7QAtp8NHe7yh5&~Q8ZKXuzy|Hmjf>^=RTJZ|#()X$x4{)=$Up=_P< zI)?^{1Z}IoQ1ZFT>qi$;SDe&3sk65JYUt{)TDucvg!ev}z8{!VG5rp(r4Zg-Ra9!> zT|vAO}W*#+2qZ-VeTT!4iA42g!Hc?`vmB>lP^T!x)J)bm0IuahRx`-zGK zZ7<-np9yo_^y_yn($3y^Dg$%H8%SpE{UlM!_I`?dR5z20OK_54a(hGI{tHJ`wtJJ$ zp=?Wd><_}#?Q{v2Q8kCMb-p2xz+b0+7zZ1N=?9(CC!gVXElU zwNW_a(6i3<$ybTf@2P4PDd*OE`+t&3Tra1-BCatBjkqSU^!r4~d;oZU2J%$$dc5LX zD*yJ@)g#Djx)jc`#?U^p0FeGdL^N*SW^A{-7E#%4$!ns0>+}x(2nOjf#D{mw&!q9J zMH@Id?zC*R#0^=tizU0OW!n!H2&=D=&|Nd+5#_LKv%#E--M?Rz(y~oeQK>E4w~2RH zw#Rwg4$JoLUAFA%_YRD{WjyxtAKK%wL^Yttvp*nTk}9}Yc;v1M4y5shR6(xF>!u2x zTd7sST!^<-K|`V{c!wl$Rq(NH78gs_CYan*!8^?(s)G6CbEtxgdE60IaLbQvcB)}R zmpR{8#@J}KW^?@H`>eQ#WKFHl}eK3t0Z?8U3Z&LeO zZ-ZxQQjODYdByWwF!t{pb5Gisza1k|?8NU8wQn%Ch6W>eRfDYE})WPF^8015c^VhL6fE+pXuqGhF(v(@YqQ~4ow zVd%gW4tr_gS*0Rdg?wFz!*DjJGl!yQ2rE0iIZ##7x$AdksuN`-{iWPOMz8#nndcvT zUmaS*At{vLyWf(-s75OlarE4d)1-csy3Yut5Svh z3aO0X?(ywaG%-_7=Mj!XSJsPps^z$k&SI9#Nhn)Z{L*#Iei~^PU)!tf=;4)N*-*2H zpBnvzob^H$#VEz2=+{A~FoANGjsx9+M%l^R_}^9NZUu+NcCD(DZac>@xV5rFCw{{A zQ0I4^Cw}~;w^M^@bOt%sMo#?G%V!4*5%P^|_8rMq1)2G}0(O>k9DTh6?dO|!w`N1R z)H*$BDgSU%aloU{$U2uCfa2%CoEB*!Uw!q&2L9Jh%x<6U7nzXk?Y+M7i)rO^QQIYf zM<$R`KAM&jy}k}o9-@kJN)abpJ*iH}$)WW+6(tx6SUM!!?+awm3 z_M~Ck*+xPwg6(r;i^(WxIlh+Ee1>RO`noLT?`)ono#JczeH>^ur>rKIuk90PV`obLJwy+!(LmTiifL{GoO>uq?=xa`meVGtJKi-RmJ8*8H*hK zhX@bi`U&fHI(edv$+=DV2~!n6vx#@|Q?E*tB9__qNd*sxo=|5M zs1N$7BZox8Fkd0*kPfwpsJ9#4I9u%=0@y!^6kW0+Q|c5<4j@7>*%Z`XU<8xXi=Xn1 zKM%oNlhePLn#cwtLGUhph3!7|I{4*hEcUN}J2FHpY2P%EW&g;J9;U@>ZwQFqcrNut z;weRr>Vv)t-}w8K9+BN+N5V~M-g3#pc^Hcb2mZ%KWe&I5JixXfb`PnOBcgXKCrNko zsv4=W>?u;`)oK2;%2VWrk zVh_AhLdWrS;D~;*WC!iRGIEVz55_B|x!HqTmrAMfzQ=HT;2~~Ad$3qCC)k57AP#%* z9jfSV55BH&yV!#(sQNbS!GI)5um}4{vHwYXFyW^Ejy>qP@xN{l&Vf6&?snLNy|j4w zARzj!>XGdM+*I~Jb-&l5^T@gldvHTo*@H`~M4E=|!9Rv*Nwfzg58m}-eI=YMg@1Jk zTU{s1x%r1jOCH_oDe>L&Xvu^22Xk()B;&)4HXHzlY=OYvrTGR`B7i7#yxk zN;;3@OzEw@buXie8EneAk~n)D-XhkWmYHw|Q@Rwn*miz3>4 z+x23|F7V6OCGy$OYc}$#`6s+!{TT!q5CzP)5bp5^6XY~5^?8pV9=rYs!`}RoZz1q? zk1{?Df^%?@;3jE_W~KfRLwpz~V?Wx@&$vbnTPpB=7eTu4C2@QwzIl+O%Nzr}Xb&wQ zTel&a5`w%?5dgvS*NMqy6Jj99b`-zy>KP8onPLgwbjVDs$D<;M>N6!v`6cy`?>PkI zGbOSWd9sZDo~1t0N0#9nD}wT+kmOCtAyWC;a^Z%IWFUr<8|8rf3ZXII3~CCfP6b}b zr(F~aX^=jXe25F*6Nc(Csc$|B4!=qgQ1|GSv^dF@2@&jN4z};X*X8WJ%qoc*A(9qX zw3xR(7-7m`yZOp%OdevkD=IILy6QopIn(v3Yjqv-g>|ZZ^s(5vm6VRy+$kZI5Rr0> z9e)8L#*QL+iJwqW@+|wm+ioWba?vltr1(DScYUqjL8=FqY5yNKO^|nE^4Us#k7y*t z1${auia<#gpCb5o^ppziGqD*QKJ{OzaKabz3!7ytc0KupEoce$V)J5*Jg^&E<=@TD zFLO8k{ym{VkArSQfZXlgWU{a|0{rKw6Vw87f|@wp*M-FHZJp1D>w+D69ApkrJ3(GA z`BHGDa4V&dpU9n5&6 z8o7uJse#Mh*I=mjw_ke@Uml&Y?_ubS&Tkt1B4ZNXK;SVJjY1v?J~@;GwLmZ*{qi1y zZHUYuV$*;uEk{3<)QG{=N*rE`{+&?Zjli{`kv*yLJV?;{(UrtWn%|hTOC&AFBj+cq zkHz1gy7V1B_?f7`w!#bEOStG93Ky#}jm{;rP4uhji@pw@+T}Y-qS~^fTd$D%OrHZ- zEMY&7Quxo#mhg3R$gad(DI(BWExoLaH$19Ee`NB}TVLywBFxMs04Sjbr6l37+8iJkU zvP<$FUAb>*9>+?{t4`8u6|THTee2o<0A*#-S&&uQdH2L_!J{~F_e7Ku$u%?PE8@)g z(2BkdI<3tTNR;$N9mmQs(xL$%)TYsQe_;lC)F&VPmV=fzs_55B+A}6b-;?HKzb6`w zZdAm%@;wrZlHWoom*^mlSMF24Ibv3F`|bzb`qqh$OFU96=f^%JzdOy*&ph|?9DR+7 z;TPod<$F;P@;&Dg>M1xlcfY7(YLV~$6j$O?c+$TD zm}=L4IkcG0x$a=Ew)P3!Kw(dJY`1z26Z?Y0iAMFNoRg0$J$RL5-U-_ zG_2?rx>=#>;POp3BG~orz3>XY#nVxZ&q?>qm*lh5N6`AjH=>&@q6pC>Ir<$ZS*O$Q zINi=Ke2QI4#kSEOaFC*sP&)1&^09%=3o`LC{FICyLQh4yiy` zRD#cwKx+RP32mfcVr2PFfxP}VPN_a|RJ4iBF(#T9o<54+vk+N;SvgXEe*dmuQghPs z>YDz(ov#gxU;~ZyIId8gE!HG z_@``uP%TDJOWS7sff$k4-EGm`Cwk54a4mR}ZlKQv2QAr4f1IdX1k#{u&@=4B4--&LCTbmQUk^R}#{ z;R~}1)T9IkjW0yEZz|>cqd{~kX;!^1Ul!)Y&cX@Be1ebI% zZZ6_i;|btuItI^vtv;{0q%*|(d-8R~V?z@nd`0cG=+jiBwBsCP`!A((q-^^pp+yQB z__cutS?Z~{xR>3vKO}{VjUA?)Vu0FKB4ZOb;K*Ii0wQXPR z9`SR$b7q2uQD3gq=X#@JW<&A|B@YdK0=U=LI$litL-r?w`AEI5^*Yf49f#I;s6L-D zQI?SoRAwRO>^L2{c|LM(2@N&Ba|sgpbiVJ} zCkOWFS~VuW^`mBQN#2&mz4$71NyjNFdKC~~s(rrs-C*aOOg>`3NLH3o z6zzGBHtr=nItRaC)xwgz7n;fqU-kFazj>O-{6am7<&{R9BtE}xL6VGX)p=?19d_U0 z(ze;r1ItNH|KU@vk4vXv*+e)c`Y}i9IF360uAMqAkw(XHkbGRtyz2<8zLDHg|z$DF< zx5d}G8HTaI+pP&6CDQsiX&Ar%k&@Z?L=PihH_4#R*LuB*+%s^wL{7hoG`UYl?-gMX z*L;nNU6?7K2_5Pwcz^Bm*u37zAqkyDI)aBqPx*xQk3i^adx`v*(rbHy9T#U}DRIzf z0T~GCSAN$}p0D*Sh>`Iceq8(Cc`L9~^PNU}lz7S7!L|+Tv*c`hUWUqK_gyRaj`dC_ zP4lLb2Znm>+)O^}P{8*quX*gC-b7jD7y_x%+N3$07AUYv;@=7{=?G!hBSlD+LCG&EIJ)zs%@-zZ#_8vEJ~R8j~+wCn*Td5T17 z9MNb+c}Go@k0a8IYTTOl8Be_K8COKXjs;=~@=gZPQ6vg>WK4urj6>)av9ekHdjssE z_*k!Si+9)iMTOdW7TaEicuIqQ@P+COpR`GH@BbLY(-%DGHu}N`@}ykjS4Bmz&h0Ib zRvhn%{(~hoaS`P00Q1^^J~gAf{)6cT(eohPIe#VJtJU@f70U;NOFI@1lV2QGU+V-~ zBkBPw+RfMcJ=j>nVfpuqegPy1*5MmM!8CIr8&j9%N#h+@Uz3y z)`Pl0JP(E{Erwb+>0Bb(4Ev7V^;LyNT-72u83euB!&eFNINzX^Ic;cW=M^e+Kczx_ zZNH+tB(WTlN4At@c1%(92RrtdII=_@t?8%WHU=SY4@o3cc}s{?wPD|xA?*|gX>Cj& zEA382u*aB}-DC`nY-&T>0_Du<&Xx|=pQl7DpQ_-KcjV`QexJ3%yMGDu(KJ+CTM_w? zin6zV9BLC~T<;uiZzlnw!DC=T2DH5u#{#)n4h+v++1nHN9X*le7JIu^=UD_y45Tun zYxuSY2KPdghHA%wy;qHH?Gba^FXr~owz=gDl@l?y=Vdm%Cgyfv0ObRdq@L|>NYnj6 zjoZ*qL55UV;mwf)f*oi0wGn=@ar~-9!l%>uUT)LLYTirR-&g0u$@XQ+{_^o_?C;x% zYs+OR?C-7fgz-=t#QApG-=&yejya)!mXbI6HVwfJoxCbk!2@*WqP!!eEq)^XTGQLx z-^~N`if%ytiBRs{zg59nq+T*zlCL?}dfVqxthfJC#q@hA0e$c<)!B}JOPL-pG@>M=nzDIhVK5wU|G^7%`IC13i#%1`-|!oEyWM$i1yOOMgPpue!$Ntj~ZPq zLFb-prM&I%1D2~Y-;P#EneR{`Z6}PXX!{$ug`{dz)*n?_-d!mq$Oz9E{=IbNMw!Yf z!@rxU%oss@3)yVC(QluBW|xBmjt7?%hkzac0&xRm3&6noXgw(K1-6{HEjK{7WeY74 z{TjtZmjrG6;y7r3-SNF|^_GtsdiYv@N8|CmqtcFs%z~D09MQaAKf@~~$ZdJ;C>IYG zXOofJ3s^6_F?_yd__^;z?}5Iyo;5F*bd+bS@w51qTY~Mc(I?JtxyU-g*ZKn`8(6XT zqnDEU(ddD>&$sq43-yk!pj6k=350Vu_MS%qZRnB*&O|X zk_?vTC2!t2eqSr^`-t6*wv;%?Xvi#XjrrPMBu+?w+|C#|zGUqMcF}OzSNtURpZJES zl5fqxaWHUx-Uq(Rb`y0DTh(B2U_IZHQq3)WdWg{enbghL!IHI?}G?EuRE>6cU1QnIeb;^!&yy_Sl=2O#;MRpMIhX)=Ly$f~f+1IwewDuuFXnel8 zwDXeeg42WTf08E0?a(V{>MJfG22U^hTHg_#b?^;r-WkZx9M~h?Uu5QN5u*EYWU5^O zsQ0zrt{C~Gny8Piq<^YCKdLPvza@*HZGJYG_r9-HO^|jJ`wLoTscQRL-&2`4N54Tn zm8E$VTbwc(MPH1WjBhV2?O3696RLq2JJs|ci@j%jRK(Z%C`e?ioll5KV`l0}>cAT{ zLoKn1`G?K$MN0OyVQgY*bgcCL7upWZB6CUmK(u_WsF?TZX|Kw13N7&jJKLntlFrlo zB^>P$&4yaW_b3Hl>xEKg-k&3*l_$jtils!NwEYuNBrA)MytT}?rS(r#4MEyLyL~)D z#%R+%!}og~PE6u{uUQx1yf2XnBAHRra>*Hv>S!EUZ|%|n>_&4qg8D{BdA7j5ALorRrl9QH;V71CCPC31H1=0cgHp z{Y%`Mu_bxE*@nuzEE+d+6x-(B8^N8CrtgqzZ=$2 zDpOdc?JKhN*oDoGx_U(uMSylmQ(Ia~nUamp1BtBXKZ{=$;8Tp)N5<@YaFHItKRxBj zyjD4arnd5J_f1><+pS>SZ}FT8K=_%0B&0IX_fHy=5ZL;ukyTNB>GX z12jUGw^~?H4wFRCey#BYjcswrwXcG}-En7jz)<0HtgdzRxYdaUnK&x#JWW#AE z8%^aU3$^m_TV%v}A}dEa>W?wx5@9wM&1aF-d8A{Fu=Og5XJ?Uq;S4559}#g=vkT0V zvuhJMdK@B4+V^1r5mykSh_g0HJB8YRr;9FkKCAXII0hb)^Y!%>QKUe#gKvi!cC1$H zU?`{7V86W*RlvF}$qXl$UB=ZkthBd;QkH?mw}}c{^+fX~``hN52WD!>69c?B#FNeUH1K{3OJ-|NstD7giID&hv zr^@sJa7*D`>2@9f_b-U>wTVwFdo4N&E7SpS`rx-%ggAZXmvr=5O32!a^MzkbElkZ& z=+wFV-u*w+qU9Pm)Ai zuqeV`G~Lj#elY3%=;_3W9=t^u%ky>S`3jz8rH2i|;?u8vJaZEj-FfZgz)lYA;FfubjkwQ)+>7h$n?6JA{vZiv*>*M%A)js9W4 z>Tm?Yk2z*(I0DU;5lA1(QNQ@ya8)GdaQ~uJHIZfhVjGqJ_|%9NS5`YPoMTy&o0}RZ z*VI+jHdluytMVtuOPH*S9|8!Gla@6!Nd5o!=&!7;t*;85W38n2^^K|#8^an{Q`Z!3 zjKpIa!#u7GTl4EzhG$ncg{?)0HXUQTkX)1)fJa)mcugW2JwHFd}MNn|P#s;R4q)F8U&hsSWLh+WfSsn0R~V4ad` ze?w(s<%)15+}K1Su6Vj&Jhubqm=WuZRK}|fCG~#85&x>nCciKRZWuXn<(o6VBF0IN zp?i*Esy|Y1+9`p!5z`N`!;$A0|Dn3#@p=y{$2Fg3mQAEfSRTpE4b5*x--J*x;b7^4 zyijOqU2~{v_3GRyq0;)Q<)Mc9+M230#Z!ulEsOMdq4I{%ipu5TkP)azBxq+(M`ns?pI4O^fTf!v%Oourlu(ZB$O>rE*jyKIPWr{I7q2%OF($w!n{C56ajc(i7-OJCN z(iEu<4H4-mHL*MrD!Zr)a$|-`KfHZ&r#sqLMB6QQod3h~PjTjt%SWhYMMJFKkiNc9pdb(-m3M5H2p8S02feUrPFhgx$dh`=^b6nEd7j#3pL<;Uvh; zXu{O_rzP6d6gX1nH}WkEp~itw0zvUQ?Lhuyf)tL_`STp@TT>@4MZHoeGfY=V6rDIh z+s&V0_1n<`6X*}DYOHL~N<`#iyNNXQxAo^thkg*whZ+RiaOoW`{V5pmW$`C+XG(JG z$4mQ<;U6v?PX5~bnKg_*)xokRbGVa~D&*YY6hOcvHa*yZ!8uvu*!?@4mK9c)X?tQp3xcB1TpWDm5C-+(0;{Pq- z4s**mK^%ud?nAkc=RSh_Z0`NIPvBNc)=S*lS?q&mgAaV8%(p zRF=fH5M~n&5_V6H#UeE1Da;(-K$wvmi|t2ce@&QAxN-^$Su}Ph;pc?+5WdArBOenk zqjC40$}A%dTS>T(u$6El;g1M25rTUO^9g@VxSH@O!d}9cB%Vh4J7Ei9)->=E9!S_q z!K%mH%#Q8>{Q<%p!al+Z!Ukq&KPT*E-n)f*`59|YpdJm3FB=GZf|O4< zNa%-tKWohk3Cjq(2zv;lgaMYT{nVp@%?CFSS}c@)PS{O&6oS}G7@&TA3u3WX4&}KF zzED5^>F@)=4G``}m`@lWEF&x;>?W)w>?3R;93;F(;@Nb12Vn)_!-N|N`v`joKOpQS z%p65N!u<#b2?K-{D+)z~*@O!T1BA7N`GhTmvY-7L!V1DW2pb3=CTt<>CF~-6i?Ew; zE8#}Madh$?LO)?I;WWZN!VnOlA4TXV%qPqze3-C>a0_7%;X8yD`!zCA5COu8gk^-Y2wMnG75LedOE^gQlsun9 zIfM;_TM7FJtC9OaC>Fb!&|d*w!WP0kP#j&nb5KUuy%;=%eO2Tq3{*oW^472deiHUH zQE%G2Wes=;2MM1cEIXfaL=FgVMh+@2KyC>0FGLe52Hm10Td5a{r>u<-#kX-C@=CtJ%V|GCe>?pUB~|gQSZqD%x;iMA{Jk6C7th(e z*p|<8{*};4x|S~TQ(pH~#DlN*YVhzp_-)dmkp0(!2YADE$TbSF`+DdBUUoBhCApbL25m++DFy$?_xhNs~n zo(WreVzJ}se_c<~e+m6hp$8A6+-GQS!ognT=j-tG1=>A_d@oWiVZ|R~vAYTVTj*be zeSd*Jhx6PA9>R*(kq5%EH_;nM5dSyil(6CN^bhouTr|JB(#}7{N?V+#G4DnT$z;5QwbijnBjocZ)3J9%|tn9VvCy&X>oI6(kFV4yY zEP1zZA4R%cLb(L?4);P}&T=j4dMdEfB@vgTle6@?fzfr;<>!A6u$zI+5y+gZ><#I2 zv;1{qva%_%fIcbtN=fq+X+}#LE`e15dmI?%O z_BgPO!0fh^GPW3es6%xL>>Y#8NuQOj@j27&0Zi~YVL8C=PJ$f=?2aT@8L;ms!K#4? zKb`ni0~3BaVH<$mkOb=nc1;rOK44wI?0TR=tfzo&0ET#5R=<=N~KQJlZ37ZB?%C})6mnFcY{BKAJNQ|IimQFYI11V6gP&C#Onzn-TX7WA4Z)3 zZG-S+T}DAx&RTC#R(|@@F9ghiPKn}RN6;N7W@Vlzb{+6D)T8gvSnNT5XS@K!mLW$w4U&fM@aZ(0 zhNlsJNZ*Z6)`_Ie__hDu?>O06G5>Md`F#X^eX*vozl}MO7hG( zV~Z4Tq#asFS4}#36EwNpc~Wj^R&QGR%A{iFj+MOPW6nBl$MZ^kkE1V*!*87`e7Xyx zPMxomI$t4m?#!5-m9qgE2&~&>PS&ioql&UB(tj`}YZefsgA{QNnEIBW$UX<(0&rU= z+x49@_RKL)ig_yHzg0%meUv*9zc&}$B-8H+~3StvM9%o$7ML90h~3Rk*9~bE z@mNIM#-n4gCnPROb|}Ee&fKi7bkD!3N&tDHS2;O|Z$;K#>M<)2i=8QL@+%xPa5-hy z^vmuvPY!4jdM2%)$GilVLm%dy5!Z)V-DzvR>oPWYI)zNJl>zQo@lQn8WSmBMTt{&) zq`zk8#$tv1&X}v|*dY30tq0jZb&Rd3r2+h;k;_r$6ZjHF2cuy<8`7n$9PW0?I+!rS zr^^yvw;%@}P-k1uOCP=lm__;b5Qu)m1&~Z+ zE1ULTO};h&8Jlo~Q_8fFt)6tx?DW*KCA^T!QMdlnhU(UnzIN2QT{d_-Gp_JlY4cR* z-^BR7gfL?fG;kfqeK)k#PL0K6yI;l`nzmtWi?pYu&q-?9!m+|Txg2y^GpL#J4`i{j6TSuN@53tib$IM#(X&z?HOo0e3(*$ukH z_GjF#t+(*zDd@6hFqRYE+zQ-H?iFtel6X@xR%FXhzBIeO7YyN*vF}zJ@2VZgyBfUD z9Ax-duJOu$88Z*gjKz-Tcg9IT=4(@yg+S|632pU@@*%!t3%z0=t_QF5*^C3pF()fi z^7VrEDI4$JiFhU7+1%&a`9=-p%cQ^b*!f}%6dm+PzH7LTxAVPEyeUt{?hlYJOnk=M zz)P|M>G>EajH1vZmqX^>J!PmK0lOZ9e%*I7eo1~V$tQDHy*A!^67kygc-qc)tIju# ze7)q$oE3}B5IMLWIC5b6zVg+IP+{raPG_cgtngAU2c5+ZI?;JrLrA$Ivx_NL@?^}@ z<;w3821LD-yH4P9fh+$RQA9hPJU6R1!;|fCHF(N~3VoT(x4pu=Nu`vhbguFtmCj8sw4KMf zo1T0|*Kro<_Y22jXG!{`c1mAhr^i<$x|+0Qq`g4WDqjfP%9ez%CAg)qc=p%};=c>& z7(!DY;SKQ2C-N|sdaAs}4h7QJ#52qpYxxB?m!o}-$0pczD6rPEE^ddueSr3*GhSfQ zg{y=+lfHWtG%%ZDITlv<;WaZ zO)Pep;QJVF#tmO_T<_0i$be*~#ZbRDDYH*eE|8r`V15KoAK(2qGRUA#~fn762=(4W@c}vd<|xukKqd68HR*pba(bb! zg!CIPiN!t>`ff=1Qy-N+ zJaT#^|D?}uGxl%3PCsm}D$%}(KQvrkjXyLC)3I^A@n6zX;7hcBjU(Y(555Z4@=g{0 z{Q~9eCNGKfwSq6Dz25@9{H|DRw&1HtfiIE1nvwA3;4=6-&D_KO+lBA^k?^esU)j~n z^$C4{z$kCKzRa7ZuJ2ReYoI-pK5E-8d>4W*W&2LRg$y8%%0A}V_~K$v%ZP=8eHi!{>nn|1!xb2Z}fKIV=R`sJ!GxC2mZ-%%o)GN*>~e#B7Lk5xX8-_ zU0+!{-`HXBooC}4vA&nO;$y@m{F62J-bQ0r4jlig2T z?dc!axzfi%0dP2zeB`U+lrjJ3EER0+j>WE|ovbpK^$#VJs?Ayd z@Ejl{4sVR2u_8ko_5)u|EcT|f`AI40bK>h#GrcZV);1NC&bf^9P8gcY=t@g5@BG%T zdcG**O})Y|Z84E)-tXVWz6bGTUIRXITb#&hljnT*IdQx_> z+#d<(IiH(J^Ue=qv2-c7dppYQNmuRsBKaO?-^sl)M}5!md`as=Ma-Cc7P&W!3}Y0| z9F`l(4#$6x`LYfhCzDje^kY!`-IG}#^1SHI1GUa_6nN!w%r*Vt_@VY0Ow~T&ak1FR zA|K_@zy+(UmV=p}y^Hn{K*HQuI%9$f_T*S3YXS|&JXr7eI zdIUTUT8m|kQMIM&6Nc6bPq}+jmZ&~92u(NqjCE&e$CEToPCqaSA4|QSBi!+6+CGQ2 z_4l)8E&V@y?MVer|i5r@Nz74KhxrY#S!hcnK5Mbi-Phjg%P6ZoSmO|!L)tUZYN-e1LH zmrH+slLZ^aFXk?X)zRq72G1JlLc{fCu^JOS48F4;iN&slUm2S;KIfQ-HPW?SnZfy6 z(xfY62tWM(oczBX!@f9vXWXLm8y&=2f!0%g`pIEqp;^+bv(uonI&;_s$;zL#d*XXhIRyJ`Ql@lF2A($l)rydCMDzo$*$H^f~OFMn(_&3kjY_0x1+ zbj?)4&U9}_w)Jwl_k;1)d+FXU##=XLOn4#7dOpMZ+brv~4DX{^){l4bUbCCkKKg52 z9edoLevy)nl78o$SSnSpt1ocEFO)@8eTugSJ<&hmbDH|vfp`F&rOcVN8rJKy=L z4hJ{O`Sbtqcz-m`+TiuxGR}HE)BC5fR_kc*vtzAqje``GZkO++d7n)e&SdPSVK?oQ zmYMB6((^U%OmDiCc9E%2*DTWCv}anz1d=)6s}+2ov|WFa-kJ7phUYVn_md2dgAX^2 zPs{iQd4@pX*(Yt>b7|gRrF*VP_iRpgBqxsadnc#gKW(?q(!9US@LcEd+?C;Z!{fcx zQHaWOcybuF=-byLi7d z+UgyZ&gS)>W(wrFOo3cFI#;nOGuK;c&Bbx2v1XS;=3+0~>M!*Eb(FNid!wwi9`Da~ zvA&?ecClW`@NU}0`j&K+*S+4KkFu`XMI!IsMH2pWlt`xV;ovFWXEM^hmG1pxhINmp zBkk1;>zxemEne$8lJh5yo+jyoX|v`4zF_i4qdgC%d*2@I{hbFF!27nxd+!+Uof+x3 zkMaIOdjFfdc-zK!f14@bzmAsQ4qaI~RhPZ*Dc&=@_ojJ&oi6lq5bI@PBA$_dj?~z3 zNgqoahtas)YdxIqz0|9pckx~^hFxXe^HX=}pVaR!(wVv=D)VS>ja98VojJ|hU@hnO&TA(J zc5+}R2X=B`CkJ+NU?&H5a$qM1c5+}R2X=B`CkJ+N;D3n&J%7>#55B0w7W4eSM9%-o zVr99-zMQ`WmY!qB3=f@Yoxi_IK2J2JuHmK0q|ngi{4LNgn=d_Ia=Ty7GMv99^+@Hp zW7`Sc$$|g(IIu-`x7b&u?91YjJq2Ip+TQYdP5n}ZU#4uy^M94@RQ2uIK{;5K3ek4L zw^iN^hMrX6b{3<~6WiXjZs}5$Z^Pa4j0Ef6s#lc;O=ze6zrs(O-ksrphXeM9L2c~z z7k}-w$k7XRjQzY%((}xu=h;cmrAg0=lAdeKGrMTj)nuO8=BBQ_&(tBiH1*Y^19qXR z>+yC?8Czb}wM-7d;9@(Zy0T5kmQ!`DHu3iQTBd>XA)9*nIcOX&*6;MCm((q`YN|`l z(~?W?P_wMREz-~S{+9{*(ejwk_9AV6PR{?5i&qxZ#jYE5vBg_mH{7E`8{T(?ezxg+ zz^|VT8uc!$K>i)m^!Ik^c+{Dy58IK{HE5py=Ue*u6Z34>%Ou+w?Bu}zNe-0B%Q-Gr zphpAqO;~2a3KKS%u*HO3ChRuhMicg!u-AlrCLA=O^{Az;Y!muT7%*YJ3Cm1aVZsIz zwwSQXgxx0GXu=*7_L{KIgo7ru9y8^e&~L(k3G+=@X2J>+Hkh!*gk2`=HsMAS_L#8O zgncF)G@-T0ly5@62?Hj~H({9xD@@p6!WI*DnXuc08%@|_!d?^hnQ+jA*5jsp6Z%aU zFk!w4%S>2d!Uhwzn6S%)-6q^227Z5!ZH(9n6SZwEhg+TVYdl4ny|u?t_w)| zKew>(7=O-bi#b)k*`GIQ+N8kIxy|Y+_u?smNr7pTrW~%}Nf})pJ#3b_!=)GxGcj_0 zTk_?>%G&nY8RxMwEZa^e!o7U(!U@s8Jy>_!E;?g9)+o!ir-|@P%eKpj@X?lSzZ2nj z3q4MV{_WwUUcW1RoYiCa?#$)kwBKG=c$QW1tTUyJ-)GtWku%mKr<8T2IYH{Thr@Au zzUv0x&02W78+>Aest_=}R@2O9WN z1GoLy=?31I1TQu4i<00~2EN|F#~XTD4g5Pv@E;oZj}3gHNzXn{Md$C6(!X!uuNnB^ zCjBVdMe3FLyapU(;71zx;RY`6mB=;6!0q{awXYp`74S6V;Zh?HCb`uJ+^yX&GI+B8 zq|nCyU1H$*N${%;JbAr9^qrJz%b{KFFBN=ve|gp5vDYE&g8#145AS!W=pXHoa(A;@ zUHY$EefK84oQj^TAC6FXhWD?T2Hs%!X^&sZfu~!;_0pLp{j#L==c)9=_2(*;ez<;U zF?d!RJT`weDEMC1dNU8Q-b8pagb%~@#@B%FW@mPU(-l0+YI$1&k2Z00fFHz(@Hzj` zEohGe>wzDPeC8WLt2F74#}o;?=OCTFz%=}2zy;4!MjvLI^gjXaM$a1xo@MpDr4yDJ zJpWdBvaP;rG$7C5Id&IaZp%p;F7L3))d5_}&6bz&xorLO8{lr`?#cjlyh@q)FCX}A z$nygk2qc%??&kn^!}Am1!iS1NU2eXiCkp%^>{R!48ZgNevo8oG{l*V9fNH91xq){j z!G8~2>b3fx8qX|~et!ng0^gVfzXZ78AC#BXx#S&Hx!yAH?zI{q@A=3j=XNXlks`U| zyhyp&BJ5hV=rty=0Wiao}MK7zX{xpZ!1lDf0Hgy-b0h?0R!(kLc`_EAi3l) zH8(u_F`wZEKMuI?XVAz)jz(jBQ^B*Xifc9ETm%1h3OrHZw}B_$jL&R&K7t7xw{lMe zE_?_yYWz`ye+m;sy9s~(t^rpW_=&(3-^4I+$vf0?{RFtkPlb^m+wJQ!>AM0t;T|Ua zZa$62F9$hu9cJJqz}@J4#-tC(Oa5GP7J*y`Q$dmco+LeX1#qFqZ}>J%C%2xo;Wugo zr3StiolEfaCE>XkxZ=-3onGD{mg`O6Zgi#rl=Oq2>vVt9fpxMC|40L1i@Lt2;O4;* z{02CG!>;E6ik$dA)%28@6amg6k^1&ruilK0`{3NnGF_+aQqU8w631J6%_&tRjy8~uw?z}tZf|2G=`+j9FxiuAke<&OVr zz@=WAUhE6%2$&H@#P5L{M^#466(tB|%1^@bwHGZ4_rvexL^o2EC-fx!cF9x2> z&q)|8H+tk->w>4}W1Sx{Q&)=(->RQ&J@zJWCFeKja=&iUXJKN6&io{OTLxUw`7fOi z^{B23P5Lgw2hghPKH$y!G$?o99s{$)vW*bH3ocYmhy%elvL-E832(HdaeotJ^T z@!=Dbe&Yu^|LHogikN7&d162+xEQz_o?CzmJ(eNFm>Fv{6Vk#r-kVhU4>HIm0e8c5 z7I49H(Q`WCgC_kQ22Y>CbEtv;!KCjl(fDnxBC81iuBpM zTQ2pgF!FGuq2~nPQtrmRb-8C5xO{iPt=t^mZFYkPfeW58IUI${1lD~i(m!p|XP=31|dVDRrb zQS--d;PVW;8o1EYGG3>bGd<;c0JvMZ&zSW2|JLc!bT(^OI)dQoHvQMuGqVgl`xu?j zu*vEMF7)?)sNuF=eGa%=xgVu~k3US~*_x!siVXaLBt5?fxX_vG2i|Ys$^GRd1WD5O zm1#PSJ7R4B?ndViP5S(;nm@TZusjHW;3@k^14bEm3vk5;V_)KS(7=}^jd%9L5J>uk z25zS>1@5Ld>Vf-BScd)+KKoJ)Q1A4UWX%y8 zk7ewa?dNPU@N9Ls0k3SE!u=>H(O3Drye`*51HTZs8$Y`g9D3f<2@AYB{GoyO{#8HQ zb$<}J(Akp2=bWPqod@W0ZU6Hu;C5a^iVA)VT>5Xm>A!YA`X_MVPv6@*wLLz~;D_SR zAsWA}{|kXjdu>UQ!)11Q(_acq;Tui*Uems_4E#0VZv6SDf}`h?>6zr7eqjpuvnk+j z0+)II#!S+u+~Tq<=U?`ZrR*|84O24W2m$ z&y4B1UYSY$YQ(^ANRt0sfxFT95OCqc;D;Kqtyibd(D(yM_VIe)Zg@U0>7O#`ZTXon zQ{(9~`uPq+|M|d0Kjb9I|Kq^j@Tb#oN}jiB1c;5g4h8Ne52u*)Wru4yH13pjnSobK z*Kj!_Qm&_gyOsMgaFI`Y{y$qMx3cmC&is$j!>1bf65vv=?xb47}?N4Jb_4VWoi&{z*Tp?_BWnLf}IG4W?b_ zw(9zkN^c$@KRi~G4Nys%t!S!$F|M|d`T$zg6^}XM~`aH&`C zkvhFSPjYdJ^j8BHIkb}M^+t;H)@+Tx!XOUlVyu9HcPGiol?J}i@Duk*U9TDV;HUc8 z=1+Q|md}kz`sX0vLg$2!bb+?qhJZ_bH|A=%O;3k`=YOK{->n1dA>eNOe=!C86X2px z`fk+tziIF+DssnD5By-0#u;1*TX(bVqGu)B^s{IzanM} zaAo)ZuG6j4f%Tz+n+Hem4Gfc%8!-NZU2Y?A<f%pDN z!)@L2AaLP-_iCL`&JdH!Ggre~w(4ipwYn;ROSyxl+V$$~+2iBff`K3y45;=lb zQoyHU;DnyOi!?pPO}E}Q@Gc{VqYSQjL6hFV@yyiqQ{alusD7Sd_|R+8SA3x1w!Pc^ zM8k(9`ENAvvY1YvX7Jo+;Dd%gd+TWHRRiz)LIX}W@XvsY-sY?u1<5q<)92}O%f6-& z7a07v0#|;Qk=wIO`Y%lS?joJg_UEURYB^bG#;vwJx)8W(Uqko;gXa$5ZvE(SgQvy# zITK9!toa&GvK=|ozyk+rdTf874ft*JyL_{+!0zuGfxDIa2jC*->yyUapBwxY^ECc) z!=Eq}k$Po+tO>N+eWMLG0{9O@{|CSYe|}QE_BzR(p5uW_`kbWxegSYPH!w{TV(Y6v z7^gjo#{3v6;a7)zn zj}+Q^Lr(5;b*d4`yue%T+;u>q|g7RCd6J(Iq)=H-{_!* zXPdl*z}@hyO#%NiaOv;8N&ad93PI@f@1yY>HEpd0uKe31zwXb#-OBwm1$^H#H2&<5 zG~u?Myw<=6({#D=ojJMsfV<)UAO(EXLW93Rt-v?|8O<_5oM@ z`eO~T{h8BHysEvD`1vOTPuBnPVRcDwne>LuRs(Q1{#*&%jh;IVp1>)Z5A$_ky#-v^ z1;e))rUD0?t>N9*>V&%+_;dwl9V$uhEd?(84E&EySZ45F2wdc_A<2JxL8UiH9l^wN zbbY%%)Ij4tSeF~P-|*Ac&u%YKk=0)GZBGl}B=OtCm$Zh9ZrXHIb%Z zRdIPG|B4Hz1k0y}ii^wURh=@AUz4ip>zX1Bjr9$a0#;q+%9^E>k(&CtQ2ml6P2osr zX?Xurb`!yka=Gs` zsZ(SXHKdw#;mXDl5>(bLZLW1mu@n@I^^5Bxp@#aVa7wNwC7WXS?qsl&@s1h^jkA9)%BsJwe^dUnd(S=V^gTI zd9_tlzoMZw96^3&1!m;BCkid0fKX*)W96DqxGvJT##+)?xgs2@ZeFos4OtvdAt5Cp zX(%-3l!E!iq2dKap%97ZE;ubzTw;JFMW;xSP4%H=m37s%VXNqjNsZwplR}}Tt5=5_ z!i`P!b+99{CbTkODKg?I&N?+%9?1)@t_n9um#|dHR;cLA1qJhigCQwwI777Gtl<1YzS zEnA+SKeseEyD&6m($q=QoV{1|3Yz%TprPDQI>Qh$r72P!s-jCRt#4dINuq6Lg-%&a zpALtEW%CkqZ6Qb@Iz)lH)fOrve99Ri`;z~aj4P-SgxeHA4X1&UWXd6&8&QOjY&y%|-@ z8td!or-n|S5*G(ryK4E=y82bcD@{u+To|IICgtWi8!K7+N!jIg3z!C*+SCwQ(Pa20 zTnlVr>&V z6UnHm=fw=fKom=;L8QY)W~V!JjS}4AqA7|5yLFAIZ8yS1Z8jy8J574gkkUgM?IPlO z$c3&Q6iUT!TdldEOQedvy`%V@eQ1Z7oLD+MNzg3C#WSmGn#4oUjGvcELrrlu7G50I zL!2*Z-|5PRB<45bC}u<&f6B~A<jF37|F*EK8Gc}l2MT+W92+M230 z#Z!<+>2*T6Sn5!tp$`K(WzuvZ&a}u#)J>UcTQOS+)K%km)z=l*h^kr{2z@2vqt${m zQMhph?H36ra#5(nSfdA(!Yrf@CvaJ4NK)8wgQN&n9mv)?!#mIvhcKa7jOZ!5;t+-H z9FgQBl%ef_AO(y;Fe8SnI5$M2B@|+~ zW!s|vw_Tgu8L-P(OYKr=m#2Js2yZoard^9r4YEtibsQ+tQ0&&?HkZU)aY z5%HC{C&=e2hRmnXw{~>cjh8^CDtv*em6718s@&XAXvJa~Br{@Fy=_^Y!#x=8j_#m^ zGBulfdg`W~GE;=WjMIlKa{Xc%VqhX@>m+tztHo`YI+Pe~D-KgwhKi?_BY!iTx>!7j z;K;gG<@>VEh0_3PZ$&uy73-X6j7O+%%0BZSV=Uvis@0XD#f-G;#J#IpmY-j^aACph zU?_LeROz-~#yOZuvm2*1HxRe+YE1N>YX~iWihPFlYQ7=Sj$&VH!*z);r&apOj%H#z zDu*pTV#;;cuN`)VOmE$>!Q-%=WGk#BGi;BWw&~grH_AKe#1`g?QkuqWfvqFHx+goV zn#KQLb5|20Nm7LqlsWi=9#<3@LUzQ2vip0r5asr_1JJa9Uh%Bt(tN0+!sJL zp8d8QL6e}<7Vp5F!D;W{WRSyvdrU$@a7+N4d5g4@zMgMvPX^9}!|H&wCZrSHK>|>g7C$>97}+@`R&qMHHr#}FzyhP-27QQ?T524FSoPPE|K`PUZ{`d4_$lMH@Yj)*Z^k`G=e-Fmt_S1E zER@c64DzfM_OLm+ILwBfEi5nl&S0%LIKQ7VScKL?hi!O>6WFCeAHg6O(DGY?{L&0v z=vD%oLedSzyKgugTA|t@1B@H{wT)cGL+fSSnGD=U!n{%p&)6?PI-!T*GSyHvnpDeo z3oBCEk0@0jn9?deJcToE1fNxfvD!>7&FrhUJJ@AmJnJ0{A9SFw!+X}7PV)kZgU}4f z$L(OPl5Kf>q_*_buB`K%SgufBkhlZJ8THPZ*gKYR-|WC!avQr^!d2sYh+ZTwj~biX z>(`spi{4rH4&bbra0d}s3xU(B679V!tV;CPKY?YIB!&9q*re$0zX9(+r9Kx+&2T}x zzR_3sXp5<)z@T-o<-wg-U?Mt04{~md&S!&%K<(t63P-D>3@}55t7nr;$Gj!@{=*tR zEI1*D+fSW{kVRyXSQlrOx)D1G8$jeNyzVYU9zy3gB`+Zk&G8cA3}V+BmJ)&CEDtI( zm(>c%tyu5TXU1K*(HlbPR&E>*kUQ6mvnFLYE%C-2(XPPS5>01oHiikx^E0zIE%GLJ zcOa?q(L)E0F|xLHCii%^-V<~@JG65vUndu|iczzF8J$Fs zv!F%8jkPv*?NSE1IFx@Gel>9=KYy@;p<##un(>@x>4ZV);Sjot-P~oTTd{ahsB~F2 zH0K<^?oXjyE)XZSqK@-0G3{1qt!#3lY4qsI!AmLj z_F5we8n`GE{Mu44uy8F1p-^A2%A%$rRBxRS`Hcb!x(Y*eNnUgmQZZYiOID% zVPWtH)BeisB(-y1gGqOKeF|;9&z7)DVQe9zMY$vf7asm}8x0m+BH&Y|ASJ~>x*8*Y zDM~<0oof?>1t9pE-OPz=QgjkRGm*uRlloq2R%Cc-a=(&#*qcTUtV8v5{3e*rpEV==XqoOaD4U%u@YF= z((U5%BRoSB7P*rn)oT!YL`WU);Ni2MVgtvo|J#GdRgvj50b`RmD$2A>*s3q|&sPF~Hsrl7Z=;OL0q-S}DQ* zBnRj7m^wy4e}SlXQ7JgwI*K+yvI}SkMyRXC8z>qLjelY}1htl%qZuToN|$U=QPC-a z&@`l-=im1uQ{>ikIM0ltHuIko`hiAjX@~<4jv#&D<=34n(d?D%Wd31QbQ(4k~L1vLTgAfj8 zMKrLgT*pnaLAvzUL~s*Qph6+##AwTipJ-@?xJ20a8`Z8&MRmDqn5XPpmMj4Qs$lZaSu8Zv|C4)I>40;DyWMN1h_3cOB8 zmFS<0k&kDITfJW#8I+0(Y5)o6h_yPw7?~$n3>NlPRcli_;=2`5goO56!0j_l&^ty2 z4g^tg;e#>i6VP5J6%;6M61q{DriXwdieK|#H(koB7_!k%6Npg3HUXaVsZ#)&B%~Jz zz8TgvMp^NHdRjxGU<^G9R}(=24=5XunaBxtlG?DgQn4`=83+Wg3S@ieQz>UdudpH$ zxzh=JI2pO_QdAah8WC$L0Q6FGH*aE_CIlME_V(J5#5lR!=k90(bua1MdCsEnfF%vj zc#>=Q0m!FFiExNcGJx&R@8eB`&m14FCHxa`T>X_$*5>g8|v`H9i)#-ET zZBCY?D!xc}JrkxipYxSXWGA%grZSLKrBRqoUs5RAg2QOZ)ugbxUD^zjcIbjD8v2T4 zPRlKN*$ha*Daw?g+Z>vJ%ZVVk;?NG#`;#fG1!7(bHJS-2bF5nDb_S#u18Z*~WR;zP zBIOg^%J1g0A=0C_5WP<>(nQ6n0Q{B9Q+sE6TI9yp=8IdPw~)DlA$9>smzh?%R10UN zH_=@aQ>i#lEu6;Y?dSkdp)|&&G(e6lDBw?GuEVX|jHb!-w={Vu-B~T7p?KAFyjI#y zB(!`Mif3e@=W&ehQV;?Ln`aG^z#b!2mFX`5-CHe=>QKw5ux^n?v6Z{N9Y3szA8-BOc%mixy;!Dkrlx(Rcv${wY585s7+loT! zO2P{TIILz}`G3U-RdtXO5;21?52JEkmaz0#T zhyHnBVeX%x>Y$KeX>?w|C1V)Kx4Ej|2`8TlB25tBn!7rOEsixZ+Avq(d<=F)S?G$`UjxmG3frLSX!B_4)metn{T9w@ZrOYdBp|cr7EZ>__6mWF@*luVmqhRYjma zpeVof#+y()od#p(P;&va6!s2Rco2%3m(I;Vy}}V!Qi$~X(0>t$<$V<|a* z+{e}Q8*X!mO4hC*JWP6|4!NsxHF~~=ex~ctFmOpXWQqhv|M84QfFtfU2h(YW3jHIh zACfKm_fcGc-t8irIM7^z$=s{Qvk4|D^5Tpjk*U<6v zSy^@=$Mt$|{eXYb_BU7gyWU^>>+^p@`oF}dWk2~Pf6(?X-u!So-+w?S9rtJsaxAMPm{%E1^8ukpX%|EauZP1_&oi+H48c>muA{lD@$-(TDHPf15@f$2}dGJc9Pq_JyGTJ$a8(_j`@8CWrjPVGayn%_IIjIa6CBse`E4*ouE!hlp1K>K<_mlr4ZZ8Bcy2zN qk$BXx%OThN^J9F~pWpM3@>u;N^v98d#+5uWRl0YOu;{bL-781#3(t+R_O`^1I zzKJ_CZqXSXX4Dba!3~3~ZZi=#+~a~)8<7Z#sDt^x=iXbD)0OT3ir?@1KhG3TrRuzO z>fCeBJ@?$T=qo)LXZMSV>947yzjme;gL~(z5^yU#x4fUa(=@l1q8*ICPVG3s8*4u4 zebe?&i5XYTA!$k@j&9rN4~{nR*StQmLDI&RzbDdbAqhAv*DubJuyN(@4Zx$4cKKdO zNBC7cOum;pOkR!mDA)8})_amat26xSR%S7-HuXB`NaqUm+JIWbWcS`G(F#ZN&DUL<)J%V%{%Y+xSIQ25p(i7Jkkb2U;(a9 zoCo97`i*>{-_?gY{&eM4v2C9Okfjz)^VYYz;|_hJHE!9m#C|b@l4G5*j#0;tylz;` zO}0cwOTF89m3CH!=P)fc>>0PTPSK{g z>;Jag>$WvHhBn5O#HPg_q^;AO^~;Z`AJD(wpnliJ7v$CV(_+VIBQ=Wa4y10M;Xp?oE11P##w{YIQ+&9uC+K9GrA7fa>;Cef|zf;}ch3h@) z{$6$cGp>JC_kY9n@9O>mTpz^wkfI;K^>Lg}DEditrOQ+7{uy=uth%nowN2fxQP;J& zl0&Y?xlv(s*~IQ&!u`uQU%~lrhSTk4MZb>gTR6AidP*rPxaA;a}dtKIE`Zn?hnB^6z8D~ABOuQa30C9qj7(%qK{*^6Zaz-O|nn0 z(4#<~#IVu0KUvYgW4NK$DGECk*D*MgaE`+{31=$KGjP)3R#!=BTAHG#;p$QMXRGUU zT)pa^_|tKp!RVQ|pM`Ta&K#U`aOUEq;{sgg;+%(bK29G_I`VK`fU{81gcaewn9&Py zUxKp~=S4Ud;Vj2lfs>9YV;}B!*gwZ@c|P~$yz@^!WX7zG8Mp7c=)?1;FFNPGCI5E* zr#=5T;nVuDryV=+kAC-q`K66jBaa-t zs{FFbQ-9c#^Z03F5BmEb&Y1X2LGgK$2Q9K)-T$4hU()_*fAgs0vp%2qai!Oue0x)I zW99|lJa*j4*Og8@_x$?0W9#y}KX_;4{7e2?c0~4R*S>zu>f&cw|NVaK{imhB_3yZs zD=#^;Chms|0t;TQKI!qIr9U*xs#$yJ)ccD+tvUJ4UC)2?DK()%iliwnOk2?JAT{iX#;$p9evOEGr#Y6YwAUZ{BZQR0VghW zCk{Bb^14C)c)Px$cJacq{!`m=(I1-^yga=&$6eM z+%|Xf1&cPXxcJ4#`k(lfy(2TFc1rdqFaP5m+_!0L zT*j?`Z+>|Fnpw3C>#7g=;el1J-u=kx3;KP(d-0^Qx1U(LdC7Hyo*(mr|Ko42Sv}*3 z=BLj4?7mZ`EWd50@5a{F@~dxLc}wc)e_Qa=`pT8Z{AtfUpQbGP^!tn6-mq@=(94g$ zI%Y%bX(v2+(Bto}eD$&ZQywdSv}VI+V~+ZE>=tL!%sa+46#w}7mi!MNIc?6_$K3JY zEe%7Cy8p#<+y6QH=5N1$;^OJaQ}!&m=k-TgU$^zUW9>cfAG_nLi;kZ=dfB2umloI0 z8M*74)wk*QopN9I`pR)9y>->S3(~T(&w4uljq+Qsozfmt@b<*nCtb3n{-+(UJ=TA~ zeW#tUv-|yHj;;))=`>fmx|26l6 zZx-3={(9_>7Yqn3`7Y=2xV3dZ{bBv^i?3SmT`;t3MMmd{<+-nX`RF5e{4hUr$l8qC z<9D|_cFzd=>3{ux%n#Gno|kt{`4<~sp59$|&C@?-&u}iElU{$q{V6x5ob>ruR}5WJ z^-#+_v%6<~{r>t#1|E9D2{&96_rr@>;+ zMb42?(#<_OvYd;f$T=H%Jw8HySEJJ#NuN4^k?kok?o^Fe`l{!(X=}TM^1MQ>Pw_@+!=+xJ4(Lp z9}-#qtCJ(+XGBT&?I?D3438}5kp7YJi=)(|OQZP7sZr`h4&+3#&xKLS>$B4%>ysFz zyv{i%vi!%#M3$3sTx9$**fWy<-xDSNJZX>2KPZYF+)?~+ToiwJHcGuXIZFL1J2tXD z)zc&6>!akWAxhl4DGL6?oXGP36eVt4et2a5SEI=Jd1mDDh58Ze=`&H%ZHglQ#3=Fb z{V3&f+oZ_)e;&pDKb;YoKPHO&$td4Q^(8r)oG5k(Rlqmd1MbUq1G(XIYtmj#X z50UgA6D4k37sWrH%fQ$blaL*^#)xPfQ}R#El6pG- zDEVh7{Hkn;yKk3xuEIMJpGkh!F%lmuFIqBi(!YJH1iXeET#3KEO9E(C#Bqtrm*Yc8 zXDd0kI;q?b(cHSkN6L$Kyu$Nn0t82qyl63KUxYqt(EF^yA9hMS?+__?Rh)eAAleJ! z&r^0#uonM2i6@;X`Oi}Nv>YMv$^Vw`8}mdDs{Agq;GBdzb_v+@)t}EIIY(*kZzW@r ze5}2M_KW!2E|K_|O3o|iNqrn42{;wMZM@`fNt3vd?z^Zjq<_iB5=Yn(N3kl$ybmP( zu)>c~{?=--^Qv)Dex8~jG32LAQ2BC6`7|#>$C=9hjsKFgVgG~Ss8qk&Zjp>9$;aBI zAcfsj#X3mgA1XQRKS)5ik=9?Oo0lc!(3}b#U3yVG=Ngube8^Vh?PKA8(tM;AYa*i7$`5hKJY(HG$ zRo~0^A5?Pkqtv^p=r>ZjZAuO-E{<=}FCslxS@eJNRLS2mM(Shu$z7;-#J}lK&6#qIuJ#eOgt$ zGweKUrj*}yn`DG~;+P8sD8AK)B&~{B`x5Ot=^wA+gCXZEr{r%}4m(k$dnp`=%4L;G z7j7wz28^dj&#ucQW20&>s!*W94rfa|N6Fcv;!K+g5i2ADtp>vs(y8-`UQ6t$3K+6wOjn{Y`4_2%3{wqla-z{fs13NylDSX z^>mXeK%-uNsQk^T;>?-iEwuZIQcj1O_%`fwIQ)vrvCC4wysABN(BwD{qdnRh=e~V$5GkSevkM;c$FnSubD3OX|a^oq`{KEUfIpC=Y3J? z;~v#6J1V9Ay3+G#7>e?{%p(8gEL9&>KjcEi|MIcY5A)uVz+C0em!N`>{;l6i{2Wy- zt*Sk0`&i;CtZNUb_QI{=vr|6S#w02GoGt|@b7=P}JFK+$!%GIgYDkUx@^2MybxXX8 zIYP?MQ~pq)(#=+Sx>dV5RN)s!sgK90_?cz#=lhkPRMCP4j$z6U*Q@$ib&>>96`t>u z_H?LveGR>Y;{#E@99pYtPZ0*ikugf@-=YNnUg58x<3#pvvG~tKWrs}`JH*;0zeAOm zQ7=}gIO$aJghzfj`+RR8C(vt&HczLhw_tT?_?<+##Pj@?6~oQ|E6 z4<#gy!7vEPPg3|mg`WXCP`kWE^^+l190A4ew($S`NG0b#k}pQ8rQPY4de$pFjePwL z{w(?}mj2tNQR>%Y2~xhbKE8gi#H%d&ln8&rxa?!8h~X!ni}u2yIg~#m%!=dXVM>4H zS4RDMPSxKmRew?3;&?Y*`h!!&jqwVe*}3H#$%x_>M`o0Ibb?Lt zcPYm)%4LSKbK8&Ma{gz6cgP5dS6TA)x`;m`wB;82JaVoq7tK=d{;b-; z7G>v?mHs74ALmaJFv@)c8feOI*6~u2G^Nk$hf4WwOMmq$Vb2j-y`?^0sPwE?dK&$o z9Q3!ze_B72g0qx9PpEd&{gwoVDtx`_pSxB69O{YVRFPlISE%+E)m9uwD?hAP_6I2r z-)xy~yv06$z`Q8sxBX|y=Ncrh-x}@mXOc#7i(~63$={{oz@-ZRN!70`i$515F5ZLu zy45}|qaL|4Ngs@(l|Q2y6-Q9``3TKnv0I_4FL@UEtH&yRQlve9QswoU(m&6l|5HeX z^4rlR(}R16!-;@P<>FA~V#JLVs$J+({*Q1Zj`}H5PJOLR@HA!U6I6baEcv|*1x@nn zEdGRnZ~#Ep9Q zHR1s2*`@4|sPs8>oRs5M8zaVh?X5xA(|*rDP%!h$$%fq#)cPD?pn zHdW%^1?@{{6a zg2s3xe}>GLruv^F6@Mn?MTx(KHgDi~6y;6Fv&!EbpGw;3$L?@TJG7{F#IQq*sE-)W zs&fCmd`&A*^|4;nM}!e^JbaQYm-wwRUAMx=pDy*;u~`BI3P0&AiECAo|1qU!r)Xag zH&lP-OvV2(}x6#q<=JK@f`GF@Yw_yZ~$`Bkpc zKS}ZbXPV@%ub2GSqP*!Sh5qC}i59yhq5&c~?Vn42hh*1W$x=>Rro`1nTZ(p&;!MZQ z636gO9C2eLzdKpt52|!8gh1k7srZe!J^4hL?rzoYBb(y5;ZTV$S8>(IR|w@rcIX%( z<)_Ju*5Q%3wVyWQ7=_y;W2PEkJ5+tNS>nd;5P!%Hi62XTL(eMZ|G5e`>@x;>igI~X z>iwwFza4fUIUTD1wnE{ZD$cm~NMM}Oe+&Gd=0mDFC7!0@-i2@&;SZ`_8~*b_wv_L+ zv^zUh{mN7I3*k;2sX4@tan0>AL6G7oQ|0Sc9TTIT&Q$hERQ_P-e{z)gkf!2cz1qNL zjI&Nue%_@f4iS#TG0^b;_auFRvO~6NFLEvIMeBHJht_n-sJg#e9m7azn?i_L1kr)e_~;gKfkohHKC@;S5R47 z;V&xmEy%Aanp9X+RC*@GZb^pQY2Zf-bqegSX5C|U0UEPs;;iA_7&v&ixyT^ zFCAZzUtZMLdoC$vUte>L50~#sD!RC~ba8%JQH9@AI(x><>?EISg5s&HC@l4tR#td= ztKFv@B__!bQ&{9F-Mfg1#jssbVI)?=NRcy}Jg%^)IKQ^cZy3SUn^H9kC(PciiKR6@ z!{o?*EmZW313-F4Dj-)9io{n^Sq5)OU+hVqV7zRUl&`b`UR6<0B&@YBBF3wti!33c zo4Ar|{Du4M9O2xP%8P1h@)s6OnNUzy=W>yqsU8$8@)eXU@{#db8M!7F)fE&~QR$<8 zkXI)wuPCi3u7r120E5H&O3SOtCgm3v`l^eHrJu^I!?A=T6MOp1Y+v#ufBr&WWp(L7 zsy4nJ>HeivMV_n~+5S|zgB6P_J=2m8oG2F;Wz^6~tcFq!d{v}l?@XiADIkbkf+N}I z8owaFkZJG15w7iTlf(heMH1yAzpSjX!1r6^z$*UNY0n*x!s)NJcm#n;!)$-@IA3;8 z^se&5!z-%F@(YU6XO8n1k8>qWsVy!ps-9X|UF6BmPtSJwJQ*`n(X3R|qE)DKCHpce zQKhOX%SsEDdXjr|{VhcQc3QiVd}tU;YpNO^nFEkI9;I&{@VqATkuFVC7$AnI4P ze}4s~LQZ~FRZ&GDMr}d}3MB~!D5_Bt}etVyqt z;iT#!VH7G>VFoea!o*I`_z?>B#N}Q?Ze#Q*20ffL)f_%kJkCisB70N`G{(jFHIkM> zg4s6wQVG9CgK1(IBP=BBrL0w4T@+@iITcGv;We|1YHG`iSjp~7*@Q)FUM~A*yf88Q zZMa@!tdE74Ojs z%g{Hf5iazbG=#scbz}$0(v}{e;rOITBYx=s#VsD~+c@8O$=1o^h#nU? z!SI=+K9V!KI+jM)GW{JM@6m8n6jUvhwc?kwiv`t12=AWR<2` z*45THiRxq;X&Bv_!lLs0iiKD)qZXSgPk2=AwHj89DL|UWWpiybZD>^Z z)bQCuG54NQSX5A1E$SA^$>&Nc#!`{2{o||h{UxHTl*M~JOd;l8>u9~$T&^n2T2}ad zLf~R;NkNV3Z7V0RCOWC05^610-S*y0BoKXJ*%1eG#p0rBKRV4zF!wnd?e6|Y7%?+y zB$~dYz~%D!$`@b>2R%wZ_g1T-h3C<$E-ZIaa~~zyCDxI{5*j%y@p!1i&jQ<$@}hDT zBxRQOR(9jdD)S352OOc)sLG|%V_!K;IbJLU!7@Ihx58Ht(QH>@)PalW-1qnyFG$0O zk`agteKkwV%StO2sk$v4g(j&ZiZ}f>l03djDkFuImObA0iv&)jQmd`mH_a;Y7yAk@ z<|_4xF0mZxX+TRC=8FY7dFO`2p8x}(n6}$bQX?qv(v-kqN2hDSbybdiV`*-!0bj)>oqXzqd`kfE-cMoSW#Ky zN2h}pRIAH{qtJj|#(s+m&u&&ya=487B{W*8k3*SvUORn+8wiCIWm!6$UOPVCXmyIq?f!|f*PtTs_L)(|) zb0yRM5n4}1$E8Lr9pi~71&wP_HT8zb>CrDx#wMeRccoFsmFHLOOGbW$@{^#=OgLBs zMY(;_D33-5INSe!O)=H^hAk)Zb$nE&qV&Gk>D2_O(KY35nAr$Y>^R^X1gM0qTMN*# zqMwp7trmS=V*?hdr{NxbP57|^Opjw@#72{UNvdmYxVEv=OYG?(8=-7_IdBT%j(R9hj11Q>wl8W(3zWtbD>orbQQwZi9T7`kGbJvsZ3G9LrWR1qoFo}PV8Fl=vAq`6@gPQ@7g02z}e5cW?L z&#A|-%-(Ei&Dt*!VkulB>HW`vMG)er3{$_ruHpGmVt$=5RLG20&8+Jh$_#x}EYlBt zhROD(AbVrqvShKS(dve>I!i{bYw+seNeXHQNlsH|G* z6T7*4R_Q!6#%F78=m5*fl&H;{v2xe5?zW!>_ITI^4h(oKYcv?Pp|ecg_`W=(m)q{Q z5`8z~y>n?XcX%%L!&=NG!eoeunLK{K=AZXF)6DX}B)z_D@JrrH8^QWb$*qc5brC3sYt&6&E@`UDdRUfSWL@a{YBS)%I#^qnACmD`@Sjk>+7dc zJ{BYYU(-^V)6*)j`C(1v0_q1w+n0krhh=F| zte8zd&sbpl4b$-?7twyZ|Fx9F=AVVN`PCS~7T|-l>LT8?`StG6t4+o{hp{QkUsPA> zSAi-z6NI<@;y;^8j)+m!a+(>M?%z0awboz-7oqTy*$Y0}!} z#Kmf0<-($VWqFamdMQ3SSy(eMtFja;yXlyWUrbY6_;825KXFYGna_|ECk3-nO=t^6X)!#|@5`^Q&R>e< zAXTvBdr4u=M=NNh>ceE8@V#RCz!U4{SPqx9q-P%a|Mc1j#(8pQwyc@>R!gqSRASv+ zIy1(_W?rz;Y^+a(!ItGIlYd{9Q~uwr3t{;pQ!Npye2di}4+2g)vNy?C3Frw&w9vG` zLy?HKo+YRDaEOIQUq1HERu?VEFQYZ^1+>~^ngWicmhiAhFk4J37&G^3tAyc*VY~nO z-YVA9PqE0D;HQlWJzp`4+Dx<0GJ+XjLwh{qvFJs)F(T(Ll{1Otl-C^yQHDytK69># z#s}oacjE^jrxAr709_}@l~Zy^@rnC^P&I-~{vm-%QTPOX#!uge7gd+z<8glx|47Q> z6k!gEjc+~wxd9qLtjsDOgTIUakpK&rv>(+h3)A^13A5?yD-CKPz^DXSeHdNH!ll@H zhHigBevRk_s9CVSWQOUD|C9lWs}Gv6iA?^N3`iAenl;#LWFrh`|F;$d`wZ8{y8izy zjcM-z?Zi#=?%-$cC^o^YqVWG5VmqZwBMp!DisDlRn*;@F2 zu`mvOO4;qPEaDHSbdqr@7_^MA4Et2)|lJSi#TdWj0R0>#NgS&T4GJ=h0PPl}9U^S!~jf8doe^~DN zWHKd1*|^6QQ~jx4qX^=$$WduV`!Bb`^^-~O%PUb*iTu4rUy1bbM&vZY|DPuz+QJzA zB~YZqBXOWUg?}2zjTylcdbRSrfcBN&(7ZQ)ADxu29PR&`W}2)z(mc+7{ijvoI{a!$ zrcM#M^gWv^!V}#)t0iXP3zEgvz4vQFxyATfcCR!+Ky?he)g_^>pBW8|j8e|9a`Y{asqem`V8UQn2RX0DZe0R>2JI z52S2#FMo@@-_#%e4djXy)vDTRe7ad$?XS(JPeyu{-}>lK8N;ja393FjOHp2l-jA7y zDXmB!f{Q))z5km^>?7xmk@f#d32QZa9^z}IXgz47d)u=;FT5&~t@h{b-j!*8CH%7R zZ0fkr;ThxN67g;1qN1hht4_^;%Ec(IwgMkKEwX?Dl4G}?F@%Qy-%l(((kNj}O(oHC z=VZ^^r}tsHn`BRejOes)|K*KA|}N zyU4K%YD>!s$JSIAjGbwIyIC_f3ts@@e`T#1Ynt-3UoJR9{Ev&JkXBGxRDixYRTf*JrSP8$GJ6L_pP5LADhHu1MVo(up+?+n6U4HVsLI7vdRc96R3 zuN{nMg9Q~Qo=K^cD!oDfwL8g?Zw?T=#4%Vr8G<{~fuz%~r;uc#=%BYLt^QiffB*g0 z0#Pi`dcH>gqrB$CnRVh){Bp_=Z4)p$$ZxJ;&ufn?#V>LU)Y{GG1GF2|^D6KMt{jd3 z0e-OdzQUHi_Aulfqd!pgsl@!u^~6n~B1ZzO(-oreC#?-p8dhx*+KD{lNw zg%vk`r^SjJzgug?>!Zjwe&4{#Z~PvV6*qnd$%-4l+h)a$->dUY@~^662pz(tf9v#44=sOcYiIFqjr$vUdG?e__G-9V|X6Jk74<( zV))05zmDNwGJF}s|IF|fhM&jeFK75n@Ei50Jqm98-V2G~{KoH(S#jfcrL4H|`?^-V zm8JVG(|;wyZ)dn31#eZqD?;))e?9XDj<+#6#&7XZGFuq_AWL^Q!|{_k=HX!Js-G|s z&zuZDh?G)Ci~8Lmf;hf13ZBH|Y%zr3S`r1{5(RH%cHnZnjDHH#Gne7&C!j>?TbQ04 z-yH?7XL2rL`nNEAB1?C-`W+jBIKGPUuV->LG5jPZr$zma5J4PY6$S5Na)vWGyBU5b zOSg;3;dr8Ihe-s-^P=F(qu_cJeC0N&9K|0l-^R)}k?HSX_)Sdz#3;Bs3Z5GUcQRc4 zRHsNKh2dYZbh8+~h~Xs+zmVx)#qdWM|1yTpWq3;zd_@#|WfXi%6nu9Syo2HYV)`U9 zf9AN(_&qGYNsOQ4by4sZ)*f;GE++pLrcVl!!}0nkc-HnFf8+cb<6q9?+Zeu#;aO4e zd{k{9LAgJHtO^_!fqzF*$8a ze~x!B{!xryXZThoU)w5~=&$-|H1X8N@Kuc8!SIa?U%}!w$89X$a(rbJJXejsNFUDc zWa(bT(oJM|4a1Wd9$?9 z7;gL~9lfxMl{?3)82?}ZqOc)E<>KF0Jlb^`=Iqqfr9~nyG zx{UF2Jd5!cGX7kKf57l6hO3`)7f)lY+qrzH%3iSbu4{2GSWF?<0_x1QnZr=!JN%NhQ6 zCa0C*jSO#NcoV}nG5jKyZac$sft!9}!3{srnVpH>ICeAsdIQ8Yh4FJ-W9?}%ljC6c zY9?n@6g-c$znnjw@z*gqZBg(NCWrGUF@7J@Glk(}8D7G0C&P_-S%Nseilxi(Dkf(e z(|>mqyp74>{B?}~6(*;i;eTVegUKm{vW7n-T5!W3+^n7wzj3%3e;LE;qTnl|;BCx* zxEwFzUuh(YYYEeb<5`UVI>ukm@P!Ot#&Gr1w&Lj~hQG)7+Zp~O!#fy$FtdZs@V6O% z7sDTC_-=;Z#c)mi{xgXVG29ykZ;66$iGn+r{keP_<3Eb!%fayD7#`2?JxtF;h9A!O zlNdgN;VBINmdSB5+{XB`7_NR=TfCLa@b8$MJcfVI@G6FX&hR>hKhN-bhX0A-%NTwt z({nk)2Qa*q;ny=cD;Rzo!&fnU9+T6?@IegU#PBbeoOXsk%kU0{zr%2y;eTLw7sDT8 zxYj9^pua5)w=sMn)6>E5sSJ;2cqxV|W|Gs~A3=rCZ1FOBjDW!h9AQ47KYmyzMSDk zTSSjr89tQpuVDD03}4Cc!x+Ab;lmi-#_$^$zKP+T3~y)naE5PT_y~q~Fx(hh&|{t9 zM=<^_h9AlB-3&jP;o3JnetQhVZ45t_;SPo$$MATDyIBQrGTh1d6B%B@@Fa$hWOxe0 zk7V-Q3_qUndl`NL!?PHk!0=p#k79Tp!%t**3ByM-yo%w!V|X3IPhogH!%t=SGKP;~ zcnibZ8NQt1NepjgxQpQ{7@o}Vl?)%p@Kp>S&+s;ePhj{ahVy{f&hSiDj$0T$iOK0; z_%{sK8J@!ME{31Z@ZAhQgW=i^sRYHy$qctKJe%q1VECDgKc3-dG2F>;H^UPd{sP03 z7#_#cO<{N%<99QB3d6k&pUUtohEHR7F2l#Mbn_VQVf-ZwKaBBLG5l=CU&rw246kSS z6%1d-@H3hG7KVEn|8j&>2)Rs}CT4boMw&EDjPD2yHEEil*%68{X_}bX5&Gt5 zBfm5ugZ!H`O~fGoCQTDCJ3?zrnkHU$gdQO`0Z7kbje=2@~Ysq-mlY`8R2r zI6?kRnkGz;f0L$(66D{cj}o-Yq-nwg`8R2rC_(;BnkGn)f0L$(5#)c5k$0;SApa&!6BNk5NskruH~%s6PZJU-f0L$(2$a7`(*y*{-=t~c z0r@v+ns7k=O`0Yekbje&DCnz9nkEoX{w7Tm1So%#P8IaICViTq(@dHs4v>G7rU?V& z-=t}x0Qom*ng~GtO`0YEQ2sv|`F9KYW0Ou3^lK(f19#-#q-ns8{F^il)RBLarhz!} zZ_+f-M){jG4X}}alcs?(@^8}VfD1VbaSJ1DSG!1x>f0L$xF7j{EG{8mvO_~O<$iGR`z!mv7X&SI1|0bO) z=zNo=0V@38q<=5yG?ShyXqQRP6ZA-vrU5I;-=t}vit;yUpP;|_!N`A}pg%Tg8mPkm zO?rW#*O+vHpdT^mLP6hS(nW&4!K8}?eU(9@^_hGUwLb70YHvW#A01B5vCx?o+GC+7 zTj&WE`cw-&%0eGwp@&=OAr?B$Lhnfo*Z(^Uz0E>zwa_0}=r=9&s}_2_g?`RLKVhLC zve5Tg=sPX+Ef%`jLN{6H%Pn-Rg)Xf%(63tP^%nX$3;l$Je#k=KXQA)3(6?CVW((b9 zp)a@4wHCVELKj)+`4)PPh0e6l9t%C$LQk;Jr&{Py7Wx|hYoW_6bdiOg zZ=vT{=u8XkvCxw(^aKliDyP$O=45*VKRr*~E`1SNMQ`v@n>Vl?W2RR}V$2k{+~#d) zb9e)uBtS0_BuNaXx@)t7Q=N9l3_RZM4NkZjn>+&^C-|NQpM7Yi!`qw{?+wh+yv;4k zkN{rw7JKa#o9R!RH#q!W@D}P*931Ek=5}~v zl&Q5a#TP&ii>ZACMK$$ks3A=~3w--x>Pg@;6~7nV-w2A&g;Hed*YP--;?s9?i~kLJ zSQNjXps0#_O~t>)-M%P(AwE$v6%V1`8bR@Up%f{8q{!E0366}^FZ^-(3^FoYs6Svl zULB9YxLz#4kMs+_@9y@vKGPRNWawiIo4vuy5}X;S+wtO|isv-&=ySl}Y76~{kuS5zEd{8K{maCkMtwx*?9TvDh%c6S*?85 z`3XsPAWXnoYXr7k4}AyzSQ3342KtB;PHX_o2z`cVlA$1r<7^e@siL(^o+ua$U&u2G*tr-qb%wYNSb`)MN=2 zEl9V_cA``^BcOX5^cepbeL6IJ>TSq;o@RbrZQkbL8Er_vdBWB;Fh=gT$iP&{2z`&4 z2mJ_edjlK2L9IK>@zfi51N=uz{>#Ci9%xf?^kea6=Q`32;Z+y)AI2(C>ZC~RQlUOm z_GFNmP22po46r^Y{lFX0Ls^)uYn#JfzL^6=&=P90&yye0TKK@T zLm)L8%cTSr07~>_LVnx@1P3P^iR~be-_tB)WqIR7%Mh63h8kCk=N^1$5U#)rB8e~b z(-e$0APZr@+rO^EP(=U7PL(fGW2i-q7^%i)k{_IK(-D2E@soHCH7UGe^{-j51Z1Ycq)(?QJ zPtoj+*HU=}B*YW0q5K1zT z)Qd7lzbEGA;hp$jJ;1NO4!%$ZC3peyRct@55&BV8nEWwuG{?P5m(<<<;ZX5sdKc;r z>h7=~S5JIv)fN1Y2>uCwr_1P{y@4+8ny=6DuG!t+8?(Xt+Ry%B5U@=NuyuDJDU6$q z_o<}nCpY0;t@d|t{>h)vu6u7_tA8*ucqVW?&;=z6q3MRW`JiLwW<(}wjYv>+gzt)b4VwmXLr||zeb$bh9pZ3PI zpGjGEYu;5;GdyXt(`Qacn2U#k(-Is4Z$`Y-pQMly%t%N?c@Gvf*WRR4J{s1gKwIQ@ zl?Q(iDIHm9bFz!^_Atm9p>;m5^iErqj#(bT;F&qCc$&dM$sMkBE;sV<{CSw1BDS=t zIqAiL%aojuUzmXO%$n`-WM<`fNY80PPuv}$g=T}JxW&6Fd(PCU_-VOb&y&UT)j5Xz zDYIr52mDh0G@(Vv-eYI)s@a}%JX3Q#^hntwv>n|&!!Hay(w~}%=`t`KDUDVf$Vi9} ztrwgy3CWq7mXVP@ljKZi`rZo`(pNqYOXu32bjTDLv$AvW_Ao6}LGK8A!wwl~GiQdC zS7=txdnnIYGjnFo%E)+{3{U#W#ESzt3GpD0(n3zKdsoewIb-Im^Jn^yQhat!T8^RO z(PY76w9t>}G3vv=p`0T>&(g2>8XCYAycE@Z zU;&F_ZTeuzhxC1iP_IW;ZhHg$CM1|CePrz>lX%{CiYJD$;DAKjSH zs;(>5wGG#d)bH($GmyM~*Y@sigrzEP!?Uto&j?Dk~*0 z-tdEXFkux4+}x4SCT|{3*n}HzV5$wM)C0VAfvFCAqaEp?McrW^IxQgufBgwb_&YBl z5r4}PocN1!!{5RL2mWRx*z}PInvjEA{1vc6usFbuG{Ec-uRDY&2gDZJhrNOGRh%C} z$kZ(R&}?Vgvq%}e-Wf^up>y5osoVUAP|N;j6isJr3;JH(pe$Av>mQ+PT(zzPmuZ2H zx%1QJrOi*fkm44isJ-bn;3%??x5pnh_6LSO(e}3Vz&7v3sWwR_b`{$bx8T$_PIV}# z9VDV?;wG@6Lx%6HruEcIsOE(qd0m_Jj}bS?E3!mq2Tnu;T;&rA|2J==h(%y9Cm}1e znw&TbSwbJI^L-XGg+EJgZuqK>x$?Ax67pmC?hjDG8~9H6^fufF2WA8-<0&f{&4uWl z2Vi>)R|ByL-r#f_d85!8%EShzJ5Y(7@I+{aS2ch(`{-#2N%Y4}f3nb*LjAxqvU+(Z z)TWS|5eT_9qo?NDrKiFP$jaQ=z@i)?b3JRJ<&J?)`y0mX=3eFTF&z{%E_j6Pkk=% zJLaa%TPxq*7}^EDmiKEz@8X`7RmiT001TzR(0?H}65=?GBXz@F@k;6htw(`Wu; zyuo9-9MsCc=Py_Vg}b}b>}yaOh&BG>=b|66n(PHn4!wgiK#*-DbD8aMdN4Ktc}0(! zB&0G4gCU`IwfQ{0^HH)}8hp>zoYkF%aBJ)QlPQnz!3rPTxlZ<6jcB?I38Exp6J!k! z^Ts;qjW@3$KfL(`+iR!gk{Dj9pz=Xf2plBB2tDsBl5$OslIXDv%e$_R+yum@o{ zfR@D@JX<(yB3>ci92PvA95zWfY?T7zdwiAd^2nS=Q`fsqm(GeR@Uh{n*Q0n1Kh1Vh zRi%j-a#ixz0=GBlp^zwo7F@PUxFUeq1nG~X3ls9Xvz+AbPK;vhjfbPCmc=IMhkXg3 zdNMX4G=RLV36$X@TM!3~`&V$^%U{yYH(RX0>)xxr+JuaW3WLCjNXq-jjUr~t2JGe+ zP}KID+Xms?|BUi24G6E+ZVK`PRbz{WF3!rv0nP{(11r z^uV_;H^vK7h5bvEsdIu;$^KQM=~_u34lw3YQ&o?9y0)lmE3TPMyQnO_*T;V$%OWo$ z@QqOxns8)CHie71DEcp`1(wAiipbkM4?|8Ah&M2gg2K%S^$?j6tV^_yzC$JRmxNYe zuv4RG+VodGr|OFkM}HEj&0gZC_PX}MH2ago63|$?y@7Y>AsR6hGXmKzH~=-)piw8h z&4|Zo-EUI~C#3~8$eD`?hxK~@r7 znz!xaNB-IWi5`lc7F~pcszm6<}T9-&jlGw$dpH0aHFzX@xW_@S_ng-+k0rP%s zXfy76`;(=NfHW%TEr|KXeL&rty~$6w&XY$33 zq1j+YJrZOXzc(`fsnPheBJ3_U2BTX@y$%u09Mnm~{3j^UNP|iwD!e_mw8h<7 zCBk3Q0_z&vYEPE)hx#Wn;!)oqR`%L1hdb1(t~yNVrZEcaiWx`@TyQ^(1|_)0V=Pj8 zy3z4QCmgjMe^K4>7xi5Pfhr6Z&}+;V0|pe{)U3{H4FB?C$ZpfGp-6+cfK{8R@tv#m zx1bS~40?T`qYMt2G=kHbFnU{co6`KErh(g!U@Xl2N%k8JK$KdZfV9?e;zt5j+8Hf^gt1adLQ zW#pGy&xXq#T3|r!8QzU&+o;44N;4BNh32*ook^W`Ps*~hiB3cEXFHdjJ(@&_KKA^z zt~Q9G_AQ7-EsTk6`Z;hmZxDChz}dFW$Cz(ZxtaBqR>-`;D{cCZRN87lHFs^K`Jg$^ zx1xhWR*u)-?*MmT{c75>=M_3oU*8+>AXvnW9e@sU^L$jrkBrq4{os$fyPqd>>-XSQ zn&nnj>LAasL zUz%;5f1DT$V@zq*D|=Hlb^RLF<+5Qg_4~{@IdWDftOuu{q3@fRZ;?kF)ntb^^#%L2 z<48vHDYMf9m|fiIU9+`6Ora;k@EM|-q9UQYwI(C*LPqLF|M6-brt(N{!wd1=hBfi_ zp%>`hVD)Zq;Nsoh)VFIt&BRpZp&2xp4zud-d`J$3J1Vy}Z}a#>Z!-pgu5Bo{Hu5=V z28}qqn79&SQ%C3I9{r2$mFP9W8<5g^tbVLTYFS84^z*WiBw_~U3>HtP6i_IT2KPrr zb6`?~#+vYX>W!stsC}H?!eFmk|AL%QiNniFn7z``0Sf$#ZXK3fDT1iAn&HTUoTvz0 zLY>1V%2V^?wfDm>1MeGUygqacx*Dv;$nj#@`A=NMk9J}P zJO@uOG>{#8v#DwnQ#)9o`0ib#*R)MK1`P_}5B8>iVzTO~HB$65Pz7@4L5@qVujp-f zi6*<;qTlV>Oump*BG|E7&xhRD!1{|v;ob>N`kU*qZEk)wV48V}7D)Se=T zOxxvLfW47^Ai?CI*#LXv06-Y>Bi>1;CDF*~Hoq65OAXQO$c_1;5RGvH+7rY7-lRdyZw{Qf???jTv=>d304d;$fv`@6W)L7IiPA8kiuH?@E`vy zj9LSu>I1f5^6w#PZ{Ptj$^(wU8{`34biwEj_HPF(4Zup?qpoBl9;3)TH0u$VV+);v z@Qv}WNv}b@Ep8!ehnjJ#`g7^XFscDEOqm&sdEaq(PoXHgc<<@MxBQGOH@yex-C4HK zVI;RFz6KBX2F^e?0P&4lkKh>zM!Ru7R#r7LDrs&083dCJWWkfAY{QPGHX9P%-6;>7xoTK6C*d zw3r1NFZXlzy!Sei?0!w8Zll}Hp;Ww}KOh`A?)q-n7GvBz*f|j;L!pBfp<(ZyGSDwX z?FgOou@V1hZ8`LqNh1EYhn_M?@${M=(hKHsW=9CCc5j9T^d@_KZ&U~>EEJp zZH^mtx0otZ{gdXnW5Vd*Kt$QsQFt_=*)}b;zu!hZGuiK%ff7hV*Q%}dU~lSca}it7 zQ^3mH6q~q@nEp)jox{DU|Hdm=tc*(?A>XZsnehH}8_3SjBfO6e`7LcKeW8Cau6z#_ zcMJUwGo3?(-#4_ydQ;o|C!L;DyR&ImMlkM2u;4xWlWpEL9f-m&U?Ff&I~rxE>(&Iz z_MZex=X*VgO8Hkp-+|puHQo{{)v`~yIDF;+YvJDD05Sgs&o!%Z?LpLI#55^pN%iaA z=KeZ*RKnam=qFHk)`a8dt#nEYn|!e>KghhaLs5%)g}Xwa_>A z&?Gx_bq`J5@ldr%r{##wycn=w0Z*Awi6sYb>i7Pkh|Kk8o$Mc?*CO_yhQwwx7oa0% zPMNFzP0#zMPdE&^`!QM)f+qfG6?96EAR1R2bMIm0C+1_&KZ1+8!+OUlaLH=o+sAmZW+^%=NM$GiEnYtjU8_|p-dg?o0M<^_!wn4@f z(F%Y(%RBl7FWP{`M?H;3nYs-h+(BV;`~7`6>vhlFVM`%zK^VqT*6srk{{kX6y-h)>?51^=C67_7K(tXM-kJPM!t5yLw% zeS@Xh)J^{5yieWUv3}ICn;-rIk+2S_m;@D{Mv}Eobopf)j|Nw^^BV$N=6VNp8T}QS z-@*r*G?eXpTg6|o9|D`c;A6oH<7t6#kvG^cqj|9-6YIvo5?kjmBfsf^54^#-Vi9o^ zh9wB1bv6p|=aYZ^0a;k1ABWD&7BG>Z+V?V3zxE%4{;;$Da_wYqu*&8yq`ViJtf$h< zLK>|a&V|2;k`znvvOUI(p#8=+dbjq@&ijj7ASr2Qn!PeJ5w8qRpd#NShCp-Yi~fbV zj-UD+y?5rxwKrh(((K*}`_55wVg7+#H2&}(Jin*?H~YH|JR2NS`p z!{WC2YTps!R`(}82~hdV5jyM{c>C#klOsLsXA5X>Umy)~g)p^e}Bh>;MdQ8WTulPxUCn z_=!?wmR0Qw47Z_kWDBKjqjjh+^m9=p>7p8L5Ul{oMUDABBk&~^ZogNd2ZDqxGdMn> z^E%aELk${@opX8)wA3F4OOGkc`1{N3apOyo?Qsz7^uJ<{Gz5h{?6HINh-8l=Zh}3g z;i-N%j1tKnYW|LDoNx^a?TEdmV_+tVI?`C%Zk|USg^Yy3v?z&+=n_={gDLtXS*&tX z-%P_u!L`d9vz}_!bv=GW9%J|Dgud~jv>Lo+qwu2hgcrHPy$F#}IFjxmr$grAdt7NA zA}zW-=585G6Vf;bezdOhBr_gT*;~-#J~nY@T#xS-9Rle3#2hY2S7CyGcWd_U9TkXn-(e1aqfOT(_nezqIU%S2O`TLEyJm=mO#(@f!4Dgl)hZ}366uS|Rc(218Y z(`0YF2;E)ETNOggjz8cYUS9&q$QL!{*vNs21ne|W`O~ZE#pY9*h$TJnb^4m0`_U#n z*!1WA_G`SVIf`d2m^pDY|e2 zauj@A*e5;pL)t_By`Cx(rfE{ld%|Z+$#=!n2Z+H$VD=1isE>;+31a_1M&R*;dZgh^ zebe4}F)4(qkY(=sYue^TX|;d>PC^rpUYay@lLfp0Ve$vXc0L#&OWP2efElfjVTX*E z5d6#VtaBindDhRcsr0OMxDy>L=@s6<77|JhfntJlB|@c}#e|#}`E3QRyVg_YRdK2^ zGo2h6&j{>>bNdHn(hTN$*qbFhM3|pkx&R5or3a>CrmM%Tl{<6K`vkToU#v&k*wKh_ zLOYQ9GZHj^B00XBoFhw)Af6Pw*nS}$MvN-%RqwareQ&ee{vn228}Q|d-iW86XK|<6 zv7X|Y+|(@nVHsYe!ze6s3#Q?58l|wNqaVU-`kFoPHd%w=ZMWhzY3Ced7hZ`@=m&do zw|lAXk-N!lEW+cq>tC%Uqd%C?MiNs$kY6oqL`hJssRxMS$K15^2dXzWw}rkIlYxzR zNE>WWm0q9^XD)(oW4|sOGoIS}Zv7sT0zYd5o7#GUWg6)Z)aEx&xb%7y`fO4pS0 zg?GiZWQ_bpY`Uj_g@o<*Fz6v_aiFW zrfkf)ZfuNgCk#hut1y9EKHp_;x?Ma!#eAL(SD?ZYM zp7-=K5+JAnG}RrUV5kCKOM0U!cMdW#fH1=Y(z$qpicxM-m=OHs{vqXe9zwWDGV6e!K`?MN;_QQ+iPSR^$DxJhH^o$mT#WPv&kE~Dtw0uN+M?S(SkG!hw&cd@bZq6YVMS{zYH~oQPJ_s6;PZw} zSlsG%?^@r_J`K2RL-hg5=)^=-lR`DM5$NS%fTI7E1S4dLS^2@=ug6oYGCcy>bYHL4 zo_p0+802RvP1N!flm_MrL<~bD()3Bs)BC;Fqqa~(E!U&?9!X&baz{6|>yh9qu?&u( z8)0cq~&W0W%W3DI-`(o1&XKDH5sk$O+ zkbX|v;E9d#Hly;S%dHRNKa*=L;6dG|^{rK7tEwvt#?};33>B`2RIot%0(^74*{GpW zq7h?R@z|o`!1W23S`>XDGUy>V(dONS3q;b4CFI8I?>?jIXv?y2h^EbG?=Z>lhkx(V zmp?-}mTpMw>9hxbL+s(Er-Yt{E6Dm`df+?C`PNG3Gpd0RGBHpIHR3J%lQI}A!?a;f z3Y~Z8JCUf_9vC-CR>Nfq)tKR6%v8eA5`F_GWd&8cx${j!j@dk6&fReB+6|pAa`u0A zJ`KCk8*IP(RbR!63WUN9YesB9sTsj%TAPRgLrEHaV>X%E4q;R50mHWNEd3&EQ9vnv z`XPK3GmWlYcI*Y0AOE#sPhDfeZt-HH5a4Q~adHPC!MH6S;exul1tcBbpckuKZihFx z6mu#@xWFEvj2IYzVs2odK37;ME`#2s|EZy&%}oJEXov4CNec=ZG@#3+6#}9*QSYD~ zDNyiGZ3i|f%aS6c>An@u>AF%~+i=y(h0Wt;b|5um80{lPbJ7_X2Tn=?LA(pM;E=(d zQX_zL#>f)+B?2$$xYD8DN_7rJde=YcN+Wl)8+sQu#!*;JBy-n;FC&=el&c(Q7}8VM zQ9D*eQl0iD+CM`H5g$R=+auS@aSMeAMw;dqZ}ehN^~Grd^k$T51^~*fH+a4HVj&~7 z&3+AyAHAbDksmo-o5dQO{TLj9@WRJpXHJYF!$fdqLW=o~5N1PG!8+|mHjvihCMM5s9P;176?QN1ijcn-Yg zj@TBFqqyw%Wjcm)=$|}68sCvXu8o;Gc#HSG*5^_J;t7zd z(6^%ba-&G8gIPNd^QfT?fi>bSdJi6h^$=1iv_(9qH}Uk*o5;n4{X(}>%xw|;RL?rV zrGNN1BL3BQpg%71^^y3_S4O?H#z10i=K=(i2dKPzT+Ts?Q(ZOOYNsgvxL4klI)%H{ z*}|Kt`$lWl;U0Cp;ZZT_)hxp!DdyDee+BODJkZt#K0cKrgVPChYIaBkN} zo4z5c&S!cqRO|e}c*l(8YsnVmK(!yxXVTK1f(sw@oG<565sHR%CBfQGmLBCte29lBdK)&smA!-V zfu@JUNMe)lbQ@|Q6mM8(!ye%ld_hV3s?I00_RF5tBzorr;~kIq=I#~s6oEnPiI|He zCG0m-`?5OUH^*0K=KNQt%}EdJ5jhnLg3zTVK0WY06<`VqE*-NJ@D5s;+h(k59Yt2Z zok|r8asQSYiiJ|qKk-nWjd}ChH%w*3mbFo6KWYb~bUL3=cJJQYxl*;y4VAeu{zE+2 z2^FBdPWA9&xOUuAFbysBko!SV%*taNh;GJ}*lXwU%7O0?(k3`RKBKu7gR}Si$ub)s z&^+qJD6Y(4k;ny=%^>Wk!;E)~e;j4{ zOAi{~I|?aS$$MHsYE$0Sw~@m^ltX-u<<^kBKhl-T+vtB}G%wtO)c~whW9Kt};0vW0nVHYba zh?=yS6E)q|u!%OsPall`)1`JDZAJ`Ep{4O3--IFKuo1EI;qLa%2iWdqt36>$`B4x4 zkC83NisLP^9rlE5M2{letgnhk5zYGhmBOqyfdduWX4ru|W&{3ZpZ4meiUBqd{tPNs^b|TLyKVDbbN-5J;7q%XeHdUJw13h0A4mhla#DcwP$Vc(q-b6z@ zX)MHT#JULf%!j-d2z&%DL^I?N@T8+_6rYKqn}`4|83C|n*sWLogKSx$A^`r29@*1te4nTOuxrf(|VJA>);GUvBNQ;d^qYVZjSti|YSpRQwb$<;t%7lJ^r(+YUXJIMN}d#6$xnbFvyxW`agi$dD-T7fCh>hJ<0SHzBVtr<;sd?M>K(YdGC;hSOQh z_SJ(_{B9UOE*0-Y^Sl3#y)%J}tGXWlgJB|BWQfL^wl1SaMXL#9Cz?70NWhSYNsQ5` z0|Ant1Q3QSu4o{J5|nD(>QY>4wM}j6(x$CeY}9D06_-|9tH!MviVN=AYUlqw_uejTC63Wb~Kok$)f;>&eZp#nb=6}VD4c3XZT?7ILJSR11P52e}8!6hF}!oDXK zk?msDcB8rWj^$0fBK$*)BE0@dTk!5e5#AZkFJn*I7l}RTCa%phHU+fVc9F2MZSSIv zbF&hxbbeNN7}S$EUt+bNW$X!q!Le3)6R&r|xnC;`Zk*efEL=Hv!5xEh?j_#4bM6D) z49>YFe~RUt){D|!DeaZKRKA+AyLk(2Zq0{@#_7QTkcw`#4h^=NMiC3!3 z>Tlovqd`saP|`U}@sT|4jw$}$A8b}Cf4eir;g0FQB>0C^LOeh36PPe{lhVj+0A~}{lfrh zZ2sE*&Zb)w6W@?mgBa#HRBD`1W~01pMU@xq^-!3oP|hW~dnZuxa}~1`^}>b>7X2U-$BAFrnKYe(sWg_&E5CfA~$(B$K-)q9yka zpSx)gxtk^w%K%HXU$O|u7xfP}JrW~#gZexDFg?n@0e`1gz{&lce^MA);e3e|YrLJK z68G_XC;rZ33WJ+wJe(|C{hdp@gptWT#kYCy?(d|5a&UjA;bB_@m8WRNrQ+M25H9Yc)t^_U7=98aqZn7CgIv5UJS;yExdQ<+Mjqc zIM>!o%lBh$8+j&bRO6o1{AbK&*L<>1Py5N>@2r%$r0xijJ2g_BS@i z_BVdoZOhZu^f#_ZroWygTD&|3AQ_IWf%}6gsaJTv6M1@Dp>Sha33<4x_Pc*DxIFDm zLU(yOf;WT9)71~`Or9Pl#!lqvU56Ly zlcx`P>n=~j!TDw6Y4Pup%afX-(c?{Hh{j%%Y@TI4(DaUnrTS@BKnB;HKL3rp^R;fV zV7eMW4^4%$cibPGS$Pawrxv!K?4;tI;6OZ<6pwb_>2xQ6pvKOxr*+@2Cd>n6+ z_6Kg}WvpM;4WxH|MDbIP7tiEj*T#!SQMjFr7aJ9(#_yiJPnf}$ONyVfx+&)?!XLxl zxY4D#a6c5#<8sV1skG;AFX}j}yJ&5n%r`MzoZqA8>i=-H8#BjHHq||NfSksG&fRi$ zkQl$(%n`jF%8~hfHA(dTRl-a5G@Jt@W*Cooq8)SEJ_w&k+P**hu8dlT(r^Qhy>nq; zeA|tk$#~(ncH31wf2Tf^#iC|<4f(0%tIp^LH>~B5D(9roU~L9|+PhOKA1AmQo*mzY=}X?mdDmlC`VWE2XxX&{Fd- zGNCm|CbVP{=ARpR)fVP+rOY|)_1QAgD;u@c%0?}BLNB{0gl9@}HWT}!k_O}>TnR+2 z86KDqzC*FVO;7GYWT*c&*)a=zmSpdk1>RH5-%94PkFnU^$A}>fdq5O-0O|eRe`I}z zZr2U+U#~ZkvF={AS9$cAopK7T-bNK4KBYh>J!3QPw?}cuEHmp@rbt=Eq~9#~&AJrf z#bg=vR`a3Z?kj{-@6KsonOWSvCR^4t*gFAzsAzpMyuYAm{m9TN>^%m^PKD0bdim!R z5_#(V6@RGD1i!)8+D`#ueTOaQi>a;Vx~)+-o1zd$?$J>R8NYIdb$KX;a8ve-2a(s|Gk$JYW~V5HLSQQ zVK=VPYC{_yenWDZJ3e_t3F}uzs~v4#0(p^4d9uf<+0th7uTT|-F_SE@GJEe+*=yR# z5`r#*7wj|hu>?=)NLLdnJcWu7#lq4hz34{%u)IVx1}7@9cQX0|!b|-ks>MiO-$!=* zk!n$hzwZSbklNQnBX0U*-!n39AW*ZvCnq}9!ys7<5TmL1C^KAmxm4Fne6Nc@g((Nw z89TT&oxH8FaYu*kxJxMz7m(& z`ccI7wL*y{yRNk7mqEqs(xQ&W0AvN)3d_J}Nt}^$fFc{}q^X!AMcO#TV;?E6aMiOs zENha`-DV#Rj#6n_f!4A=$~XEzv{Q@HPI~`0KGG*cR&|B{Cft#8mt;j3v(B7~#b^3_ zOBe3!6k|1p8U6G9y`!je+5y5ii+_Bfa@WbM0eSHlL`!P5xfXOK;`^S_)(twUXf@o$-n2~`zxmYUFu!&bx~MOpi-E>OZuo7ymD`> z7#t)tw0keUXLrS5&loXa=+B73=2$Vfi2Mc>1IBl3G1!y1e5410NRPV^{2MZNA_SRI z+N46T)FA`|Mkq9PF)Y|DKbm$d@~){b`7R$g=wK%x=U< z#9%IQqhjz2L5mpVYS{=s$g>uM=j7jWN}s9ruTJKXNdnnPGd;YB*J4;P3ewy^y<6%z z=1jMb1f;w+ng|V$@0@JTU%{)UY9Uy89I}^z+GU!@ZVabbni*SE{$~YMUuO^G75-&2U0W75*`N6i$qzLLI^%)qgDJG83(y z)nPdtNToWFR1P-0$A@-tYmbF23Bu*R8|>n% zg^QvQs5j0p6TX>6LE+mrXaj7!mXR{x`fV|MX>+-~Y0p6o{blghz{_fSXi^)Da?8=UpetHJ7j3~bts`iB%3 zCJa{pdkf!e2Hf^NQ~z+qrT(4!tulBlXJkE3!+#z-toqEXVKWouvz80|PP^DNdhb7p zrJ11n(auTTg~EknX?n6Fi+8!CbL~L?1urbI1Dy@kW&zuSB+mtz)Q8&wwktu9WWx7f z>*ZDQ3Exvm>X`67m&e`{P{MFIgfL1ft1&ExeB-o|yJhr;O4vDB$z=C>tXJDuo+4s;s@7TW~d2YOb$uK`sZXqGj5 z-_|wZvdFT8Xs+{XcV8kbNxH~#6zLrEYm<2FT_~muY1)NFmRD}F*{K&|m2BF)6u~?WESi@hm?sW!HerD0e%`S^EXKhM@Ju1Wqd!e1pOYzkqI`A=wB36X zxRS}`=j()P$;NkryiGd3^Lt*#j_+&-(z^hr;wP#$rCh~b8{e5o*^-a%aNOGaKY|x( zqoi1|ZJQ-h`o>R$H8I0I?+Le*^4miO1)P&>LF!>ZQqNXt-6ZwpR>jshNzJ90QAsV3 zDs^2Bd^3q%C3Pq-29eY=dF_zYb{=;}Qv2?Rk<=LZj9D+!;qAH=+S`=M{1K@bjf@?E z+$@Yy98A=l`@!7-%{f+*yK2tsTihh`BcKk=86l%&61teENi-*i(haIP{Umj0&O9D> zL394(cAJ4}z|3`h2l7~Pi!F}>&0h2J6cEHRa+$El!AR#&?lZjewK6nmxAf_V^GiQY z#?KPUoY>ND18T-wSpN&gWZc}7h{@Y~-Nl2n_osOo+ur+us1e9$d(Wfs?^=5=poGcW zyKd;%rl{L1_C8D+H+vtx&1SEt)qe@^9L#pw`=wM(s95PHVy>2Su2TDtizq8j>Rc(a z9ynKBSBJfNpD0Fz?*Z4bU{1#LYlxaegg;IVhcPL2N#bft5(*MLhE zvz{SVkz)T9KjQ5_xC}eHDd+VLUdL(Gp)fvF9JT+F&fWe~W~w0kE}`)Q;+cCDuat`Y zC0V|rtI5R~I8HFReQ|Nyg@dZtJxJ$Ju~|I!7Q@wDbOzR=H#K_`R)EFucR(?;dyj%0 z3I4!1QNYGTvCqH&rxW~vpYhJux@|y9%z`mnP(kkUdl=;x`Mp#^cln*bE7~n+E&_Kl z(S7lP!Suy(yiMvK+|0{ZebEiXoW*DSgDEh5*Yw3G%DOXs5iid@Hw%AaoPrmHWrpK% z;(I)mF(AIDskClR!5_}o;yYf-XFeEOPQ^HM!*fK5itoWv$*vwkvtV))-yP=-D!xUe zbBJ#>kGmtjkKGg_zA+ung!PN|w=FFsiZyDN3x^zf)@juK!#n+*s&QY7kh7tna2MBA zlvl*{WeJVA#<8@8S27*|o~J;bOkR&xoJ;1>5m7@gEMk&h{T-#a8>X`;a_NbPpExxr^>)yc!VQn|>&1-9*<%5k>Q^lA;>Xt&bJm z{dg4>-DSGQTr}^)=Ohu`qj)ihbKAjdhv?qPW#hi{a> zasF+OFv=mpPEX-jDqz4)LW9ef=ad--QfjOD@))F;|r|`_` z!99fuBz1TS3wYcGPvQO>Y=-J};l_R;pKGtN<#WJ{XMs>e>jh%vGq|8q&KMbv!QW-6xklQfRf7CUhlNz^>l0O zcZr+W#$DG_JIlW=iS_4*nvN`nijjvGvmfPENoH4J8 z>{r=9vVGrR3RWU6BI0OXmm+ZNt!D3-${#Tb0~@Ze*-JCeDi+Dgi@1f19{G_m=ZCLWo7S*N3OV?GX~||(LzRj+`aKjl3t`Tp z_%g{=NSzm^vpEq{yzCk5RhxFJ(sbyv$%XtfDU9Im_I;yZOuFpOBOD1Y<$HA5hmqYz z3Yjt|qAXeQOVcsyX{26!tuLjD)=&#-7V}r5Kaag$NTL{}cog0jbP5wFr|Ng0JJ2XQ z`6&N>i0AtPaA<5-sxs-ibF9#*@sLS$hwXvJ@7zy(`b}3+f>AUE+1EyP{L|BCdkGP3 zYy`x*BU!2-1GIO-&Z73CelZtI)840B*Zks7fO!k15=^Kd6F+}@| zGGQv;D><}Jzv@a@@oAkLUcGi43mIPfT$(#8R0dY3_*(xI1)9Yv*O1EB`YE*Wy>{=< zc|9RL`epoNL%6d#@XW4CHlH*%*5AU41BzhRtZ(xDw`z4Z?Wk!09llTP9 zKzctSd);q+j3O4BjupFs(R>Td!lA?I8T7)>Ox`>Bsplg~5X($^=Z7t#C)C~#>OeoWXW zmEJp+7@aZ~Q{og%HXuSUSrpWkX9Sboiy!rkJR8AWlhwO~lE?xhLGTuBh2=hVI{4MU zne3kfcf{o`YTMeEVgCt)hiLJd0s-Ogt)RR}Jh{kLeb86t8+nh?BeHsIINX%#oghir zzu_3dzJJ)L%;q-R`dAjk>LImdM0kBIQM$qx)kukDO_AESCj80(ExmZ(NCo!Z*&m3r z2PeXI?7=jG|6}&xa2S9+2uzYTz315k#(rFc48_=k`@f&m9=v+FvIo~uM_<|=97Y`v zZVx^qw_Uae&q>XSj!kM0ex}fUHTK}zPNXyq;fb{esX|1YJ-88eVh?^S!A|W#F0sBE zd$8&(NO83XFDgouJ$Uu9-LVJfQE2SJo~UxM2OpkE4(-z~1xL8IdN=LCdQuHy4{{XK z-0Z<+Dpz-V@IF)wY7gc~;uw2S55!>)eoYbG?ZLi8WPg}MdvJkdI=DS}L88RigHM5c zrS@RzrT-m!u)X8IZV#rz3tM+P?7>H>qNk68fbcoX2Db-9RrWwNzn8<`LH2iI4^}T% z_TcQLBEtjr;Hd#xV(mfEuYdXzec3z9sGc_d)rFYKak6jz5#gc-Kk*d#e*R$5uk8bN zWx0CGadB}crib|~HpNb!B2U~srDEb2IxJ*7YKt$RUv3p-?ANt`__U-%ak zT0z*HYuwqvecV z-^k;`?0xx3QQPsH&--xGYv{_bB020S>8RYAU6T8GXiV`DoB%-nZ)1EZjl4ScVev6b z+Ws{s<+JebVV8LAoSE=W*sExR6tNy##jX6ixRl~S;!+NwH=X71rp4c49q(b%p-BfL zr;-ZdoELg0!vke^HKg}n_zIk;Smc}eWXV~tjPCuXJakv z_hXH+bUXM0f&Q6adTOQO|CK4Cy$^m*4B5GUIb9;af?l(bSB*d61?yuFq(hW#&O-P( zy#)Jk0M_vqF zGab@n%kiKHqB^Ez8ULgla-KuB98)4ok;h5z?=8wBZDcvlu_7oSnZz$jzK~eH)*RE5 zff#mflnwIBgvNmB)Z|f|GQ5y)G4Lj&LHfz0LtHpd7^+L9ya6HIQObo~XgB4rE+>s0;d z<0=lSBzMH7O+pGGBIW2i-U}l7jv{%nN2n-ymi6DQw-5!n=#_3#bR9M8!mmZD`<83} zA2yAaZ({;1rM^rwlH!8i9TP>MB#Wa6{u7>2p?xM+TEVB@%M?!dLV97dEX6*7^uiXj z#5Rcq$EZ-wf{^s7c6u4R@%Qcn4Z0unAOhrW_YNcpOUuB2I{lzXC|h=<6Q_IK60v)q z%;BK5;&$B+GMmyJC!d$>kk>T$o@gJ)3#Q&qvef;bb^O!)A38&i>&_3g&ZVYMk40|L z6sr4j?3mczi`tm+2dd{HGNd{#Q!c?!?XNm6d}ScMJUV0E5$KHb%Z+}KJ_+y6;9dAp z$Rk3IL^zEIn2%oh4#74=rW3KXPZsWk|0b~!g9arIFNgm?DDZi}wV{zUsnIk@&^zIU zypuRLnz%D1F54pq;`$fn`>BiGOh-~;Iu)P53%*NOQAXzCXHKMX$!HVpYSP6=!lyPl zXGv6BX1J(R$}?#NV6lX`JWA#_1tomNY_cnF>O=%ODy5Z`@`XpG=#O*`z4f&`BErmA z0)Qe)P(luG|NG>)HeWWu@UP$SUdBG^R`NC{An| zgYQRl&5SvUII};rqHlwaYqJCrC4NEsF|v)cXaEScY4rUvKMg(V<8-}r*=TvQdi`=y zTiTfLpQJij$A_%bj0)ISzFXd+>vB6 z{O>SZKl9wfbNERLhF=iic}MI|d@4sg%c!ii^GX0x4;G zOyp8riNC><-W9-9z4pqc#Wc=}Ln+U=(+3*m(RS-Jp9XA$fhBGK4u4yfLhl(~lKT!D zJXfC~<&vG7)#Iu1D5oAGlqI=4h)`rAWXgWme!a2jL<6hWy8lp>=$-J35GNT+6SsHW zwI#IgNonfWDFw14e7A`y4bzBl-w!m+Q9v&x4VfiatrnF&kVS7*=!(%`9Iva5>YB7p!{sUVjm1@w8XsbJBbR5`C&V1Z}(cMl`d2%?D2(g@3ph$%a{3l2@R2 zPcW%5Y58#~n<6ngrho5XTEe}j%Ku5XjS)h3 zgjd3wa0)-l1_;$+^t7~X(+9+e#O`g2?l;3Nr;y?uQaCsqfLtc}8Cs3dDlV2R5a=_h z2Fc^qXybc0yi|$YJK;YO4vaP0^2-G8hh}Ok$Og49gw8B!KVVM#?DW3w3L0d#d#6&SaLT<%&FUL1B;14fGo{H8MW~B(l&V>SuYzmzoBovNyL?+Obu=))4ZODmfIZ(W z65&`K>1TWA^cW4JPOjAZdZS`yK=Lz1zZp2Pw#U~pGL^>k8~cb`4ypIGTqRnd{m{!U zP;EYKj7%dPtYAm~`+RknmwZAs{Yn0Ze*oX`Xj*ru_mld76}K-$Zk`UESwuz6=~#+H zJ{Is@`AFY4J68=0w0zX$Ey{hq;TxQyT+}{K#oQax(A-7ojV}S>q}r#O-YV{xna&{w z^kii!MbV!7V8b_rN2lW#tXfo*`%L3}!`BByX@=)65}9A5d$D}dh?B(e>wZX*e(j^8 zU*sHi-w{%`nPJ~TqNAZW%Jm^>G)$War$j$yNpE8WW$Zf-ybGn$aa`p1%@OJ#06OKZ zpOGa};OFy5yP~&BOWUG3((7(uWymOxeCGYm>!Vj@6(rXL<8~l>;{gcRNG|8g*!lWg11`}yoX5ZXT+iZ{vkQDP=^*qx-OzYov-C;^>UxmbL8cuUl1qf zvGC<04C0!vQEwNe%Q2w?Ed}qdjTW2R6FMZOu}DMkFzG2rK5qk|ul28_$CO^#UEF>_ zI+hX#jT(@SkpATNTJrO?ybdwaU&D`U`zK!omYDe?*0=IrlD4yK1N$r~Tc4J$GRb{c z3ch2!ClRMK*RTWe3KmHrxs+8E*dZ3a*5Ta!d zD9TkNO67<~E66=+j2w=5BL9UCx&P*g&po4xsJMNuSc2RWL39+6irdr1z$*G7G>b^- z)ZTpoc9MOhN4Uke>%A)#E>$kE?b(Q@ROknvtH$t6s754Sm7rsvz0qy7g;7GSa*baS z6~R2Ww_Iv*q$m8!d}$ex@L%WAGW2Kv0ZK-Gy$90_!Y3ku&haZbuU6X|R4fMwm$WZQ zkY5~DU(0A}Bm6!EN4xo2ZibB|?UOv@D*l!5=L8wpX+LzTBKTbF>5kBaN^sXnJ+|+2 z^(qM_eX?2!?q8u(3GQ-v8sZ+&*f|;1W4UWgH>nRyrX&1|$-yyvup1 zO2fJ{L)ttCX|0S(DeX={u*aB}y`&F~Y-&TR@ZH?n){2 z`@(!Q4F%U$L=I9>_V&*Mb)t;x9f|gKEFdcUKnO(!w7nI_0=Z}hJD$0+w?Y1gkEFWA z-mcYY&IKk0Qkl^;ob7?ZJrAX!>alO%RYP04#oYFaxqZ$yx9p*EJm&W7^v0LP-1haM zeCWeU6}7!8Rri$Yx6v+(=~7{Zw}lQUZa>+tjqoE4BUddJJ}u@sT}T-pktpalp5N(AFx>Exg$JZ@_ds5X**$5Me7^j7Lp1~UVm13d5J%jba8^h;Yj-UH>_%i5g?OyXjQTzN%)qfVh^7-PnmuVB{ zG@oxB>1+8B$mr@xN2#3H4~7rSdAg;WQK-Ka`wE{cZhKzb{F1hA+rd+(tQQDL^Hqzx z%j7Cq7U#b0yIdUzAIeZ>>f`X98YlVvi4>#z9xtMQ!$~dlWr}lAhM_JD3Iv58f``S* z;?s?4Kzd7QY-gq`B3tR#VJqJDUEM7#@U_n7S$$Z!G`c=_R=V^8)R;rG&r_89h_7X~ z(n~BhdS46zn-lp9eZOQtR`D|e2mYx!AOahtZs6BhbZevsmV5f>Pzm&f4;SL-3rX** zeRX>CaYr_dD9-&%(Gxi`G@Ql~K7_PzPt4aAFcz=9V6-KFEO@!K*jK<_EPoST#xOEq zL2WYkmcC>izXg)I?Z>$AKQ(r>jBhNc&- zJ=e|}F8c}};r*Oi=j49iyLc~A=de|E2K&}?o|LL?Y10FQ_U=pBqW!uH zv#iixah703zG6pFdnz0$f}x?@@P1J`?iExNI>#wH&i<;SYZ#!UAB*fD;P>_YUi2>1 zsb^p7{!-hA2%!hsG_;V$&&HBVK= z_O-mN5^oD1K)Y5+npUyIDV<*Q1(?a`^1_n#6>2r1>WHyYO?R?bd&Z$6zLp0;B7N-u zAtsHHsYfUSpCt^`#8$>1w!s(4+1CoIBdT|-^#14C4oxL-QCnX)|9DX`@6gg-lIav` z;$c>{Nv1^|C;5xm+9TYLJkh^LDfn9GI>7ViLPM1&brnxaBud&o6-Bai7LvD?@wSxS zF{&bn+i%y8N5~jz>L+o%*AcvlUGKH17UzAb3=r+fJ6q2-sja=_h!wW>3Z0AuGRhgQ zDF*pwrJb80iaKRS7TO?|*l$Z6d+T%9TKFbPETeJh)PV1opY#Sm-pRO=obnpgmvO86 zny%3RGj#R46;Sp~q3nE9o&mZ1LEle6Cce_Zh>@*-irXSu{fG-J<1JH*w(X(Fl_|^z z%J>_;t4MRZ-RrND`W?3|U-x`vEMFlT_D(e58N8g~g7#cm-obfQF=~C9Ne4$%c;siOlCefnS!*QH*}N zG-~LT7C~%1m*LaGJZE?u8uZ3wqkS!RKrL|JG@B;y-X9eX( z%F7HLs{CtXHs_Q2)Gd$bjB!L4VXE}L)@)>1-W$3eJX7fUma0Psn!@ogBoIA3zrK3KU|#YJKsnueA3l2Gz8u;?;TVXL@xs8;)I z^KE@oH00sFy`^A(L4pWH!TnMGs>*AhI?xC3ORQ|_JAB;?;mTW~VNuOGI{YzwG5EsQ zk-qPDQT?X;$&;cT@%Pb1iMzucuK4O*zZ_n&`5V(&vXJ9^zuFB>oPTzMlWpH z0%taWdrP$ddjq&Gp7i9%I{x7z7n%*=E`fI?ySM?|-o*8_c7Q-ujko8QLCx$^z46E230YfVKuvu_r$d^YJA0}9y!h5DaX+tA#dEj)ytcsNlJ+rl?%CIEI%#Rg z$W=Z6n6LQTP?WcqDB-ev&2}{1z_xxc>7DRo-iRK&Kp4yOO7mRHv&{4$4C2$TeJFh^ z1^xQ^ngd^R;A;+i&4I5u@HGd%=D^n+_~IN`R#jWoP+eg)u4xQat+1AqH&#~!8$%7% zwacbWE3dDws;#uEN9 zYHIwobs_)C@|x;O;?nK3@K&uQ+;W|VM|msvL)@=(r{S+3#a+T3;@-%85BKBTZ*uQP zoYS}^?UK5>#?Z8BRjVti>O<9awZZyOgFgXSSrvlTY15Wfg(y^ch_Vf&s9SPoRYfT4 z2>;?$)uHA7LK~HTdU8Zd$}1fhPPeRaO^prXs%tB1nkuWtsr<)9a~P+y9{>oEW0%+0 z|IgB2UQ<(75j@>mN$KkvrcI;L8mcs~y0)>ZAryVnP{rfQDr-*N%BmUVjaAm-LmQ_Z zT6wyEZu!dUWw5iZ)?Y!|P*vqH>j=xARasNj&`{Ul*V5!)p#^7}KU!avHPlAq^74kN z%3w|Pl7{kzHPfcm)Rk9K4NIx2Dv=Yp#x1W~Q8liiZb==L);KP2*1~ZG)k~KKPhMVC zRTC_2ICETm?OCgX2qO~LP&KZxp<-NKX>sGYiePztwR%+V zH&PibHl)<~4M+T|${YQ{6u7ag=`}S8bk9^g^@r+AJ;f5|px=~pApQV59BHQc4>bjk zPy(oSfjCCR9rhRZ>-4GB-CETvpo@tXRD|XMC`vuHvjqKb+LlZ~ywNaS# zxzXftF_PlX$*!oWtF2N5$EC4xOBlzWN$|&6)Im~eEOvU`;&UeILRFv=men<^DU9OR z?@cv~A8*V~Fh07IH1$6gznwluquY6M_xy9lH-;*M14KGCcxU>kd<3gk)YmvM+4(!9Nu+xG6dFaauAv$&S6<^VS)n!cRfVOq z=ZA9TpOU@-VR!Js{%hk;kl&nav4z@fI0>>7nlO3#iLtgc36A9HjeHA3C~RW zEYkc5L9EX}W0)TrKf!5#@E~cl^)=-cRmF2BgqBXo$)3@)bZJ#XK3-7a{PN=YIl;o6 z8PPZyEazE#@(S)Vxn<_9#`YI z6?optgpb2Fl!ugWdyZ*=aX%;X`hz2pF8=ptMIw(9T1P}8+X?H(MIzTzaR)P#(oOg* zVF88f$caRj6V4nTi8NEWrxU(Ocp>3^d_;9K;Q}hNmv9r|hzaz`sL*kQrx8vg+(=kR z_&8xH;TVLVT;3Bl5I#$I72#AW=f{N4Q!zb+n-8_DcL_@olo^xg%OM=M5q43rhfSsr zNVu6WOxQU!61j#7+Dy2KF!LD7N4TB2g-qxT9LIaYPQoM4^op3YZvC!{lHa0|9#+2rpg#Co2l&6yk z@@B$r!WJnP^Z(hDtDA)ck5it^`H{#c@L$%6jX*%N3H^it!ij{Pgr^bq5Y`iJCv1`T zECjuVu#|8UVHaT!;by|u3A+hDA?zU>u?Oi0{e=C56A3NWQWp{W2~Q);Cafn65VjDO z5?(`CM!1Qvp0Jy+nebV{PQox@7vX1wn+Y>%%-w|YeRdDwRKo3qC4`v^kt0Gs;dzAF zgq?%|!dnST3AYfI5k5}XMfd?>KjE>+V}O~!5MdeNdctPH*9p4`Rm11olu^ToN0?1m zPgp|ONmxeMO?aNbS*(5!VK(cV`sJB04f)JHzQy@~u#6enB9T94u&$@RWnZ{{!tI29>OHUudI-A-`w2@|Q!W%) z_8RaJHlG9EQF1-!l0V8Y`+V>NFS~$rD8a4^kwbakj9j8zGB1WMluhYc>H~OJ3w#IO z-wIyRmtF$Dcy3-#{h)}tnN&SW6jD3wkuZA$bb`0O19=Be=cT+yQFdPjKAyLC5|8K1 z%i#m~%C11JQFNUf!3R8WEp(xf<-E~1CAigCZ$nOb?)fqJ3A67256ZLiC*UD||4+$}{L6j@zkqMP6THOB+zg+1 zF8c-LAl~M?C>Qbk_rM1fboafaCoKCF&ls5c`;j}s=B>yNVdlf=3EE@NW7O|q(Dg(l zvYN1~2YEf5bWeegu=Hu zERWDC%E(-sdg8E*^jX98Z(&9{U`hKt_ff>lA(TsCZ*ngJ=FHcktP6lGlt^3>Pxg}R z0!GtOm!ID(VAlhiDUg{NnHy4PW%z4{Wn_|Np0rKCCB%_6pr`7+sK&IRk1WUOn-IKeH0zHALf~%G8x|PHBm1m3v%wQ3>v--8LukR&Ysu z98aC5J$aDIvm6ybS-kv{@Ntjl(kh5HUk#USwpnpZp(U87FRnx*aR_v42~B6rFA%QXHCsi0dY_}8XMoZGoiJ=UpxTtrMiY#Tot(I_LO|^y8^diE|Wkyqz>Ek7O`pn@4s*J)gVGAZ+=Qs$1d85vm{sLSkiLuO`7UAsp?Mp^2Q zhGk3zLM;;`$^lbe@moIw-&}B8C)(wmIsBAikBE6H;MXeU)!)f?4E}Bo$}8he;IV*rfukdlhvi+I?2v(xomm;3shlByx(>$$`2YPMv1jS67NB3p6n;j9SorS^~?VH;d7;MsH?xrL6U? zOWWY-5HiJ9W^?}z|3q{~+OKg8xsKwNHrbUOiRAM??We#g(*|khYdy&Rf?>9zmJ09_ zM=nR6f1Wvzr+=+yL#pJJ#a&Ha2NR|>>b!*4&B#FudD;3{bkjy)oxpz1f6;SB?~?c0 zv}Ic}Mi)PVMtVFWeW3u>kpa<;<(^9Yr{_c>vZOTaa7}jt|B$GZ)R}SY%O5WE$>k`= zo{n;CcH`Y|>9-$3nzZM%#S&SRHu*X<^2Gr;5ITeT|$Z`}|j-^XV(r=~7)74Q7v zQWtVLXksKjTA$6SYxh_;WP`UO?NZNWcAX0UW?;#qm{HD>SHcW~{j+Xxi0-2-L zeg;CLW5w{LoOWo-d8W`ihWn2PMIzEJ(%zx7G&3Vz(wzq0N*nJE;10Z!?lkTj?R3u# zq}xck)9iFlCZ^j+y0h(c4-BM}e)9s-Rq;OUSHO!hvQq;X0UQ#Sa$HNBv&)ffmtzcg zrG2DbLV7MqH-j=$#yitP}XYz?F}RIY~VUgPx~4 z5q{pu<;bVakx%wogk18-;$CQ%?FBl)(;Y4`m6M1-n!3UMr==$u`HPHk!hg*KZ z&E=?{-oStyWUuwCi|YDq2WUSv?G|uy6>--vmM433bqIj+cckJqIeo`j8QD#7+NWT6 zeMUMQc|!azW4?|hxoj#lqJnV>!Brmh*Jg^prfe>vg}=63qlgidy!{C})oZvDP4p;c@0pDrMB9X%c-!?pko#R6?-SJgIV;Zt~UUeig zPVn&^n=8HnJ2DH8MCeO&Y4V1rR`3FL%#3y13Wt}FPYwBe03T*bKKt%8AMx&SM&0Qa zeD{GbRAcb{5zoSn4+C`X6yFHsI<&&n(}hX!?M6MF2EJ$OOg&B5C47U{(9?5OrP+0(J^zPe!Up5 z7=ONScxpKy`l?YEzi7DeIHM5Jzmm_J z48B=k9^XUYJBoRRSwi2xc}U8K0$aXCe<${@p-;7cNIH%B%NBe;2Vb0=4D>VY&P`O* zigraFXB&Auk9-QKKb6ny|4u%_@7Kv^1M@C2rj@n>p>g96+%Wy5SsC@7{pj?%2;_|6 z;txzjq~Bzo<^$o+uQa}d{!K|nz%wSk>s+MzSs~JIVm{|xaHUGhuHwqOrqB#(n{?RM2*_(oxhK5JuMTlm*%<3q-s_+sg60bf#otrC2H?}$WZ2)^7T z_+sg+9t__-;5)XHF(|?J7RGJosIKU&CPhTL8W{p>M9x zcTy63vHWAKGdcfm1>fV;hZ<`cngm}gedi2@?*l1stMT*h$EkAD2eI^tADXByjrS%08Su7`~|ft#_r*_!lxidldDd@Ey8K_~<(&r|)&}Jx+NQzQ5u$yUB|xZ%$d=z}9X7r=d5Oh$k!yMBAql<~@QpxT6uvwgUtIf*#mC6Ji@eO$ z_?`t{O}()zACA~r{fb|iR3DrNePig7{~h{%DCNClr}(6wag{5548{YeBv?@Axb}?U zsqPAH)^N%HR`UPs`zHUR|NrJ+D&=PgWk2Tshcn)ob~nb`t-Uzy=k^rOyJA2STZ~Ml zs4Wc|Tq)RR@}GRL4~Nco)1F?bFT5)&(bZ<`WvuDAi}AFy3lr1D&A}8fV(wY&UNO>*Q8@1b=bO|`o{X0*wsA5@m0+KX z#os-VIT+8g?mSR)8ApLvF2`8Y`C|v_r$1T!T+5a|CyIO=0u5ZS%6k1E!aJxR0mO`r zrP3!DZ4ZuxGDcHzjFXiSZS=wPWk?g2G)e0-s+$*5n~AXznkJ_o7>AE(cF&RS_%v;0OpuJZhxuJ;|8aawkh4H)N3vGY zN%BqL8htNoo1(ZFr!?{vN0;dPD_vCyz4MK{V5l)p;g<}T4&uxN_zYEviSR=Q8$xz;O|_KmnfSNaf;Vy!3re}=LCiT`P}I=#_B z%wK9fCF{jbWp9*AB+f!Rjy<2C$CxV*MLvnMFLdbkH=8aC?y594#wS_3lsP*i9nVc) zk?ZzP8QJK+I5nj!#oM0h`De;#{*(B8^FET2d1(p)dR2<&x)keY@|RigMA&tErg)F> zT%MvP*L9pLCGrg^o|{vwd+c;Bu;Jkp@AawHov9ji^#sC>RBwBx^+KxmgOS$Tsou{= zT0ck|{Y-}Sbei`M8P>~b-Ul6zZ|c)snO;!U+u&NmD@_6Xu% zw@*siXrejbV-)iVcz>8SkV>UFB0i1IV@oRYl%9unA&=dSzh_EXrsp72&Ct#B5KV*Pl{|mh_c@+Z zV(@$8@DKWaWQzAf&rh5MSA6; zdVk}wIz6ekd#oEh^8W>oj{T&GWeI~inPb0Qx17H#rxSX>(^=C_lH>z zdA&WuQ@VzwZX1rWgdS7|v)42|Q-8Y~e`LxYcck2z`kW`Wp5xndB;|-VJ>ECct-pJ{ zokOkbhj_0YYW3`q`uB9}&UAr1nJ$ovhUO?%rRR7{EScx0vS!SO%q8CY_OQ;(RBz|$q)R}NEa*7m7db&@m`&7 ztxdbxBcOC}evt0{G~KRL@a=nTs&{)Tt2a}7Q?0+K|DUHabjGXnqrKHurRH?{L~p%y z7XSapuk62Q63U*{VgH~1$4UA>M}WAB_T~I9u+%L3O=9Sb>-_&^(s{xWbtUGezsa)U z@%eqxbg2OwV`Avc!}(ug4whdcrmrDibKrj`2cFl>E%Ifl`=WScJmia9yP98*DPOYi zi{ve7{;$%Vti0X3DF@4wA=*Xw&hop)(333O#cb5~#Jd{TEnc$xZMa*S!C+ls?LqgO z(2o0mg`YOPUx)u44%i3h*x2n8vFv`=+>`Yi`}vHx=kwy8*Tp@5Kkm6J?)gsh%#5PC zwwPyz)73R+kq((*(pR?*n2}T0t?M*pOs}bH`F8!xRF1kbO~`bWx>lR_c3&dhz}e`z zPX6{A$BVv-y0$RrE*H~G>MAva=+Sk{`opOjZjaqDt*I`L39WujkL}Oh&XBTPEQe4R zBZlhQ*My_*)PHPvS%-eM>D=Pi&jyWJ$19NkL^S)>k+{Ga4NX`ij3V#&|$*3SVGmYT54g!LwDHesg;yG*#*gxx0WG2wO-_M6ap z&{9{X3H>I_HetYor6w#hVZ8~PP1tF|E)#AxVYdl;Ot{^I{U)@2Z}K;x--OvF44AOg zgk>hIH(|30J5AVS!p$b^Hertmx0|rvgw|G*zX|;&%r;@bgrz1dGhw|6n@!kh!Y&hT zHet63drY|9g#9M89y0lx&~L(Q69!CJYQi!T)|;@|gqq|}o6v8DkMcct{N>?|ZJ(X*JXV@z+vQle*Rt(*EPRM%+re1)9+quCW8vwR zZHHswLoM50$HMUrx}6aH-^0#4epmPitIP1+naacNKATd@pP2EpG6=t;ZjAgO9RozbiK1eJtDGiiLl}YBqjYEPP+fO^EA(Fckea8Mv%eO=OGn8v3(_@|1}kVFY8g4 zcIsB%cZn~%oyF^cG>_1q*p5aTc$wj+-7h&Bc&e4CeQa z;8`BmewQlvH>~w$d}F;~+{+4{s2@HA&Z_=|%au*c6!Saf#kCL zcg4UD7Cj&zlyH@+U-nq@YIvy}6U~*U1M4*4g6Glg8jxw?GnuCH?S4fAN=^J*RQyb9 z`$i4OHSxC_{AIG)4415*l`DIQ#-I7R1{7(qRRP?symtW~Y2rA8H-O)S+-{K%3ApU~ zO{GKXhG!md;X}VHB?FQZzN52Qf>xSpszy*(0uQQZ2aB`h);Pr=VxO`V9*R4tLJeUOj3UJ|D zzmbP5jmGjYej)WzcBMv~W8hPPyV2PJ{3h@OO#jN3+b5I6e+RgfE3;1H4;%c?F<>L@ zt80e_eBZ#|0WR^oigkQhZ!On6Sfl7P@*{KQag&(JwrM>OJVZ38ay-yNr~CQxvp$8Y#1>rdr6%ZC3z<0;W#>$kuK&-OSxhtTL0 ze@@czW&N^T>w&w`d7FvfBLL|Ka`*V{Jyef^AD zP*)`bW2%1R;4c7oqhB`Obc2rtF8tqY_;1VYB}wA{Gzq*LxRfiiR}=0xbbgQ|zI-w& z@o$gQ^9z8x(fJb-fBg=P-{$|Pz=aRpOEg^8R?8)y04w>4-5w=?^l_b;^P6cxr*G_{Zpo@0ommXW;KWtpODV{x1bL z4~}3zzsA#JVc_WpX+Crt{cpF63BZM(?9n>D5ew@A;9~EZdo_aB z^lR$|6MwTI9JQvdRFt+G-?D&9JL)oewbaBv5xBG?%lK)weV2VE6@N^vZ65 zXS*qv?dP0t;QiBdd?Oat!@xyf1y~p_ z*CO=`48`Tf&q)dnJ$*W2o>zyn4LtCoezwbg25>1?Ssb6AF!1eiZYr1Ue@1}YX*DMO zd%{t={RWJ{+U@9S;KHBo*L6I*A9@FHC0B=N{I>oN1DE=JK28qzB%#Fb@7H+pOy(y6 zSMp%$cdCK60C(fh)e4T?iKiz?{LhoX_a3Y1X+A*HZ})4?04{p0)9@kP&~p!P;X~OQ z8c=26Pbs*0a0D525Tsn2L;SGp)@VH9fZc-*50dZ~VO?6Ms)C zNa)FYNdt^nTYlhzKM*Gmmm7Guagc4h_Nop4m&RlJArat0f7tj}Ha!z^H2#Ip==jY# zuxUI^r}F z{|4ZDng7nQ>iDz}@8GVmrQ3NXKb*Sa%tCV7vy%J`{3&3fwJU*>F?%Y>(S# z>gd*Y6`XM%qi+`&_*KBATwQVfulE(4)UvrT*Rh74LvwYxnwM#SeA6jc6>w>Hfv|o) zNC(yh244TN2FQMca(&;x%l@LD)qa)y-2`0dzsA%H%}rg;srcr>5llj`mAoCK>9PH~ z^9($@K*RHNVEr1n8~>jJE_`S<^=tF(sAJsYmjjo4%ZwZvb7XBY@Y44+k+yv9bF6!P z-bMUq4T}V@Te5H(lbVXqBhmF*2M3hs`32W z@aKI4Z#MMXdS=mgb-k3uwXe+vzS_Y32LCn%$Nx0_5gyew^f-4uECw#+ZTXXq_iGdX zX9k{`t_hUAQ{_4!;EtyVxZv5GrQ_S<9vw;I-vnIb&5A46yGi2v@-+U=u^NB24y@x0 zyg5!DZnWbYe&Y71>um$~f2yBt{*0WV<#2PHp2-F-bdLUq&d`?6THs1fj?!?Oo*x)^ z{l^;rCLLIh0e90IuO)$}=WF@dZpO{dFnG=c?uO?A;0K#H&frFc=Nr~`L%*$8w;TKc zGk%4eq^`^YT`uc9U9LBEVC?{|?E4NKZ=H#sR_IR89N@yYY~v@``Cg>r<9GdCBY+Hb zU18vzFX-n}Ouly+c);i@TZcRfT=?JIq$A4r$a3w4;Zt&G^grrUU3I{fzx1Mh{<(p# zHSxO-)Obv@w;oaPO(aLqmjqrgOXu5tj>coU>()mGULU8IPAxL=4IIm@uKR%tojq^p zXVY!7UN-Rq?`n9C4y=QV4Ikp<{~`nT|64~)G5p+W!;O6IYw)~f;NAbyfQ1G=?0DTS zI^)Kps(=fhw|_$;&NKM$1g`X+k=s)Zo)NPRorOB0?VslX7dctIF+ZE`g&gjY<|9B;Qo&_d|wmahoTld+YMt3o2?6hyVci?!2Kqz zGuUGAbkEUzFoE?xaAg+^-zJ+xM=#d!E;DYtw}BTZIP*%cXgs40d?j$@hrFf%rTVqi z4qW80KF)u8RmC@n9l?atbb0Gdd2PLMm4RD^pBX02y9Qon{Ki@XAA7oPccb6XgaizH zIdC_=oex~-?2l_-uPHp(ovSn+zrnM|8Jf;gLl5qRy3PmgCO=(C;BOi{@%>_7(4Ees zlfdTz7dZ*U`Mnnyc$ujX(@o`YzEkJU4^0WGbAEGj#&bey^x*O03ONWY{p@#D6 zP-Ah0oNQQ8SeQS)c)lEvSUS66-faGjRi};DH`LXS&9)q8IoB;++Q^aL%j!7$lykpV z)K^s!wWiV;M;~0B09GgNh3XREm2wiX3tYwL#Pjm%Wy?cu5gO_^c)ZH(O?`P&V^u;% z9J<}KA`x6sURzOBlMq{q8Cq4{7~(kX+N$z~K_ZmbE^DfBi6MuhtAn;VXt|L?m6Px* zF4}m*(a=(I=InS5FAgSQsnDo1SXx&Xs^@I^Py(3)$4%>3!C56I%*ZPV1{<4}1dH`K z-_f(YgQw09l}rr=tLr40RnZV?3~>nN*b1w%s)2*OAqUY}5mYB2H(J49WnFMtP2Cct zp^{Tt8-wLdtF4MU&M)N@=*qEEvnS`cM+z<_gJ5|>L;0FuRc)wYjkQ$H{|;6*tyr;! zB#tLe<*W|HL=6UK&dZxq7%ZGy5DXG|*4&eVg+&HfR4`AnY^)0|FR$g;bgSUxu?eaz|4j8ZF;NI%cn&8T8OOX+caq5C%j_-{g2yLmHtzf|^bMxjD=Yw^@oP0ae zQy1jhC)n=d39#8g_XAyH4_uuX%yB)AcK&#Me5qZ1C(N8Vzi>ftLEem#Lg9g`{H4K) ziMCG_2tzK#k1#645DVTn`*1iZmJ4$t|}+l<^=77Tw_m&CgQ}(s-@*kH6e3$c=7yP zDXCxx&gV4oicn5tsCa$>jGsR>m@~ejJR~@SHC2s`>R49Z6iyA!;UsA}3%Znq6KtY` zwRM$MDy79TewMgsiSfBnCRf)sRyEKWtor3#wOCOgpw-VRoHtvJ3Px^2%M0Z+Vf*xEbLe*k;!`N} zEw7zCF)BK{*4_(pw7^7bI);w%$qrhN5^odyB@Tuz7_UgMYuAX{E+dR>5S#?T_P`3kqjs9i=8@R(LP zc09&D%GnrkkF_mE?~wIZegwB_bwYVVLtR6#VZauSA3I5iGc{t`x!uNMY|{7%wn5TJ z@PsR?L)CS)h1H@8R%QpklpWD*N8jN}t*EZWgN@~)P&q#|6+NgFW$$SDfD*lozg?v~frSJhTZ&tXLjp@WG-wpj7y z^c$)gic#7V4gEl?men<^!STXb#*C<@>fnZ`o0v=tw`^PVzuQa6nGCy(wbb5`@=Xfo zPYU9A=1j3m5v)cw%JHYw>m`LJEC@RNIO#7et*WYA!dd20{$xrf=gX@~+e5I$DYo4V z<{|9V+ggGdF0ZMns|ebbawq;-ob|Dlh$+3p)aT@gJvTkvDTw$=+!N$;75&=8?&ZlZ^G48?SXa@q1mey6XyB5lnY}Uf$RXcu)2!W75 z-+my!x+Q1gFXL`w7US53ZIbTCgn`I7(c&;w)r&@%kNiyz&aX!!@F8VYrP4jcC3AD> zHq|z%v4ru#66vYe*VR;4tSKC?%xHoiyUWhSB&}SlYnRrkyr2&{zGRtWjqxDmP1V{yOv@cN+TK*LI^wQ}opHmfUN3pLpRkg7&r&apWj%I8<#!6a_!+s&@!SPc<<;#L~ z4b?E3VTb|w7WUBU#nRf0(wKIlXA-n#LQAq0R-73&tlzn7pJL1*+lN|Zi{fJKo?{=Rkh_yYN~=u>O#v!Y1sYz zSjSnL$XKRl2ggrts8T~X4vSS?D~1*aZ9%YbqEa-4Qpm}U2I)e@ZuV4h{dFT}6jL1g zcsa2xNSssVKC&=n1>jtGfXIZ(nwp%6jm#LJ z0BWk2G?X{2L5lG&CXix9quo^X>_KezH|iX?u)^715Z^&$GSQ1o(8rFtx63ohQ63qP zn^IXtYpYwMl$Q+iei3hHmaXs;3$$CxBB20UVMJSlU5H(_;k%;C6_aASzp?$siK~{E zhn&hiv6P8*;jG6(Z5Nf}u&k-P0snqUT|+2dJIB&s%9_Mz?xZiyuwl-m=mZy|6-%0y zx(!&wj~+TmY{1kYA8|x}fnNS@Htxd0guaz=o8m<`UcEc}`qi;YHAe-q;u%a4_TKi0tS=8M9osOgd|#$=$57&1V+|#uo-s z=*CW1QC`0*;~igOCT0p#D|4}GF0(Gq$(}r!u(1c5cYp4AV&kgv`oej*voQ=;E`sib zF#nX)+x!30R7L(OYZ#doO<_;3=^;gx|9EBxlJ@5kwPt*U7CHxgjgU{$QWNz-2Xk9i z&YR5;Q9UVSa$`kPsA@G{J7X)3$-uz|K$8vROq8KxZ4wxSvxlb!8=Q}}gdj6`_Ix0^ zNIZsY<3`vsf;lc@015McQxzha>?@WjP==nF1>D($A2Fo44$`EI#nf|kMrZEc>|jD2 zDh=b9_H&t>*A4e8AM{2k%?ZX#JfL?b$N*bZdfWz12mFme#_ENU(LQEuyh))thM+13 zx>tHOEn#A~a0;3#xMH<(j-hOK$C*^|jGfjtX(v7SxT;ZZ$%?8KF``tS(@utIlNFWJ8rP%^0!hag^pCNE_awW?Chzt~Y-%w((={6%I49cnn*I9xJ{lP~4{*sOMp zL#t`!sWJs^t5IdmDyvqoV5la+<&4T=Oe;&Knw5yzNe0%eC|dp-^J+}INJahcb33$a z81Dt_TAOo>bTXi{x|+U9tZ|QP_tE6GUO zsSBcmOo{Uqu^D`kh7=>6i7jXl^|Oi5 z#153`LE30DiLpyLyX^f<6z|W;OTFNBfR%}DAhE~fDCWS(iEaMux^NpgvE|Vo@tvqg zaqTGqW|Pits%CKvqX31~rp1`5`hU$`&x>SN6@H)$qEWYzfHZ0#5G6zUaBp^nZfNop~aS=B<;KI1eMhPOAy(sYy2xR5b@jLgN z@7*8oR@Hk|)sTYj`|8!Z@7{Osx#ylA-#O0%KhW1z7Rr>4arw!442B4wUvbFt{J}iM zfQxf5vgXx6>Yw&v^yLdbrH?VF8SnDm!EcBLIO)NPjXa>4v$rifJgurkjGd+x- zdJ|zNwx`|+SN-G{5gsVX*t@8$q;bv&P^6VhE$nq0_Y0y@a6tMP{wblHWCx^ zjuy6&(Wa}UzR$;)P=|Cj=u^%r!(mCdYXnjBtOUf=ed!b)df=edww$@A0EK|uL2N)N znzUDP#0Ju&Jvt9Voqg4YSX(pE_PtKT4=;x3K6nWA*?IBjHDA(&5I{4zj9jKQpN?qAgHz(-HTI` zi<=`}b0J(&TJUhMgxXsQmx+kr{W?JZ1fqjPgE+8Rq8(N{qK!p_3a~8)(^hod^DXs1Z)8l>y5cCR3WhWV3NBf0;q;{=Y(j+-SVdM2#B$} z38e3W*!{9++=c5rnL)0}{qejc2*?58d*dD;bSJ|0v^#=t;MG@7hco!A4MpwrY-$Z* zJc!sq1AS43-NSK%A1tmhyw)(IVm22XKDtqfd-b`e5=0*b=r3XQ4j_WlZG$~?B)frz z5c28Do1G{c%|x(Datk-dbdhq(Hf0qa4@Pvkj(_OV6%f^Yyw0o3<-H9OF_vUWf{kVI zx|FdRIVZYij(U<HXoWH~@If#lNkAI>(#aCekv+udHen!ww?l zT|nBzh6+|Y^eZf1TwXSgs5utd1svUyiOMo)=@f1)oCnr2%SeTq z+&acScQl!Kg~K2d8_UjhwkKjF=6T<_JBHF|`WWQaH8nF0k=T+Y$E+`7AXg4beT%$G zT&#laXoX`Bm%iyhS=m*CmPO1KF!GXU*;cNgXpY_BFO%y+w-kpqX6mWZK{eAbs?J+# zShUchDKl=3ri+15I=WYpfwbOTYg1WU!@^(&$5TYU#l%fNlVWgk2%H8IpWl?~?Ftj( zb7dG~ZGq_WsLZEad6}xApXfrdI|v=W39)=(I&C>Z+6usxG+V$FLH)9qS=s@KP=xB^ z1L6Sf5u{isRm-h{G#gh2c>*yi*cT!EZ0`JMO+vpbCAgU=8`)gdQXlp;lRrg%E{`My z2eMv3m#B0Da*9OYjVwWt2n7EHc)O86<-PEu!F8}JLz!@w(79Q2lI5r@-85<>P603o z`@DJs+q6iaHQwG{`7t!n?6{Bc(#ZtcT+WSxE#bG(fvaq&?< zwMjRpUZ6fFs~W3Tk_`zluiiwW6+sucIsG6XDTv7S%v}en<+nMTl4VF9?s}nd)%Lel zE1%26cEUKY(>k)NfQ4w&w_t^Je(Ck=+%l?gH)~C~wOu;url|lF6wXikGetv+utmF0 zWa$G9nv&~zz7ozxGFl)U@zGKTH8cSieIVkBlXehDpA8oE#`bM&4ndE$l0FmCs|(h_ zM3{xs*&lK)&uiuP%f%Rh$k%|A7Y}GAO;aNLt(2gASh+lHoVX>^(UeMCE8&R+cbn_YQ z`ab2QsmzFM^gM3iJFtI3=aCHZnNGh2OIIY(N79J~MdqC+vQI@aof;rlX*e<0hlR05 zb!y>=;O|T6n;UIgNuZ`@ReJ|DEb{!uf>``ub^Af2S$LR?W+xP0w}EY{CBY)UQ5YL0 zc9;7a2YagvzshtnL?%B(h2ER*A0mU`0%=Y9$nSz|ZV20u^pXgfx(WBa=Kau2C^xnu$t04VC{z2v$@v(2|819|5-Z}h*N_?w`3WmaM{VzF zkf|bG83G%Pr9`!oR0O-+6D%vrh14g$Pp3;+U~whBX4W$ZrLB)@EzoVNkxAwN#FrZ# zOO8T?qfTb4fGUA#w{-wb9Wmz#BQ=2yekf7n@CU0zX@R&$KslbmJ?wFTp0>8JORE@4R6vrp)!N&6&+!r&~iFb|HA-_OVpX)rgB(~A~RXmh2A zmJYt89y$e-v~(E-!$cXnxH!~F{;O+rQWKV1K_i_MRcS~$8?RhJb7(if6N;LXjot^p z6=F}j-hVtd>?BkA%Tg~W~jc&fl=ax>B_zem!FXPJ@6 ziLb7n5>B?vSXpakZZ(~0g}}IiM@Ne*aXu!yqD}OYGzF1p zR;8r|Rc{Y)%Ayvy-h(wvN(}MrYW-4137h$f1gE5?wqkHBX63PL)Y>kDVzDAbYRc*%YzOk7cf11;wIqKB)Cc6KkG=5*RWat4+ZjFKnRj@J2cgQf>D&xb zEA8=83JH}@HrLMLC`Qnt19@f9zl(Bls^y4B*lUL!q~PmFE`H=Od{}gadqFY*9Riy7F!gdo4)uo*l5+^jc zc%4o0IsCEy`|?2eu>2d|8xnr>?HK+HUhh#n4f_A)&-6Ez|F#>m|7?fl2$%masQ>!g zTA!usG)_tXuC}F|mB}&um#sw#&1>OX~mjEqR)vVEJYD&Hqcs?+^G% zYH+fqmQ-z=p!xD@n-uhKS7W1w< z{zU6N{+E`ciZHYftj|*3?;X}JKGk}Kjl&^3W{<#nET2v4AKH6`4wLtuS@mlC_+4H% zE%#zGkF5U4>c3*$;q`kwu=1PM{^jI6>%W+s=k3BW86sDM@1gzp1--%7QG6GF)pP6V sLf9kwZl_%F;2FK^AMswI@WXa_PTrxEtN-G$*0*>(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 Interface Methods // ============================================================================ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { - printf("[%s:%d] Begin: navigation_initialize\n", __FILE__, __LINE__); + printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); if (!handle || !tf_handle) { - printf("[%s:%d] Error: Invalid parameters\n", __FILE__, __LINE__); + printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); return false; } try { - printf("[%s:%d] Initialize navigation\n", __FILE__, __LINE__); + printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); move_base_core::BaseNavigation* nav = static_cast(handle); auto* tf_ptr = static_cast*>(tf_handle); @@ -200,19 +234,19 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle boost::dll::load_mode::append_decorations); move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); - printf("[%s:%d] Navigation created\n", __FILE__, __LINE__); + printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__); if (nav_ptr == nullptr) { - printf("[%s:%d] Error: Failed to create navigation\n", __FILE__, __LINE__); + printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__); return false; } nav_ptr->initialize(*tf_ptr); return true; } catch (const std::exception &e) { - printf("[%s:%d] Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); + printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); return false; } catch (...) { - printf("[%s:%d] Error: Failed to initialize navigation\n", __FILE__, __LINE__); + printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__); return false; } } diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt index 6de2889..54e5b4b 100644 --- a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -25,8 +25,7 @@ add_library(score_algorithm src/score_algorithm.cpp) target_link_libraries(score_algorithm PUBLIC ${PACKAGES_DIR} - robot::node_handle - robot::console + robot_cpp PRIVATE Boost::filesystem Boost::system diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h index 2f97b48..dd78342 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h @@ -1,7 +1,8 @@ #ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__ #define _SCORE_GOAL_CHECKER_H_INCLUDED__ -#include +#include +#include #include #include #include @@ -32,7 +33,7 @@ namespace score_algorithm * @brief Initialize any parameters from the NodeHandle * @param nh NodeHandle for grabbing parameters */ - virtual void initialize(const ros::NodeHandle& nh) = 0; + virtual void initialize(const robot::NodeHandle& nh) = 0; /** * @brief Reset the state of the goal checker (if any) diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h index d57e0d3..60504b2 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -36,9 +36,9 @@ namespace score_algorithm * @brief Initialize parameters as needed * @param name Namespace for this planner * @param tf TFListener pointer - * @param costmap_ros Costmap pointer + * @param costmap_robot Costmap pointer */ - virtual void initialize(robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, + virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0; /** @@ -154,10 +154,10 @@ namespace score_algorithm double angle_threshold_; bool enable_publish_; - // ros::Publisher reached_intermediate_goals_pub_; - // ros::Publisher current_goal_pub_; - // ros::Publisher closet_robot_goal_pub_; - // ros::Publisher transformed_plan_pub_, compute_plan_pub_; + // robot::Publisher reached_intermediate_goals_pub_; + // robot::Publisher current_goal_pub_; + // robot::Publisher closet_robot_goal_pub_; + // robot::Publisher transformed_plan_pub_, compute_plan_pub_; std::vector reached_intermediate_goals_; std::vector start_index_saved_vt_; diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h index 209ce1a..09d0a93 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h @@ -31,7 +31,7 @@ namespace score_algorithm class TrajectoryGenerator { public: - using Ptr = boost::shared_ptr; + using Ptr = std::shared_ptr; virtual ~TrajectoryGenerator() {} diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 25f5de3..7c7400f 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -370,7 +370,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose bool goal_already_reached = false; // geometry_msgs::PoseArray poseArrayMsg; - // poseArrayMsg.header.stamp = ros::Time::now(); + // poseArrayMsg.header.stamp = robot::Time::now(); // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID(); // int c = 0; for (auto &reached_intermediate_goal : reached_intermediate_goals_) diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 3532a2e..40c2f82 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -52,8 +52,7 @@ add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS}) target_link_libraries(mkt_algorithm_diff PUBLIC ${PACKAGES_DIR} - robot::node_handle - robot::console + robot_cpp Boost::system Eigen3::Eigen ) diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h index df33bbd..7e33383 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h @@ -27,7 +27,7 @@ public: * @brief Initialize parameters as needed * @param nh NodeHandle to read parameters from */ - virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -70,9 +70,9 @@ protected: bool findSubGoalIndex(const std::vector &plan, std::vector &index_s); - ros::NodeHandle nh_priv_, nh_; + robot::NodeHandle nh_priv_, nh_; std::string frame_id_path_; - ros::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_; + robot::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_; std::vector reached_intermediate_goals_; geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; @@ -98,8 +98,8 @@ protected: bool follow_step_path_; bool is_filter_; boost::shared_ptr kf_; - ros::Publisher cmd_raw_pub_; - ros::Time last_actuator_update_; + robot::Publisher cmd_raw_pub_; + robot::Time last_actuator_update_; }; // class Bicycle } // namespace mkt_algorithm diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h index ba58a0a..e38182f 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h @@ -25,7 +25,7 @@ public: * @brief Initialize parameters as needed * @param nh NodeHandle to read parameters from */ - virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -55,13 +55,13 @@ public: protected: - ros::NodeHandle nh_; - ros::NodeHandle nh_kinematics_; + robot::NodeHandle nh_; + robot::NodeHandle nh_kinematics_; double steering_fix_wheel_distance_x_; double steering_fix_wheel_distance_y_; geometry_msgs::Pose2D goal_; - ros::Time time_last_cmd_; + robot::Time time_last_cmd_; }; // class GoStraight diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h index 1ec8602..ee20972 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h @@ -14,7 +14,7 @@ public: * @brief Initialize any parameters from the NodeHandle * @param nh NodeHandle for grabbing parameters */ - virtual void initialize(const ros::NodeHandle& nh) override; + virtual void initialize(const robot::NodeHandle& nh) override; /** * @brief Reset the state of the goal checker (if any) diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h index f8f6acd..2902a79 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h @@ -24,7 +24,7 @@ public: * @brief Initialize parameters as needed * @param nh NodeHandle to read parameters from */ - virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h index ebaf309..6740036 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h @@ -25,7 +25,7 @@ public: * @brief Initialize parameters as needed * @param nh NodeHandle to read parameters from */ - virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -55,8 +55,8 @@ public: protected: - ros::NodeHandle nh_; - ros::NodeHandle nh_kinematics_; + robot::NodeHandle nh_; + robot::NodeHandle nh_kinematics_; geometry_msgs::Pose2D sub_goal_; double steering_fix_wheel_distance_x_; @@ -65,7 +65,7 @@ protected: double current_direction_; double velocity_rotate_min_, velocity_rotate_max_; geometry_msgs::Pose2D goal_; - ros::Time time_last_cmd_; + robot::Time time_last_cmd_; }; // class RotateToGoal 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 cd5237a..c5ccfb8 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 @@ -24,7 +24,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Calculating algorithm 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 b0e1aa5..674c847 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 @@ -44,7 +44,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -102,10 +102,9 @@ namespace mkt_algorithm virtual void getParams(); /** - * @brief Initialize dynamic reconfigure - * @param nh NodeHandle to read parameters from + * @brief Initialize Kalman filter */ - virtual void initDynamicReconfigure(const robot::NodeHandle &nh); + virtual void initKalmanFilter(); /** * @brief Dynamically adjust look ahead distance based on the speed 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 8ccb9ff..d4839d5 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 @@ -23,7 +23,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp index ca11d0d..9f88ab2 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp @@ -6,12 +6,12 @@ namespace mkt_algorithm { -void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) +void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) { - nh_ = ros::NodeHandle(nh, name); + nh_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; - costmap_ros_ = costmap_ros; + costmap_robot_ = costmap_robot; steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); @@ -19,7 +19,7 @@ void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFList void GoStraight::reset() { - time_last_cmd_ = ros::Time::now(); + time_last_cmd_ = robot::Time::now(); } bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, @@ -40,7 +40,7 @@ bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs:: nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) { nav_2d_msgs::Twist2DStamped result; - result.header.stamp = ros::Time::now(); + result.header.stamp = robot::Time::now(); geometry_msgs::Pose2D steer_to_baselink_pose; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp index eeb4359..59ba24d 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp @@ -11,14 +11,14 @@ namespace mkt_algorithm { -void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) +void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) { - nh_priv_ = ros::NodeHandle(nh, name); - nh_ = ros::NodeHandle("~"); + nh_priv_ = robot::NodeHandle(nh, name); + nh_ = robot::NodeHandle("~"); name_ = name; tf_ = tf; - costmap_ros_ = costmap_ros; - last_actuator_update_ = ros::Time(0); + costmap_robot_ = costmap_robot; + last_actuator_update_ = robot::Time(0); nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5); @@ -72,7 +72,7 @@ void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListener // Best guess of initial states Eigen::VectorXd x0(n); x0 << 0.0, 0.0, 0.0; - kf_->init(ros::Time::now().toSec(), x0); + kf_->init(robot::Time::now().toSec(), x0); } void Bicycle::reset() @@ -84,7 +84,7 @@ void Bicycle::reset() sub_start_index_saved_ = 0; x_direction_ = y_direction_= theta_direction_ = 0; follow_step_path_ = false; - last_actuator_update_ = ros::Time(0); + last_actuator_update_ = robot::Time(0); } bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, @@ -122,7 +122,7 @@ bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twi nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) { nav_2d_msgs::Twist2DStamped result; - result.header.stamp = ros::Time::now(); + result.header.stamp = robot::Time::now(); geometry_msgs::Pose2D steer_to_baselink_pose; @@ -205,8 +205,8 @@ nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pos Eigen::VectorXd y(1); y << phi_steer; - ros::Time current_time = ros::Time::now(); - double dt = ros::Duration(current_time - last_actuator_update_).toSec(); + robot::Time current_time = robot::Time::now(); + double dt = robot::Duration(current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; Eigen::MatrixXd A(3, 3); // System dynamics matrix A << 1, dt, 0, 0, 1, dt, 0, 0, 1; @@ -411,8 +411,8 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan unsigned int goal_index = start_index; - ros::Rate r(30); - while (goal_index < last_valid_index && ros::ok()) + robot::Rate r(30); + while (goal_index < last_valid_index && robot::ok()) { // Tìm kiếm vị trí điểm mục tiêu phù hợp double S; @@ -426,7 +426,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ bool goal_already_reached = false; geometry_msgs::PoseArray poseArrayMsg; - poseArrayMsg.header.stamp = ros::Time::now(); + poseArrayMsg.header.stamp = robot::Time::now(); poseArrayMsg.header.frame_id = frame_id_path_; // std::stringstream ss; for (auto &reached_intermediate_goal : reached_intermediate_goals_) @@ -516,7 +516,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ } r.sleep(); - ros::spinOnce(); + robot::spinOnce(); } if (start_index > goal_index) @@ -569,11 +569,11 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ } // Publish start_index - geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, ros::Time::now()); + geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now()); closet_robot_goal_pub_.publish(pose_st); // Publish goal_index - geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, ros::Time::now()); + geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now()); current_goal_pub_.publish(pose_g); return true; @@ -587,7 +587,7 @@ bool Bicycle::findSubGoalIndex(const std::vector &plan, s } double x_direction_saved = 0.0; geometry_msgs::PoseArray poseArrayMsg; - poseArrayMsg.header.stamp = ros::Time::now(); + poseArrayMsg.header.stamp = robot::Time::now(); poseArrayMsg.header.frame_id = frame_id_path_; for (unsigned int i = 1; i < (unsigned int)plan.size(); i++) diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp index c40a715..1f8fccd 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp @@ -6,12 +6,12 @@ namespace mkt_algorithm { -void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) +void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) { - nh_ = ros::NodeHandle(nh, name); + nh_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; - costmap_ros_ = costmap_ros; + costmap_robot_ = costmap_robot; steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); @@ -22,7 +22,7 @@ void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFLi void RotateToGoal::reset() { - time_last_cmd_ = ros::Time::now(); + time_last_cmd_ = robot::Time::now(); } bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, @@ -41,7 +41,7 @@ bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) { nav_2d_msgs::Twist2DStamped result; - result.header.stamp = ros::Time::now(); + result.header.stamp = robot::Time::now(); geometry_msgs::Pose2D steer_to_baselink_pose; @@ -51,9 +51,9 @@ nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2)); - // ros::Time t = ros::Time::now(); + // robot::Time t = robot::Time::now(); // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec(); - // time_last_cmd_ = ros::Time::now(); + // time_last_cmd_ = robot::Time::now(); double angle_rg = angles::shortest_angular_distance(pose.theta, goal_.theta); double angle_reduce = std::min(std::max(fabs(velocity.theta), 0.15), velocity_rotate_max_); 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 09475e3..6e07711 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 @@ -3,7 +3,7 @@ #include void mkt_algorithm::diff::GoStraight::initialize( - robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { 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 d5cc6aa..d23d651 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 @@ -5,7 +5,7 @@ #define LIMIT_VEL_THETA 0.33 void mkt_algorithm::diff::PredictiveTrajectory::initialize( - robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { @@ -16,7 +16,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( traj_ = traj; costmap_robot_ = costmap_robot; this->getParams(); - // this->initDynamicReconfigure(nh_priv_); nh_.param("publish_topic", enable_publish_, false); nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); @@ -43,51 +42,59 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; } - - // kalman - last_actuator_update_ = robot::Time::now(); - n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] - m_ = 2; // measurements: x, y, theta - double dt = control_duration_; - - // Khởi tạo ma trận - A = Eigen::MatrixXd::Identity(n_, n_); - C = Eigen::MatrixXd::Zero(m_, n_); - Q = Eigen::MatrixXd::Zero(n_, n_); - R = Eigen::MatrixXd::Identity(m_, m_); - P = Eigen::MatrixXd::Identity(n_, n_); - - for (int i = 0; i < n_; i += 3) - { - A(i, i + 1) = dt; - A(i, i + 2) = 0.5 * dt * dt; - A(i + 1, i + 2) = dt; - } - - C(0, 0) = 1; - C(1, 3) = 1; - Q(2, 2) = 0.1; - Q(5, 5) = 0.6; - - R(0, 0) = 0.1; - R(1, 1) = 0.2; - - P(3, 3) = 0.4; - P(4, 4) = 0.4; - P(5, 5) = 0.4; - - kf_ = boost::make_shared(dt, A, C, Q, R, P); - Eigen::VectorXd x0(n_); - x0 << 0, 0, 0, 0, 0, 0; - kf_->init(robot::Time::now().toSec(), x0); + this->initKalmanFilter(); x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; - robot::log_info("[%s:%d] PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); + robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); } } mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() { + if (kf_) + { + kf_.reset(); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter() +{ + // kalman + last_actuator_update_ = robot::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + double dt = control_duration_; + + // Khởi tạo ma trận + A = Eigen::MatrixXd::Identity(n_, n_); + C = Eigen::MatrixXd::Zero(m_, n_); + Q = Eigen::MatrixXd::Zero(n_, n_); + R = Eigen::MatrixXd::Identity(m_, m_); + P = Eigen::MatrixXd::Identity(n_, n_); + + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + R(0, 0) = 0.1; + R(1, 1) = 0.2; + + P(3, 3) = 0.4; + P(4, 4) = 0.4; + P(5, 5) = 0.4; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); } void mkt_algorithm::diff::PredictiveTrajectory::getParams() @@ -127,7 +134,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); if (inflation_cost_scaling_factor_ <= 0.0) { - robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, " + robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, " "it should be >0. Disabling cost regulated linear velocity scaling."); use_cost_regulated_linear_velocity_scaling_ = false; } @@ -158,7 +165,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::reset() { if (this->initialized_) { - robot::log_info("[%s:%d] PredictiveTrajectory is reset", __FILE__, __LINE__); + robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__); reached_intermediate_goals_.clear(); start_index_saved_vt_.clear(); this->clear(); @@ -207,7 +214,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 { if (!initialized_) { - robot::log_error("[%s:%d] This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); return false; } @@ -236,7 +243,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) { - robot::log_error("[%s:%d] The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); + robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); return false; } this->getParams(); @@ -248,7 +255,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 // prune global plan to cut off parts of the past (spatially before the robot) if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) { - robot::log_error("[%s:%d] pruneGlobalPlan Failed", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__); return false; } @@ -269,7 +276,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 } else { - robot::log_error("[%s:%d] The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); + robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); return false; } } @@ -278,7 +285,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 unsigned int start_index, goal_index; if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) { - robot::log_error("[%s:%d] computePlanCommand is Failed", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__); return false; } } @@ -287,12 +294,12 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 transform_plan_.poses.clear(); if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) { - robot::log_warning("[%s:%d] Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); return false; } // else // { - // robot::log_info("[%s:%d] Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); + // robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); // } x_direction = x_direction_; @@ -353,7 +360,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 } catch (std::exception &e) { - robot::log_error("[%s:%d] getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); + robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); return false; } } @@ -373,7 +380,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( return result; if (compute_plan_.poses.size() < 2) { - robot::log_warning("[%s:%d] Local compute plan is available", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__); return result; } @@ -395,7 +402,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; if (transformed_plan.poses.empty()) { - robot::log_warning("[%s:%d] Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); return result; } @@ -404,7 +411,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( if (transformed_plan.poses.empty()) { - robot::log_warning("[%s:%d] Transformed plan is empty after compute lookahead point", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__); return result; } auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); @@ -811,7 +818,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf } catch (const tf3::TransformException &ex) { - robot::log_debug("[%s:%d] Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what()); + robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what()); return false; } return true; @@ -840,7 +847,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( { if (global_plan.poses.empty()) { - robot::log_error("[%s:%d] Received plan with zero length", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__); return false; } @@ -927,19 +934,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( } catch (tf3::LookupException &ex) { - robot::log_error("[%s:%d] No Transform available Error: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what()); return false; } catch (tf3::ConnectivityException &ex) { - robot::log_error("[%s:%d] Connectivity Error: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what()); return false; } catch (tf3::ExtrapolationException &ex) { - robot::log_error("[%s:%d] Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); if (global_plan.poses.size() > 0) - robot::log_error("[%s:%d] Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); return false; } 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 56df13e..c624de7 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 @@ -5,7 +5,7 @@ #include void mkt_algorithm::diff::RotateToGoal::initialize( - robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { @@ -18,7 +18,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize( x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; - robot::log_info("[%s:%d] RotateToGoal Initialized successfully", __FILE__, __LINE__); + robot::log_info("[%s:%d]\n RotateToGoal Initialized successfully", __FILE__, __LINE__); } } @@ -58,7 +58,7 @@ void mkt_algorithm::diff::RotateToGoal::getParams() nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); if (inflation_cost_scaling_factor_ <= 0.0) { - robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); use_cost_regulated_linear_velocity_scaling_ = false; } double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); diff --git a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h index 5982e68..22ef701 100644 --- a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h +++ b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h @@ -4,10 +4,10 @@ #ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H #define MKT_MSGS_MESSAGE_TRAJECTORY2D_H +#include #include #include #include - #include #include diff --git a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt new file mode 100644 index 0000000..75ac25c --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 3.10) +project(mkt_plugins VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Find system dependencies +find_package(Boost REQUIRED COMPONENTS system) + +# Flags biên dịch +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Dependencies packages (internal libraries) +set(PACKAGES_DIR + score_algorithm + nav_2d_msgs + nav_2d_utils + nav_core2 + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + angles +) + +########### +## Build ## +########### + +# Library 1: goal_checker +add_library(${PROJECT_NAME}_goal_checker SHARED + src/goal_checker.cpp + src/simple_goal_checker.cpp + src/equation_line.cpp +) + +target_link_libraries(${PROJECT_NAME}_goal_checker + PUBLIC ${PACKAGES_DIR} + PRIVATE Boost::boost +) + +target_include_directories(${PROJECT_NAME}_goal_checker + PUBLIC + $ + $ +) + +# Library 3: standard_traj_generator +add_library(${PROJECT_NAME}_standard_traj_generator SHARED + src/kinematic_parameters.cpp + src/xy_theta_iterator.cpp + src/standard_traj_generator.cpp + src/limited_accel_generator.cpp +) + +target_link_libraries(${PROJECT_NAME}_standard_traj_generator + PUBLIC ${PACKAGES_DIR} + PRIVATE Boost::boost +) + +target_include_directories(${PROJECT_NAME}_standard_traj_generator + PUBLIC + $ + $ +) + +############# +## Install ## +############# + +# Cài đặt header files +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Cài đặt libraries +install(TARGETS + ${PROJECT_NAME}_standard_traj_generator + ${PROJECT_NAME}_goal_checker + EXPORT ${PROJECT_NAME}-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +# Export targets +install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + DESTINATION lib/cmake/${PROJECT_NAME} +) + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") +message(STATUS "=================================") diff --git a/src/Algorithms/Libraries/mkt_plugins/doc/VelocitySpace.png b/src/Algorithms/Libraries/mkt_plugins/doc/VelocitySpace.png new file mode 100644 index 0000000000000000000000000000000000000000..cb5e660ee1f73c785da6372d0d0a964ef787e4ca GIT binary patch literal 34570 zcmeFYWl&w;m-l&-kN_b8F75;f9^5^+ThQPHcXvzRLN4wQ+yWPO3-0dj1a}DTFo*n} z{&#m(PfhpC)Kl|fDn%|F&N*yZd+qP~thECb!Gz(ajV6GDYBFc)N)~v)oh9)4=anl!7(S`)6nf-`)={j{TIK zTg;I(6A~(4zIU#;?YAD+B9Ez*<*+*0K#+70tYwP#Yh^;kU&CLUk2AWC_^=lzc6uv3 zful);AHI+3%13-hB9`SQ$&xrapLRW9LKUEo0JI3ya*2gf@bnExVf3j(p!Cd_Pahve zk)A$sO`kn|{5O6VLiC&ym||p?+1hzN3=hN#q}k__r0m zc^G+AYU*>dE1OJycjOyuY7{clDzcbvE{f853|As2lcN10Y9@QT@d32()z%0H)KcX; zoHbRHkgo!LCHK>BfY@1;oA283x_yRE}; zP-2R|l+$daLn$_{ojhk`O;%UsErGx{)F@Os4|f#`c687}=}0&hNY4;cwi>wUg2<&% zj(dSuH`$V=cn_DgtaX9oPqVo6e1DRq-{vo$mG#-Yx^GHE>dDHJV?4RS2eNTIC7PLRUCYwF?Y_=nc)Zre4SdB_4F)arjO>9xdT>o2|A>gfldPMX-9r2W z4ix6+HcnppzJPs5A#|A^P`4!e$1E(RcHMdT6H&*O)~p$MC_ZplexLgm6&eIP>}z@T z{O>)CzIV(HGu`OFzjaEoC^_n?2&M6|_$2ff2+XH*AltNr+DyUHeYTVR?v8|~3* zv8~i$PWwSY>F}eCm!i(N`O)K1{;%kOC6Ej`db!rLk<*p;eLQJ>zkumYMM1^LGO%{w z)B>J?O1V^waA%yV<`3y?Dvc@4z7OR^A*wL1qz4MMVrJ)SkusXTS37@Icd#T_qlVDS zu-B7f-9gv}OrJX;2vpk7u!m#UcU4w@*9;7bAcqnHbD(GpJzk*-)WxTM* zoOBGJW9n}2N{L-`KC&J5cF#Qa^utU-{Kt?>Px=RhgVpN$eX6THo{tIejjDS_(m|1# zeXh#Ce6b!z0(}K+{dNB@Jd2#GLOR+!x8@Ez?@n(_HvDeTa9E>YM?>!+9}=;Y zG;9WGncjL{oJPyeO3uyZaa*#1YMC-Zu|V${&@l-Mhr`sm{ldd0QOxl-Pjdm2Yiw?A z9gc%_@BZ}DIseW;t6og<;_rOhwwp2XB+|*n;Y!gvqcrY4a}j0JPD``IV?933HZ!y9 zdw7%*x!XGGCt^h_THNqKCj=Y=-~vErKzsE^K6CeAsEXDmOno`EoKxX4U8Tbv6}DlC z!-9EQO7^7(sti`6oYH@pf7bFA;T~{FZJavBM4h~Z{Wf9YdfQDbMZIDhE!3pXj%Gc| z+lCt4y}ww*_mA!2a+YPZk6PMM;H}U2f^3B|a9+W3L7-xVNlTCG_SMmW>l!DdEafkS zOrWnvSWouwhW+MtVKY3F8XE-a!gnWZQD^-@_?FNM5>Y6bm@%!T>z7FQZIzyZGE z-vmN)yMId+;09n~i-H#-qTOCdlm^+)03Ygql$Mz|G_n|ys-1G)m}?qb-|*<1^Bwi% zjP?9S@F?^HT(K9q6JHi_$2->d&u@yJKl$C-YDc&2^JPbLA!fGBcw@6Jvdor!x8^xH zgwKXCya{=2+|4=3R7bUSMW08Hb4gzAy?wI27Pby)78p;r%g-Bn6v7~Aya91THN^Ur zvlFJRXm^(MlkWx^6j)xL+qr{?Dx^9fsnAQG#!+Hc6+hvGF0?5(N{NpNL4{E&s2k;( zRgDXZ(StwqjG2&!WmlE|EQaM&#iODXL|-g3SGMjWV7`4p8BhhAom@u%-bLPrh8f>n zeD3vdPiHOEGdP;*_DVAM4WGGv+Tq!asTf5%7B2a(GJ_pt#pSJ%GHvcP%S(7+CzCj_ zTnrsMTqCOzANh)ce&JJUW&yGC4}x8=SJQ>xH5M^diAmmTv^85q6*gxHnbnkJFrd0G zTn~>q=g7U^RDwhpi@$4L)Slmi;kLng2B?n9;das;aRmHTu)@Qe^5g$mI|lQDz~+TX zr&VQL?_#3QLLx%5T9TSJBjB|+T6ZE=Akl*Kw)pd;CBCEPHL#61u$(u4W>Fzs)m8cO zDdfv|ao5fjXdWea1hJHEh`q4S)e~%1)4}V^mvy$vHR!PZNfJ%;1*jBxoQqe z#pS{2(Z4`;f<}bl;m_Cs_v0TU|HX~vGnLrRO%ljme;;)Qd4+oPdlWYLLu>mBY^c}6 z?;$Zt5<}w{DoxsQcfWArqc^b?!68vTy`L5_=qMld}PF-G`1&ByrX&h(sZ9Bp)+mYgFMr}T%`tXdey=G*7=42Cf7f9Wwuy+zS z*QBsKi?g$V-3J#>3db9Sie}y&8hH{=-lHlL<==OV%yFMo@c54Lm2>>#zJD=O17q_* z$o%l%rGN-n^HLO87QyVmK6c_9LP2y_O?Rf(nG7e?bFp@wQBnN(&@+JT^W%PPUTN9W zUT#|j*qY}(7S!+)V{;J~i^q5W+;5h^g7bM0Nooc|8m)}?p(U}1xgHe#2qeXz3fT3h z@g0-5atwp0LQ^@kh$KhTGxfZzBurhhhFuqwBN{>bn-zIcy&%$a)IWy>5)(T zvhB?vwsh!@ISI)+gNn1-*ID4jZ6VSJZvE(n8Bhh>M)UH!A~(hE!?1Zyf!#qjMfk&@ zw@#Hs2j^<@cURBDYsT493}|Hz=zT<^-_inm9wwbk1W$=&j+6UyajAJk62Ny;Bhm;- z^mbYcLA6Lrxf_772#>4Yicc`8iof|8P&cnrbc-d|L`@fjI%ai=CWQ9(ByBmCsc)J; z$~lkla*bPjTQIj-uoB{w5Q*pt*mj0bmrr#Cmb1tMUP-5z8C4?cNk@$I z<%g;pUq}Q!3b482LtPVNj?d692qSnFDan8El`aXVTi^Lr@ zE)9ghMt*BCyrv3hi)Gxitj3jvC?d>)=<3T_V4jMQbswD=3pV9QA?k$uy6ITBomh0^qjNI3j{td9LfElu;U!axgy!kJS1Ro3XMLrH4_J@pds2~Tp7j~CD@cOPDmh!mg(!tv_9qXHj49%SIE zITMpZQ2mTQyg(Qc++?`TzkRH~SHGv*8C9e?rqX?|%_p5ey$yF4&m~<~dOuO}6_pfS z%RL@{OWdNvZ!gePa-H@KI1R7qp{ep|kpbq?&D^d~3_3m8N#*xZph)*lDUwr0`1yS4 zcJksu%oIoMB_^Sk&f)Rs@|5%N@R)(@bC3)#E9C<8@CPBbx#Z26h=E0XFlzZ(ni2vD z%O}T-w5_q5)cyBX+i8Mi2M+M%(Uq@44c`R%2&j9EPKYBQI$4(1-`RL8jxOsT`&{a} z?w^W>IW(NeM4XURKqH)zkxvsJs70zMFwr$6fkXLyN^JlX$yRU?jMLwGB)B zH@y2XP5{C-ae4Zn=(yT!99xx>q5uhASA}6u=s@LQ*t2UnMn`55PKhwU=PDTm6FLoe&tDoSx51~ z@W3i>BUCj!{Nc`UJTNG%k~4O3qj_Xb8b0Ct(j+_sF+V-i;3z>=NJ2GpbF!*p`6hE! zIXgRl_CkNDGmTE(=D^-RmCqvsh-s!X&6tGaTVOrH)F(Cl?v0Nh7^ib|iQcdDQs*zq z7Sp46_~FgSK+AxJqG0eHjHR{ti)*wR5vs)LJOxCveCJ>fM*nQN0~e=@5>VEg&(b@rH0 zB&&>Yg^_`4->}$NRn4G7&pEa!_Tr$!^vI6m2O$CWC;PJbwX70n2oDJK8`-LoBljEm zJ{_)_BS?a^=W%T`A%hw#X$$CL!{{y&oWH^DyP2i6rG|QLWwDZG8@x1eJ}(zz)0O&Q zjUe+np?d-$7NnQ$anCO)X>XF`#wXhpv~e;qPx%|p$83wViMW+t);%Biqy0kc;_i*? z_Qr;)?zh>~&y5aeCEA8D4HfUWCz`9Pr<+SURpt)JUL<-Wk{sI$K9U=C&o5>#Hfe_r znIb;Il$0T;YG1Dxx#mmcoV{kZMOY3xI92C{yhxfpMi@*3p(_r4kUnF;>f8Ls2mSj? zMEtXPEp?+`Bjzr*ra}@L$;_-8R@<{DVis=EbY~jBL~I?e%-@k{VC~LCJO!+dJ6MR= zvR)<@^+HC6K>UFm|AYOgI8GXln70PH(U$md`+aaU`1_O_tMJMEB8+ZnB=FWSTkmt* z#(ap%{GGVW;fMubwRBbO)m=piACS&%bE~gi*tu`-1i@({=FXc0>(@BoQ9UiX)7pWD*l^$l1(ShJ4*$ z=yn+=iBRL5X?0W@E^R{NrP8UbDR0=_0H7zfapymy%)Z=PeS4G zeDCe!{PVev)rFIz&Wp4v0+p5**;NLSkn5D-KEX(U1W z)5Q)FYUZ&C)8wM`?%-J!d!0tg>4oBFKGHMQ{R9&fM%&&}y3~PbpEc0}vG7RIK52H& z2p6VlD8JoM)F$^pZBsjwG-C@eMN*E zo34nBn(oftNj~&O)xi#RuV%CzI!Lv!+_FypMx_XgNZhNL%B#&qn$XxcAV#JUF(i(l zpPY+H&A(i_Ed)e8=paDPLzK9r5q7FQ`tgg@d;h``h3PiVK8C$39R}jRt=!d^sN!G+ zqShs=Tv4_ZN7Ti-zt_`?{f{Gcjfsxz#}7}^iVnK+Up-CW*E+1O&b<7gK#9CcJ(U+A z8BmB5$QHLtxKD{ydua1C^O*wP}Ng#cdy$VILNNfbZdXf3cabQs=mF6I3i z4^`g+5vZ`upGeIik|?21x(tN1zy2l&1o?W5XTr6Iq1`Dq--m^|UwlO{t*HYxb7DHH z63fHRxvv=`S8y$=(*bi9HeU8*a~37|M8u8n`b*zgD6_M~@#0^U$*!vpq83i>y-+NUN%W(I;Y! z3p4^Q`g=+&BmywYW}6j5Aqk84y>awV6*~6dL4y&Aa!Lzg^<<2KXq^N1ZQXJ$Qdg$~ zS^L1RsP>~1mp3LxkO+wB^`o2XE4frHpR1CFlh;$vTTD%3#*9uY4u2&5s!3mcyxsFA z3QtT#qy3%5a&unT(95oce?a9y2$K2XWipR9fDEIwt*mTp-T!mr=NPfGd%hl@Fkh!d zOwI~FgDjFi5X%56-)dY< z1uF>%!%yp((cap73KvV8X1&Bgs+?YW2Q2YrGI8m_jJksd|L8RB6gFCwc%H@DOGk_C z3s*oo-V8P>nPl(UmxtaCSHCpvI>-hrE;{ffZ%|)~b;IfA6*b~6FmQ8*p`U2pEFQ~| zqc1)M8H!T=Vnt^F`1xuamc~)vz)E6xe?O~)rcBnv^eheATqhd=k?x>BS~i1k&!nGa zHPdiDd}_LK7LjClIo@mUO_m^8N>qAScvMAfI9%<6+%WuAm}B7 zoGl~GVIvUHTUwWM3O`nHdEJlSV%M8$$hc)4wA5H-s1+roak@yInJx_jf`)igyR!bS zyMF)}PxRNW8OXJ-Lf=os*OYIJEr%{EyQKaa!E|4EToUR$oMbkAKU|!8Q!hXC{UOOx znb%3}RTKV``*)%Pc?!H~kWEpr-Lcz#o);mwsAHjV@K{#Vv6dySWv`JO<>|3=yj?Pj zhUf}3b@B7e{_*Iwk?}7P)Ln^EdpRM~fQRin=F^dQl*0THDf=o-YB?WlM zjP`^Od_tp%#s->x?!ZCwxmuxy8nfuP(oRRd` zw%TpWw4lfm3%f-ekY~8#JYNRl`*X@Z3bi2&_+n-q#I&jKN`T+()GYL$Y=1vb;7gNH?dKCy9of~jScsQ`KrO#q6= zNXa0~dKdCzy!~GzsAW@5scVj<{rHd|Tc5AO|V!sH?7Y~37snyFTscZpO@vp~8gUe?3{Yk=XfGU@GdwRb~qiP=T z{5{Y9SK|WUu33jK78MwpX#rZ|_8!D1i0MQJIP}`7`>TtILXo#5KO4KfDl6}WERNDO zR|)T`i+t`%08P{#pW}ankD=mnNNG(g4iXUp=k%zekd@{-Ix)SlgNo>i zdu;9}_G!tK9Ab*G3NihxQbDKuk^&EdCV`Q)d2+>K&mK+air4)1IACRI@*atDSZ>mv zsK)u1B{Bm^M|$K%cq~r@18T#_oW;)hA2;Pp9j4OVgYa>x4!}nhG0+4^YjIHuQe&bD zWhzHK^iOZU8(-ddM<>UY>-Y-TqYchxGg!ojCRl6c69?I|wF*@w;u(HiOn4z}j6oOs zHclHVzRRoWhDAun)BU(Py~P$~Uj0)e9+aOnSCoMet5xpV${Gh{nyAtxpy$_8b~5Ui zNLy~^^i5ms&mwp8k5v;HNV_>D7Guw3JxV$;jxOr8d@K4PDeXL%cvFNsGjrDP-UQ-0 z+fb;>8`017MNf2o-U`g(9Ns@^=F~$jl<^h2V0s`e;RI-qihb{;=`;vhn`Xn!wxA^c zK{hQdJKR#MX7Tpq`!e$7{^@0?d5jhyR$3&j47ypGA4N#e`5PQH+%x40V$Qg{=t~~g zgjGaibL5v@nb{NfB6F+r))JO|G9!>fb3f#>dfjv>Es)W(yFBbx5ekzz6BJVATR!f~ zRznr1s+{PwRcTnB42wnXd%M&J*No%}5g^JwmR)ue%wWdSEk>k>eEyE2V z%76Y4MusD(RO;09PbkFVkUjQl&t@RRcgv5sj5;3Uk$KRg|?0p22*pj|a2@ZcQm39f`|#8?*4%i1^1V zOXW&Nn_#k;J&g+}%0)862aY4x16Z;^r^hhA?I|NT8`d{K{PaI)p&-8hO%hCC5I#)8iKECR`F3%r4^Ho6F5sp@=> zc*|@6JU6&RLdX#is+hAG7Vk&vk$C{Z_nrz{lrH@4V2-PKM3$?ODfM zy^UQR&^)#~>;AkNfdzqpOJaB4MCh^&-Fi7)vh^|)d~~<=CfNZbSn5}gv5UhjR@U`W7rr6+BpqkKZzdw~J0>mSk#J`MT8(`g;SJv7%`mm2SV>6x zpzXfQqR+!onFlQVe!n%teFxTjOCl|z=x^4}b6YDy&_MODe~D5q-f-9bq5u|ZwBn4y zwLIkLA>y&*{R1${1`kseRoj8IFE&+rTbkje;&&Ir-_>MrZ*l7CY29@1SC}rKY7g#b zLzQJHu1y8L`?FUD7tlxd$C+}BqnS9H913Cq8~$=L$l(3%a&r?|Z>P~T=K|H^dh?Ni zB$v|c@vW7=_>u?KcQ6Qu0`@=@%EsjxzX|5m7bu6>I;g1V!N-c9E}$jDI_o&#EO41P zMl9Q&k-?p&A~{KLR31*%SH8^JIo3@mYD)5#aH>2ee2irF8nM0*958H^mFB0$z`DW3(r}6WH1o8v zW-tt#sc!xtJe-#)JNas9#yN=Y%knh_9V{t1Tl}T&vU}9M();`dEmG#U>!DVbwC1

G4ja3zFuHrlgp<;D$e?^SN!C#y) zpLJ_6A1##pvVs}vK*%=))16OAfBjR$F8}4rj&5w)aIB;@G2YXgB~Jfv0|-QZoLAC2 zkIS;7t_B&$h$sxqOtO~!lCqCZm;1Ep-)aGZjmeQiU9HzLaPlZWcbjSUh<5D2UiW()xG6biPm_rGC4aa*nW|!afz{2lBiM%*t>jGh(1BS(0ocr+!%+I;L%TYL#y-lrBW_N9j8a@oxQxXVr_hF?@*=^`7}{f_a@@WF0dwwd?*N*!@r|Xep9ac zE)DS;6gC= zk;CeRUhsRT$j(gc9K-uE(PT)Wg}6d`hYBiQC)GDTd}I`(QueYWm-*t_Epy$Sa#D?$)q?*Bd!Ps zjgf^G`W->e@5sgt4HLU~yrv32TTB(Usy;>hWNi#2Etm49ypWBA1|sb1RnO9LzYITh zS`ZKkZhWB8z?szbR!0PQ#y=1sz4#>v2iw#`2za#p*PADm4p0?1^_QUO&)OE33S09D z3`6kZ4b}tl=nlkYRZQ$0b~isj!c;;rUueW6(I(!D1(s!ke!B-$Qv&wLtR;vzZLhs| z5nb-Q>FW=M1_l$Kp;AzhLP#MI)`^bipH#3?WqR)P&=>^$*nrZVY3?&_P;KhoT$Yo)kR>zPuFg3@zLA!b$ZuUxrVO-xlD?g9^|=#lqX- z9(;oM2d;uI=(Kp8tb9BUl*x>oq9{B@Xi#3ck%fp7qQ0_vzIx288*{8`d@}K=P!0sD z6$29Kzs2$_;!RHSKgf77jMHU_e=uRlfQqA4B5g0K2!6+=MFSPn{%cffrz&S~I0k3d zpV`N%d0uyf2umClXWp|;k^U{q`FaFh6b_olNn(}gV*UC}e&Fo*M?-L}89PWzVC2G) zPf%d)*w+DfpP$EZvnL_Zy&|WmLY@Nr2hl19KBo(nMBob;+1oKGWw-2hu8x2?@MDj` zn@_riWR`+ev*i0br%81L_IV0D244&x04wzcr|`SQt64gYWFUaUaWEf3sBX;3hN@pK zw!K3yt`H27h)Y+v;d6vdQb!};25|;9`#GcJd+|l-nGjD-^@pqJp2JMbkPbrHHq+-l zl3(X3d`MrNlqv5+Z^fK6T${%gEul3%w4*i@D*MXuSz0!VhYV-*-&JX4;7bV9P$(1Z z2E(ORT>sRt`ooxZ+~~5(>4LCU{a$Bja|0=d&uLlcmojAS6{y9QDnKv`NhWu$E6l|E zF;IxY({-ovOe95>ddJ~LOXu-cHdHyHYZDI`5UJ<*dWuUR0M6?wd;}Kf_U_q#;yQz}~2%+H31H7(XrtUYvO4EONNt zw=oFhU=-Pxl?Rvlfn4=+yPTHnBA$?2$p-8WuhQ~d0F&^y0Ou*q(7{<Su?v`O~tmHLuEEEgHDUHIuIDT?a;W%`2JZ>Fv8DF)Bq5P8gQV z8=2=yg+xBC7fYwLZ5)rYDoqS8NXmKu3Nfq^g8 zVe4)pW7xYC<~TiCzVM#78?&l8(QSv4%Y{w`1;31=*?#5y~2^!k-$sa;wm5!l!vLf79(u4oGGP`)v=%X8Q3hj@Pin)t0-U)X-Hl+|K;ZTN*@pm z*LB+`di)}i#$QatL>m7-PhfGBNCQ3=5b#9qM*WH)B%tx+=~zbsoJ#++1-&%;z*49c z@ul2>+eTq@-!NP~n#bQjB(%4k=(uIfjDT#H=LPNeyfGmn4fe%-9ILGK#rBcA)}!x7 zT=`fii$HS&`|9exyJo?V8-2QbrJ{X=c$SfcZLb8}Am zTHAc(`|>dGJ`7AmJV<1xWb7FqZ5xbGE^Y|J6C)NJSB}8Di+u4dk_>9|r@+^9^*G%5 z;QILh6iiW&)*pl?DWm^Iy~YsH=+p)vP79*T#VghKpYy|XVwvzH-;?1mJC4x% zYjJ*o^O1lPMvx6>=a3@7VG?@3UddExzvL<|@srH*urGkVNUF_lT7r>kRw;3?@e5cC z_P2O(`s;w9a4T1}b58UY-WQBy^VTaWYLAX-69)Sb=qPlCET}K>oLO$L#T_&4NdtE< z`GOVp!Q^b4EfJ$lQ~{_9Fc_bER#MGLO-)4%bNN)DeB zwf?&d-Zdl;29Y9I$>*Bt8*;{j|0RPrkZvt4#E(*yWD}T@`v#Rm9}ftZ-(c(SI(kO> zL>h>c&s|Q5TLQ7BihYR0r|YE!ogTC@E1%B+-j%WBohGiWtw*r$#|_oz7}6b&^Da%_ zaXt4miG)RbN4+c0a)0k>%9r~DiD1I&_l)s+@aJb#CoWzX9W!IcL;pLn_%T*Zr&Z%C zV)TEM#;dg5O-I{y{P%N*a4PS}gOuo~-tjl7yH(C$u&*XeUv^P@5|F+3Ox%<_ksj11 zn`(4wp*pn(~iXWHR_kJ2f@*IPGAUz8bqg@E2r&J>e_L$acxvptYN zNU_RsL~!_ri|?*=QTW zOwSO`lsDV!bsVBs?JaX*P+p3>C)JRe{_LrVth!Si;;OHtlROY@^*$?n+m^|h1DE@2 zd@$U8;}S#ogh&B{_&!~(W(`wG`_}%3|IQTk@rf~x>#J+i3ImLFp?Z(K)?KP9PrVML zq!YB5rZ*{Rw?WJ_lA*m-=Xy>tn?X?QM^{coLJ1X*S`k1s`;Ael%*9;{5yB6N=KVxF z-hjSM3jew78^{cQDKChic)V!keA2q~x@h;Lxa>&zzW<2GLvMDl*^}{mca|;a83dN&uM~u!x0q|B)e0~L@W?kb02t5nS+~_@7P}3Zg9p0goVp}1= z<}Kz`o@vv>-q|>7Rs_<$XU_LyWWs}BR3V(qawMY0K$j{GFRQA^Iihd9o0sr7Sz3rn5CtHPmUFR;rPbN2Okj{9Ef{N_b1$$;GNK;DI+ z&nzOV8yhlRf-gYD=u{?(qiwklC+$*SEySx+qY$YgUG_zbX4=PT^Gvy>fYJt_hVbiA zPoGTcME_QJ-aKq%SZSoe$c*y~sO9{{zqylV(|EBzK1k(Y$nguybFNR_mVD1vOY1?R`l;jAjx zUT!h}o{K3YbCJt%^llm(g!x*E(uC(!S_{5uujdKI&Jy7Qgg$_+e<$iId_o!cJs7fT zMsQ~u{&9cn*ffX{f-1xW!T!1K-LynDQg`L>C2w+;w+dtL4IsKrJ?c`^r(0cHf4!-V zJ9b;ofirjiF!_ZnlutQ4>G(?WE5&o7$Uuq8WT`Nq5Can4>;{yo10|(e{SeP6G|^9) zR6-+pAdofZ-xs|!ZuqVB3J6C2mTrT-9{X0q_7L3(L$H(k^)CORvlB^dRw-W0fnViK zvv?WV0z_;(l;8#G2IbCYysWg56GYyvGrh`VCulJ+KWWbXBJU%C$MegQI$ zz3eM$q>KbB=@Ba#d5~6+8o{`wQQdx1^-$9b?1JU~`3BF{O4y+$ReX@`0#Fcv+{yT6 z>bcC%|2@+9mpGtx&7ym9r}@kGgP;eu{U^crhZ0$2XveYz@21i{9NjKj0D>Fir9I}V zDfNyN>`!+G2;(vym@e3^^iD)_?I5pG8I|C)4bZ+UY_ogL33&Nv2I~U9_QqhgZg;GM z6IGvG2sqVrOg=};+BFA-fvMzJ(NZ+{x24aIGx9T~#$h(RxcEhF&)@)qIDpL6-s2>i zec!F*RxP>u+-@rFWA&hAdrgAzIs$Y ze86k(Eh+WYpyM0fZg#iBVR9Px*(}OM%ptc%T+;d)PTBud>F#nf4D!sqb3cr9)92_0 z@X5<;D;^yWp|2~z`H{BPRxriHV?(pcbwf;P^DHD{%O^QYwQskMQ*?qWh8ElC4fb)t z_f2uLSOT*AqA?Y&gm%%p)dw#3lmG*f1W{Xg>nU5Xd~s86?zDXf=k!oLrOS-`@wiIG zm$!-pWI^*ieWJMf{eW6UcNq?0+fKAhmYeaJ@B5Gzk5`@iRutz`?2$Apf0qtFS5;*;wdR8ZrO(r`EAs;_$9)6&Ui_m(8t`~SYzz$!R5qbS{(%+KWI3=z=gZ0BbSz0Q-2zW`S}A2 z6yiw~zQ=}`eO?W2s^L<4Jv6xj@H}AWZt|cH7oZdDXKqj4?~sFsX^b4aEfoh6A{|U^ zYgw~o)_oyI_Gs2%!3*fV@{4YS5}$PhY2rchHf%ky5XR55LH)99JP;>BPk) z*0K9@A54c;|8+R;8Jidx?h!(x6zdmA#C&4VCj_w&R75UO(~XAXum}n9nT6Q5+8VKt zwco2cl1C?Lh}?^(z7#)8=ne37uso3Y%WXL^&HpQIo1FOVoQB^+#I7pP002s5 z;xQ9H-oBPg(j+qy3xO)x?TQy*v0N{+my64c1^!a03@UkzH$xV6_Nmxse`jNVZ+Aho zKRGYj?mu_T;wbg@A}o_Xo?*YDh_KN^j?JjcmWD%#?V-lSlSK=XH-0SoAQ}elHDA^w zE)(@kR_kRVmE7=f@F0=ysJ=JCb(h1m%4h%6Rjp(Rz8!*@zR)5a%knL;;p=^%MpMy| z^C83i|DZ1AmYWx%CzTUWQ850@%OlfqBl|s+Sw1RGX|(Xa)}Wg>`Y!ON!$}S{a(H-U z`lq8JY=p2sp>Eg6QftvT+SV?DcJ~JkZJt4IkBDT)Pcud3!s=gP+DgXMwVP@SfYi%{!%ABMw~ zn@Xu|qWT2*;xegXkIu#Eo4X*wiq+pG*k}{rBBcF{H=pv>0 zbh87M++m=ENM%fM#Z@&y;g~k=F@SqH6Da2;YQ=HZTQw?7_FBzTZtOdjeCy7S*2$@O zKi|B&gyOvXHFCJ*Udk;+M~w6I2wXOU@YWWQzH$^^w8fJQ9mE7PJRI9*a!2G}*aq4v z;K64r=2y}kr0^go2Y-?zU+o9uvlB~>zC<@dK1}mP&0MXY-(UA@#MGGF&^B1kyNO@s zyJ#s-^10SS4i|rd9jHy(AQyF<<{&IW6m|MxsE6kR$Y26m$sARTB*L3nEv{F7Eez)7ip@C}=Uo-zXkNtnGcHr8 zS{SXgQ}YR`}g49(-9j`OYO&b^e3k~Wb`C}{yE`?!|*Te_;m5<2pkBs z_VQmU5%}pT-47fKYe+vp1dQv=7W30B{~QGj!LH=fmIwIl8r&B1jFxv{LK zNtz7lZ}d4T6q!&GA@2@Sg1c>Y9-z*k%8APm?OQ}Q&d;VEu^C_U3h9~gEeB3-AA|{$UTt!DXvN`Q979Xvh;O6R606IB9Y1gNhKZV^2 z#i}0}qOwG;YwXU8c-{G&MrZJS0WUbjwEXxWBy$>*^|BU&IYYfAsW6+OHw7HBeNYX> zt?%9dRKQ%vb0HCCl2K^Gct{MDJ++RVOhxQY`RyF{EwWajH-pXT$;)^(sF+LUkj)faW&xF7o)8$1o2$p=t!HNfqdVFon-x*cq=!y zypCQD?1a1l$R9v3?PdT|O5P1MFK|?m+q4BY*ZPF%z*cY+jlm1iNo-mj(FHclyHx~a zLsh4Ty@VG^caD>V>cSY^&{g(wdquGs`#9ii!y*eXPZant^o!+c=Kihh06G(4@T}rDR?$prG{grMjF0!?b}}mS)8l{TtgPg9B|Pa?*#b@56vzK^5E<4}KzG zujWdTv2@hS-h~ZcoHzrX9E$%@YX6$N^ho&tBPCDScxxV(qsNPV@Tm!f zUC5s+(+g-4^ICl}L{lV!kCbP!g{f_in5%6}+#&GK%O7Ae8P!Ku@k&RxT9Fafl!RP^ z1VL)-UtX+R+6IuLgUXoPs9mi848&Bwg!FCb7w8S}(T5SfT+7_p2U}^&g@MbssXau3 zJL!Krn12i%mFVITDnSdyAz81^$;9vllZgn)fi<{n8pz&{f!SS*gyU7oS86TZJjnm`G`)~cpN&|xQY^i;_g!Kf)fxb;(qOfS zy6P)C;&D%wZ?5JM#FPkJRYrOZ4QEg#qGR_Db=^E~w0lk5jewYz;}&)I$zcZSWJAc< z$4fwFDAXm5Bp0KJH7G@Zcf8Og%=AgVJZ!u=A{aU@j~VkBYZw{5%zIovYasgPhB7N{ zhjQ9?E=A+sUuGvGPjKqJ&Q8+LOfIpR#n!vi&4r}s05QuMPgX_QZfZ{hohHArtM_8@ z^V$kpKDWt?XQK0KQ$$O0nu+pLU< z_^@Oo-|!yH6FyxCq^NaPLM0%o`xg7imb1ECj9)GT=gyA^f}%Z0(ZF=sdmozU;0qkp z#2QMe8RRy5+0hQcuiQ$f_GjHoHDFByWi{#6_n3t%|5gid&*O&pT0V}yhmTZDkiN3M zhNv*?Q$PIQ#|3x#N3MT~v`7hJx&+N)Xso0?w!&U) zWh#FPyt3~I7!?2H*ea6XcME$5$-O#c=egFi@mF=r_-u98gA?iX0%LFNzDr|Stp#y;;%6%Z%TDEeEr4I@KLkJhGVA+jk%KJRd{ z(DL86LdAdVH*gn}{+i!Ifisv8s)F=Rz?0#RPDxMBLg*HCe7*)MB2_U*fknmHGbhD% zE!(Du(ga#?*hq^)fHXBYJomqm=Vo^tE*&wbubQ8loU*7Gbz~Hy9jE=_sQr;HhpgU0 zU8`-5NHDAO00j=H8Fw)LDd4o+mPai{zzX^!4OCIC;Q1@T*@oh)k0Jx9?GTx9SJ-eO zo5f%v+stc#R_Tk>G6RPUK-MDvj@+K44`3j`|BUqiND1{%@vH$6RU0U*Lk!Lq%Y5C^w(HJahAN$If=}<-5cRo@n?-QJxHTYk>gcxs3nw ze5|5Dxb1Cknb`4Z_z;i{y^e0QpK>j8g1}!7;hV@I1F8ET#WmC zj1jNhE`1ut4o&qi)R`G9Gx9MKG@B2bPDTAQs`3W(?hOF}P{KHT>k&UJjd+_Ri{SRq z&1IflSRAY*k|O-eUa<=hPwu8b?!A{ObB;zkHkwPTgW)hOYub!cEao%Z<+|}(?Bn&_ zq!4$yZm7dSgNOb1dkg_>HrgrFPNvM-&_b;kaCI^{gxBtJK;5snw`&J-UsE)2;+Ci1 zKdS%{EMem!*CLgnI|q_GC~8A!LQ{Nk;|u-g>T}yuh3Y@}7dF*Brj-MskJl>>z~&}` zdNY~{V3~|>?1g^**s32S3=;G=d@-U^#@+maj46R4&;@D&GIJTC3n+Y?+!~JJGOjw8 zOf1&PNcoO2JWQ;$>+pAer;@sH?PGNzeRzblh!Tp!QB>XsmIV(BcN!g(SwaMYWI4cH1)_%WapqXax-${1uNRmh9hJgkdC44rQf&>#54I@&n62 zuiU<)ZT`!}C6GtQU4kS58BkNsx0*9>MNhec1vU)tkz=zB`Ew&4R(dw7Me6y?xC0@2 zQeX*8r%gl;W|cLe*i|=H=o7Uj9(0?=S@GbZBhUX?_wsLg`QLmKBo-&j-fe~k$00n} zK)kiql*>azGd|itq)@JpX!J`N(p55^f&pvPao9+K2$>kducACEf4vTb$Ot8f>Ja`B zLZBmLP(Ao6gmB0Qk@}>+MH{Gc^Lruj) z+LZ$d+_?X_V^tT2zif`Eb{($U_~++F>$jAZjNicQSya}1Ji0mBMSpM;>aOdq<3tu- zG@gSsJG$KfNE)Tw#8HD?E@C;a8<(-AJep(v!p4%P6RYgVGl;!~oRp8|>Sii>KaEsz zW8;h?>RUrusFkRdfC3hSgIyq$z3XwOV!RN%foPB2txd+q-BWH+>B3cWK&xGEPX9E$ z`p3F%8<>SJe=r7?jfNJ*rF0iy4JN*CvfsZABib4-IgnF$`6&~~r9WR?3yu{JmenUq zb@Q@GZtG?eYJ-`jzHgo&VE?kH2_M3Y(v6T1aXjXWz7Z!{f%aXDx8_(OWFoq7t&(n? zboET>rP2S<-djgi6@Bf(7$_k~3X&cv$wPPO0SPHZ>5>lVZbV8NLAs=+rMtW1(A|CL z&b!d}_r7D?zrOL^G45a2;ef$D`>eh8+H21F%x6Ap?a0>RW|7=EoAFnNltw)Gy=W{W zYg#ftm95NT`Pmm+g~k~ojO(S+hTY~LhYY(_{%&wx@V}vXocNNw^!mTpae;sbp_X<^G8d! zFQHR*Z|Pg&*qZAMP;{WcNyfh)040nU29oRg6|Je zMBcyr#DGWtj1E6nj8WdbMBz&}k7-`%NLhQ?+EjL)X~-n_7a*2CqJWN%jvw&a?~$k4 zOOJ>5D@}@)%R0?YSgVWKiVrWBaD95IbstcMHf){GhivOY*@}yoOLzO71+4|4*hAyO zMAaj!hP&I>heA*F$E8G@H=@o)q2*B-?dodlSLNTZv~^x4BAqV@Ve*+|3K6N!esNo# zAiXWT%7)IJe8^MqynT&?B$|&tkA{vLWjN@-?2p(bzrN0}KP9sZkgI9vT*-)w?Cb36 zXAWADWViFmCaw{ix=z7^TErf*21?Skol_A_dhT)_ks|iM`2$9Mz08bR9VBy6+>4ks zQ{(ozIXRw)C6k)6Brw`iY-h z4R<@Ixk6`cxf)*%`{P?9qs+k*CfvcXuOfee8|1w+EX8}6Ir0v%{nxP%|96g6{roeIjQddMIT8{< zAQX?!A|(VXY>TZnqd>aptZ8IZuBWO^Nj2Qg4pj3Q=`5@q z#+Z1A6GIIlrJ0=D4PwaP9#8sR6Up3d&JOepw^EK2%~fs5l6|d2H()J&}zU3;PYhbM{fh2sOfu^K5R#GF{<|)8``fCSV{QfEIrm z-JI3bOxxhPe&(h#b*oRnk_*%6?VrD~olmRgG8txLLx)Gouf_(^-uRc=cxOMgFF*Eq zo|i#6RirBue9<%P%UD<2UXxHK&Y!QF{ycL{IY(c2l6z(^bMnf~nVT-?jLx2c_PJ%q zTOC%2u9?w={cd!?1AHVuz-sn&{5HUhU+0^|e&N-_WYzQPPnPmtR2*?Ci=$Jb?eWX8 z-8tUT337;6bWqh>Hnvoj^Zk04@d@(5;z@G%o6SMZ6zB_=-IWItjLC6x=Jl5sM&)X% zM_K{9zV7(}5|+n`ySN7z)uu<~nK-73t~pQ^U0m1+`L6TQ^~t4zTA`MhwdU(z=tA79 z*X^^xRNK=Icj9t4Q=11I3#u=S6AiHP9ZJyo|p0)CHEgBH=!mC9_mCb zrJ+<2#{d(=qA|BBW6!W%3)T@?OUj0(Kj>YJX*)=?K$G9awHaeR%e$uPe)fX8)xEM( zOC|ppT-dOocCD?bMSJtRhQlRyP;x~226!?ev~D$Q4kbGCZ8-<15V%zyuR_jNUA0GGPIA5yML;bW znr|u*Ox0~>Y;pOZW}E=a?Lg%kT3=()@(!VCUp~Ygr^lti#itrf5BhozE1WYESp7rz ztheWC%p;t=ud1jAy(^wqVQUA&-J+-3>BpCr1ZD~$#%?pB%eWx|!od1={s%V137pL} zMjO%o_x{cR(f1q_aIL{hSYfPBqO%!8s^_Zei5(WqBz`B=+RE@*4Y+5`+a*J8I)Q^G zK^0teRvO=op+VB)Cj$fL>yz(9bCS`UKlq7C6#sBtF5`h8nurRdQh%OA`QTc?M?2W} z>d*CjpQES%d|{>jW=ciHI^n}-XB$L>K1B}G_TGWOjbY?W^x1g-4DiGRL;t`%0X?vrMYLxR;4n{Y&mEI})MiHm5%8(j3PZGC?&tH%nDGM5}1=( zo>Q_Yl`E6lu@va>(^65j`ISjnLrWo))RWUT@ZQYqOnr(=^05g1n@wM@&q z*!I+!1>qpKT8lSW6^RmT%}2NhW~HWkxpXkSIXSoE;ma|hQ%A$1G=bFYsi>dZg45FO zbzm3>>AklE1#5GXcJ~fk?=93)2<(z>K)kpaF3e|DSb*BtTU*E}ednxmUMaEbH=jUz zFp*2I4?O3G7DwP&!;`5{r#yPNl)L!$ocvpD&*Q=C_Df*o#BRQZQW}-N=EW}?9=F#F zMqdH@e4MGh4y-mvsoZVZ``Vb|P1u~<3Pm$PUtFZ1xr)R1l$@r^S&3FKVwkXp|OEL?CVwReL( zok$QUEoo=w#D1bE$t6 zUqcUH>QcSr-{g8ok{^E0SXR6wlt_>g$-+Y(k55fm-(L3Y6FO}a_vZD@2WY~$t*8fL zur#9ITmZykGVo!!YKZH~YD}uoA$_&VY}OWog%#Y&G*AIffM-}5_Go(ACV18bbRr@> zIRB=Dc(+`(< z@#qY5ZC$UJDjuCbB2`W6O)x$1pStTjm+UNamwxmp5PQZryDQxnBoTI`o0GP`1MUL1 z(FR)3Zie3YlN7w&{UovrsD;(jInGll1GJRF$Dq}xmC))@gUSd&fV^{4&PMuFET z``+3mf23oAwiV41ZEtX~&tQcN)jxK;kf1}>O(>8+g<_{4=zex#uMrz^DIf5VowZ(m zW_U1^e>7dgSV$;($$BhlYulP=%e z@hQ`CeP0Y1sFcfF+g9U{lQJ7`ZMo^T4PZ%Yk=(;0JKG^H1Ivib5E1PniT;AVS*G+$J9t-CzY`~bwJVt5yVcRSR2 z4^ex4Z|;%V|`m=-Dq?FYQimj+c0T56k@) zfDFQn3A>TLqI*XD6e2^%fjv<<@U~K2EGR2G?0w^hC1@6gdlg`8#WA_x3dR5dtu+B>*D{m`6-d;@g zehL@eTHM6BOtU{MSf`yR_r%%TloZ=uJPeO&uBWeLD{>`fy{j87YsY`s2Axa;M;9E{ zh|*DuG>{{oS|=z^3dDlt_p>3loK2Csz`rVK!}DG(XR~5>g_@H7*v$XVM@hw$reTcs zNW>e<%06ag1UI?rCYpq1#cpO(ySO`S@#$~b@AVs)yN7Mxv}=aD@?R~l@?kXF>%+gU z6D+&1@d-$mb5AKna>AC{`DiFG4A!kQ)LT~K!pC%ro&WG&9*MC&8!{&-HaWejF3;eb z1YNS-qetU$vG@z9sIh}bKH0zBya19V7N&J<`?NTkY%XI1KV|?lyheBSC>$ zo8}dYNbIna6_GB{%-NV4ZA{Ww($LcRau((L*sY$jcsMr2T2Y~+e0FzgsE>wmZk*`m zu*gJ1&TMw{t?FC(fl`!FrdC zz|0UL5e0|QST>5R$CmlS9^~Fv^wk|!`s)Q*ns1v;y{7z7TA=;PRE`%F#VB|r8g8zC z*qi%?FHK~U^amm<>3v zgWlX`<}}hoHKN;gSHDB47iDIRVW_UlqzjhdkE62(N1@@DX{oV*>C6Y4} zs@<#f`3feXKpD>~{kZ6@LuvMJ-Du|v_cU!4ZCY&ew*p*`1Lp1NCz^eHxshmPFHSD^3vgex--P-7m#nC);hfuL78x5byCfGtIz>Ie!!l6=vKM#Chy1atIz~iP#;G)y$T^?lj~*AxoC-}V7}H9^8Tm)lwt~92%(o6u_a`+P zX<9}|_)}IJK3`qvr>i5VQ2Z@-hTmF}M~9ybs8vrs_>)Oa*QC{jK$ zV~J{|k94L4!&8UDT_aMOAb)D&<8A;p%+6MHhxeGu<8G48y?xHkbi4l5ym=q1w=Te1 zbe+&`b?-CPCD)kV5Sb5fwmVZZs&LX7F!L9s;`k^B`EqGFa||{*(H7FrzVj`huau>^ zg$>TA80XIsP^-waU&Vy)m;qigH8^>JI;}(I{qGRqkcv$na67{4#pwlp5@P& z`X_<pIb;XhuRFc$|7f*~L*N7h!!8FV<-FMKo=xLg~ zutbGNWr_p$pKtrX1xk(AlU>naF3bxf$E}+e7rsKAFWST$SL@k*$%NKr-^F{LLlaXp z&Ngtmck+~*jb3t{%=nC+}YFX@3**4Kil?q;q5RqVXkxj?tNs2WIWG8_lU+tbBf6WLFivz zFMN1%;~8&y!_$*X!x-j$U6*6Ll)s=Bi$}i0yGcf>VP9Tm_L&pg%aR=3 zIu*jR;azBsi@!pieJ!vG9vEme>_{=uu~h`Uolzew=r4p{vOjK3bz`v{1DlkYTUl~d za#sau=Ijpz5#4MiF|j~9NO*iARm5)ib}%I{JtHGikd@D+g+Ch@cwuR`Fd^yxM+-1w zWNx*vNZ|QDd?kUBe1^x#yP+C+;}NRF@=w}&dAz3Y1Gz3*RcCj!Ib!>J{{&k;qY)J* zs~ozSPZWf8csgGs3XoV3mDVZM7`-mpEOD%sk&ZGTvVe zI6EF{ADvQjs-BdHApi$JpLw55p38VjXW}n6TAyG9X7pt6xfvf6fZ13xSn-1X$fd%v ze2(5_yEhyk)7ARD9D2znrKUukV0+!%RIg+gdB~&kiu0G~-IDKWp>2jJse=qqX2;Y6 z_u4fouO53ME2$?C-P5d{WwdsbksW}O_6jgkosT?ks&15Gq%#fJgKh{BWDqZYt8~|+ zkNZd&uSiCbkfMV7sqyr}^(8r&V;`?!QvVUHSK19oF!G0E19(KH8Y=U}t@lk~nc}Z3 zb`~;u?xP%kZrJzS&})rko>ykZOWi{@HdZW2SngN9ZUI&)*a!|5NJ1Q<9TnQhj4~o> zUeJ@|oLkEKH2+dpH_4D4Stq!?^t>S_+e2<~=;AT!f0HHOF6qy!gVb*9RyN?6k@u>6 z%k6fMVRCnn^v4%xWT&R*?Zz8VFfMCZVBcTR+njg?_@RHl;5SK z+%d><%FkBa)rt)xs~f6gm^%ul!NFUSp(suXMgP8ETYDcunWSUv>|JY6!CxSq>1ddL z4I*-4q9F+Wl4$M5T|+XO%z-)_x8>|#zVOxe)3@lRw{+z*aRRdvsEH4S9yfDYY@=ss zGTM=yw1S4Bb9?O8qX?pL`q@%L-hGNmc0X*3x35mUtof$--8wg8I$u-cbBZQvBti5u z-BrYf-{_^b8>G)r+)uika*SI;xuz-k_ONJMJpubEbU$p7oi$>^i8ll)NMVsR^a^b? z{Q(l;1jxGqKfvm8PAd5hR0h6XvoR$K;E2~KD5tovU7mDJ2UM7GnQWwG>ekk>+N<>y zKKP3It$$7A4LNuph0?UTzOp*I6(2Mp=How_kXk55hHo)l^By(Pf3pEdA7{GB0iH&K zQ1tNKC1@a1`uMucuTiGLEBVBRw()hpNMrnqy9Da*XGpln|M=HC`v3p?F@i^{sRB^@ zo%N024zJL|%7e2Ce(=5lr1*&Lypy9K0ax2=7FY-93GZ5EWc!Lls3A?l&i8FpNs1fe zv(n=eXJmWvmtG8gEWANIN{P!Rrd!|lTEDAA@#2&8~l*{iWDMte&cAa@39j zIZSvI?|gcF#4QY11SG;wTWj;C_JE6n)KN3Vu zv*a^pGmTy!wJu7a%L_tgaqy_TxsN7`*k|?(^h;w~Qta(TGBkP4ZI-1As7PTO9u6dW3PwV*aGV^)c03Rkf-HwO-#mneB}*5SZk8Ms)JtbFmT!gKgiYG~ z?S)X|8Sv6G`glHI# z)Smm1z{FEET@HlZK}?+F+ zdWi6r`Z*q!RXA2v{{~26Foy&mQ{d(cSg6vC25sGN(3eAG;rMtyc8g*Idw)po&cAvT^WV`oLcX_b?TQS zzr4;NDUVP=fgIP38Xw9|YV6=Gb36 zYCU_v;abGHtVqGi9Lteu>+D-6bO{VdtJ58 z%}!-Va+_zk%EAowI%dS!KQ9;hYOge^>7sn@;-dd? zFo2xgTQg9H_mlS#MX$USIe>YwP~OMmQBVSWBSDVNdyfs!{z6I z!)%UH#N(s$Cmk0;`q;5c^q_zEDPY4C?NRHLK;$W`yCf~B$>LBm8{-QK^o<(nsNilt z>xej^s?2!c%Az6H!i~M8`=M-~@6d-qijBEMN>v|jyMIu=#se@);SSmt%yvLAp+bBw zbWVRxl@Ntxj6G8s2P~R0aNq9wtD{nLzz6&J=?>LXAi zg;H7FV+;>bfcJ0}iwLWH!+|W1;v6REjiy&-7;&mEhBFf#MEI9Ld*9KQ7M0e1gNfH; zCB@}K%QjtLSBdVYqI#`b12@Knhx^%q)dhb}-Yx~X6^2J4oJ{32k{P7OaDJ{6@4rvK zrY0zVH5J`m_()XeGkdGTjQuKKo&3hxCzdhNAFAKNVB+sT34Z8EFiL38+nS%C@Mwt} zTZ_TDl%Zog5>g2T?}&+h%dd5Q-i8E8YqE42pP^|1hWtX;;uISOn8iDCv0owKB)RfL zI!`MJ%bW;H&wBu8o1n|Iv=#;u7BVic&`X1K#-4YigU{9oCG)CQe^nH-0ee2zFUyB2 z>;?=jn~oUeRHqzWHs+;2n_O#Pjy*0m8BzaHRX0v-dS$u8AFuD* zn+AU%%@jWx{}#A{ObrtUH*EAejo8-aHG?GF)XsN!F_h%udsr|)HPowYe@a@&XWfdltILPB zNxoARPa}<>N2{FFSO9!!#=Q@W1;Hy)W>Mbi@jAIX{+y3nCpD7k;PXLUQv&?GY)t4| zm>6c0izKO&YZj_mdP$1o(`{omVy>{OTqk2if;fSz&vUIVnOZFoSH^FNT=TM}gzZIU zE%-&OTf@}gqjrgrn|1HaQnPxp;Wzyw8@sk*)f5M-oHw9Lo7#tfR<{ntf<>;y;Y?YK z49lRZIef zx+a4qbHe5#m2J1XT7>0c8Pmn@Y^mmJHd7t7g;F@95dS=d*Ht>tRYQDYp& z#A^V`>lLaf>!l%p{m#kceW1PAvZYWZ-M*@dx3DZOqrjkaP-4jkhw)7Ix6FAv{zFW` zX$p_eYcgj6j*Q01@8?>HQ6>^LQux@ecvuN>AE0NGNoxA!kd0c}l&lz~pMZwc_^~%* z_V*Q_cxTBPo0G_-gpP9@vTXO(eCZLFwVpHgnb6~kcrz}xZP2BW-FdLHyV$Qb9KP3e z7-XuVK-y)!BRBKcbL-2Vx;uP)O;Y&N&Mmn$?S}ty*odd3UJl~}2xm#Thfg(Q29qlD zp(pJ+ja7nL4mr)<;w2Jm6_&YnKjVHL_u!kGf2y+pK?gnI7>50j;M(s+r=DhfF%Z`Yy@pLkW{q ztrB64*Xl_C+HsoHd9x*a7SE#TvCQ=$8-S@juYC@J) z(}U7tOKW@>M_FP1TexwC?WazPQ@k(P``k)jA_6kI@&Qtl;ti)QEK~`_2N|%1?A$G5 zFH!e4mO_G^bYV~zgxOc)@{%euDKzTmVbVkH{CHcvl4-zIK8JXz zMd>(wmQh(GBeaASwj8_UIsTzvKPB(Y?isk_{5Eh^`S$CHk&x8-+P(-*K5)GsRQ$!i zx1>D`tH{hiRt_^9|KPPWc~E7b4@%L2=C0$do$v`E%_DWbnFHMfO@)pz;~I8~Jkn<5pyx7&6M0^NIZBa6zSE{+vNbNF%~BJRnt-Wv%q&Cp0@@ zSuBo^ecb9#76<=T%hlu4qe1TwH6u;s4r~~xz$_di*plG}STMsz)~S|(U0??1G7n4@ z^G;iD>JtQ1r_OovKRY=FV6nzkz&MySiu|{A^nAn#Pnc6`0f!l4~8yUPA3P zz^5bXgSP_gd;9J-*#a$IShhaF2Ahvhj-HeeNOo;lqi!flIBimawE=n^M!KMOv&4;7 zfce7y?W47*JF}8(rFH^jQevUCc_reI&EzjJ9v&aunn$HSdGcB~z+-S)MO&f#d&Ud% zPd7b$Gaxbh;_N-9+EiXMa|}41^LU~sY_cN?nqM$?H~enWdDu_dL7iv)>v^QzP4~zw zSkZNIB;pRoI%^s2Ygzjsj9xq3Gu_v7RI<4=(pFXO0IaFL|<5X5A`BTlJSL*I9R(@o@)k4etV`~p1nmTMs>B}qy* zR(IvnYJ#FjzLjOk+}FFbI9_S@YF0jY^%qFC;vR`O(GicZzfiEZG;TE82JzEPZVgnj zW(h8@@-<-Mh4Nf&1@|6_9u|8)*UUZvn6|GEg;Y1g?PQ?=6!`MbrQhmT@u*Io0}jb! z5M<%tF=+5$hv9=MeH$X?S%bGm1+I>1zEJVYFH)zpq!k`HV8U_%t9~Q-U>aSrIFWmO zZWsTXHW>cX$f<|Q$<`i)D$?+;XDX81b3MFAle_9Gzjlv8a)#>VfzalBWx->Eb0(&1 z+fQ+(;u0-~7qvB0lEKg3`fnn1ttR|gRm@A_8%Ci1n~a5<7*38p>eUa&L!`sxoFgu7 z$1iQSNFJG1s-Jd$Ne~iZf34b7EG?xf-o?p6;iD2KBW(YD@>@+!1G@m@B^QQA0!`|X zE6AC-o<-Mj!!yh_G9`QNvD~z$zTqWXU^WCX%^xNc8Ue!ix{DRa=X&2=E+c-QGmV&xK@(8L|Z*tBz zI6ObD-r8tAN{F{@-XGx4Y_#+V55Or1sQ`+eUtR?gh6DZ@cqgZnJ~7EAOwTeX9gRTet3k6m_+DEO z&t1am{OD4gN1>Rzy~Hh28eAJTdS}d>9|uU%@qs2snwGh%{IXEV^G6{Lk9C!SFxIcxu%ohf zd!y)&0UF~;ae+HaEMEAF1bHdmHOfdrq1-Awd&0Usljb4a0-(fI6Zs8(N};}*l>T`q zO$9q&f|XKt)DTiM*1zKV(v%72-O+z!SZ4Bxmk4^67Q^3_Qg>oZq|Yh8{@=0x>titA z^qFR4@?ook^=q6(e?WZ|7CjR6w7+{TF!g;yyMUs8`4Yc7jidb-xF{)#>@BUWt& z2#34lZc^3!I$=+t!3?I2Gc{Y9vpEzm@xlVIsJ!Iq1OaeK;a^xalwyyMd5lQ-(joy- z;ieq(?(xIbhktJIW=__l|5Sc5ylA3dFkWP0Tl-|NZWvFhW`o&K^F}EW z-hl-JaU29dsJ!#Y97v^Y6?G-@QN;k<0fHOJx)8gMTcIXZHD4wD)0%51N#4~MjTqh5 zFhJraY)%n7^6pclKmO_{D!X|H(TWZf+C%RnX7m)Qx_^VLxa*0l`rHO*FeBm$?_CK{ zser)_0Qc=1?^E&u-cXo|ETjyX^3g8_LrbY&+{)9IW^$q+rW-~%88B*VF;1|Q4oTl! zFtQySsDv0-A(Pzc;hCAU+e6g-!QzRbWJ%+y)NtJus7gxSmaL#}eoO3CZSr)! zY|Tbc*G?$AJSO@PFma>?JS&Ist4H8gwI)nD!hR8g9FnHFOS42|3Wu=hdV*U2O#anD zkJM1u(mm|QBzN~OG@Ka5jv=j#>1pRMXG#{O=5B&2<8Qq5R$lqSu*SEQ0Z)k%LP+it(8<=vD12l~P#(0>cwCFjX#Aqt3`fbxtZkl&Z znp$lkg{4GdOe^-?{$*>sa@hg`>|=eBod$+f>d2?b?=JToKj5xKyo+@(ZZQ!>i8&rS zX^As1Zmw0sF%@j5)W5>!!*YRHJq4w^7`panXm|Ar3oTH5!_bWWX&AK@cOaJcgnC8Y z+{R1diO*>iZ&3KlL5-3K@ob$C?!oOn?+NPRyCn*rNq(LDq@>+_)Ph~4?4U%&?QRvA z6tecN=Bh9^ji)k$*8A@L6-J>>>(g+O!NEZ0U0AG+bof=2VX+s=K3u_MO(fV z9$E4*{UF&RkB6Ib&?;9yOt%!2XnFi|7NDRZfvhYy zgD+7fdpuymc?F45PXVAsURW66vy|L&*bEYY>sg3mc7gY=>kbQfY<{NE7ZXtN5(lE-7hi} z-+LN>TyQb%6V38sgBHHqHIpgbEI^baH4NK#jwSYKJ^QBTc;^nZIde`ESmInuUz#3( z5#aY856NK_ts!|E28w%t0nZrK@&fBT?tGdgaIAH?n_Mn_ElRZ**7UlQ~5D<0z2G`r{z7R*f zE>NBTKqw%DJDjPTlZqb{DFyFri+SazimiJ!1e{m0fLk1yz5(dNo2e~U8dVTP2pHy- zdM}jA09zE~woeH~*52o5Q`Ob#H~yx2H|T}UJkvUz`k-dwtc2FG$V2i`dqjHDgtL;V zQQmAUVhZ}>%d2_eJA3@74m5V})aKp=l!HyZk(h*k3#h*XbuYAwzpJqSE~G)CM*CN( zYx}!j&Hvdkn#dRQ4wqn*m{f7qonZs7QxaYaLT=n;5g(b~$#y5!$&bX)yMqvpK1xUA{82kYJABzwP-hJ^IUKtklrCzPV1 zJkPn&M!qMLfTH$?0p$91=9>r~bWF<(tgSkB6TltC(mk=-hyJj925DdWO&qBbK!;?+0fnEPZ<<71_N#%r$W#eRqyV=k zenI&;Wy?Ov8}!7k`=@LPcv5A)?x{f_kJ?MTo9gFrIHj3DV0!lER(A7BUb&5QfhS&a zS=$5zGpeLNetqWzz!{0q{bS)ti)7a-r<*nLE;}o@O_26l@e!HQXr`EA`kR;S$IaBx zFi0N@1pG5mEb~vGws)re@ehNvSgL8+Mv+-b0-oO}P0O5(FIV~gaUEgs2hef_5>wFR zN09p+H8cQ`8pucF->w3uu`*$p^WkT}XcFE3Otp3I-7xh~z4(F7*%O!XB2Xx}sYi+) z+EjHtHoaNsruW|lYbUnJkqF_UUdr=#MfPIkjqP2Ht?-|z|=?}}_V04kB*#BpSo#4#&p#Qi_ z1phO|78~v?xMB}fDNBsEm<@jz_G3b6W!=`@k3~?xTSvg-dE0K-aa}xA$J}gOQ8H-) z6$!GdR~+A71^7ttUU>K2_thQ(oRJ=&x>CiS`KQ*xwWR1yCpT-jWN#~)F^${+PC?3e z3Dk>94S95n&-9-|7kX<@tVtiXo|Of6td?1r7JOa0+uY&vGz*cHdazM?Ks97ex8lj_ zepU40pq<#Vblw6G8d>kXjA!8-Jp2oMR<=`m#3WI&EY=nAfr6(eYWex+aF)UBe_H5B zyIJB|i(;oc9jRL+MiLijJsJh1f1o5j|E*T7E=GN2QGp;1icqfV)0H!^21)i_#ECB* zf@5Y7jG&$DLq(0dev5Z%4QM18sHmINN#&<8XW=ro8g?v_EG?2yKxubDvSxxYb|PYl zcBdkEjRUHV@j7ZBP3kzO3@dV(?D@`d^Za!c8gRLiuyer-RiHjnz3r^reGJaHhp`E9G%u6`+l}nAX{7Z9fGd zQFQ(+Gx@-J2-MoRrkYeI9oZ-Etd6Q*Yarh$#++`Ej>Ks@h&IVk9?VQYHrucA%+ z80y>QfBq)*&U>k81393hhmjtnoV(qNX+n~DFxwnhH4sd%;VuG}t#`*lUGvE<6*yPQ zHlQddM{WAxez49DUFfk$jum0amrPYq+N=;B(&wtLWZcG-Z~y`gq|e3x7%1S!C_b~A zpnl}MR3uqK2j-wqH+T@H|2=ge!S)28UFu@MA`P|XS;ce%VLAroBpggdAd1=Tv)hF_?&)BXhZda|kFA0QWwAaj+(yBWYjm5AO}l1xeId7nL; zFlyQG0!=YPTwo;l%6F@s?wo6*;+>@9zY_2N?bp9^{)zejOVj_KU;h&I|LuJ4;#+ds Xg#p74CQqo8Zf`W9Hbhk8;ij*RVq%_hkAPu6@-BQxsA@T3W z`OZ4)oNvA}YyHg3nx(w&6ZgI2+IwHu{ql*NB>n};3n&x{|Dlwa0t$7`8-+rf#Kwfb z;p`s!1^>ga7kQ|N4L|PKhC%RW92+S$dlZUL5BU!*Qz*j}{*uQ*T-D*JwUL9fp4~H) zrJjS0g|&l)sXndKGdp`zYb)A&%<#`$S`!Bc8(tQc|Na28wVg3bKw=**3Pp>0DE2_n zC2nQ>xu)XosnEtCUTa+e9&1q>iaz-qz3;5Vdl}{|R+m=0T=S9X^3@TenDSA5EsYiB zGPhzKb*pDFj#QVP+D{a1-Sd8NWxAmtzleSN@T7ir0s9ss?~gADVS!uYWbquXbu& zA&hx6knY8;uWR-F``eqFBj)*ZOib+qX(dZ_KIrdx9T`PLM3SD|nw*;Q(Xg&2wV!Ky z;N(=6+iI>d;l7P8c=(G%Bl>~E!_YEjZ{ED{a3Vz&mB#Piv&N%%TsCx(KMW76nQ0X2 zpYQ4E+1TlQTt4cW*H`qcm57MQ`^}q+#~Y1=E(cSAmRfJxSASMtv7hf~E-~$$oSl8v z9~~2eQ*nHBN>%BwsF0msdpLK+V(fFFnwz1aVO0(bKmV6uw$TK+1pb#5_VcjD=c-+I zD;btQQzp7^;Uu^&-NwMc(ESm48=H_&(!)dGwZyfW<1HEYoo>m@oSYKJnAN#h>zew- z-i*y(Zy&3d>MYNAFW9lczVtdU||fVquP00`>*FS|CF6QH9dU}S=!CP>G?_5%ukXo+P&KKiT@m_lP z@Zqq|G1_sob%FD{gI{CNp+&w&| zV1o+{TBdgvd!tLMtDIIFUXt8oHSM|n=mQ7NXt}kuyQVAY!h-R~0zI^%f{!h25nHh9)a`K(kQS!6HPOlNg^%{@g ztgbsInTm|J-bv$KzJ~#KDx%y5vwOhsc51TT7g^~q)z!9O+~_FSGO+o8JgusR=H~Ng zXlO1Q^;q(XiiW#K^NC*6C?ZNqMvpMLxcg{Hvpn7{VKglVn={Bw(-d9~4XIwgaYHS{ zl$Ss`SKa^l_8i@Fv#p>K+-(FDc;|^VlEX*xdBV&c1WBzaF-`Jjk8> zF*6ezwpMJuBmU!)TU{0Q#*Y#OsnXKYq~zqjCOz;)q2Atn9JRN8xU3HD;JUwt4@+}! zbfiC0V*0JUy;Qv=_2JvDtfw~(jg0jBGUaWYom+ef1%rl*jmvrC^_Kdw7W%SCmX?<_ z#=RgkP{zf_7lJ*KotZpge8+@Yr&Z;o_v>54#?Fo(JaYj4l>m?5duk!iH=D`n-hQ{) z`t@+k>+Ix7mLhD{*VmWXpFDlqYZ0U4u}f-IF-!KjvQi{hdFJ%@df<1%B1)1gSAt=$ z^w)n@e?Hk8omyIYHK^l7g(`5}HG}VBQifn&@INJG+$ zLV-`9yqopwhYAe}?dRp%-m{@cL_|o*$~I%sxO+DS63%2KdcKL;hCpL5T=WdFFL

8vQc-{Uce=Y%B7)TrZ%)6B0sw(6Q)QS&3L`4(P$4 z3e5T$#~cT>RI6P%*J^&RDVv|{4(hNOwNZexVdd8xG5)gSA!E^gt=AZ!*Ahy#Fi~6E z4~E>5;K&^BV#6*rAFA*m3MD#95?1s0J69nQ;n(> zb~gkMCea<1`fyN_%gd1RosN@>s%+Ds^P++`8_)AP6G zw^_Tk51hEqo;`anI zcRznza{X`NboX60XpfJN)wOo^-})DlYm%W&zt?LC;YUnT z(%|&?usxk!QAtU!{R1bYGqdcc{L(Tqh@~{Ov@}jk)VBZT<>9#uOCoMzad)mG9v5}6 z)yl%=u<&GMxEQP2ZNH4tYf#Jiq25aQglA*2cyLm;N$Mb%$JtN!ZP`Z&{Fl%6z0Ly1 z#x#+{GE{C&LvZ!JZYj;y)>hKs^5$UeDe2P;IdNTGTGZs(N$nXH3kwUf+|p?muW(y6 z#qrp~^1kH$>4J`8bzalt-ygHT#mtPj1R9F*-KTy_t#T`>izFm~C47)ik&==gETn`H zk&_3aV-e8ay0uVL{w}i}qnlLqi%TJjc5i)RV(EQO&RsO&yV_q=&DDc*mO9IJ;&C1RuF)LO7veP4@oFDe` zWg(@Cx;i62KYx=q8oHRHW2sVNvp%s1c6V17t%#_oxRg}Tt=qRvZC?|K;L!+b!d3-; z_&}L``uX$y?rJyZtknh&qe4|N*x%+%`IMX}6(yw`@V#aTA!%ih&6H3?5KXPh+Le@) z+DDZ0G;gDVR!7TaEZ1v(=b>mJSAPg0#K9pZGI{WwNT?RnGRhyY=n3xdJ(b4Iz3>7NR+<_N- z@+4g9(WA%_qt1l!>CV;nYzAVwx=A~Z@ZjOCv(wf4KZ_~%TWGvE%xPG4y-~Cf=X!GH zqC)qvs9l6zTwPxs9l2&%9&Aj}BM56KD%3Y92%DIK;uiQ#TG`3(eTX;=Ms4qHhexFy zcwBJaTTCAp<7jJZ1I`MB%){{LJ#A@VU|?fwD=zqXZL*^~;G>||uOTmBT)@Rm^J)yh zNAJ2Xgky4i=PC-~lIiYIYekmf$<3{t+o5`;sS0DoWkn6Y? z4!b9%^E|^MxGJoyOqSqzv}nXF%Mo%L+y$cAD;yGrq~v6gk+S4u`qz}47bHWk*2l0L ztLN<2o}I9SUgddl`)PVoW~P*@<+Eou0tjhB!C*AB2F`g)(s+~%+d;HSI~(llODZUM z!i&BO07Kg@vD6Sk9uklpVlJETr|=nk#e5f+zZe?`Pvq>d45*$?rE4OGYFi2a3WD7q>2ayt(R0+dyC6WGLTS zxTjI4^Hd>^Nf+&}U%#Y!`IYmv@ga-2tyQ#`#l-VDOGGnkePHTsY)olBOq!=zmld-2 z#l#nR{+uV2if>@a(?k~5|K7cO8(pG=YVkK#M$1_+38}}X_vaP+!KWO4!MX3vypWLqpC{V`Jm!m%L}KQFI&} zmoE5=eDEbBBf|tBH`^LL)6PACfm#fi)^|29d)C&SB1z|0*UD1+8nQ)2Guz?eVb5&# zHfKAFD?~eWEl)MI%YY%%@--?kbTl|uWG#o^&$Sa9Luy>vdi;^N}m zJsCi3xnDW^-P2kgnX8X@77xFM97l*t&MXF{j*>~uhAOX4@2S`6;e4ztp~;)XL_!)G z8pCJL^cK24swmp`&L}bShd#S^xuLN!R)9+)OR>yl*cCv63w#4ffL}HmaBtnc+XR?w zBh2e~CV6>(V*`bX6L2e2n}8^z#}&H?Im@6Ujz__*w83ccFMNHcCMUZrG^M34XnP($gUS(-1F5oyirts;y%RLb zuzkzXGDyW}p`r6X(^&v{Mf;+K=T${{R%qvF9b0z{&laiPg&_46Za$&!Wn;5J%Cs*s zJ3Fgd<-}xWX4VDS&*V`AY5ivig3saW0Z_c$CBu~nqaI(un=bA7`Dckwk$Pl%uDuZuAgjd? ztBaePD0B48W(!U2!XEeT$;rA`3zhR&_w;+*iG}>Ys4`*{01Q^MzFT)S%J5jeY@7oa z#mB+Hp}Cc}xUpgL&9I0p@$pc}I6gkU@~Z6bIf$uZoe2UOnrr@H@)dn-x`nEfzkXeh zU#tz!$jIm)WLgbuXb?f_kD8jA_uSUhkXxW)djU^8JsYjFX4AqqjD;y&xmZe;1J)V- z=FN+##y}kocYi;>I{m^)0GrBQA;v~VWu_-Wrw7+yu{I9p6S?_f!((Dz!WPpEm70@V z)t)#lwn5FUo{hG8%yv!UwTbPc!8|QAl$@MgZm6kj@0Ngo0L$-ES!rqMg6G>tZSj2M z`CqnP^?6Q>2iK*sC?&95znxpm^I!@K!OUl;`?a|v!NI|4>yKqV?A3~V z{QT>b5Ffw#7kYxmXszsTMs{`*TU%RYkC>R4=u(ypx%gL5&Czpml0w92u`C~V>Mo8G zp6Nq?sy>vM-#R;M7i8IFKSV9g&N?=?7|FEe&^<)?pKX%rw-$rAOT619v}HRT%&lM|mSKj+&aO%`49b90*k`NoDg+S{A=7E)7EzJgrr z4v4MC{b7>r=}MX9DD&^z7-G@EUfNwshj63txVW*L>P7h*gQAIo9vkD%6H~*+iQ-V* zOs=hkHZ_SI@8)|oe^SccfW=1uAH{te+BPG027Y)9?exRs$38{l1%>j1C)F`v?fqM_+X}NW;?(OVE zQqMfq(Fud!LjA(>?Z*$Z&EM!8f3HzusVK8V{vc93xcrKc?8?^>N=;3jgv!XgH>MP+dNwBO7y7fwsXY($`~m`` zxdShelOu+O-sKYG0+q%=tIjFC0A@P01i&)t!NLGX(v1u6LRPOE9=;6dAN+G+tb!e> zBKY@)3@CUVZZXJyxDH>^D7QKooj4tsvvjnzt%pKz6I`$1`*$3Yn{pPt%M!0CeD){2 zXgUSL-o7OvV^a5nrVc$XFD2jwf933ED4YsS zdl^hh)e%r;nRmq`?(zPQJNde6V-@#ZcQPGTsDwPPA|Wl|`CN=q;)w0XdS9%@-d>WY z>5u9sPWQOqw;Q=b{mLYGF5_{c-~mKEHn%+sWMdiMNxSBB`Xddk>kJI~lXX7t1wATv zC@4zR$Ct*mlbsBLHNqC>26rgl51hvHI)3VJsry_}!Kv{Ru&$V*qGItLG!P@g!)ZOn zdkl0t5(Mr*bwLk>HwG$OvZ1fP|E2TK=aORL;yC^ybdcc1!NQxyD(uD8)M7aDDl667 z;&`5B_M1U-jsZ%h=B%d~%!d^3!L=Tvh+?^|tsL9l+>n842uem7OQa(url}cc^s1RSm1z$k{J_V`d96Gwcc0Rcf z7`*s?43Wsy`!>R_uW-x3?Vz2R6@R zmx#A-d#456Wo)M34P{{mEskdx@4!Nh`x>D&K4-SP<9Gr1!g zymQQHOkqB1RZf=0HJ?5)vf0l)A|@iDjg(HK;CCrF0H$=X8r4T6Na|>ml{nd;aG|LuHn+9O;YQw-6YU ze{#ItdEVP-6v42spe{sqb!>cG+|V%fJ&%1lVyzrTb>3*nV1`UqjSvdOAjj7ae)0!`UXlW%+GZ7K6K>B3JVLNsVI&VJXLFzGSJ;n52+Zl zykUkLLlh?5vE1}>2Nr^(p^Gj|kU2?X=jOJrH z`X%oVb0*p@zFqA`-O1t`TU!unl5=I~lAY>!pz}Y7w5X<=g9}Z%>0Ml0l5}c4%a|8m z9m6|HJ$~E*W(gRPg~QF8hj=^=YO0_G%FL`RpTIyY0F{Nh^)H;$>0H++h0oMO2KN=D)@d$Zh_|fZ?B}6IvuPQFnbFF*E`;VCHdJ9>@*ZxFSV~n6~y$$LyKnPQgZa>b%jT5ov56XcZta5ndbG z++q$69NJzdj*804BbEH$;p#axVdTFDqb6m=_JRptFchJ~jq4$}Zyqh@Ujw+$$;mlX zVSh7z^oQfEVMYyich4)xVb#ZSR<$|vm4;>;Go;H5;CKpB?z<7=w)Z|@bCY>hYYi~ZDDXTvT2_Z9 zhEnm9wYj+f1aN;^g_=YhdiBM7496?l^kz6`*RK~DAjL5XGSNfJzw2jnx~)R#{yH=B z7BngWHm6um)TkEf`}cWGU9Dr*s-zVVpc*Z+e9Qb4%B)Mm%l7>>XQysRpD`~l59uGT z)t-4m;{0mXhKyMN5<)y(_>$2XQ3P28X6VFr;xt+ zH|o%lZZ@>-&`_?`)X;Law0#b6GWJqIEI{)|BbZPbArk}Gs3J;sz=}Id{kUGon^;gF z;z2P7rT98|ZZ(toNWSdyJ<1S@dxqGN(l@~90-y(jhAOZe<-((2MZwcTJD~--{~wt4 z+hW*}#{bm%I-$Ra54<~qfK;?th_!@Zss{b3LIV`4R2?7nt+n+JRIwQHhE=)>6^5v+ zEdJL}Ds#t0TX0K=8w{>{D=*LEk|7CiVZ4eP!E{TNWOr4)lagq`e{>gn(o`!QZfm

dBwL_y6#{*A1Msc(GGxGjARxeI5E0Ie*|t!T=bo6sErbEAKqN%^x0f!Q z2N>Z8Z7_nj46-8b?)*rP7hDkm+T!Bk{yQUIzVIV43~;~8*7uv6Q-L(e06AP1J_f?T z@P)ZDJ8>!d12L+f5U$Rn5VRow&^z!OwjdM1-i)q27KdbAY8tPmrlz2ws2B;mEmd3yn2i|K6vd?BM*Ihe(Bkt? zNl8)q4Uhu^RcJHS5IivY#?KD}TG~DO6aK$KfHRPhO4=Y&@Ic$=zU?#y0X0813ZiEN;Ir8H^X4ot#O2%C-rj`C zlt#OrjFglx*k*8aDvigEe0Dw-KBeDGOPG#>fUI#DG+EWDdizwgwFyu#{Aq%Q9})_K zw%0x)#W@j7av-Inqdbo{B~|Z|J$lcI=I`&{Qwu=GVf`l+>_(w_7{GZ55-jT0ZnnJ&Qr!aOQ0rV_~Q zLo^)mSm@-Efa{KTO3Jn4lM`ZUYAZ))6QqrTGPwxd*4=%G2gSynU%|7xYQ4N1mIv?w z)ZGN2_X3L1GMRX2&!8xMzx%^Lw*VX{o9nMBnR@ASh<2%|zmk_tFF4QZOn-UuTJvZ9 ze{lh1=xnE(ULGuFBo;vbtf{$q3W5SGo=^_64>gY+h6pyiYjyK1^!n92s3<6~jg9AF z=e1wFc#+Ci91DO90}G4W9G!Nx1^TMcaG8b)i^=YmtEcBFXXbk-Z_#NZs1_$`y^3Qm z<%t;@UcblAuE$CN<<{%|Y?VLh@J{}h7tm@d6HSZG$aTJ=lX-gxAQdwCVG3oV3_*EA z)hTWH^5sjItEiTmk-``X*(jL5yrvP-rZgy2g*=C33bJK;*o32z{{G!pWmD26Wwq+& z*1Wx@g{mkNAb%lhmq{P8mL-Qur_Au6JWc=rV8MR0?CiyAZW0m!eG^UX?f3x8nwnmc z$g8LX!gxpztRF@)a@k?j6&pFd%}Xfnwzfx$aia?w7E9HU}a_1wY-bKpg&0v%AiIVXD0QVrLpPh=@E(0LI#H|H;Hz&Y#$sXhteII z>lAGIet+N*0)MW*3=3!oNmbra3o+qwXc^zOnffHS2%A~;f?meeD8Ek~cmuoz? zZ{r}S0-`Zkym>!2gjncqPC}PZ3>st%a&ad8LRMq<;Rgx*%J%1dzOtKP+e2`RA_P6FC>`tM)|NQYk^28&4gaCdj#fa%?}J9luw8KUNJ ze?Fg&2g*ww&?O*2g^30dOuc<9_&`LYLEJhMrT~P3XUF!(+lgml(5aoApC4Soqk-o4 zTD1(8M!=GYt!-hh%=Kh0UEr>~tgRJ78hS8>o&kT7 zfP@NTeki!j9S2s=PS?+X6k+$}?DY42x4paIPOMOjK>zFP^TL+S^I1|uV`HR_&dkiL zgOIC^=Pf*go&ZQ3$x3Yr0$0GeK0}z=?BH9kYi-RQFB`6}N6S{uZ2Zq2r`={I2?93FCB*#@&phFbVw(w8u`A_6)(pSL^s z*Z?6ojRq`#haPI}$=($Nn<69sgO%b=$lS!Fr2Y_sS_Wo`<*#fOd;i7Wng9PH_Yl6Q zxj+_to`;8LV`IZ)+c*v|c!VVm-4l6vv|Qyo9t{!IdZ;fK;P==B1Z9qwA*ef?ht%p1 z;6o!?kHm=z)fva*3u|ov(-2^dh=9=)xNPbJg`!+G-}tRcj`+U4J(bcQ3|714kuyN%;xfir>RL~g{hB^hKO#*xgl@33&Uu{~M%5?!ml`#h+ zpZQQcigGzVuyuzf_rlL=KA1Onw@|y`A?y~Q3XCtA!(>ffTia?k3_hNRPlwS)rnh{L z_j(Lb1M_|kfOmk`!N4Gpu|&QC6=R`sX9AfH!UfR-4{aZ5{qf^I7Ooeh47%t9G(tywm^9;ECTw9ErvJoa;wFzp1^Lo?J`!IIR^Q`__Bub!i)PnKzA%n^IThJ~4e zbGd?4p{m&Ts;}DtJ)4B@RK%%`ie_S0%f2-qp|?sf77;PAe5)L(%E|g89fBVF4gtHxKmT1JON$v@zv@#&Qm?LqBC2og%gZf^hCfWW=(MC{ zvgO`Ktf}t{gsLcH-+ni|m!uO!xWh*IdW{q%L`XhLCY`H~(BqP281r%(k2px)yk=Mt z(DvIFN}3ty(C%6;2?`sa6>jcwW=BF1eKOfQ<^HMY9sV57Io0fOfgMm=&H!IfmNL-fn#qi+}fiWW`!#0LUZ7_giJX>-XN7P)=%Gjp%ItIs8c4d@E1a9*W2$|U{k zn)m52PWjc2YP*a@v=1w~GVB~17BW%S7dV1&pEh%; z^b+-@?I-v34I_mvI;!KX4B&Kx;(0hc|Fb?;ME}!mJN*T@jj>)YED~1B1=kDkKA2k2 zMK0R%XFo;%cq;98lNa?SBFcJcee+Ady!9eHrTR=+ImQT?lu-1n<|ndGo@Zw65p~4Z2c3zHe-w16eK_ za8OV$5;~usNLU=1CYFG=!H;LmgL}THfA6~!XO}hpzrD;t^~UfO#5$6lqMoXs)be=n zNbzVfl=6Lh1}h_)->a*ri24%Q0amnf8Ci9CzHfK(BSa<=Z_1=ORp14!ev)`0=i4oh z_N3|#TulDjpNrj5d?FwGi-Tlo4tkqDsmWXBno^bpQ%9EP8j45*A+@HOP z{`k3w;JaxBD}{Z*z6eNN`X2(+XVyfeaBl~ z=?b#!He+N_iYChW3bUwOQGVFx^yhO-xAL=EzV7hDA`U$-A=;g@|HvY}j04-^XM)e0 z{SIDb;Rh>{M^8G3M=&h}BLhosGgt^|dh-^GF9l<7cxz`oa4*5s@)(E6(%o->xOkPa z(xA5v`<9**o1To3pxKoU{~A^u?gdBn>t9W}+B+KqlrEOCAn#tp$GK}{(cTtVZ_+R) zdYGBVE#slpCk>y3m&e}U_kP4std}M>h>5J`u>U`JS>F`m=V+ zuAHxk`xv%Dg4}t0r;W_VLrom)OiHd*-1b>~-{5yGpMwPyTyY0~(`ez^xj&yvy-VgZ zQ7og&I;X2;#(bf3K-xplssq`@BNKc~2c<@zk70i~WUTqPv#{LV>TF!;6OQ`ha&A$K#N9UkIS} z6E@O555XFOks+zFv`3yC0R`D&pjbSnXyeJ`bW@k411Iv)Z|R|PiT?HP(S5u9QVxyN zocvR+I{W>8K$B8mfGu<@{xC|sPy3)3Qp(`?%LeJ&JI{V8ORaVg*x{{B1vgRP2S^?@ zYSowDPB!-uQajt;8?{D;?f1jHyBB z?C`Nhw5XUvZ|j}>2XDIlQWZ%~zcELQ%-*9d%cjKP#g~vgFT9#pp!ZH}uYC=DlH@VJ z5A+sZQ?O0HK`Vd}U}@<*HLtq1wr51QnAlzlB*^ck43H)Y&cxXKOe~_mdwbFeB6y%u z1$E4?X`yk87_DIY4~=6Q3T*@>jdGmkmj7&ku}8kGl9(8G;PemP>lw2Rx^4A8XT%GF z>^;hz_Fiabh=XULF$M@GUkMq7FtjIiyAaTye)kOItg1a&ppK zi5{#aBj*$Kl^*-T|K#~%(exxDK{ABPF2KIIiHtLBZ0_%~$`=pg#XQ1FHztEml}xkyn3=xV4yhms)NKESV*OS_IMWyC2v9xmmqb7U;7O ze}S31ccYPpSYFbptPCxda8}Rq70xIjc~XZ(#1Z{Deh6Sh5WrM)oFO!csr;})PlC3& z>+aIC{<*n1Oc*}p#0Pz&R_y1T9DMCgq3~$vO-$HG00gVHL3}Xpmu!>{+uO*V^--^I zA>r!DU+Ei+8)8=2l526_pm?sIc80&8gs+VkA@Jv}C`*g2zC_#LhsIf{BMz9arls#? zJ_2%N8@~EJ&&=T?KYH;+>k1MyZ{^+;rIE)%0EZ#g66M_?#~h-7OC+8=?}J}`z|4A_ zZTH}X9D21Ju2wh%;co;;$aH*yZ-l#dmE{Fpv)`{B%}Av|8IRM13r&*T0W}&gs%fLy zcZi2cAPgoGG+7W2MJr)|hy5ziyreSl&?9~f(mer0SMdoV>i|J|zOQg=hquM57@nQX zK9Hdtvnp=!yRBWj<1rS;2Bx;tO^LVgDF9uRhS&$d#zOjazdFb}5r!OkTAy$#W;qr9 zF2p2XA%@t|Ei3XT#9UAN5N^mZ^~kieZ~miA)3;k**Oy8thH^>bs4gD`WjZ1$gPWBO zqsRV7T98&q<~3eb8n75U!+p zd$)p@M^`nK=At7d9{#>>FbO=Vmf)X{Hgz8tLq>xS+HjoZ^yS$^&V z%aNqJf6AJep8g82yy|WLn%83&vc`uWgw_kVM^L2HZ%BE4zH_krA!vlqD}{(y%NPm+ z*Js)D~hFoz({((5to8CvN<{WxwFZHpzhnU}@ zort(6g>~yXL$3z9NhMORyOUxQ10l*nU4}d-%5159=)*9@5nNf)v080uIawJ1y9iCngU_{C zhWBD1r_oos!bc6u&91Z!*?uMhILbxcCi-l=qKo|3>IlV=1ErX$0Ag&Si0GzD;h$2u zSf3swN%_D*-7?+IO?n6oQgWT*YL3-?^Xpl{?hXbERX2uk?Rdc09Npp0eRMHQ=YAKx zLhK_hDi{K9R{qnex9^cAu%kL zU1I-&H!KmFR=F8&iJ7kz$@41r@0`AbVRqv-`Z?ELCq4 zcZUFVE69ph1e@YIYQHnj3KHl0nE zB7tuq+fMx5yf;TbfNp#y_VRPEnvl@1vgAaLXvJS; zz1u_hk{3T9;CFrV%a36}nYpm_3jy}vVwa9WewpO4oih0_s6k>m*U?1+3u4(2R*~*&fcJul| z3B_gJ9W5WNn{OL5ZXX&}@B^UbU&BMuX1{sr+C!QKVoV5lqNBD&5Zh)Dp0x z`LF8qeW#2E+k;PEyE1xdyIT7JxDt0CBFk9xZm`e5z?J%qg6xoOD(dfmb5M{vq9?Po z`zz%^$3RcR2yLm2EQ^J{4&5~#bF-V$Vb`%7vX)pM8Dg8Cpggtse#;xeJHY3^El-1j zn4`6@>dJq*;vx(Wp!`gNt^-EvU!Y3{eyH@j5!FSmtfIev-WIWJ(PZ>&Jhd>lVjW~3Zd1I76 z5ke>m;4~l5g(*Kdz84B4eeiGIQ%A9A20!7Yn0o+ru3whD|5Tl6_xY`>Z1;Z9yo5;M zw2Waa2?&DT%y1BGt{JxqG=oA@gH{%5MSF=F{g353`__ z;gRCAUpssgYIhmaaorCip6r_2k?8D_VIw0Hd@0a@DvF5fG0TzR)XVMmi+q}3^pD`EeUhb7L03&`px&L`nLPQj(X$(84q0BbE9M zL|vKjvKUr))v>pqXR$Cy^&|Na!*!Ex2HIX45HVCfeo3dk(z_iVY1R;W?R+M8BCf~( zhU8*mNRx`W_pkCD3tq-D=6oi*MrutEig3Y^5^mhpKRAwrHsaDyxHbN#<+cOPYc!Be zJlnrXS~fy%o?FI*o&Z#Hm50xV06x(tmqC6u6PK;Ntig_p)Vte%(Jw(fk=EptF0*v@QktOGb1jL0Yn648o*Ol>6Kxxv@(i7v4@~*rN2Sf^h;e7s4P)H*QfW@&WTF@<=M%~OWU^g zUm=xrNt&}JWm6L1dBDQogpW27i%iyKDFED1jW!dpdLGjEH(fHALRdh`=f$AIQ0R<= z_)iD!@gHh<;s_4|RBCJ#(K#&HS%n2YHO(8l6+C>pVvoJRIPliGaV|(=A@M-W0p2M! zR?f)kJS-*R@7k)jORN4JTNBI6fTB1Z5cS-NLfXdo8!J_4kwt(V zjYM8G{JRRi@jXlo>^9=xTHOMC9UeL2lFqXFsG@J}Rs=A2ch5@FT5+oA9BcK+p*-+ z^sdAV?jt9jM5~tbJutzqJgOW;{D;1h2%T0dYKfN}d@S%^3Exe70e9Qc7m7ap*O=qS z{LmvrnwKC$*HZYuJ?*b{G+fpU7hXvIzf*R)=;3{hQQF<}&#SnQ9y>$_Bo`#d!bKl0 zwUe#zt7Ahqy!L9FxOhACdX*|5{rEtg%q-OmI8@Am!~j)d|5qXV$Ah4dRqp@yLKY@0 z7%szNW*RXK4g^<^{;aQ zkm-U)2_|~wr>Q#RG5S8Tp`3I@u&>@%x&V*+f5B{6u*Z1bR8W3q8UH|F{(r7$A=MM3 z>tfOR)jfDxap~wk_RPRVLh7Iw1XP(<&J!NwkTru|zN|-BTu3*kGKcGXTP}jy0MWPL z-br&(xrv1ryd#<{=lgNW)5aDF>-5R?E4(Pr{c&d(AY=n;MFinU;ga1&IM|>#KqxdC z(Sym*VuPB!9(oXepB-~=z@>u2ap?SVgA3ll@E74B9)S8Cw29@;>)mYQZfA89Z%40x z6Q{JJlYRJ7;iXw@+-F6Fm)HC17ezBomOf@Ex-H#mc2!O@JFMvYm?`OcL~Al`t(K8z z8LAkTUP8?D$~XNJ_H5*%7vV9T>tPOD2J~!g9VV>h%k0 z`T4H`zlRqTV^5ZU%gD$e19VU*G* z4Me(63UxW|LHClFxIaDb)YCl1ilh-l*z&5XK`{7cY=sGNcx0p>P&vr=a(-l;Y1Yr7 zE3tcEJ&tB<6&S-?7-7V&PHqez(L+>0JBsq=iuROO@*LwKXH*jBkM(nhv)VM*L}$e4-f>WgurA zCBbNv1yQbpj4k_=C(Qq-p&fPnaaMGJlw>N|yuU=>qFFAJk6fY%6&iOs&Jj}bBLdhMAY zNXHPltBP|7d`gP5lE8a$aq$3=C?krs$jBt%G$7R6@t1V)a$@sP5Tip8WkRo?mI%6`2zLav!K2b^VF+1LZ>VDE-rDQI_p(3V3IQ=UQ5+IiVWwwUF?^5%@ zWZ76QTx?02>YtMklxH?k$bfj;P zZOQ`A+ff%$GJ=VTx>>82*y8#m6+cP8eSod9#g|A8;29-IjP5>F9OvUZnS>s;k` zd1A^Ad=WVRf)IFLpqC+}4;{PQ*Ittt6t~;9q7YBeb@l*0X#%+PTRq60RL7_*WwziBHY~(7O z2cR-yJycMijtLr0KX5ys*IO6=JmO*iH$VjFzzrQ!`aM0Zk1$t(tTj~YB?R)d5YT^( z`q7wMN`n02)vF5}CS6GbY;$Iz1A4IF2q_i_jZYx{H2tM=0jebzERVS{JM}3jZnC;P zGVDsol!2U5Iy?ZOfiZu)bw4zlM?DZIV&3}p=HgP82%${l z=Z)9h$bDM#^-83o$V}D;JfgJZHF77VaF|NIL2+3%}E<^`rupBUkZtAQ(wWON%^s z@St@4_N`k5u}VO>mLCCDA+QOX(nEmW1dF-$Sx%)Yns;pN$Gbb9ZD%k+-eDOvKR=%W z{sX6gGyoIBc5_-hV{d$Tt&|Mj6GEl1i_85Z(40Ydyq(|A!SI30@)fiV^ucxe^Y-CF zaH@y6nHeLs>jK?Pi>dL7SYAXs42J$=8H}$9nB<5O1s#RRbb$Pr4Cg;^E!RY|u>qM5 zs9{);jp~5Hw^@%xL(9fS3}@qr2NF z6&$}S<1r1eH>a2Sv%h`&X0x!LD+siC)|f|56U1=Fgs7@2g?gFAOAzzXEh22tF1ybwIW)1kniOBt77{BAmbQs3^?h;g!BqzTWCS zdPc?z!Q?CwR#wb{rw0a*29Sd*YLB;Yy+5>wfzQJc7Qv)>86Z2{x- zjF(15B~POuQ#iz&)nBc@qoifq_B~A-g3@`>6w{Lgy;r#w=j@<1tDGm&v+6vuQ|Kbz`i`_O+593j+I&1 zt{Tuefz`j+B6KRR1_vszf>aA!i%cSg7=i``kKJ`0Z zu!86Z6wRw&{c3ByLJUO2Hh_gZ{V|YQ1k~Oj;IY&W12!nob2mXLal1GZWU=0fi8`e% zDsXlM4T$HSmJy;rZTK4G3`yV;&=nMgeYQQ=(2L!F275(Ii$WpSYyh$;!nBXa^@t9o z5h4K6RG84|&{{Dvn?!UUA(WgVpaFO{Jb=*QA(opA70^*X|3wT`&k?I?gbR4O6mg#K09lOzY#1cSda9PDh-;EIKof-Hq6h)u^_LZ579t#RU zl)!;njdSg>-9QNcT%qJqJ@@NZlJojcgjNd@%?+WmZ6SnyjF4li#^t!J#-5mMmDir& z14lBuOsL;O5~XzW!r{TeSU*J-1aCNMtSea@w=Ir`1Y~@Ol|yb00QNOKGcy6ZQJXNt zmhgxOz1~3(Qz2|y<_Q5G-}KVR&fD9LksMPnZWJaV0-Yy z13S)s#h{^J99?|99)BbzhU_}B2*9A`EbH%uuF{Ao(JlaNWjN7;bGBmGBFd=!L%++# zZ-_vl_^zPf&$a8}5Kkay*SJ?`$6-uMQHhBGMVHRg(^KKe6C0I8+oitSY!rBJVq@JV zPLv>CPAkKfT_h#Ffe4AkQ}@{9)zof7=lvoLjdgJX1ROM4IH*e{N{x^~r`oktb`;jR z&+2gXrVkuABZK+|qTk48gZVnqh-w4CUgTR&V8+s+0QolM5pc$16oDibjTX`KrF3V; zmFSPxfL)F5JO?JFJ}U)A9qcC93Wj%EqEPL~U@j$x2`#ATq=iI9M6L@6jHkCla~Q)L z5$Dh=>{>nE))!?!maZp+2jCs`a+14P2&w1^{sjy?rlF2GMGV(eY z&eMd6bp(JZU@h+tLH0GX)Bxng&WalFQOZ_+*=-UT7kBj##4*2sQe6y3$w9V-q6>Ce z4C>XSl8I`!a^%LK?f3;}FTHv9?go$@xX_;iB)AA-Ou#@}AmNb#jcTfRFqs{oW8R6H zs#y~%aBOBUlp?EfULLo-oGnPouwF8tqEQ8a5)RPw5E4)t)CE1ClBHDz>?>nsC&1n12Q~RIE1C){$kMB{5X2V@D_shQyg!A z^ignz%dEvbDCppPDbdKzLl9l1K>=|OQaX?OL4gAtY=`;=v!Y0@0X8l;lm(3z!n?F( zE*b=j!?1QP02DK@3%dL3gPHQzF7rBMZhe8%Ft*0*JBKZMVG&nPvFeB!-adqbT2eDJ z7xI=iK-(Au$JX3cEvTF0n+D0YbSyA=pMnY}?ek||=r z#vNCOOHAKh<#T!j$3D@*!BqjEX#Ng5!Mu{JRX54ttGT{PAjyF2s8rQ)#LEyDf>sa; z{-c9P$2YgO-oP<+hp;j42EnrsEe6->V2;`~h%sbP*_#gL#asORGCb!9C?Dv=Wq`Ya z!7@P})(VH9EbBjA3%=$EP&9L|hYV=`^z?SKKS&q5Qvx|md&FRF1}z2=HvQq=}#IWC-21 zv?lb1I1eb?q&#<^v!kPfEaFoP(tl82hI1NkK!j9C&_Jg}4Hp^&wi~-cmrDA+MNjHW z@D6O$BGf=31`QFH>yW7P{1l|@BW*$I#+m>^vrFKvW+A0`px2FvEha4&$}s>H%Trhi zquxZoB+>wPn?NrO5T?1ky*;lJId!S4>j@eyU>J~(31v@bJ4>o*jKqiz4MyDJZ? zasArw)?ld6a7dJ7rf8r@B_u;aDH=3MMI|YXG?+^?97CK2Lq&s%iYC)ZlTf6A22nz7 z4KgGpzk9vAbAIQ1e|>*_e|*<h0oN{W|Ug|O->HLh8Q#hvHB{nnX0VWupv7tw@MBV5+)RU;sh{oyIH)VaW^jAYHGEGZMtEczguRC=fHN2EpN4@fj{>0WX187hb2>jTX zUwcI#idEAWEHKhSJnlHT$*(u2vStG3`V6l+fdLGCbG_D#8M^RKI`NM7_mRTet0t<~ zSVW`Ap+~xY(o+A1c_xU1cfPuvtMO~#Gf0kzpE-I%DFowYT8O7o(vl3_$v}kWml0S~cNysWB;A>ykADOv>+RCOM0(wcF{{n6 z(?bf-RBK@!RC?1JExMs5>0i}=P<@0tQ=QW<5g?+74DO@JvcnQlfbPl2gKaM}XZrWG zj)w;+f2RrKIdw>HIm6xTGMRdhW1g;J-1X;Lz6^`QcgIaA-*wbo{sgtvzg8HV~rB@%|(S z)lEwKBqaDRw)e~eI7D(ll*A(XZ#1?b^bm&AjLzx%msT93zI6)8k!?ePPKlCWq)>Gh zQxyR(Dwt(a28&oqB%hH)_xc#E?+o&mm00~suCKNE1q9w84SsRkM+GTTH)6adU3T!n1+jtc|J<<47ds?bQeQv#_!8PF)KpHFBE|;v4DYkf2g{z<78ra`w1j5A;;Z zkkvTdUe$xhL?H0Vgo5lxj~<=E1AtzQI2xmUAb?{B%_unU#H^$ZCCu?4bQ+f7)6E9lHZg0n%uA?m5<=Q7I_rLq>;=&K#3_{+ff_dxJux*NFtsqET8-P9KHl z%fr_KVxZnmBDZ>aYzYcl5K;nG)18@eCJ>y2e}mKZ%&#v8Fg~|Q{hjpKKg43-< zATHwVv`#_5QjLdT$(NwIn$~$#PZ1!km6a7^%!fq0qUa8=)XVt>W@g8M=CyX^$SC1` zu8S3_81$2ueRV62?QU}~Q6rc(lz7ZQZHM%f%ey7DMZ`rXEwMV9Qu`#Wa}++J$KUTW zwXmtay3zZ!BIEdCmDOO-uk6;Kl>B8tp_SuKnc3_$blT$8a^L}zXj4)FN#y-^G+)2( zXls52bfa3+YVhYK0ltez&np-&T{T_nxM-uCMg0WKfv3Pm}qQJ|&w_&Oj*^3+U;R@^oL`dPfC%jk~dLafW zna)RY(~j2osJroC3;QFBe`5zRjtR_P!h+n3$Iu^AdL}#2qb*I^E6o5@3i|M22Z{rK zqO}sDoKSp7Ny$&R-YsuU_k19Y&l*Xy62$E!#*HK0G`9ap%kRf)NG_#yN-?iS9)_7z zy?N6!)+-mcgUG3viTE{*!WPI`9KCbtu>w$+vg?r77dp08OnTeO5Xe|2!i0jB5Y zIWaDa1;g_L=@A6u5ma1Q8U{T>QS`eV0sMG5F75`FD3oQ{kH!joEN0EeO%7x&qz<`x zLJKo2hd(6-AV$Bs*MZ|SVQ6IZ_(KxfXY%S34%_fjrGl+d{01XA9W={<<33&y6TMiR z@1m}$ORk0*&lJwXoXYm=4dYoHeQ{O@Q8q;*)af02!EkIMLza2E?<_2Ja4s$bOEueZ z`Uu+Gu?2_;YM)GgyGHh}e`n09W%z~R%3wz`>58ueSW78j5)Ez((c?}!!r2%ne>V97 z??Vo@OjTl8lelaJLuD0uIg1;k-OxCJHP>9a6X!+&-fD6KPxYkX`eARB=*QvJ0;Li@ z4tvKl{0THV2gYX{JJyMKWjrT8(4qmnUpe2`ik3T0dyKz6T%1=$_x5%T7y# z3Rd3fBfPP$rQegk&AyRz2zQDwZGF7Nsr2zTw&QR0r0hS%CB&lS#sV` zmAZkr?JZDuSbbXO?+1HXOTPI*oe71YCxi&KffXYeFfcS6#&q}g9z!Op*I&Hk01d!w zub=R1II}(6rTR?_BbrHP1oZXT0p37|x{(ioe$^H&9%Ndma>jB_H#xk_Ds@i!@sPdU zx6b%mm2zgn98S+Pdc1C`c&5!mvj}UmgC*LshWQePa-6pp-=t;cCaujY49+AA&UBj~ zIo^vbUuMyX@lqZtOwR?p2#OAATmDbWG0hPIEhfmQ!26Ox*&105_Ej{Q7w?TKS~qH5 z92IsUy2DV7rn9?x(&7KD3Ci;>YD8B$b9*%Fi0I4LA`g6GJ1XVaJ09B6*>x)Z_d3Of zTUaDbzIyoRmLu%)k|wvt&P#?2--kElps3>U718?s@An>$d&Lf=>d$}w-JAV3yJ&vK z{^E@>Iuv&^M`0WTLK;_+Gp!VjHqqD~443a=05&$btguMOigBPjA%F ztzYIcCTL_qrHJzPYKt4TZ+LFE@Ike-YXSmE?9I7(6RR`InLMugWwK)_$@vS)oNozg z50biagnOH>;8O!gPoY;}Pyy|DW!Cd!*Yqc|?x`Xi^LWZLd`-hKwkzgw$PqA~**o{PVLOD$hM zOeg-^QIE&_^)58<;*f{lVnKqY@@l`Lc$-@$?B{8!N?-$T$rUq+%d+tdhg=QzR*}4g zjIS$i`+^Z3a<*r9+wenJdyp503g8QIk+EIFSf`2&Ue<}(+j4I-?U8|^D-NBuxX;`2 z$oFha7}`~R@%2((OYj`}&R_lsmkM_i8xh933;ITQk>S@w z-iqHkJiL$PN6!xGyOg3129K~a8m8uTYnYfbOrLh?MoG z=AwBWwNJQAUe-k1(U-u=U4=RRKh+_{42D6xygN>iKyEo|)Tpg5RCwE%hfhu(Uz*#$ zYkkE)@{|2GtXwGo8Eux9lKe$KSC|)fWV(9Ujiq{H1+RnSPbP7qyAWU5K`DE`;36gc)7J(z4f$gk+eHg zU|P>KpLPgejEo!~yT4}*YKgNSK~NU*>fkC^b?ig33MkuI1|!q?deX&#;aKB3=;bfm zv_bO-&^3~#m^FWX34&LP#)Y^GxoY?mO4#99i*V!ul!*^QEXx{G<~j2yJc+30YGJv1 z7u?p4o~xih0UBy#j~oA_AK(YuWxrszziDQsS>z#F5$`hx8*4sKlH_~s2OO8VSl4Qe zJz=%xj8h}wI4SvY9pCMD-9&zo`#U;k!^XA8Z(nNUxy2ssr(LdI<<$In{NH31RJnEd z@oh+}xVk@oFJQ2U&wDcXOmLp=#!C-HMkN&xC5Ct{H^zYyGw0d^{hQUx(^UdC|2T1n zra_(hAYghbon1uiPwV72uE2~#HKn`zX>Y7=&85G2E$pShu5m0mMZp4cKQ>Cm|Jhh` z_0VB9|9HtQJt!d5T{<-k=+#7MSiS(zbFu??Q3wEc`WqtN5|mq;O7fxAj{!80>NA6xqK z5nyb4-nneDc=gZ0iNbrKh=Qh4Dn+91;R=pOUY-0g@IWAlO+XDQ z6*|k7age_dRuiq|S9iq%89vl7Q#7D+Z#q88-D*;*z(x<_e)H_l08doYxi;%SG^n=MosxHIM8~p}cv!r);jA97+!e z(R4^QwDl_rt5kXcp(pveK>3>;(k?(NW}|SJfir;YOrc>9Vnj)spK(;&O_Pl6!AQ=U z_W1cqlje;mAcom<=WaS%U8`gxpsniCU}76Z#U`TRWXfx5T2P_seXqIsmi#DnqpPR~ zC`046)o1hOvzVqunNA*^E{-Gs%8^BFxL&&c!=82Es!$DT07n;@dw!R}jv(ZTbXcso314MQL4+JQo z3skK#wx-)|gOCf+Vsvy4KXKDM8N9@XF?tCY)><`6bkZc}Obs-ebiNQy0T3uvZyunY z!K25x0GY#r%j_GbhXYoC;)X{?9)nckYdVj_%uMRBedebAu64bJA*c%D{0T`^!M;Ok z#|BIboNPmyyv&!yGeC|D0eZcS4Db;b=28`NcPiMGd%m_CUmdtxy$a|xb^ZVwz9lN$Qk4BbrlyG90F--Ny!|iT18)D{TRG@bdI#!cz6^8kODOqF5G0~r;w>A+XMIE~R zp-&&3OBCsfSpKyD6M6%xTUh7y(F2OWAY1RDY8nPeL`PRw4rDV((s-@hVk`$y(OvA1 z*i`SMc1$YgTlO0N2G$PfM24|mpa?*i?mq62^pnPad+b5#m(XN`(od~@u4S>#u0?!_ z8PElSOk4{%7pjjcVwM~W4-a>25*HIQwB5(uu&SobvDa{T3hH+@s5%0C91bkO$+;en zpO94mi@l8wHybu6Tth7gJxLp8)hA^uL6j#HXn$jMq=%oRcxc{pufu@FLH~+YE^a@C zcWm%nNF}o|?Nhw~KmSFj){Uw1&N+qQ+AaC2Izxvaw4GuA&M5yF+nU3C%$1nr$2ji+ zG=gbx&8O`kf&@h?RTcH@<6&XhJI62={Jx#9yoXvsB=v+Q$DCTThB9Z>7Bps+dA9Fd#~4ShNlr`zF-14^!luc^ zhvOXkLtTp{Q|4ob3gd*D+kY=$)A}0HPUp6@KeyN_eb|4s!Ky5JA?FENNuI-0$grG=B1$C0t zMa`|bt!*}>x;EC~v?q&U!-h>$Q``AE)V0uNUX4Ryc z&qSXFYpx?s_`}bfH#TuaKT@3l7p6XsTi79?6`?2?O}Y5J?XMP=?xA z!cVYdW{F5G2q#h=XpMI856zvO^AG@m&AHiJ!7Xbvj!>ie9XWgFyHlyqljVbxh^8-H zYIQs$#15s2-%&;fk;M(-Ug}L+s}`AsK(VM4Zul+i&<#+%4=RZeqSL1#C4i}V`2V61 zI+pPa!{IlgqPcFZ#{p%m_+!CNIn?C=WlcemE#u0)$1bg_6+q}r%@P?s`YiCnGB|PP znybf@rpURyEfwX+q7Z-Ge2bs7c#WQS!{Bw7eegOcAU^5T1TAVTm1SRMB;c^ULYF%W zt2d*>y8#Aqiy_&nhgv*#sZvst z$p5PK!FG9kyO_!UC@um*KQnUz!SSnCc(a*o1da^EDEx2IRC92tf literal 0 HcmV?d00001 diff --git a/src/Algorithms/Libraries/mkt_plugins/doc/std_pv.png b/src/Algorithms/Libraries/mkt_plugins/doc/std_pv.png new file mode 100644 index 0000000000000000000000000000000000000000..0307645bba8310c8f389c0a0cffda0de8c91e84c GIT binary patch literal 31906 zcmb5Wby!wg_b$BXZb@kblomufL&zH@!ob^bZ*{l0r|So# z9~b^awEOQr_y_hAg)>Mh*o4*1-1*642YXgw0r-oL)yl=iQBqLw|6Cy8;A|-vlGaOvAgstO z#cS#wZ!GKq`-jr4P&$nGkcnKR=9bt8Yzx=#^zW@LBY?!Ohms+O6R z-HpX(!w!R=Z&o)+^brd9=@Z12XNMn-Cgi4~qN14@9TE-~L=t1M!f#_^jgbrRTVY{h zfB3DUQV7Ni^yA-z5K8zpJw2{`25x6Z2hmh(RDNRr^T@~`MfPiYdU`e02KSamYtB=K zV=G59UUs=;CV#|8-Y0-7FMQjwE48KPz8uN@`}Z%#-~L)rk$yi-R;$c5Nu}<->^`>K z>Cu;fJX?>+$w}7Oo7P)>FBJaOyp?!@%i^;|K}khLqJO(IBO_yNtLJ{x&z~gD6^wuW z{J|$7X@P5Az<}pAtovvUp9X9%_MfjL&fvP==#3FqEb(+t$ZLBcc;;ua>C!;4qm$D$ z1qIA#Mu}%O{huZl`tpZrX=rFX4(7ArsU8=3)l^rLGcy|qiYh88nH=q{hebsNCnhG2 zZajO2X**nYFMD~jOHSFugq=pv;A79pzqtF2fBzoE+WfY}uSRvhCk=^s(_K4l_ z!M4XlFfBO~QQ%{(-@kto zt3NuvC?F7`ClIQB;D~qn^XJd?qs^|xlHu3c-u!nvNSPMD>-FYkI4 zJ2*I4NJPZ@y({{R6OF+hC;PL-7C#wYzn(j3H>ma~9(}kNqKF?bIcbuko~LY}X?n~^ zE~+k#b6070sMN-4@N<^I9u~sQ&Aq$}FB%62N2|)MV5h`lTB~|*&!hTaAs<$c|Lv@2 zH8nD+o-_0{J*n}g3nvGQg4O#o$&M~AK@}AesZTdU=BV=d|y;%Eym53&6sI_*k2Yk(;bLJr?}?_pgzK zMabX3dPx2L=4^MByF-pT&)2FdQ@H6bt1Dz*?&OzR_wn4kK*=cklrE0z-m}0!ESO#l z^=h&>?)xEWX)NkI)QpUbLe{+(FI>1V_c61w*wF8IoAzL99*dWk_jPh|ru*usxYt&W z0g0IX-}EG#T{lZ0`sOBwPt3L1$7s}Teuvahx)zK}Dh{&&_L3OYLc zUy(6)p)zZXn&;2oaa!E4$9BBi9albbz(JK zW>mc>8IJ)yS1;r&KuH~v{Jj&PD+ZlG)PW4+J5?vbn zqI_tuwK~QkznvCHa{lT&{VKNyJn`^qn_+fdynMOwYxuSqp9l7f$mr~B?u^V#r_hjEvdh>_K+Rl!WwKY%e!2)kka4^I@k)x|?-~8!%8_RWp zL{oU%7cX53&=XL;eVY(X85;NeydT!3D#HOEA{>I+o-wF{zLPDX(zb{~8V~@8+ogXN9Ox51rz6NV}Dqho|p*@~= zy<3^(@z_}V3rW*f;vZ>pEXRktZLcn=Il}z#8P+m#tGumm2*hKH`(7fN_2Glb+IRz+ ze}pW4(4nc;dv^uBsqg8rJLGe#!_lu!SfQj$jYs>N`6I0`ST+FxQne3vJ>Tt4Cp*+O z+!3q^g4>y%9PNMW>S}vV&HJlBR}5AlgT1}|P%TXETu(MxV`F2HY~B>Dj{KETU1Pp*0e3U&qk1suh%9bI+_M)b|3S}w$uG$5rwcq{R>jc>@KTYZ#}QT z6&J2sZco4#bo0&~D(4D4VumX>v&XzwYq*iQ-UoVme7)ACoi=T)t%Sh}Y!+Rq1$Gh7 zpHm`=5j6Z;3n^Fa-XN^e%rbd)ci`i&U@`rnu=EYrE}-}E@bc2Au$^v?$Ml~YsVX=4 z`DnSR{f(I2(0jY#Qkz7C6_!shxx%Z&L?Wo2mtE!_q|DAtPZPMiyI(f@Ms7D8M!^vj z9v+U)ugaLz?s}!`qRSt{OP4P{{AMPMUXpSGD?bTgy=pt~&W?zHfC7Pv|GrZaVk{^w zE^gFvF7Cc8g=(()1um|I<-ptL#q7@^&3TXVx2)kC`}cVS1!)!VV^dQ2`S|#ndvn!X zQ{!H|c-JrM{O=})JfBvPyyZwkLqi5k(NI~9_bwq~0kkc8ZkypE3za$;pb@)co!pxg3bJ#VD!jFCTvYRyDJ*Kr+9n_|a>aAQ z?5tu#N@@d`RBD$j<`9yZ$>FuLl>4Rg*RKcPrk!hCadB|i`1tOwx~@E}a~P=%j*KKm z^W|hq#874<8M~w;!{Fc`ntlH{SCsketxZ5;{p0Ot{kcG~H7MKv`$gA7f zQc07IvKuZlUY~41G1hvQ+$pQL_+Dz4AP-MtwdZCLz>W)?oY($3UG`B?i8eR|oVF-k zR$7XKm|0q4`Cr99*x#Sudg9{JyHoJK+p5TAUiInzbRr7NbPE0md?_#Q_qKCzaHuH* zsI)QGLEt&zETR*!{^P5!Z- z#wQUO8K&sy=trM6SBSFnyb3YK#~T9o<}za(x8@$E2ob9Xjo{VPXIAbiv&XWsvLdsc zDLs!1zn4+cQvrmw2GE-#>8g;ek}|%!8nZstW(sL{Z>@=Jb-y}SE!*Vh8`0NS?HClU zUpKb1BMDc02IcgTL{4>{5hBmM)!U0s2bt_1f+l9!lReXc!NF@h@g_qhj~~^#=|?Sk zlRxScf+(^5`<23>dL36mK>>!312@V}^;$^9+T=~-*RL!YdgnV!k$#{$;%h%vE2C3p z8xGNA+8joqRp%=Mu^U3h;@h>2gNuvlZ}1|vV|r*k$X!Jn`>4tR3N}7WS&i#xjiL2M zMX%Krzc3hvlu{Ga=XE#*R@RIY+_{s`tg~vkT3H=V{xg(jRXwv*nD#{AJex)n7AMwiR-`U#2 ze&AG+bt8gi2>#urhU1*v@fHO_(syqHkNEu6yWT;Ck|?AFL{w%w_zZf1KXdrn)3tzhu63o# z*yxQN|M7%C*jgIgOb^@^tUG%4`Li&zE!Vk)R)$LN6gu3S-K*L>t~d;{yn8p&O@iW4 zlqvFkPVvT#NBearkfAVt$z++?+Z&kY#Cz1f*4FKxNZ?PQWZcDGr_#r;W{7#RrtDidN4Mpdb?b<_)4G0azgSqf; zY*eV3j_+A~{pO9yw-;;!pUsH?WM}rwcJ>#%jaysUt#BBteevB)tI{P`cz!8U_YJ zC?RNexX8=fQ|2&SMp9f{Oty7GPS3%NaUoO0bwT|c4b9|t?leUsBi7q@?l|qO8Ka?Y zU|=ArrT|$I1MqKC#*K*ag#|)LDv(c%07t=hFnug2pkQQ-1kkoLTwd6I*+X<AMQG{QLyhe0e7a`e zz^rdC^wCMWT+}JIkGgEyTHn`44RwE@+<`?;U%%LM(*mgnKu1AM9c({RX}t08JM>}l zs;ZQ*?9jmhj-M=UV-8egZ}?mBB=*f404o8qJ_q&CPxoZsZ-dX4u6~EGEP{r~W$_OQ zw5d7kJC%SRp#d7G_7r*h_U*57hcO13)fQUAX6Uzyt-9G2m6dH)v0o-8hJLZ^nrvh7 z3o0!wbxRu@9-fL2tgT0}ezB=?B%=i7&dyF)L`2ih@^E*#gW*%CkD{N>BL~}yp~a?c zlTBoD%@BN1a0f==B;rIt!&WE<#a`Ri#LUursf%-Cb#kNLt1`rl;^b7^DvBy9q$rsG zT3Na9GjuThI^IY9DjF1rLT3#rMPvE~=j|-Yw5J;c2Ag{a13I`2H41M? zg+0^MpEo9)*c#s2Bs|f%fPetNv$s;giErHv>b^|~`$2(=Qbsty#{@S-B2ntqt*C-H|9(9^I(9ftZnF*C!3 zrt5V8vwyx{5rPh}Pd6bc>5_rR1dg$TgCHM2e+#reEq(c#!C$_7c`fbvxom8t$4~v> zaJ8<5>b^AF($Z3pLOMA_y2;jD_asyu$>UFkbpq(JoERTxJx3V4w!TgQZDpm~!a24e ztzwhlwT%r*z|FIzR~VwSfEHNW-KG8T;RE_4D0X7!=2o5+g%Y6%VY*V0R(rT2g&|)J zZT{|(X1vegPB@es)vsT_;!{ykK|SJ^bYI~^q491Kb%x18LRc8SvAH>?I#2xC_BJ&Y z*Ija`Yrl8sC7jhK7bl24E)>DA4SjoHvHM z1_!mJw|mtd**Eh{Aa5bkX>}5*5{&` zq(W3uB`5Ur`tO4q6J5{3Sn?j$Vd+Bls&yY&m_Cr{QPu8q1g zZdi3^;8a&v6YKxR>an_lc4KIt2yNU{eqpV!%PmDk1WCo@%D|lm;=ro^6Vp(cT?By2 z30SfsF1N@&LP_I^Cj{)_u~IQx^mKp2dNz}TJ@5YiaCD~sj}#tz9u>e-$U3kls~pCl zf0k#D9fvq_HAuWX;met^0CXRHK)aB}ewI4V1R#z}Y9CldMN3B)_BJ{MfiTl{DAZI_ z!2hUQPAcQQ6BHFi^5e&kUzM&lD5C+qP>8^NUI77>S)b~D&3n(pDCh-Mus0EqNzSR+GxKis|?Sk`9(K3EAkmsYh@opK?u5?B|HF691^vQ$tBE=g-eXKqZkOJnIIssX!v!4 zPy~{x9P_EK|K)cx(c0xCb)I0=+>d?zg}FFHG)6ri?!A`vk)#pO%dvx&gHkDyF2J}c z#26sKK&g#p&7VhFS~;}s{}+DFEa&*PBFXvFJ2%amy>?ef5VSQc)35Vo>XP0hhJ}mr zTfiY2!HB%N^vi7N0dWzCh=>df4P^iV8YnUjblV>>MS#0>JUQ}Yl=gVw{YF??no)k+ zwC}3JXe23ad}@ zdJxJ~LJ5L=+M_EAJtPXPs@zvwfwn@BKs+)fSX_B}hE`TNSB*4v>U?Y5&cj&$heX>V zAR|)-Qp|C(nE)~;m4$_c2hdO}+TQonOAUDq7a_ah6A=Xh8I9HYcrD(8@j(s2x3sjpc#g2dc98OP*Y9-q+cxFHW1y&oO(1b&Ap^t1SO_HQ zZkTB3?(xtT3b@h;KxD*BlIpX)Q@~}dA1oGfh>Oz!{Au{8{pHsueJMs6uacpDozG?j ze@8}usta>n{9`;)>FRNMyqmwwLQfw~!J#D26=w`J-eW#1vB-5%!)0}p34rDA`FxH1 z?m|O3`0lAMU9#7;wCH3{kDfd|-tOQ1dvZOLlub(Ne3$GY-`%@;vTVYD29bJ*L<|Io zK1EwwzGxP?Pd$Rrjsj2Kkn=FJ#12T9me-d}$7g3l0jnAVC?BZvlS6y6auPr~bV5c5 z@JpebcM{$~^SAr)_#w1NrhrfI2?$UY1Xw+E_EA8SsT9=+>v(-WJ1yhgI|QK9m5q&! zlCFFA?%jY=g*Lz_Z+RzCAQ-r2uDh@O;jX>fKNZzGuqi}6ZT@_uK$>fP4i&w;DsSGr zNiKJ?mkBwUmM8u;fL%hiAm}oH(KtEUyubWpb7Lc@yu2K(7S^RY_-xH+y@3$b&g_pw zBe~ReP!($1{C3nCBciVUid-QK8rz9~|1Rj1T601dR&reCFeZS6C5c#C54T1$@giY> zo)$lDLh7SJ0kvfmyfzq9ncZ;Sa$I~oClWT%6iUPv^zHk13P3T{+kbQ=j{&2yUZ4yQ zi-`fYf`XQo+e|p=wUqlOl{x?>giwW2cA&sw$bZx+T}a*5F#q-LT4PVomv(da)qW{M zo2C$wp)!~g0z`o){yDrStKr?HK@KEL)aH-XFw7Dwpb9QwVPO{o8$c3`Lqj@%CuP^J zl0Q72qL63jfV~lP8F_ek7{TuW8`Q&i$7g16k%5XQ+)(}^S4OIA(oBtw18&7}p^O`? zfF6QCTfquk&FV!hyM3tFu?YzwK#o1KBp!ZuEs&g!PHXzoe4?SR_N-G3vy3Ca4ewf& z=~(_dcRH*O)df0!{=`AFw6%@v{V~v@(#+KV%MM}yaQih_ych_;i(sY927uA9O_F}> z?Bo;xL>+9UxELs>KQID_y8E%b45;Ec#$00H?IDXp4I`(d%&;pws&$qlWGgc7pdqFg z##X>jSzhQbWN~(Ob_8(gHHgdZb)_0V=v&xFtzyiRlM|uKmjk|h5y8a7booRY6%*4> zfW*VSN;-5)t%Kc1J@EivUx%I+z*Pi<*W{pO+6`Dt)>)m`}s=($7{Ns8lMdo zkmj@(gt{wpRYk=$fyA*dqeGJ`E1znFt6r9tib_jMZw(<+W?s~ZwY!AdIaiDQJEd|NKnd}fMWUu<3uYIpchE$k!3MTnE`;pfx*xF*ad)d zw)l3ISB{+rUEgxy!Vqtai}e)-q#m}KVgMLx43-1jDSg03(I*On`@ONXr3mex!6C2{ zB^|s;$;qbwzQxi>IA4Iec=_p`kE=vOxDk>^krPkE9Z{IFiu5w#$Y>A+~Z1w9M-be`# ze9{;QdU%Z7jpzS>Ut<*(ri7Jn9kQ8ALKFmqn8RqZ*x1uR=wr~?kn8|5m^A5b2!Pf3 zNNQ}J=$w%nvh!H*CqQKoqA#-I$?R4FFh1ws@vwW*Nn_qr)LE@P6gX?v>4Xp%wj>m^ zBG?OQZf!;R%%SyXfNFyyB8Y%s#f6fjSM5Ooq(mDm(-qHaHvk3~IZxk4_abU*YaN}P ze@ivusYk{n%lXLyFCGKA2Pip)+}vDfkS_rgTx&h=(7SLoP12QcbGA#_#pMdHESI4E zF-p4NFZLHIL-hUmq@TZAjrPUAe^bK4_2Xtr?eFcOTV_x!aACJp2uO1+9Ubori~zjg1X0U*yoh!p7QLX-`UY9cpMKq~E!a!2nDOJ3l-P zx;?g9y-AwDuNwlDv*d9PZr#a2e&@q-U^=HDF||Qw{30!l40vZ05?Ch8jAYMMBKD+vRhli#>DGn9K zGBY#J4VMdskTU7m3_nPNZ33sIfy*eegtXTJYn8y;+Z)9VFu`Ijo{H+%QEZRya8Cd} zhoAq)4RA;HIpufVR*su9_h8Qd>@jD4v_(N8X_vrcKBmzWr!5ARw~q ziPzxwWbgtoR#sL*1e#ds5SDAxGWV${DS=rtAGZ!u^U@IdA1y#wSPmC-3=!K zVy?vgG4D1?3ghFc(QWyM9Hu{-$|E-ijuU?)NSVb4k9-G2$#wE+m21F%7jR|g5O?etHVn;)zskkA`<^m#c_Fx; zWjC<2z4zC&sPtyLKV>XU3+XdGLXJVRC5_Qabj04W0uNHJWBy!>{b)^( z_WtX3@yoY-Owbv~kc`*V3z>eXSv>QA@pd1>xA&CQzgnnz*pG}%x!{#6g`bUkAxMW~ zttT9t@9z_O`h8BsKlLPf&*%a9(OZ5Jy%eXj$;{Ak-}2(JaH#X&6nlb!@H75cA8WWa zM?H=mu4pp85O#JW>w5|sY%|N1VJVhs2$FD{>S)$UE|OsT+25e4BL>1VS%sGw4(IgV zM*X9YCn7Xi(+oE`>*qRuu3ND94d`AZ)@L3GKD!n38zqjSZFNeTrP`p9>gP_r`aVnR z4dX`GqF;m^UDcWXyVpg(Hgn^h9U}3#*I39}iCEJiR^Tu0fh71g@&B%1y;c5vZT{1p zz3r`gh<|V+wwm{$iJq9?#k*#>pQ5dw{WtFSMJ0^+qy}yMetOrPl5i;rgg$5DjV_!= zlAwRYxM zn(4tdDIi>Qe;u$3+m$QlAmaOx*{$l!&tbzVZLX> z9(jQOuBu8(_IOKeZ-4(9en6>BKWu)d)&u?h>$CZt>xs&Wis}wQnPe)Z7pltwz7y=A5 z&tbx=SMT1xS3DFK6DzN6hrrGgZ85ig>V1*MP)YyBwOj^d?VqW=N0lG)MmryU47^M- ze+&$aJ?Ld$#Ki@s`5q8J*=>Pc+kD~xKz>co9WgX z7){Lk_j*y^>L!bpcJsdpUcy6&?R$t1VsRxD8FwQ%5;o zXUQ=tk{t8l<;SnT9J7D2Jcl69-e^xWu{F&7ZXAWzVOo!7@ZK=JTR)rHqy#2oI_41s zk4WOfE=j!zlML&WxLh6^y6!WS#x;yDB`j4vb5)*9|AYU!{enkvve<{Mo#3qB5R4E1 zD{a00@wuXCo~K!@diFL$7?DpivaFU9!Z#qDf$YG*dv-6}h4E>Zr`5-ppFT`uA((IZ zFF&~{RJkun_f^<>>1<6iytbL&e_b`j?pyZ?7by}IY0>q;63UCiyKAP3rKx-mox+Hq>qV50}q$Wx_?yrMPm;_v5VJ|N>E4VN7zkhbczog8z?tf;ueeE5M9BLPNAGqn z-R$g?RrDz-sZt@rjiss7`uyz6qMOanjmM?Xt`7(Mwe8=FSZmtUmzL;^A^z{z2~KZW zL|x9luvD>tV!%ThBkhzXlWDfUXN-b{nDAMe2ux8^1Nv<0M5KoKue%DJ-x?ArKY1#2 zk!}Jb8mKyWV>fBhneOKJ>isq?{x>!hx^;z%=m*bA{}#D3VMo!V|B86stiqcqlIYD$ zC4Y2ua~Q7@Y>HNHO4knf;Yi#tNBo(6N%xE#g^pl^3wF~8165U-G#GD~KA;@ZtNHc)Qtumv1H5oBhXmlLw0dM6!rIDgCS#@_C zOQ*(d$lzPw&LYm9S3u2lc|x2ort*39q%$J_x{kwwm{{>$L_B1GJQ;K*&9<&E1Q>Sx zZGJX#`DW!KYy{uhA{f?NMrc|ldXdlBMI}Mx;*JtbL~d>4Ce8w9S#16o6V0bDZh4vu zirzcB>Xk~frD?;;9dU+a9HdD7wUM(H(cQUGOrDA#^?ek5{OA-3^JuDW78%_q-WjcR zP5!I+yf{78L-cQ=`xr& zbu}ZfZ_Ecf%MnoV>OnLIW1;B2e({p#RA{DHk@NzO@Wvg0Jo*J|DyQ;WYfr{yyu*>3 zLFA3UZd^GxKR*xcOe2(*ZeUUlce;N542Dh`cmN~w76OIvFyh?11r7JPDT{4SssH}C zgo}6#CL4RzA;h+Q#HuRu3gzpo<*_Un)w-}}gVc9+erliQKliTnIAJ&gGj9Dbu)nCJ2pacv;Km>AB#AjN zgRbgPS3G3jM?9;e#V%vidR-S0L|p6Qn)O$*RFcP}S(j!vIPysEV~p*z^A;MxYy!zZ z4y0fHl3E{yJOK4f*`T|M>SGX0GP7z$9%JTuDnId%fj3E$(?TrO8M5;1naYSzyQ z#UPl-hr3D7mWRthgLsm2^$5csIBJFToVIp&ikIY=hk_rByGJ{|Iv$qUY22nlgMX^h zvq_;r?y;h%d9NJ50{-0;NourNgT8KjdD(96C9F>Y1^gpVPOLPDogRX61fe0Msvh`B zMCDcD574@vno?923X!4rvZ(Dq5k}q71`%xg#J+rqi4>KUh1_n@o`Cmo3qN4aCQ0k_ zDq(cXqrZik<~&UUpA26A{et5#th9w6=|ybh`s7-H_7_~(&8S-4lnC(;%h`FvynbkM zN`1tXM)Gdbx27x9V-iq{xQ_EhQW=Jm|x^EMdSF{WQ`-w z?rgb{$M;k`T|a!JSMBqtFi9_jw$j7IB!))1?~ZyXy8d$vaz!zvy0}aSS2asE_p~qG z6)MZ;=5leskTUptO?~#iYhLAx4)}gx-_yTS`1MI>)^F^oD$l7FdxN6OrtnRI=r^fn zKkw)ra2BR8JauAPPomykDAhkCSz?x^mThD38cPHCp^)stp1w{bG4@6L#+BV z1;B39;e)|i$8D~*M^YFq<_w5`Os0eZ>1G4f@?Tc5Scb0(?0IKZF6N)JHM0iO*APRM zu^=6_%MHG|?l#qwD-pE~oiA?Z*Dg|0(37HhRpwQk*yrMndeNnt7kPGzD~tY`O+Mrc zriRPQSHr)polQpTl{e3aOMT#L8KZt;A~!F{pC$-+Nc_OFld&b%1NsOXz0-)MQxe0l z3g<+!T%pbEnn?UDG!ee;?ZK;6p~3z1B8>U$vPZo6@6Ih|Ock%(WIW3a34~N=afMJ$ zPbzJPccB}#d3iOQFr_==$&mcr>EU%_b(cx1yyL$YplMqc(zDdg%wTE-MB7Ie3JJRO#LLjMZ5I{P0jWU&#^{>H|i$D_*ssX zql~$8$#ZDu>dGGv*=DNiLzN1gvwhWyE)Hkc)dwe6X!%`Xu6ui1pmHcCGCpcjW%s$_-)Z_&DY4l&i7`jyFW?gd-PRR7n#eue+GKZ`YmsW5v~)=0(dP&`aQhnI)P5LG{6 zlZ1l6BRAPcQVwa2iq8WIl8xb;M-vmn7NQ>F)x$Oj7TPd|Q*pm|`!*64w!!9Gh(JWx zn-@VJ6>Mdaf_`#SzId(w-p*m(I{0CVv`|UP`HzpPU&0L%;RawLD6t!+LFHGVk+Pq2 z4$BGu#w*bD>C2_Pc{BFt7K_Kj{P5=fbdqpLdA^HJF2&%zz;0^Ui{S{Z66^r^r+2Vs zZ~rf6c%y6tMZatn5;Ex|~L_2<&lg9>_ z&m@i~`q`T-<|@9ubFeG-?5f2Fx}iVRu;P@@yW1H2x~51%gEfwY5|X1&H}5S^0jmqU zAaq5L2?c|*rU%?FlbjxM6X2Qre)x$3LDaNc-ug5RS7ib#^p6YLhuexSe#HJ{^63xY zZ@Nli_B?K&O`seyol5yoc?-VJSldzjiTwIA!T=vLy{W4?kkmn<#Yxc}h%WWYKphUp zD_5!NQftHYXH^{+cWOK^-c_W~hm_cb7%%@FpS68l&l4WTjJ`$C?djEcJvtNz!u0fL zlMG1SU!b_a(oj;|hVm&AF0nT@pFH3@!=?JgEZmIWLOSdqWh1HU?o!2v}Rox0lie$ z2iAFwShpmI|20IJ*mi`t1}nuV*KCf1eF|;6qBN3p0!@px2fW#+y*sm6C9<`u?wa#l zUzO)3wpxB>fT1F!xz^7_@sRUG=|YAQXW>_3Ji%~qm@tZ#ZRnh%T8eCYGwhry%btOU)>#q-ivE@C-ffv{9 zj#HV^e4*HksLb$jm!~acTX1upnTTpN5J{uqAepC zSbaKj_rq!v0-E}TDSTw8Jl*fM5+kzFGt>cHV;F3sJJm#Z6c4eQ#@ZEK?knIoTbyHs zc_B6IXzpH2`u0t*Z{F0@;<D>IR|B7$4MDjtTDxOG8XuZ@0yTya!rpP{bW3s_;~A}QtMLpXb&#Lbw*7#iB9E)ivYmA^H{90 z0gpmzfYWxmdKu61rL&{#B|Mxg>zZ1CbinK?K@Kl*D_1(4DRZ2D-=V0u0FPd!b+?2ER=yKsx zl<|r$d3sL6iI6sj?hEabb?`fj1blmoC?ic?kD;XXZ_L22o(JzCLymNYyG-Ug_VKKl zDQ6w#PakGDRzKZ}=S^zAu;x@E?5xY6_wzew!UCa`xtAKWLcAf@-yM~N}#tGEib2z_?yh&Oo?hHKAu{4q0r?g#ZzF)MqeXtv;Qthb5(XZTW^f9H zLl@3=;Q}`3Gp^P79)k-F3ygu+6Z_}lU*!Y2r!%c-aqZIQjXS3#UY?wN24+rTN$QxE zcX0IV)U7NspXEnUe)_}nb#%QZD(TXy4}_9BNjUlBNvPVJ91sJ!fR_NaSn&|6D-5Wv z0(6HSyCd$aherT0v~+bjxViB_59q4@=^D^mFJLF=@1*lTns_!yniVbP-}rgzjtf_& z0CAd8zhdRBD{0qF8i^@wKc|vFD6V^H!YtQ6GageVfqF~~Ca+Hkhx-mr+ayYjh=kOQ z?V6S-BVgHa0u>X;1FsKr#ova#w*ZgXpp{^*p6(w@)9iMCCQn_sa`)UJkI*cv3LQ^Uj-DpmI9URyS@jc72QZvD(U;zQW5YUZV z58ezARf|hWEtY_+ehP$xk7TpozsCl0WvC6Lj;^Z1?ScJkEhoILpI1W7bAPH;B#-adC9?zsq&q+B#3g8%%m& zCC+==8A&g)25vcPmf4vZumWcOu}K~prUf#&gToSrr_LSecA*9gd+5fy->I)12t^xC zjt?mq7<8tez+MFSMqO$bo~iQ~@4@zQh9vqnF~)z2jWrJxo1&UzmzBTtXy9JEh5;M& z4s*Xj$p9r9Kq9Ez*g*@A3e?dh1G+0P%`EEEptu)6iz%n1+dH3~4zf#tP--lli@XJ z@OGiV*MaU0WT@K*(sQd!*S&uoquM^yb^`94qR*cdtgJ4=W)fH%GGkKL$%Yp>f4)wu{Y7JN4vDvgQOhbwziFcTl3ibDYiZl$};)y zdmPw#`B9O3Nl6LMAq&VXKu6jNyN-?vy_dijLJT^bCQzf4`1o5*@=Rq9|7a7ajK zA8v#6xVLRa`X!88zUBP~cYDjHPpdnfuRrCgbYG2zNDTEoIo=6`9aS!sw>isoFbOF> zj-XEm;~?spm0Yy5O$-0uZ7Y~Ga~_@!3}}IA5x>T3yAecKfscDW7*95bOHO1vkJTP+ zbf6yEq@+O~In$7xcZULHb+}Lbymy&t%#%*Z zj($3c2Y#auxpnLEeGjvGVbOJebL;6?w(^1z>*4Fef8LCZl~&kmCcd$^OB(w-;xL0D zon5T!sl{I+il@5%`V9jKH^%+9(^oe4k1?dh11=Jhu2mcMw>)~%CDSxpQRTU}_2b8R zYj^PZqTb(2y}rIG_;cJ>?Xxp7cO4BnINE__ZLL8 z7Z{?Hz;pSFRQcCcohnkfN4mPaU=#QZt~YbrmA@6j=IyVp7M9kOy78as`*(I~=Qm4B zxI>8P**|^yq^hdw+|$;TB%%zyi+>x_9R-rncq~sJfI@OOukG)g#v(Zb0}qHyKRTZR zCt(IEP!r)KMa6$3?qk$o+ypnI+RWddsW)%kiiH()3#4|6ncX*6M{A}_`wilFH9!6B z-W(x7K11Lnj~c3}aUk}KHn1)43=x3A3Np~geO<{glT7v>z}fQ|tiIDbpftU}!;{B< zr)t*Tk{>l-V6XzAUN!%=N(OIuC}k-eR#*ghK99h?+^(fLKxG%8i41GIx{89q+txL{ zd32N+98colPyZ4l0skMD&jafJ2Q!uy7VL6zW7g5&-`MgxzL(bXXT^-kH!mXtQ&dzG zOai40_d&oaFOR^=U7w5)oCIIAosPF|RZl6Z6j#X()W~l^J<$+e)3pgZif}2I9HL=B zVu#h#)YLE$4X7OjEUH(<8;74rEu!}6k9rlV6{oOHwZ1yCeg6CzLC_;7P_s5Do$_aE z<7C!B?rz%t>eAPz`@%C_X`oWtLrhyE8^He?dMyyw^>tzo6C<3{Kp##(#bpEvV$|Tn z?D>yk+71J?yF)G_hWd-oN;k{l5&&)^gXuGuiuf(;21Wb}#(3eP_K zRc64A*+8!hxdh!Nc(q<8j_0m~jtn$k?|SAxw)T~9+lP=CNmaq9eprZLVqq1uVqsw= zSBqs{^Y!(O*gStS_-3}GR7yKE?G0>`fK}t8iV;ZKo1vZ+yRRDXX@AB>m0(YwO8>BU zlKf{ZVa1Fe=C&B@KkwhaXT5ZZfF=S5OhEFmZR4M?8w6fk@G2EU9bh70{AmzO(CWNVx|U89cY${}|H~WSn?ykz6(Kyj(_|QxHD7K6}O> z`1CkhZ@Ua9QJ~v5=vfN2$2+?3KYSSaG~h;l{(J;_iUQc(8e#lJcEeXer8Wf`xkG-I zMevGor+Q$53*T<1Mi!)fq1oBo%zj5s=08k|$5Y%#uYue>z1xcX+_@k)#Dt=yrKSAH z7wigq`(1v~{{9H0eYCVeTV}=qw2QJ{8=V`-3z*s54>@~**NTUhPegSuND;VGkm@Q!_=ot=s9fl{E zkanbxw|c}3=Z0Oti~w(s?b4;(@4E+3kT1gt2LR$(!PQhT*abH1qSpmpIxfuM5`52hH*|FioS5->|5YvU(_>#gKq(N2>=Hd>lY=h}tbsttl+IaqtMp zKYU05W(V6i$>BR@W*1WZPCP;Ue-5Iz9*pclCe8ThX%1xqKP})zVxcE@q)7Hz>+yR} zY78J{B64za;^N|3$EsKhzPG>vEqeI+-Me=*vtmbIM7ti_xPK-kL4}c^Nj2SF8KDz* zG>UivVif}T9Jqo{KH1m{x__Y|fkcfSyfH2AP|8h7N%=rmclaN+I~;1VG*n6i9+ANO z{Cwe)r7vBR-~w+0+vWPn;i@T=0ysj2A8K&e(9lrwpGO7;(O}(W17!>N`%pXO?{7z2 z_x^&a2Ma-sc@A~QSHT;H2d&my8Sk<|S2__3Vgs*v&go(S0RbU8Vn9cVCMN8JL`3Up zr$=oIm!TQBdH;TBa`N!K-Dc1)qfU{#$yZ6B9R2KkY}G7#xFmwqgLd_yj?Qxs*lPU* zX;u^JlYw{KG+Mi|RSCH4ZmKjr`1ZBHG69D%9QvWhc0fp^a|ufI_4Z!0+32r3p@zq_ zDNCK{q>vxM3kGANgG~(lk`=rTs6hu#!67Noh(Mzp&V^_Jx$3+A#bUGXlyFQ%gOQL{ zpAI~HD2p#0*mvr|41`V)2NxHv-_aBcnm$BzpLh>_b>cvsYY+_k2`sla*4NRjAhjit zQ2tE2n~0EwLMk|kSgFW`U`d?ZQE zuNIo%>Fc~CZHX^l7(LpAndEnKbHg0};B5j`22(yk&~S9=m;`qBFM#+uR|JrVMd#bc zqL!eP#*{~ewiLOC4&&3)oB%8{`apqP%{4_pK+qm4X1}epD|fOU^SjQ^Z+ZuW=Ynu& zVdF+@^N+?cBYSKwLS+7|)r|Tc+L@ogBLR#EgAxx4g-7&qt_!b_7ofdXqt`7m3V;Rv z4UC}$3to^;TDrR*f}mCR2nxoFVcX&J^)PZ9!$t-ZO6f zli5((%m9D1FIUN|4(QnYNuQ8*19Yj|xg6=8!C!ipcn&N=B=M1*KBw_RC z&}v)BDncm09P`tLM_Gi zN8R9x3k)=PczD{S){i0~su9AtU*?nME?cy+BCP9S(kny5{$xQ40$V zRgUnU1ibQmJGsv?viVG56z6eL=##@k6BRVmUw&-WZu3 ztvFM_Cz^4m7*WxV)m~)zC`e0N2nf3BSE2mJ6bykKm@)r6J1>fDOvGAIj8uSd9q$l~CD$2b> zu2xV>6$|^t4&SD@-Y*fo>c9I9ur{Z(v>wRGIbkJ5DrGv4K~Xn@Jgu<_P6IvwkTf8f zpYJbBdkUv#Y3%Cq(d_r6om>F_N=CPp8N4ND=pVhEF8;6PzC4`j{cC$miHut(vxEjI zL{fwX+i;pth7vMFGL)1lgfi?Rr8rcmBb5jtk|CKhRWcJY&(ldr(tCe&e!urT@AF*0 z=ij%>)#a+azr$x(>t5?~uY0*Z+BOWdcK3QF81lY!x}KiiTNDqMbab46ub@4tEtk18 z8N}fwm!oUf(A67p!k~|diKzl+5mgN02Jc-HRn8535M@|%Ht*{nba_*$A8q58PJLAd zEVPp~>H+mtr*Gi}Lol6w6I@7n#fE_KJjBw}r#W(e)()O|#ev2A5iqLPMU9bpHw=rz zR~1ySK9&zey-wG_fCS|vZX`8!wZWJH%QnmD`*2ErX0}+1V+r=}3H?4o#+OQdl#-Hy zyC=;K8Wyc4HC4d^+bR-buq*a4u?wulO}YjLH=>O8psnd^&sR&AFK49;Hm5&H%Y=RD z>g=pJxp0M3)yCN>bq^{A;d=F7jrR1^b)C!mx$s>oxwra-i?GIZf%+~_YAC$ZKGe@3 ztF<4Wt%_1iFAuK_i3etdmq6x=3&wf!>(_8Z#g$TzQgJ}_DJ^$+^wbBn(PotW=>d>%S&9dW78k8bAX;aLx0 z8b_z2^xS~*7V<)gRqlxVm-#$9k^j0msSDT@pnNz- zQPu7+y(FIp#E5?p*=0s1A-LK7D#xc7-*p3TW2_sNGIlo$mR~3F#wqtVxOw z>c_dQ)A`T7{$_o<|0a(GO<>7=$SU2<^F}zqMV;^4p1;SyfQOB#=-~ZC-{@#zqM&2^ z+LJ$ju0G{i*Kwch+yZ!WQG~1IPn=Mpbrs0l5+W&0c^=z|XLR;P=!1x6vf=pIZQln6 zk!)y7dmUnf%JNy1iy}s9Uq1tBv!oL3Yb$lj-fZ%rNq&5&$r;Qiy96$T^1+@FvKdE} zzmKGoe9yU_w^XQ;6`(K*MQ%mFixc@jzvmxx$`Ht%C}f?51l(1)D5&14J*hPZn1h&9 z5ll@u(;1WrN2+)zw7d5U1JLnwm)!_#wl9T!y$+R^Zhu}=o9S;L^s%9uWwX%>X2WD- zU*Kb7W1;!^c}iW$LsHL+qNY8-rc@CT@8iRAu4Bfej7Y?Df&mObf5r2Cgy2mivH*v< zxVU_fevvO{4h{)H5=mtX*oAdJe&k$IJGF2@oSHUy{<>%)^KJIsvV}>d2^5T4;Wpd#lUZl97;Lqk6Mg>~x z{E%V3Z9SNUM7NO?l>)eSjf`xD;9@?}D3n^b-sAqIkA;$`&HS^jJTY$%Y?8hQ#z0yuVw+E0Fw$NABY^lXYACM&KqeoVV(~Bdvpw))Q`V? z`}PeIDR<|a6~)BTO0n4L2J0Id3ZdW!A4!%6ild6wi!|X-N5h^_(&>5uieSUgXp%*! zyra{#tbo*V%u~wB}oQ}^Q#TT6?|O(?6f+8nNuKl zjt;)t@%qs_-q(vy6U+{(7d5ovIRi#X>@ff9<%EMB-LuN1?9oTb;}JDamtB0d=51?9 zgVhDa#W!ftNou~V@`N=s%DS+zU%E9r>d<&^mZ_U!^2?;k0jDNXCzRoP`PM#rt@nkc zQ}#U(dP^uHBR*y_y!zfPQ)k?>cQj}FNna&5t`X*@dYSq) zjO1aNbF}elvo~I+dt~vhMo+V1nU3G+S)*9(Cn6obsk`Vxn;E@E^wMrF`4A3;XE)=m z7C(2Ub}~wAE&svU##Cj!hNBe9*gK9$g|)x2Tohi-+$XZ^!8>1aV_nu{!+VqCmhM{n zwsyK5N|kHh@OCAOtkMBNp%t9$-qDBq3x&x+M=)zd#ZCLF!iJ(|iKow&QFiQX62~pM zEidpT43~1c>O3hS(-?1h9b?k5YR$%KKgRwPiEi^Yb>;PUGS|^&Z8?3LNL`U!y3+dM83q+&Fgpi~8OJJJwI1@?&-O zBBK#sxsobWxv@0+%6d;1WlsGJXR7pS0ZnCHkxp*@p3~iuSI30Ony9nV*K6!Lp*bvO ze5mgaiEhrN9tB)0LpEOF>)bn3Yyv!L^6p#V;kEqf6Xl^oVeMRR5f7b(@`l|UH|-c>kTq;IACsgWJictGT_e@ad|`=+%+JIK4~KKaA&g0Nt1 z`t#0Kd*g%~j;SvhR}ZdaRy(j`^;j>bhej6HF>^9l(IF*bvvZy%ewFRD%i^^ieeVY{ zr3cu_9v2DG5ixA#`Mu6J6{|M(P`8o$woIXS8cs$$2Mi^{&Uta3SX(-@<@H&vDB)2D z41WKEAtp#}?J6ezP{ezlO8EdCfLy2q!Vtc6FvlT17xm;xUeUn^Ecc_c=tFbNyxP~r zV}t|(mU`{DxJ<6lKYp8D8wJ%rL>CPtB3{~3k1s+}3*q9CZ z8xI}ua2j2cAN=;a#>!0La`n^tUVcFjRaOh~1sC%PGo9w|-0K>cpRa#u*@Ol>E%6}k z?ur(cmJTKy)X)UIMx*D$da4cl2esbY2Rqg{yI$FoQglSE=)-lYG|fTdEBj~Uok{Ae zqGBI8o&|hBIp*2)^N9%wI{NyLK1pxfc-jTW_H8krVuz33w=R5q_Ppb(XR?Ls#(Z~^ zw;8y!3v-mgSijvTG&B?%>gDaaONd5Wvr*&%u z56=E|;7{((uJNY@Bb`Q7*Elqp#f|ALt2!8GS0cR(v-YOr=fceTnQgAiP@|kMr?Qkr zdpT_DzD+@-qIB-X=94B4!H!o)n}x5YDg_SU-L>9@gxB zSo5j-8lA2rLoEn>W_Q%}gP+Wyo`)^QBAs1b^d|uUq8O1M467BAZ)tg9R)Gygkep(M z`~~#y8d6T&sH@XM@~5)l5tZs}Yo(ttD=*HA>3G)@A=%BiH_x2TQq~jCm8F6o?8D$p*MYW~fjRxg$~z4PjM#69KIF3kvztg;pLK(iKDE#_}Q4kwU93V>V-7 zs49THO9|iZgDi1q(*8AKLsJR1)lKelBITu<<9;^rb$%>*FN|HJyiJ}>^$Zr#kQN@9 zn_3fH`P#_$>)^4bc?X5$FhfR)(8%bPWYt0|8qF2cZ<^$~!gc*E&Q|k(e*a;HcWhdY{a<&7 zxQ1BktIQmChQy3rqH^g1=i7Q!&l*0NV~%i~nu4{_JylGs;IlA;hC0oxlb!Z^u=e#L zw{u%uh*aLPH2EDd(DX4LL7cEIp+vntdU@?bmiyK{vAHd56vuOEWnu9;#F}vRIdp@5 znndn+I5oj&t4q{d&095oYLD$I+_b%2`KFYV71y%0pE*RX*TP7yx2huhuy4Dz0_J)#ZzE-YDH+6b>ZuAnT=HGJ*Lu;!onfP zYK)d%lr8kvVkg$g`CP<4*ixq;0dZrE>A$XsY6PZwE&2SGnO97oSf#pUSD8P`rHH{W zCm;E?&oAi4y_JtS*^7koI8^38IT){_Hm|^A#Y)G;r7^YM=pi3`RVZiWd9z{pRaqrv zr7e8YhcJzYcM_Xv^5$u-1h;iG%^R$8;w zT7;+yWfOZByR(d3>Wai`(<=^|PB)|e@c*)%mc_<0v&Z5Y$K((4P!3n%jzh0US4nAL zqQf4#XPF-1?d<9s@p*bh&HTgghwa6crQmZLlBashL`Gu74K5*dq?9?L-FNkt;(;v= zjuOh`TBClrR*g!S`zxVx7*C;hdnUhTGxKg(zVpC{2pdMMMg&n1qG~z&)?Y;j*YJ{OTOVa>Fin>Y;Lr6pyl%OJGSgT zr_P?ax_pn*U6x(Ol7^*$9gXY(e8TIeIWL!n!amo@lBJ)`T6X^0&6Dz-C(8cPIcCW~ z$=bQotmP)zA_FUnG4?}li?{vw)pqNGmog_zQ%L`Bo4UO&D~Ia9$o<(l7*1D`j`@>L zKU3?!-%3lt+I+avK1{Cxn@@Ozx@!$HmLgldGd2g+c?EegBOS|3v-hsc2poFzZBvEY z(d~w72b?2iH?y@Z4q7xTS=H7(yoMHP7kLye-SfYEOOgUSaf8P6hU;NbQQqQoMw2}r zdyyyT^F1;kv*OcH(t2XX^U5$C)|y+fq_df@SmQ3Luc~jlxMO+w>0@tmKk;>Pw-vp_ z0y^=x$ENDG9(HETf>IqK0d|CezKMyOh&~gd3Ft_Cgk{#e7i2*fQ-+Ui=E=o5z8iwTntbn{*8L>j1a&O8ES#!q*9$0LIUIvd zIPEoTvYl>O5xYjn#*r+I@EOjV?Fs_PX8FSRj-ODmd)BO2ZNdPJCL2kLZ>Zod38 zCO_C&W94*=^lq=qtnGcPMrC<$3J>xL#0?4+8U zB}K3y;UgrwXPjNtuowY3{)A6K)J9Lh_84+=3QD$AGa|d_dZiU`**VUoVzLH-%f;u3 zeYy~V-5pwu2;fJ*G9NlwFh2_3jx-^IDFn4W7#4d{;YrJ`~s<6LICiZ{ALiBh$WZq~q zGz^uh%H1I688ib zCaXj{?PZUmrO-K8u~-E|eY?_kx4*&qi2f*pwT`(o7Wt5~3|RDYcPXY#Z1Dvg)_=EM z2J>oO`?5Lfuqivm@%hW5EI^{Apd} zu=I?3juPI%@8OeOWQLTK$W`_O^EJhU2E|=jA0=f2$hqQ60DwTbuBhWVr|0JW2zCTu zH3uFaIhgqn5F81lKePm_cUyivD*}ma` zh(LhiMd6FHuUfPAiHT+N;#SV*9))kiFl(%QEhBNHba~+SHxC*+YkUjH!yM+vP?^;I zX8@y>L@py5zRlY6T$dR3uu!?=;nH!=37({E=`tIaHr8yH)2WX zcY$;2%2CN>apafdPRZh?n4Ma#{Ham|dmgbSWx0pxs5tBf7JMBGojG`yvHIb=EIEhz z9yT22?fh6>&9XJ%w5+BGhY7*b)S6LjwL0H$y#}+^P9cn1r>8kaR>x#JpG6_H<34$X zx{DZhr3bfPm1xP`1)k$hBEgc}ATm&*S2Ost|q1UZUr8U!P{E{@Cbo5|&ddvc^{{{aD@Zh z3OrAMbX6t%MvaOmQZ{qT3Yu%$16CpL)~1LthDgy4?}c2aonX^I35@6kf{N>Kl)w2h z{qxa7vej?boRD^=SWnH z6%wwEWenF_&krA!MtWh#oz~fcnB|xAZel@(?GbfE9EfmPG+`l0;o_7Hot{*=Mo{s@ z(SRQLm6@yG?K!RXD~-Vw?1wq<`@V%N;Ow<{-Z<55)~qh~C3nM;UfCtDk>&S2TNLJD zu;|5vvuh?Xw~LtlDFTttB}6|hjuvJd|mTsHL# z+oB_a?B}>MAQbKYl@@{zJ_WCtty)-eq?^7W5E%rK5K~`b1;v)&jvEYhO@a`+kW|CC zG@S4h>(d}dcTXMWPf@R`akn$Y9l_vF{u4>?cE+B2QD*rMD1na!;GqQSGk8Xn!fzl% zK7Ge9eDZsC4oO)2D}@83f?!)I_fsrf>fh+0PCZMRuYhdk*aqYog>RLJHF=M56UgNL zSm&xE%S4@-!G#^xns(HY>cx-kEWkCw5_{{LJS7B&YtwEzXCSyi>VO1zjoY~naV)-E zHsED>A8n~-TCylo2=BE&agiL#pjw6&oki0xKk#}Y(#9lbd8<0iu)eGI;VUgU>&_{5 zsx)I!F2c7hijw~d|A8-Hm}fpU&m)2w<$ooS14KL-&xJeEKh8A~+ZdY+tkSkpKf`)N z`Ob}#mOLN5k;57s><>}=M+W4Vb2^+;@!H6j9SM-BeM15D_oi*w=BpXTsCn%cxZP@N_0DT5(l`t=q_ z_T0L@KB`o3wvy@54f8Qp%=PHZY zv;P4?hz%7sQ4-$%X9z(ji5Zu3{~bc?wg~2^4e%@c1tDm*g-p+bbnx{)AyB z0N1YXBwnV|jUEp%IpVO%tvB3|s1*);y*NL5*1L$%(Wgg_L1CmRTY1xl^Fg5}e+0BCnP=bpJL6LGE8_xGc4T>A;ca3gX-Oa3^YeEy)|)asOHM9@zAKSD zv8wtvUHAd}CdNy-#v#N9ARVI%H+}nxaM$*))1r!~8uvJFo z0b>5s)3OML;i3Fa@gT1 zC@gk)>aaU1bO><=K43dzE2ppi&gg3SqDs5)AOq~EBqk%x|M z+8k_%*pR*65AKOgDY=)4&qSd)X@1kOFufg(fVy!yo9aw-vS_0k_3WR)+dDV-aUD5? zAnlqUND0RMyY(V$@F`+L`}Cq2-CjnD_>JppOr$WF~SKVddwM&_MY|`{@^pL z6FB^VZl@c#_{)Gx!(N&W$b_ikYHCf~%H?lJ87Cd#aM0ND)nm(<#~h7+{`#Rd#{T-H zCaGa>lHr0teER4*JosT*1O@mb**$@y^Kb7nr7z#@|NPkp#E+sH-L{mv^TI=pmHd%GJWD8wiyQOBt}D8`L3fgjczrE`!T4zlQjL}-~{cBr9r z>Gro>-<#i?;q?0JHn2LTRf833?q(d#yXbto`@*?%sdpa+sK{QN8dyJ!&jf0Uc$3S5RL74hIMH+y5-cbj>|1*)Ac>LG=loTr=F|Ipa3#Q}eNr{} z%p*1l$HB`L99sNHM=e&K&7}4Tt&04?t9<|st6Fnyi{sX zi5P0j(qN{07h+?@u)EWs3GcGwa%N47^XyaP4=1Ol!XPg#VTtV;4)Wa#_oUIJ47*9N zrGs9+oH=-3JISN#ALSgSO2aWWl--caZtsd(By8gBF);LXyYN=b=*{@}_{`0tF*(s0gb=jU2a?I7kI&a-Z6LDSAbS18M?An%vV(%fbTh8JvlucV49LYc)q12 z`dN`Sc>E6@KW2geu0c1r5u^bL*x06?lj@Lse00$6tetB%4}yvhgBM^uNE<1zQyzVK z(x5^*+!2yJIs&~E7`OL`%{b#6ViS}x8oZf@73{-%QR3KkfiNdTP#(0hw-ps>y8feQ z!HZ|+;3&mba72?aD`Sip=3y_k=>?_e`mDRt#nsR8R_G@Z&}^?^X7-@o&CJZ~_KdYW zY~|T=mx^{B{}PqyvSE1Q2sk-vT3Yma7JINx2_<`atSty>VxmTm(_L_K&eYDeevE#m zj+Y``=)%H&L;tD*i&QhTKLU%1;lVqV(3qGIEHTn3EL~t6#MO*5E+MFkDnOT{)}H>P ztq@1&N9(mMs2{RAkkV+0c!&su6X{zt60-pJ31YYz=8qg?Q2zmjelh9W1fJPaqa+?u zIz}Y;UsNk*rI%M$7JsS_nt}^0Oq6 z&wdpUU#tJ#4r*Mux5y61HB%Q=E+`3;CIhez4LpxBgV1gZWN68knn_eU{O5ani&Jvz=eQd_e&MBN`w+D`j1=0u>N-BbE>iH;U?*c;BG> zB&n@jNgj=i_`%E#MiHhZI#fu#8*NVC0O}K1?B8;dU*&zOHj*a_Wmur$-FmGt5;8Jn z=%aXc_+%UZG|jGUM5wd3cbTDKqe8P~bUFTK^3(bEgn@O?RN&`E@Q0_M%)Wm#@ZQys z5QC5-4jy~|8+m-Y1!L1Y5JE}+J+%BXwS;+eU{wMD7(O8ZJmPqBv4=J5IPOZTIqgs>-;!K*kNcJ;`r&BzJ`=L-kw zVcmMQVBlEXN=QfmH}%XnFQuXwo228>;w=_8oOuW_p75<;HjkDtv>iBhj9=O~>%*hz zqH1(B&{I6LZIp#VngZc0+WYv=+wqT8%u5>X@728mIqz#44@1!=rV`B94f*rEG9_hY zn(z$jw6(Ibx~ASgoUIm6ZBz{2Ht1{X(R1$)_yjsi@(6wQPP*Hq1x~j^vFsHu&1a#Y zNgaMnv@zvGet(&34xADi&=F>AbGCCLkbdli}|^QzBMSE2!&VE zLVZVETH43;Wp4N?4^~SU$abUwQ4ctda$ug}AU_n1i#DFGd%i(HdFq3j9OTv4cY~*k z27dcs!%@J>OmPQeajm$x+JW`A>2v}-Z$(DNSN%i{P;oV87>a4AVtE0L`n_oLdJ@#U zim2mB4utxvb?6We;fbT$O}!#%u6O54;I8>Sa9ZoOZau;wv||mPpsT-s1A6Ap%+DG< zglQhj8LvR4k0h>h8!qha_@$YhsV`qfpma-O@j3a}m;j2aRZpF2TZw%6XO%RN8leWw zi*dpF;(1sT|7klqHQWJ>wtgPPvHMZHP=Y2laLF4`Wb>;22Pkq2Q%`pN4r2w*KmSgb zXJ7?Rn+jE!oUT?HxS+)Fp~nu@en2_8x}qQp@}dstoRbrG^q~#;vn4W0Q{^KXNymb~ zjy-lqkdIFSZEtU(7V4~y;H>lKbbxzi%1X$kxMe9u{xBQ#Z z`!W5L_BT}V-BUih->#JkZ~lv-8=CDFg#w3jE-rtdY2Y0+q$MRV?W>!FZAOhyR7L~* zg40k(XAdjTKK}x%S;4(M`Sk|ckl6G8ho76il6ty7D44)JB->Pe;k|#c*^t}>WP8en v#3cTNFA{nF6QCnk`8O5r|MxG?dn-?8J(4}}s0RJSDZi^9q9yM&yZnCu2p_rK literal 0 HcmV?d00001 diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h new file mode 100644 index 0000000..cf891b3 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h @@ -0,0 +1,74 @@ +#ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__ +#define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__ +#include +#include +#include + +namespace mkt_plugins +{ + +/** + * @class NoSolution + */ +class NoSolution : public std::exception { + public: + const char * what () const throw () { + return "Robot cannot calculate error line!"; + } +}; // class NoSolution + +/** + * @class LineGenerator + */ +class LineGenerator +{ + /* data */ + struct LineEquation { + geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */ + geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */ + float a, b, c ; /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */ + }; + + /* Save data present position */ + struct PresentPosition { + float Gxh, Gyh; /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot */ + float Yaw_degree; /* Angle of Robot (degree) */ + float Yaw_rad; /* Angle of Robot (degree) */ + float error_line; /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2) */ + double distance_to_goal; /* The distance from query pose to goal */ + double distance_to_start; /* The distance from query pose to start*/ + double distace_start_goal; + }; +public: + + /** + * @brief To caculated the equation of the line + * @param start_point start point of the line + * @param end_point end point of the line + */ + virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point); + + /** + * @brief Calculating error + * @param[in] Lm The distance from the center of the wheel axis to the center line + * @param[in] pose + * @param[in] rev + */ + virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false); + + /** + * @brief Calculating tolerance + * @param query_pose The pose to check + * @param goal_pose The pose to check against + */ + virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose); + + /* Properties */ + LineEquation Leq_; + PresentPosition present_pose_; + +}; // class LineGenerator + +} // namespace mkt_plugins + +#endif // __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__ diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h new file mode 100644 index 0000000..eb26c63 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h @@ -0,0 +1,57 @@ +#ifndef _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__ +#define _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__ + +#include +#include + +namespace mkt_plugins +{ + class GoalChecker : public score_algorithm::GoalChecker + { + public: + GoalChecker(); + /** + * @brief Initialize any parameters from the NodeHandle + * @param nh NodeHandle for grabbing parameters + */ + virtual void initialize(const robot::NodeHandle &nh) override; + + /** + * @brief Reset the state of the goal checker (if any) + */ + virtual void reset() override; + + /** + * @brief Check whether the goal should be considered reached + * @param query_pose The pose to check + * @param goal_pose The pose to check against + * @param velocity The robot's current velocity + * @return True if goal is reached + */ + bool isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose, + const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Factory function to create a GoalChecker instance + * @return Shared pointer to a new GoalChecker instance + */ + static score_algorithm::GoalChecker::Ptr create(); + protected: + /** + * @brief Internal method to initialize parameters from NodeHandle + * @param nh NodeHandle to load parameters from + * + * Loads xy_goal_tolerance and yaw_goal_tolerance parameters + */ + void intParam(const robot::NodeHandle &nh); + + robot::NodeHandle nh_; + std::shared_ptr line_generator_; + double yaw_goal_tolerance_; + double xy_goal_tolerance_; + double old_xy_goal_tolerance_; + }; + +} // namespace mkt_plugins + +#endif // _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__ 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 new file mode 100644 index 0000000..86064e1 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h @@ -0,0 +1,300 @@ +#ifndef MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_ +#define MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_ + +#include +#include + +namespace mkt_plugins +{ + + /** + * @class KinematicParameters + * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics + */ + class KinematicParameters + { + public: + KinematicParameters(); + void initialize(const robot::NodeHandle &nh); + + /** + * @brief Get the direction array for X, Y, and theta movement + * @return Pointer to an array of integers representing the direction of movement for X, Y, and theta (e.g., 1 for forward/right/clockwise, -1 for backward/left/counterclockwise, 0 for stop) + * + * This array controls the intended direction of the robot. Sudden changes in the theta component can cause discontinuities in odometry data, potentially leading to localization jumps in AMCL when rotating in place. + */ + int *getDirect(); + + /** + * @brief Get the minimum linear velocity along the X-axis + * @return Minimum linear velocity (m/s) for forward/backward motion + * + * Used to ensure the robot does not move too slowly along X, which could lead to odometry drift. Less relevant for in-place rotation issues. + */ + double getMinX(); + + /** + * @brief Get the maximum linear velocity along the X-axis + * @return Maximum linear velocity (m/s) for forward/backward motion + * + * Ensures the robot does not exceed safe speeds along X. Not directly related to rotation-induced localization jumps. + */ + double getMaxX(); + + /** + * @brief Get the maximum linear acceleration along the X-axis + * @return Maximum linear acceleration (m/s²) for forward/backward motion + * + * High acceleration can cause odometry errors if the robot's hardware cannot keep up. Less relevant for in-place rotation. + */ + double getAccX(); + + /** + * @brief Get the maximum linear deceleration along the X-axis + * @return Maximum linear deceleration (m/s²) for forward/backward motion + * + * Improper deceleration may cause jerky stops, leading to minor odometry errors. Not a primary cause of rotation-induced localization jumps. + */ + double getDecelX(); + + /** + * @brief Get the minimum linear velocity along the Y-axis + * @return Minimum linear velocity (m/s) for lateral motion (if applicable) + * + * Typically zero for differential drive robots. Not relevant for in-place rotation. + */ + double getMinY(); + + /** + * @brief Get the maximum linear velocity along the Y-axis + * @return Maximum linear velocity (m/s) for lateral motion + * + * Typically zero for differential drive robots. Not relevant for in-place rotation. + */ + double getMaxY(); + + /** + * @brief Get the maximum linear acceleration along the Y-axis + * @return Maximum linear acceleration (m/s²) for lateral motion + * + * Not relevant for in-place rotation in differential drive robots. + */ + double getAccY(); + + /** + * @brief Get the maximum linear deceleration along the Y-axis + * @return Maximum linear deceleration (m/s²) for lateral motion + * + * Not relevant for in-place rotation in differential drive robots. + */ + double getDecelY(); + + /** + * @brief Get the minimum combined linear velocity in the XY plane + * @return Minimum combined velocity (m/s) for motion in the XY plane + * + * Ensures the robot moves at a meaningful speed in the XY plane. Not directly related to in-place rotation issues. + */ + double getMinSpeedXY(); + + /** + * @brief Get the minimum angular velocity for rotation + * @return Minimum angular velocity (rad/s) for rotation around the Z-axis + * + * A very low value may cause the robot to struggle with stable rotation, leading to odometry errors and potential localization jumps in AMCL. + */ + double getMinTheta(); + + /** + * @brief Get the maximum angular velocity for rotation + * @return Maximum angular velocity (rad/s) for rotation around the Z-axis + * + * A high value (e.g., >2.0 rad/s) can cause wheel slip or odometry errors during fast rotation, leading to localization jumps in AMCL. + */ + double getMaxTheta(); + + /** + * @brief Get the maximum angular acceleration for rotation + * @return Maximum angular acceleration (rad/s²) for rotation around the Z-axis + * + * High angular acceleration can cause jerky rotation, leading to odometry errors and potential localization jumps in AMCL. + */ + double getAccTheta(); + + /** + * @brief Get the maximum angular deceleration for rotation + * @return Maximum angular deceleration (rad/s²) for rotation around the Z-axis + * + * Improper deceleration can cause jerky stops during rotation, leading to odometry errors and localization jumps in AMCL. + */ + double getDecelTheta(); + + /** + * @brief Get the minimum angular speed for rotation + * @return Minimum angular speed (rad/s) for rotation around the Z-axis + * + * Similar to getMinTheta(), a very low value may cause unstable rotation, contributing to odometry errors and AMCL localization issues. + */ + double getMinSpeedTheta(); + + /** + * @brief Set the direction array for X, Y, and theta movement + * @param xytheta_direct Pointer to an array of integers specifying the direction for X, Y, and theta + * + * Sets the intended direction of motion. Sudden changes in the theta component can cause discontinuities in odometry, leading to localization jumps in AMCL during in-place rotation. + */ + void setDirect(int *xytheta_direct); + + /** + * @brief Set the minimum linear velocity along the X-axis + * @param value Minimum linear velocity (m/s) for forward/backward motion + * + * Ensures the robot does not move too slowly along X. Not directly related to in-place rotation issues. + */ + void setMinX(double value); + + /** + * @brief Set the maximum linear velocity along the X-axis + * @param value Maximum linear velocity (m/s) for forward/backward motion + * + * Limits the robot's speed along X. Not directly related to in-place rotation issues. + */ + void setMaxX(double value); + + /** + * @brief Set the maximum linear acceleration along the X-axis + * @param value Maximum linear acceleration (m/s²) for forward/backward motion + * + * High acceleration can cause odometry errors if not supported by hardware. Less relevant for in-place rotation. + */ + void setAccX(double value); + + /** + * @brief Set the maximum linear deceleration along the X-axis + * @param value Maximum linear deceleration (m/s²) for forward/backward motion + * + * Improper deceleration may cause minor odometry errors. Not a primary cause of rotation-induced localization jumps. + */ + void setDecelX(double value); + + /** + * @brief Set the minimum linear velocity along the Y-axis + * @param value Minimum linear velocity (m/s) for lateral motion + * + * Typically zero for differential drive robots. Not relevant for in-place rotation. + */ + void setMinY(double value); + + /** + * @brief Set the maximum linear velocity along the Y-axis + * @param value Maximum linear velocity (m/s) for lateral motion + * + * Typically zero for differential drive robots. Not relevant for in-place rotation. + */ + void setMaxY(double value); + + /** + * @brief Set the maximum linear acceleration along the Y-axis + * @param value Maximum linear acceleration (m/s²) for lateral motion + * + * Not relevant for in-place rotation in differential drive robots. + */ + void setAccY(double value); + + /** + * @brief Set the maximum linear deceleration along the Y-axis + * @param value Maximum linear deceleration (m/s²) for lateral motion + * + * Not relevant for in-place rotation in differential drive robots. + */ + void setDecelY(double value); + + /** + * @brief Set the minimum combined linear velocity in the XY plane + * @param value Minimum combined velocity (m/s) for motion in the XY plane + * + * Ensures meaningful motion in the XY plane. Not directly related to in-place rotation issues. + */ + void setMinSpeedXY(double value); + + /** + * @brief Set the maximum angular velocity for rotation + * @param value Maximum angular velocity (rad/s) for rotation around the Z-axis + * + * A high value can cause fast rotation, leading to wheel slip or odometry errors, which may cause AMCL to jump during in-place rotation. + */ + void setMaxTheta(double value); + + /** + * @brief Set the maximum angular acceleration for rotation + * @param value Maximum angular acceleration (rad/s²) for rotation around the Z-axis + * + * High acceleration can cause jerky rotation, leading to odometry errors and AMCL localization jumps. + */ + void setAccTheta(double value); + + /** + * @brief Set the maximum angular deceleration for rotation + * @param value Maximum angular deceleration (rad/s²) for rotation around the Z-axis + * + * Improper deceleration can cause jerky stops, leading to odometry errors and AMCL localization jumps. + */ + void setDecelTheta(double value); + + /** + * @brief Set the minimum angular speed for rotation + * @param value Minimum angular speed (rad/s) for rotation around the Z-axis + * + * A very low value may cause unstable rotation, contributing to odometry errors and AMCL localization issues. + */ + void setMinSpeedTheta(double value); + + /** + * @brief Check to see whether the combined x/y/theta velocities are valid + * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits + * + * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta. + * The speed is valid if + * 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative) + * AND + * 2) min_speed_xy is negative or min_speed_theta is negative or + * hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta. + * + * In English, it makes sure the diagonal motion is not too fast, + * and that the velocity is moving in some meaningful direction. + * + * In Latin, quod si motus sit signum quaerit et movere ieiunium et significantissime comprehendite. + */ + bool isValidSpeed(double x, double y, double theta); + + using Ptr = std::shared_ptr; + + protected: + // For parameter descriptions, see cfg/KinematicParams.cfg + int xytheta_direct_[3]; + double min_vel_x_, min_vel_y_; + double max_vel_x_, max_vel_y_, max_vel_theta_; + + double min_speed_xy_, max_speed_xy_; + double min_speed_theta_; + + double acc_lim_x_, acc_lim_y_, acc_lim_theta_; + double decel_lim_x_, decel_lim_y_, decel_lim_theta_; + + // Cached square values of min_speed_xy and max_speed_xy + double min_speed_xy_sq_, max_speed_xy_sq_; + + private: + double original_min_vel_x_, original_min_vel_y_; + double original_max_vel_x_, original_max_vel_y_, original_max_vel_theta_; + + double original_min_speed_xy_, original_max_speed_xy_; + double original_min_speed_theta_; + + double original_acc_lim_x_, original_acc_lim_y_, original_acc_lim_theta_; + double original_decel_lim_x_, original_decel_lim_y_, original_decel_lim_theta_; + }; + +} // namespace mkt_plugins + +#endif // MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_ 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 new file mode 100644 index 0000000..03154f3 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h @@ -0,0 +1,42 @@ +#ifndef MKT_PLUGINS_LIMITED_ACCEL_GENERATOR_H_INCLUDED_ +#define MKT_PLUGINS_LIMITED_ACCEL_GENERATOR_H_INCLUDED_ + +#include + +namespace mkt_plugins +{ +/** + * @class LimitedAccelGenerator + * @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time. + */ +class LimitedAccelGenerator : public StandardTrajectoryGenerator +{ +public: + /** + * @brief Initialize the generator with parameters from NodeHandle + * @param nh NodeHandle for loading acceleration_time parameter + * + * Loads the acceleration_time parameter which limits acceleration to a fraction + * of the simulated time. + */ + void initialize(const robot::NodeHandle& nh) override; + + /** + * @brief Start a new iteration with current velocity + * @param current_velocity Current velocity of the robot + * + * Overrides the base class to apply acceleration limits based on acceleration_time. + */ + void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) override; + + /** + * @brief Factory function to create a LimitedAccelGenerator instance + * @return Shared pointer to a new LimitedAccelGenerator instance + */ + static score_algorithm::TrajectoryGenerator::Ptr create(); +protected: + double acceleration_time_; +}; +} // namespace mkt_plugins + +#endif // MKT_PLUGINS_LIMITED_ACCEL_GENERATOR_H_INCLUDED_ diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h new file mode 100644 index 0000000..e1014c1 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h @@ -0,0 +1,221 @@ +#ifndef MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED_ +#define MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED_ + +#include +#include +#include + +namespace mkt_plugins +{ + + const double EPSILON = 5E-2; + + inline double exponentialFactor(double x, double k = 2.5) + { + x = std::clamp(x, 0.0, 1.0); + return 1.0 - std::exp(-k * x); // tương đương (1 - e^{-kx}) + } + + /** + * @brief Given initial conditions and a time, figure out the end velocity + * + * @param v0 Initial velocity + * @param accel The acceleration rate + * @param decel The decceleration rate + * @param dt Delta time - amount of time to project into the future + * @param target target velocity + * @return The velocity dt seconds after v0. + */ + inline double projectVelocity(double v0, double accel, double decel, double dt, double target) + { + double magnitude; + if (fabs(v0) < EPSILON) + { + // Starting from standstill, always accelerate + if (target >= 0.0) + { + magnitude = fabs(accel); + } + else + { + magnitude = -fabs(accel); + } + } + else if (v0 > 0.0) + { + if (v0 < target) + { + // Acceleration (speed magnitude increases) + magnitude = fabs(accel); + } + else + { + // Deceleration (speed magnitude decreases) + magnitude = -fabs(decel); + } + } + else + { + if (v0 < target) + { + // Deceleration (speed magnitude decreases) + magnitude = fabs(decel); + } + else + { + // Acceleration (speed magnitude increases) + magnitude = -fabs(accel); + } + } + + double delta_v = fabs(target - v0); + double max_v = std::max(fabs(target), 1e-3); + magnitude *= exponentialFactor(delta_v / max_v, 3.0); + double v1 = v0 + magnitude * dt; + + if (magnitude > 0.0) + { + return std::min(target, v1); + } + else + { + return std::max(target, v1); + } + } + + /** + * @class OneDVelocityIterator + * @brief An iterator for generating a number of samples in a range + * + * In its simplest usage, this gives us N (num_samples) different velocities that are reachable + * given our current velocity. However, there is some fancy logic around zero velocities and + * the min/max velocities + * + * If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s, + * this class would provide velocities between 1 m/s and 3 m/s. + * + * + * + */ + class OneDVelocityIterator + { + public: + std::string name_; + /** + * @brief Constructor for the velocity iterator + * @param current Current velocity of the robot + * @param direct Direction multiplier (1 for forward, -1 for backward, 0 for stop) + * @param min Minimum velocity allowable + * @param max Maximum velocity allowable + * @param acc_limit Maximum acceleration limit + * @param decel_limit Maximum deceleration limit + * @param acc_time Time delta for velocity projection + * @param num_samples Number of velocity samples to generate + * + * Creates an iterator that generates num_samples velocities reachable from + * current velocity within acc_time seconds, respecting acceleration/deceleration limits. + * The velocities range from min_vel_ to max_vel_ (projected from current velocity). + */ + OneDVelocityIterator(double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, + int num_samples) + { + // if (current < min) + // { + // current = min; + // } + // else if (current > max) + // { + // current = max; + // } + max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max); + min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min); + direct_ = direct; + current_ = direct_ > 0 ? max_vel_ : min_vel_; + reset(); + + if (min_vel_ - max_vel_ < EPSILON) + { + increment_ = 1.0; + return; + } + num_samples = std::max(2, num_samples); + + // e.g. for 4 samples, split distance in 3 even parts + increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1)); + } + + /** + * @brief Get the current velocity value + * @return Current velocity value, or 0.0 if return_zero_now_ is true + * + * Returns the velocity at the current iterator position. + * If the iterator is set to return zero (when crossing zero velocity), + * returns 0.0 instead of the current value. + */ + double getVelocity() const + { + if (return_zero_now_) + return 0.0; + return current_; + } + + /** + * @brief Increment the iterator to the next velocity sample + * @return Reference to this iterator + * + * Moves to the next velocity sample. If the iterator crosses zero velocity + * (from negative to positive), it will return zero velocity on the next call + * to getVelocity() before continuing with positive velocities. + */ + OneDVelocityIterator &operator++() + { + if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 && current_ + increment_ <= max_vel_ + EPSILON) + { + return_zero_now_ = true; + return_zero_ = false; + } + else + { + current_ += increment_; + return_zero_now_ = false; + } + return *this; + } + + /** + * @brief Reset the iterator back to the first velocity + * + * Resets the iterator to the starting velocity (max_vel_ if direct > 0, + * min_vel_ if direct <= 0) and enables zero-velocity return if applicable. + */ + void reset() + { + return_zero_ = true; + return_zero_now_ = false; + } + + /** + * @brief Check if all velocities for this iteration have been returned + * @return True if the iterator has passed beyond the limit velocity + * + * Returns true when the current velocity has exceeded the maximum + * (if direct > 0) or minimum (if direct <= 0) velocity limit. + */ + bool isFinished() const + { + // if(name_ == std::string("th_it_")) + // ROS_INFO("%s %f %f", name_.c_str(), current_, max_vel_ + EPSILON); + double limit_vel = direct_ > 0? max_vel_ : min_vel_; + return fabs(current_) > fabs(limit_vel) + EPSILON; + } + + private: + bool return_zero_, return_zero_now_; + double min_vel_, max_vel_; + double current_; + double direct_; + double increment_; + }; +} // namespace mkt_plugins + +#endif // MKT_PLUGINS_ONE_D_VELOCITY_ITERATOR_H_INCLUDED diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h new file mode 100644 index 0000000..f1a5973 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h @@ -0,0 +1,77 @@ +#ifndef _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__ +#define _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__ + +#include +#include + +namespace mkt_plugins +{ + + /** + * @class SimpleGoalChecker + * @brief Goal Checker plugin that only checks the position difference + * + * This class can be stateful if the stateful parameter is set to true (which it is by default). + * This means that the goal checker will not check if the xy position matches again once it is found to be true. + */ + class SimpleGoalChecker : public score_algorithm::GoalChecker + { + public: + /** + * @brief Default constructor + * Initializes default values for goal tolerances and stateful behavior + */ + SimpleGoalChecker(); + + /** + * @brief Initialize the goal checker with parameters from NodeHandle + * @param nh NodeHandle for loading parameters (xy_goal_tolerance, yaw_goal_tolerance, stateful, check_xy) + */ + void initialize(const robot::NodeHandle &nh) override; + + /** + * @brief Reset the state of the goal checker + * Clears any cached state to allow re-checking of goal conditions + */ + void reset() override; + + /** + * @brief Check if the goal has been reached + * @param query_pose Current pose of the robot + * @param goal_pose Target pose to reach + * @param path Path to follow (not used in simple checker) + * @param velocity Current velocity of the robot (not used in simple checker) + * @return True if the robot is within xy_goal_tolerance and yaw_goal_tolerance of the goal + * + * If stateful is true, once xy position matches, it will not check xy again. + * This prevents oscillation when the robot is close to the goal. + */ + bool isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose, + const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Factory function to create a SimpleGoalChecker instance + * @return Shared pointer to a new SimpleGoalChecker instance + */ + static score_algorithm::GoalChecker::Ptr create(); + + protected: + /** + * @brief Internal method to initialize parameters from NodeHandle + * @param nh NodeHandle to load parameters from + * + * Loads xy_goal_tolerance, yaw_goal_tolerance, stateful, and check_xy parameters + */ + void intParam(const robot::NodeHandle& nh); + + robot::NodeHandle nh_; + double xy_goal_tolerance_, yaw_goal_tolerance_; + bool stateful_, check_xy_; + + // Cached squared xy_goal_tolerance_ + double xy_goal_tolerance_sq_; + }; + +} // namespace mkt_plugins + +#endif // _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__ \ No newline at end of file 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 new file mode 100644 index 0000000..e0f1059 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h @@ -0,0 +1,189 @@ +#ifndef MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_ +#define MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_ + +#include +#include +#include +#include +#include +#include + +namespace mkt_plugins +{ + /** + * @class StandardTrajectoryGenerator + * @brief Standard MKT-like trajectory generator. + */ + class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator + { + public: + /** + * @brief Initialize the trajectory generator with parameters from NodeHandle + * @param nh NodeHandle for loading configuration parameters + * + * Loads parameters such as sim_time, discretize_by_time, time_granularity, + * linear_granularity, angular_granularity, and include_last_point. + */ + void initialize(const robot::NodeHandle &nh) override; + + /** + * @brief Set the direction array for X, Y, and Theta movement + * @param xytheta_direct Pointer to array of 3 integers representing direction + * (1 for forward/right/clockwise, -1 for backward/left/counterclockwise) + */ + void setDirect(int *xytheta_direct) override; + + /** + * @brief set velocity for x, y asix of the robot + * @param linear the velocity command + * @return True if set is done, false otherwise + */ + bool setTwistLinear(geometry_msgs::Vector3 linear) override; + + /** + * @brief get velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override; + + /** + * @brief Set velocity theta for z asix of the robot + * @param angular the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override; + + /** + * @brief Get velocity theta for z asix of the robot + * @param direct The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override; + + /** + * @brief Start a new iteration with current velocity + * @param current_velocity Current velocity of the robot + * + * Resets the velocity iterator and prepares it to generate velocities + * reachable from the current velocity. + */ + void startNewIteration(const nav_2d_msgs::Twist2D ¤t_velocity) override; + + /** + * @brief Check if there are more velocity samples available + * @return True if there are more valid velocity combinations to iterate + */ + bool hasMoreTwists() override; + + /** + * @brief Get the next velocity sample + * @return Next valid velocity combination + */ + nav_2d_msgs::Twist2D nextTwist() override; + + /** + * @brief Generate a trajectory from start pose to goal + * @param start_pose Starting pose of the robot + * @param transformed_plan Transformed global plan (path to follow) + * @param start_vel Starting velocity of the robot + * @param cmd_vel Command velocity to achieve + * @return Generated trajectory containing poses and velocities over time + * + * Generates a trajectory by: + * 1. Computing velocity steps based on acceleration limits + * 2. Projecting poses forward using kinematic model + * 3. Sampling velocities using the velocity iterator + */ + mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose, + const nav_2d_msgs::Path2D &transformed_plan, + const nav_2d_msgs::Twist2D &start_vel, + const nav_2d_msgs::Twist2D &cmd_vel) override; + + /** + * @brief Get the NodeHandle used for kinematics configuration + * @return NodeHandle scoped to kinematics namespace + */ + virtual robot::NodeHandle getNodeHandle() override; + + /** + * @brief Factory function to create a StandardTrajectoryGenerator instance + * @return Shared pointer to a new StandardTrajectoryGenerator instance + */ + static score_algorithm::TrajectoryGenerator::Ptr create(); + + protected: + /** + * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding + */ + virtual void initializeIterator(const robot::NodeHandle &nh); + + /** + * @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits + * + * @param cmd_vel Desired velocity + * @param start_vel starting velocity + * @param dt amount of time in seconds + * @return new velocity after dt seconds + */ + virtual nav_2d_msgs::Twist2D computeNewVelocity(const nav_2d_msgs::Twist2D &cmd_vel, + const nav_2d_msgs::Twist2D &start_vel, + const double dt); + + /** + * @brief Use the robot's kinematic model to predict new positions for the robot + * + * @param start_pose Starting pose + * @param vel Actual robot velocity (assumed to be within acceleration limits) + * @param dt amount of time in seconds + * @return New pose after dt seconds + */ + virtual geometry_msgs::Pose2D computeNewPosition(const geometry_msgs::Pose2D start_pose, + const nav_2d_msgs::Twist2D &vel, + const double dt); + + /** + * @brief Compute an array of time deltas between the points in the generated trajectory. + * + * @param cmd_vel The desired command velocity + * @return vector of the difference between each time step in the generated trajectory + * + * If we are discretizing by time, the returned vector will be the same constant time_granularity + * for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity. + * + * Right now the vector contains a single value repeated many times, but this method could be overridden + * to allow for dynamic spacing + */ + virtual std::vector getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel); + + robot::NodeHandle nh_kinematics_; + KinematicParameters::Ptr kinematics_; + std::shared_ptr velocity_iterator_; + + double sim_time_; + + // Sampling Parameters + bool discretize_by_time_; + double time_granularity_; ///< If discretizing by time, the amount of time between each point in the traj + double linear_granularity_; ///< If not discretizing by time, the amount of linear space between points + double angular_granularity_; ///< If not discretizing by time, the amount of angular space between points + + /* Backwards Compatibility Parameter: include_last_point + * + * dwa had an off-by-one error built into it. + * It generated N trajectory points, where N = ceil(sim_time / time_delta). + * If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which + * indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the + * actual sim_time was much less than advertised. + * + * This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true. + * + * Nothing I could find actually used the time_delta variable or seemed to care that the trajectories + * were not projected out as far as they intended. + */ + bool include_last_point_; + }; // class StandardTrajectoryGenerator + +} // namespace dwb_plugins + +#endif // MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h new file mode 100644 index 0000000..6b8d82f --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h @@ -0,0 +1,61 @@ +#ifndef MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_ +#define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_ + +#include +#include +#include + +namespace mkt_plugins +{ +/** + * @class VelocityIterator + * @brief Abstract base class for iterating over valid velocity samples + * + * This interface provides a way to generate and iterate through velocity samples + * that satisfy kinematic constraints. Implementations should generate velocities + * reachable from the current velocity within acceleration limits. + */ +class VelocityIterator +{ +public: + /** + * @brief Virtual destructor + */ + virtual ~VelocityIterator() {} + + /** + * @brief Initialize the iterator with parameters and kinematics + * @param nh NodeHandle for loading configuration parameters + * @param kinematics Shared pointer to kinematic parameters for validation + */ + virtual void initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) = 0; + + /** + * @brief Start a new iteration with current velocity and time delta + * @param current_velocity Current velocity of the robot + * @param dt Time delta for velocity projection + * + * Resets the iterator and prepares it to generate velocities reachable + * from the current velocity within dt seconds. + */ + virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) = 0; + + /** + * @brief Check if there are more velocity samples available + * @return True if there are more valid velocity combinations to iterate + */ + virtual bool hasMoreTwists() = 0; + + /** + * @brief Get the next velocity sample + * @return Next valid velocity combination + * + * Returns the next velocity sample that satisfies kinematic constraints. + * Should only be called when hasMoreTwists() returns true. + */ + virtual nav_2d_msgs::Twist2D nextTwist() = 0; +}; // class VelocityIterator + +} // namespace mkt_plugins + +#endif // MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_ 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 new file mode 100644 index 0000000..2416b17 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h @@ -0,0 +1,79 @@ +#ifndef MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_ +#define MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_ + +#include +#include +#include + +namespace mkt_plugins +{ + /** + * @class XYThetaIterator + * @brief Iterator that generates velocity samples for X, Y, and Theta dimensions + * + * This iterator combines three OneDVelocityIterator instances (one for each dimension) + * to generate valid velocity combinations that satisfy kinematic constraints. + */ + class XYThetaIterator : public VelocityIterator + { + public: + /** + * @brief Default constructor + * Initializes all iterators to nullptr + */ + XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} + + /** + * @brief Initialize the iterator with parameters and kinematics + * @param nh NodeHandle for loading sampling parameters (vx_samples, vy_samples, vtheta_samples) + * @param kinematics Shared pointer to kinematic parameters for validation + */ + void initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) override; + + /** + * @brief Start a new iteration with current velocity and time delta + * @param current_velocity Current velocity of the robot + * @param dt Time delta for velocity projection + * + * Creates new OneDVelocityIterator instances for X, Y, and Theta dimensions + * based on the current velocity and kinematic constraints. + */ + void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) override; + + /** + * @brief Check if there are more velocity samples to iterate + * @return True if there are more valid velocity combinations available + */ + bool hasMoreTwists() override; + + /** + * @brief Get the next velocity sample + * @return Next valid velocity combination (x, y, theta) + * + * Returns the next valid velocity that satisfies kinematic constraints. + * Automatically iterates to the next valid velocity if current is invalid. + */ + nav_2d_msgs::Twist2D nextTwist() override; + + protected: + /** + * @brief Check if the current velocity combination is valid + * @return True if the current velocity (x_it_, y_it_, th_it_) satisfies kinematic constraints + */ + virtual bool isValidVelocity(); + + /** + * @brief Iterate through velocity samples until a valid combination is found + * + * Increments the iterators (theta first, then y, then x) until a valid + * velocity combination is found or all combinations are exhausted. + */ + void iterateToValidVelocity(); + + int vx_samples_, vy_samples_, vtheta_samples_; + KinematicParameters::Ptr kinematics_; + std::shared_ptr x_it_, y_it_, th_it_; + }; +} // namespace mkt_plugins + +#endif // MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_ diff --git a/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp b/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp new file mode 100644 index 0000000..522c45c --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp @@ -0,0 +1,118 @@ +#include "mkt_plugins/equation_line.h" +#include +#include + +namespace mkt_plugins +{ + +bool LineGenerator::calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point) +{ + /** + * Phương trình đường thẳng có dạng: + * (y - yi)/(yj - yi) = (x - xi)/(xj - xi) + */ + Leq_.Gi = start_point; Leq_.Gj = end_point; + Leq_.a = (Leq_.Gj.y - Leq_.Gi.y); + Leq_.b = (Leq_.Gi.x - Leq_.Gj.x); + Leq_.c = (Leq_.Gj.x * Leq_.Gi.y - Leq_.Gi.x * Leq_.Gj.y); + + return Leq_.a != 0 || Leq_.b !=0; +} + +void LineGenerator::calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev) +{ + float pha = rev? M_PI : 0; + float Yaw_rad = pose.theta - pha; + +/* + * Tính tọa độ điểm R +*/ + float G_xr = float(pose.x); + float G_yr = float(pose.y); + +/* + * Tính tọa độ điểm H +*/ + present_pose_.Gxh = G_xr + Lm * cos(Yaw_rad); + present_pose_.Gyh = G_yr + Lm * sin(Yaw_rad); + +/** + * Phương trình đường thẳng (rh) có dạng: + * (y - yr)/(yh - yr) = (x - xr)/(xh - xr) +*/ + float A_rh = (present_pose_.Gyh - G_yr); + float B_rh = (G_xr - present_pose_.Gxh); + float C_rh = (present_pose_.Gxh * G_yr - G_xr * present_pose_.Gyh); +/* + * Phương trình đường thẳng vuông góc (y - yr)/(yh - yr) = (x - xr)/(xh - xr) + * và đi qua Gh có dạng: + * -B_rh*(x - Gxh) + Arh*(y - Gyh) = 0 +*/ + float A_he = -B_rh; + float B_he = A_rh; + float C_he = (-B_rh) * (-present_pose_.Gxh) + A_rh * (-present_pose_.Gyh); +/* + * Đường thẳng (he) giao với đường thẳng (ij) tại e + * vậy G_xe và G_ye là nghiệm của hệ phương trình 2 ẩn của 2 đường thẳng trên: +*/ + float D = A_he * Leq_.b - Leq_.a * B_he; + float Dx = (-C_he) * Leq_.b - (-Leq_.c) * B_he; + float Dy = A_he * (-Leq_.c) - Leq_.a * (-C_he); +/* + * Tính tọa độ điểm E +*/ + if(D == 0) { + if(Dx + Dy == 0) + { +/* + * Vì hai đường thẳng trùng nhau nên sai số giữa robot và đường đi là bằng 0 +*/ + present_pose_.error_line = 0; + } + else throw NoSolution(); + } + else + { + float G_xe = Dx/D; + float G_ye = Dy/D; +/* + * Tính khoảng cách từ điểm E đến đường thẳng (RH) +*/ + present_pose_.error_line = -(A_rh * G_xe + B_rh * G_ye + C_rh) / (sqrt(pow(A_rh, 2) + pow(B_rh, 2))); + } + // present_pose_.error_line = (Leq.a * present_pose_.Gxh + Leq.b * present_pose_.Gyh + Leq.c) / (sqrt(pow(Leq.a, 2) + pow(Leq.b, 2))); + present_pose_.distance_to_goal = sqrt(pow((Leq_.Gj.x - G_xr),2) + pow((Leq_.Gj.y - G_yr),2)); + present_pose_.distance_to_start = sqrt(pow((Leq_.Gi.x - G_xr),2) + pow((Leq_.Gi.y - G_yr),2)); + + +} + +double LineGenerator::calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose) +{ + /** + * Phương trình đường thẳng có dạng: + * (y - yi)/(yj - yi) = (x - xi)/(xj - xi) + */ + // LineEquation Leq; + // Leq.Gi = query_pose; Leq.Gj = goal_pose; + // Leq.a = (Leq.Gj.y - Leq.Gi.y); + // Leq.b = (Leq.Gi.x - Leq.Gj.x); + // Leq.c = (Leq.Gj.x * Leq.Gi.y - Leq.Gi.x * Leq.Gj.y); + + /** + * Phương trình đường thẳng vuông góc + */ + LineEquation Leq_l; + Leq_l.a = -Leq_.b; + Leq_l.b = Leq_.a; + Leq_l.c = Leq_.b * goal_pose.x - Leq_.a * goal_pose.y; + + present_pose_.distance_to_goal = sqrt(pow((Leq_.Gj.x - query_pose.x),2) + pow((Leq_.Gj.y - query_pose.y),2)); + present_pose_.distance_to_start = sqrt(pow((Leq_.Gi.x - query_pose.x),2) + pow((Leq_.Gi.y - query_pose.y),2)); + present_pose_.distace_start_goal = sqrt(pow((Leq_.Gj.x - Leq_.Gi.x),2) + pow((Leq_.Gj.y - Leq_.Gi.y),2)); + // ROS_INFO_THROTTLE(0.2, "Goal checker %f %f %f %f %f", Leq_.Gj.x, Leq_.Gj.y, Leq_.Gj.theta, query_pose.x, query_pose.y); + + return (Leq_l.a * query_pose.x + Leq_l.b * query_pose.y + Leq_l.c ) / (sqrt(pow(Leq_l.a, 2) + pow(Leq_l.b, 2))); +} + +} // namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp new file mode 100644 index 0000000..91fa33e --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -0,0 +1,106 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25) +{ +} + +void mkt_plugins::GoalChecker::initialize(const robot::NodeHandle &nh) +{ + nh_ = nh; + this->intParam(nh); + robot::log_info_at(__FILE__, __LINE__, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f", xy_goal_tolerance_, yaw_goal_tolerance_); + line_generator_ = std::make_shared(); +} + +void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh) +{ + if (!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) + { + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + } + if (!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) + { + yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + } + // ROS_INFO_THROTTLE(0.5, "%s", nh_.getNamespace().c_str()); + // ROS_INFO_THROTTLE(0.5, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_); +} + +void mkt_plugins::GoalChecker::reset() +{ + old_xy_goal_tolerance_ = 0; +} + +bool mkt_plugins::GoalChecker::isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose, + const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) +{ + this->intParam(nh_); + double dx = query_pose.pose.x - goal_pose.pose.x; + double dy = query_pose.pose.y - goal_pose.pose.y; + double xy_tolerance = std::abs(std::sqrt(dx * dx + dy * dy)); + + if (!path.poses.empty() && path.poses.size() > 2) + { + double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta); + double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy; + double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy; + if (xy_tolerance <= xy_goal_tolerance_ + 0.3) + { + double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; + // ROS_INFO_THROTTLE(0.1, "Goal checker 1 %f %f %f %f %f %f", tolerance, old_xy_goal_tolerance_, goal_pose.pose.theta, x, y, theta); + if( + (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) + // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4) + ) + { + robot::log_info_throttle(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_); + robot::log_info_throttle(0.1, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); + return true; + } + } + else + old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; + } + else + { + double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta); + double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy; + double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy; + if (xy_tolerance <= xy_goal_tolerance_ + 0.3) + { + double tolerance = fabs(x) > fabs(y) ? x : y; + if( + (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) + // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4) + ) + { + robot::log_info_throttle(0.1, "%f %f", fabs(cos(theta)), fabs(sin(theta))); + robot::log_info_throttle(0.1, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); + return true; + } + } + else + old_xy_goal_tolerance_ = fabs(x) > fabs(y) ? x : y; + } + + if (xy_tolerance <= xy_goal_tolerance_) + { + robot::log_info_at(__FILE__, __LINE__, "Goal checker 0 %f %f", xy_tolerance, xy_goal_tolerance_); + return true; + } + return false; +} + +score_algorithm::GoalChecker::Ptr mkt_plugins::GoalChecker::create() +{ + return std::make_shared(); +} + +BOOST_DLL_ALIAS(mkt_plugins::GoalChecker::create, GoalChecker) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp new file mode 100644 index 0000000..29d3638 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp @@ -0,0 +1,178 @@ +#include +#include +#include +#include +#include + + +using nav_2d_utils::moveDeprecatedParameter; + +namespace mkt_plugins +{ + + const double EPSILON = 1E-5; + + /** + * @brief Helper function to set the deceleration to the negative acceleration if it was not already set. + * @param nh NodeHandle + * @param dimension String representing the dimension, used in constructing parameter names + */ + void setDecelerationAsNeeded(const robot::NodeHandle &nh, const std::string dimension) + { + std::string decel_param = "decel_lim_" + dimension; + if (nh.hasParam(decel_param)) + return; + + std::string accel_param = "acc_lim_" + dimension; + if (!nh.hasParam(accel_param)) + return; + + double accel; + nh.getParam(accel_param, accel); + 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) + { + } + + void KinematicParameters::initialize(const robot::NodeHandle &nh) + { + // Special handling for renamed parameters + moveDeprecatedParameter(nh, "max_vel_theta", "max_rot_vel"); + moveDeprecatedParameter(nh, "min_speed_xy", "min_trans_vel"); + moveDeprecatedParameter(nh, "max_speed_xy", "max_trans_vel"); + moveDeprecatedParameter(nh, "min_speed_theta", "min_rot_vel"); + + // Set the deceleration parameters to negative the acceleration if the deceleration not already specified + setDecelerationAsNeeded(nh, "x"); + setDecelerationAsNeeded(nh, "y"); + setDecelerationAsNeeded(nh, "theta"); + + + xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1; + } + + void KinematicParameters::setDirect(int *xytheta_direct) + { + xytheta_direct_[0] = xytheta_direct[0]; + xytheta_direct_[1] = xytheta_direct[1]; + xytheta_direct_[2] = xytheta_direct[2]; + } + + bool KinematicParameters::isValidSpeed(double x, double y, double theta) + { + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) + return false; + if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) + return false; + if (vmag_sq == 0.0 && theta == 0.0) + return false; + return true; + } + + int *KinematicParameters::getDirect() { return xytheta_direct_; } + + double KinematicParameters::getMinX() { return min_vel_x_; } + + double KinematicParameters::getMaxX() { return max_vel_x_; } + + double KinematicParameters::getAccX() { return acc_lim_x_; } + + double KinematicParameters::getDecelX() { return decel_lim_x_; } + + double KinematicParameters::getMinY() { return min_vel_y_; } + + double KinematicParameters::getMaxY() { return max_vel_y_; } + + double KinematicParameters::getAccY() { return acc_lim_y_; } + + double KinematicParameters::getDecelY() { return decel_lim_y_; } + + double KinematicParameters::getMinSpeedXY() { return min_speed_xy_; } + + double KinematicParameters::getMinTheta() { return -max_vel_theta_; } + + double KinematicParameters::getMaxTheta() { return max_vel_theta_; } + + double KinematicParameters::getAccTheta() { return acc_lim_theta_; } + + double KinematicParameters::getDecelTheta() { return decel_lim_theta_; } + + double KinematicParameters::getMinSpeedTheta() { return min_speed_theta_; } + + void KinematicParameters::setMinX(double value) + { + min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_; + // ROS_INFO_THROTTLE(10.0, "vx_min %f %f %f", value , original_min_vel_x_, min_vel_x_); + } + + void KinematicParameters::setMaxX(double value) + { + max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_; + // ROS_INFO_THROTTLE(10, "vx %f %f %f", value , original_max_vel_x_, max_vel_x_); + } + + void KinematicParameters::setAccX(double value) + { + acc_lim_x_ = fabs(value) <= fabs(original_acc_lim_x_) + EPSILON ? value : acc_lim_x_; + } + + void KinematicParameters::setDecelX(double value) + { + decel_lim_x_ = fabs(value) <= fabs(original_decel_lim_x_) + EPSILON ? value : decel_lim_x_; + } + + void KinematicParameters::setMinY(double value) + { + min_vel_y_ = fabs(value) <= fabs(original_min_vel_y_) + EPSILON ? value : min_vel_y_; + } + + void KinematicParameters::setMaxY(double value) + { + max_vel_y_ = fabs(value) <= fabs(original_max_vel_y_) + EPSILON ? value : max_vel_y_; + } + + void KinematicParameters::setAccY(double value) + { + acc_lim_y_ = fabs(value) <= fabs(original_acc_lim_y_) + EPSILON ? value : acc_lim_y_; + } + + void KinematicParameters::setDecelY(double value) + { + decel_lim_y_ = fabs(value) <= fabs(original_decel_lim_y_) + EPSILON ? value : decel_lim_y_; + } + + void KinematicParameters::setMinSpeedXY(double value) + { + min_speed_xy_ = fabs(value) <= fabs(original_min_speed_xy_) + EPSILON ? value : min_speed_xy_; + } + + void KinematicParameters::setMaxTheta(double value) + { + max_vel_theta_ = fabs(value) <= fabs(original_max_vel_theta_) + EPSILON? value : original_max_vel_theta_; + // ROS_WARN_THROTTLE(10, "vtheta %f %f %f", value, original_max_vel_theta_, max_vel_theta_); + } + + void KinematicParameters::setAccTheta(double value) + { + acc_lim_theta_ = fabs(value) <= fabs(original_acc_lim_theta_) + EPSILON ? value : acc_lim_theta_; + } + + void KinematicParameters::setDecelTheta(double value) + { + decel_lim_theta_ = fabs(value) <= fabs(original_decel_lim_theta_) + EPSILON ? value : decel_lim_theta_; + } + + void KinematicParameters::setMinSpeedTheta(double value) + { + min_speed_theta_ = fabs(value) <= fabs(original_min_speed_theta_) + EPSILON ? value : min_speed_theta_; + } +} // namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp new file mode 100644 index 0000000..ac38e00 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -0,0 +1,47 @@ +#include +#include +#include +#include +#include +#include + +namespace mkt_plugins +{ + +void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) +{ + StandardTrajectoryGenerator::initialize(nh); + if (nh.hasParam("sim_period")) + { + nh.getParam("sim_period", acceleration_time_); + } + else + { + double controller_frequency = nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0); + if (controller_frequency > 0) + { + acceleration_time_ = 1.0 / controller_frequency; + } + else + { + robot::log_warning_at(__FILE__, __LINE__, "A controller_frequency less than or equal to 0 has been set. " + "Ignoring the parameter, assuming a rate of 20Hz"); + acceleration_time_ = 0.05; + } + } +} + +void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) +{ + // Limit our search space to just those within the limited acceleration_time + velocity_iterator_->startNewIteration(current_velocity, acceleration_time_); +} + + score_algorithm::TrajectoryGenerator::Ptr LimitedAccelGenerator::create() + { + return std::make_shared(); + } + +} // namespace mkt_plugins + +BOOST_DLL_ALIAS(mkt_plugins::LimitedAccelGenerator::create, LimitedAccelGenerator) diff --git a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp new file mode 100644 index 0000000..4363f0c --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp @@ -0,0 +1,76 @@ +#include +#include +#include +#include +#include + +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) +{ +} + +void SimpleGoalChecker::initialize(const robot::NodeHandle& nh) +{ + nh_ = nh; + this->intParam(nh); + robot::log_info_at(__FILE__, __LINE__, "[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_); +} + +void SimpleGoalChecker::intParam(const robot::NodeHandle& nh) +{ + if(!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) + { + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + } + if(!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) + { + yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + } + if(nh.param("stateful", stateful_, true)) + { + stateful_ = nav_2d_utils::searchAndGetParam(nh, "stateful", true); + } + robot::log_info_throttle(1.0,"[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_); + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; +} + +void SimpleGoalChecker::reset() +{ + check_xy_ = true; +} + +bool SimpleGoalChecker::isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose, + const nav_2d_msgs::Path2D& path, const nav_2d_msgs::Twist2D& velocity) +{ + this->intParam(nh_); + if (check_xy_) + { + double dx = query_pose.pose.x - goal_pose.pose.x, + dy = query_pose.pose.y - goal_pose.pose.y; + if (dx * dx + dy * dy > 0.25) + { + return false; + } + // We are within the window + // If we are stateful, change the state. + if (stateful_) + { + check_xy_ = false; + } + } + double dyaw = angles::shortest_angular_distance(query_pose.pose.theta, goal_pose.pose.theta); + // robot::log_info_throttle(1.0, "dyaw %f yaw_goal_tolerance_ %f", fabs(dyaw), yaw_goal_tolerance_); + return fabs(dyaw) < yaw_goal_tolerance_; +} + +} // namespace mkt_plugins + +score_algorithm::GoalChecker::Ptr mkt_plugins::SimpleGoalChecker::create() +{ + return std::make_shared(); +} + +BOOST_DLL_ALIAS(mkt_plugins::SimpleGoalChecker::create, SimpleGoalChecker) diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp new file mode 100644 index 0000000..0af5486 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp @@ -0,0 +1,264 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using nav_2d_utils::loadParameterWithDeprecation; + +namespace mkt_plugins +{ + void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh) + { + nh_kinematics_ = nh; + kinematics_ = std::make_shared(); + kinematics_->initialize(nh); + initializeIterator(nh); + + nh.param("sim_time", sim_time_, 1.7); + + nh.param("include_last_point", include_last_point_, true); + /* + * If discretize_by_time, then sim_granularity represents the amount of time that should be between + * two successive points on the trajectory. + * + * If discretize_by_time is false, then sim_granularity is the maximum amount of distance between + * two successive points on the trajectory, and angular_sim_granularity is the maximum amount of + * angular distance between two successive points. + */ + nh.param("discretize_by_time", discretize_by_time_, false); + if (discretize_by_time_) + { + time_granularity_ = loadParameterWithDeprecation(nh, "time_granularity", "sim_granularity", 0.025); + } + else + { + linear_granularity_ = loadParameterWithDeprecation(nh, "linear_granularity", "sim_granularity", 0.025); + angular_granularity_ = loadParameterWithDeprecation(nh, "angular_granularity", "angular_sim_granularity", 0.1); + } + } + + robot::NodeHandle StandardTrajectoryGenerator::getNodeHandle() + { + return nh_kinematics_; + } + + void StandardTrajectoryGenerator::initializeIterator(const robot::NodeHandle &nh) + { + velocity_iterator_ = std::make_shared(); + velocity_iterator_->initialize(nh, kinematics_); + } + + void StandardTrajectoryGenerator::setDirect(int *direct) + { + kinematics_->setDirect(direct); + } + + bool StandardTrajectoryGenerator::setTwistLinear(geometry_msgs::Vector3 linear) + { + try + { + // ROS_INFO("Get linear %f ", linear.x); + if (linear.x > 0) + kinematics_->setMaxX(linear.x); + else if (linear.x < 0) + kinematics_->setMinX(linear.x); + else + { + kinematics_->setMinX(linear.x); + kinematics_->setMaxX(linear.x); + } + + if (linear.y > 0) + kinematics_->setMaxY(linear.y); + else if (linear.y < 0) + kinematics_->setMinY(linear.y); + else + { + kinematics_->setMinY(linear.y); + kinematics_->setMaxY(linear.y); + } + } + catch (const std::exception &e) + { + throw std::exception(e); + return false; + } + + return true; + } + + geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct) + { + geometry_msgs::Vector3 linear; + try + { + if (direct) + { + linear.x = kinematics_->getMaxX(); + linear.y = kinematics_->getMaxY(); + } + else + { + linear.x = kinematics_->getMinX(); + linear.y = kinematics_->getMinY(); + } + } + catch (const std::exception &e) + { + throw std::exception(e); + } + return linear; + } + + bool StandardTrajectoryGenerator::setTwistAngular(geometry_msgs::Vector3 angular) + { + try + { + kinematics_->setMaxTheta(angular.z); + } + catch (const std::exception &e) + { + throw std::exception(e); + return false; + } + return true; + } + + geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct) + { + geometry_msgs::Vector3 angular; + try + { + if (direct) + { + angular.z = kinematics_->getMaxTheta(); + } + else + { + angular.z = kinematics_->getMinTheta(); + } + } + catch (const std::exception &e) + { + throw std::exception(e); + } + return angular; + } + + void StandardTrajectoryGenerator::startNewIteration(const nav_2d_msgs::Twist2D ¤t_velocity) + { + velocity_iterator_->startNewIteration(current_velocity, sim_time_); + } + + bool StandardTrajectoryGenerator::hasMoreTwists() + { + return velocity_iterator_->hasMoreTwists(); + } + + nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist() + { + return velocity_iterator_->nextTwist(); + } + + std::vector StandardTrajectoryGenerator::getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel) + { + std::vector steps; + if (discretize_by_time_) + { + steps.resize(ceil(sim_time_ / time_granularity_)); + } + else // discretize by distance + { + double vmag = hypot(cmd_vel.x, cmd_vel.y); + + // the distance the robot would travel in sim_time if it did not change velocity + double projected_linear_distance = vmag * sim_time_; + + // the angle the robot would rotate in sim_time + double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_; + + // Pick the maximum of the two + int num_steps = ceil(std::max(projected_linear_distance / linear_granularity_, + projected_angular_distance / angular_granularity_)); + steps.resize(num_steps); + } + if (steps.size() == 0) + { + steps.resize(1); + } + std::fill(steps.begin(), steps.end(), sim_time_ / steps.size()); + return steps; + } + + mkt_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose, + const nav_2d_msgs::Path2D &transformed_plan, + const nav_2d_msgs::Twist2D &start_vel, + const nav_2d_msgs::Twist2D &cmd_vel) + { + mkt_msgs::Trajectory2D traj; + traj.velocity = cmd_vel; + + // simulate the trajectory + geometry_msgs::Pose2D pose = start_pose.pose; + nav_2d_msgs::Twist2D vel = start_vel; + double running_time = 0.0; + std::vector steps = getTimeSteps(cmd_vel); + + for (double dt : steps) + { + traj.poses.push_back(pose); + traj.time_offsets.push_back(robot::Duration(running_time)); + // calculate velocities + vel = computeNewVelocity(cmd_vel, vel, dt); + + // update the position of the robot using the velocities passed in + pose = computeNewPosition(pose, vel, dt); + running_time += dt; + } // end for simulation steps + + if (include_last_point_) + { + traj.poses.push_back(pose); + traj.time_offsets.push_back(robot::Duration(running_time)); + } + + return traj; + } + + /** + * change vel using acceleration limits to converge towards sample_target-vel + */ + nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D &cmd_vel, + const nav_2d_msgs::Twist2D &start_vel, const double dt) + { + nav_2d_msgs::Twist2D new_vel; + new_vel.x = projectVelocity(start_vel.x, kinematics_->getAccX(), kinematics_->getDecelX(), dt, cmd_vel.x); + new_vel.y = projectVelocity(start_vel.y, kinematics_->getAccY(), kinematics_->getDecelY(), dt, cmd_vel.y); + new_vel.theta = projectVelocity(start_vel.theta, kinematics_->getAccTheta(), kinematics_->getDecelTheta(), + dt, cmd_vel.theta); + return new_vel; + } + + geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose, + const nav_2d_msgs::Twist2D &vel, const double dt) + { + geometry_msgs::Pose2D new_pose; + new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt; + new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt; + new_pose.theta = start_pose.theta + vel.theta * dt; + return new_pose; + } + + score_algorithm::TrajectoryGenerator::Ptr StandardTrajectoryGenerator::create() + { + return std::make_shared(); + } +} // namespace mkt_plugins + +BOOST_DLL_ALIAS(mkt_plugins::StandardTrajectoryGenerator::create, StandardTrajectoryGenerator) diff --git a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp new file mode 100644 index 0000000..9bec193 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -0,0 +1,73 @@ +#include +#include +#include + +namespace mkt_plugins +{ +void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) +{ + kinematics_ = kinematics; + nh.param("vx_samples", vx_samples_, 20); + nh.param("vy_samples", vy_samples_, 5); + vtheta_samples_ = nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples", 20); +} + +void XYThetaIterator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) +{ + x_it_ = std::make_shared(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(), + kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_); + x_it_->name_ = std::string("x_it_"); + + y_it_ = std::make_shared(current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(), + kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_); + + y_it_->name_ = std::string("y_it_"); + + th_it_ = std::make_shared(current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(), + kinematics_->getAccTheta(), kinematics_->getDecelTheta(), dt, vtheta_samples_); + + th_it_->name_ = std::string("th_it_"); +} + +bool XYThetaIterator::hasMoreTwists() +{ + return x_it_ && !x_it_->isFinished(); +} + +nav_2d_msgs::Twist2D XYThetaIterator::nextTwist() +{ + nav_2d_msgs::Twist2D velocity; + velocity.x = x_it_->getVelocity(); + velocity.y = y_it_->getVelocity(); + velocity.theta = th_it_->getVelocity(); + iterateToValidVelocity(); + return velocity; +} + +bool XYThetaIterator::isValidVelocity() +{ + return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); +} + +void XYThetaIterator::iterateToValidVelocity() +{ + bool valid = false; + while (!valid && hasMoreTwists()) + { + ++(*th_it_); + if (th_it_->isFinished()) + { + th_it_->reset(); + ++(*y_it_); + if (y_it_->isFinished()) + { + y_it_->reset(); + ++(*x_it_); + } + } + valid = isValidVelocity(); + } +} + +} // namespace mkt_plugins + diff --git a/src/Algorithms/Packages/global_planners/dock_planner b/src/Algorithms/Packages/global_planners/dock_planner new file mode 160000 index 0000000..c5c5514 --- /dev/null +++ b/src/Algorithms/Packages/global_planners/dock_planner @@ -0,0 +1 @@ +Subproject commit c5c55147b754158e16278e73710ebbee76e6290c diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt index f660ff8..8e6c322 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt @@ -42,7 +42,7 @@ add_library(two_points_planner SHARED ${SOURCES} ${HEADERS}) # Link libraries target_link_libraries(two_points_planner PUBLIC ${PACKAGES_DIR} - PUBLIC robot::node_handle robot::console + PUBLIC robot_cpp PRIVATE Boost::boost ) 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 0af764c..8709d62 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 @@ -1,3 +1,4 @@ +#include #include #include #include @@ -25,7 +26,7 @@ namespace two_points_planner if (!initialized_) { robot::NodeHandle nh_priv_("~/" + name); - printf("TwoPointsPlanner: Name is %s\n", name.c_str()); + robot::log_info("TwoPointsPlanner: Name is %s", name.c_str()); int lethal_obstacle; nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20); @@ -37,17 +38,16 @@ namespace two_points_planner name_ = name; costmap_robot_ = costmap_robot; - // Bug if(!costmap_robot_ || !costmap_robot_->getCostmap()) { - robot::printf_yellow("[%s:%d]TwoPointsPlanner Initialized failed\n", __FILE__, __LINE__); + robot::log_warning("[%s:%d]TwoPointsPlanner Initialized failed", __FILE__, __LINE__); return false; } current_env_width_ = costmap_robot_->getCostmap()->getSizeInCellsX(); current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY(); footprint_ = costmap_robot_->getRobotFootprint(); - printf("TwoPointsPlanner Initialized successfully\n"); + robot::log_info("TwoPointsPlanner Initialized successfully"); initialized_ = true; return true; } @@ -57,24 +57,25 @@ namespace two_points_planner { unsigned char result = 0; - // Bug + if (!costmap_robot_) { std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl; return 0; } - // check if the costmap has an inflation layer - for (std::vector>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin(); - layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end(); - ++layer) - { - boost::shared_ptr inflation_layer = boost::dynamic_pointer_cast(*layer); - if (!inflation_layer) - continue; + // Bug + // // check if the costmap has an inflation layer + // for (std::vector>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin(); + // layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end(); + // ++layer) + // { + // boost::shared_ptr inflation_layer = boost::dynamic_pointer_cast(*layer); + // if (!inflation_layer) + // continue; - result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution())); - } + // result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution())); + // } return result; } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt new file mode 100644 index 0000000..eeffc64 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt @@ -0,0 +1,134 @@ +cmake_minimum_required(VERSION 3.10) +project(pnkx_local_planner VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Find system dependencies +find_package(Boost REQUIRED COMPONENTS system) + +# Flags biên dịch +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Dependencies packages (internal libraries) +set(PACKAGES_DIR + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + visualization_msgs + nav_2d_utils + nav_core2 + mkt_msgs + score_algorithm + costmap_2d + tf3 + tf3_geometry_msgs + tf3_sensor_msgs + robot_cpp + angles +) + +# Tạo thư viện utils trước +add_library(${PROJECT_NAME}_utils SHARED + src/transforms.cpp +) + +# Link libraries cho utils +target_link_libraries(${PROJECT_NAME}_utils + PUBLIC ${PACKAGES_DIR} + PRIVATE Boost::boost +) + +# Set include directories cho utils +target_include_directories(${PROJECT_NAME}_utils + PUBLIC + $ + $ +) + +# Tạo thư viện chính +add_library(${PROJECT_NAME} SHARED + src/pnkx_local_planner.cpp + # src/pnkx_docking_local_planner.cpp + src/pnkx_go_straight_local_planner.cpp + src/pnkx_rotate_local_planner.cpp +) + +# Link libraries cho thư viện chính +target_link_libraries(${PROJECT_NAME} + PUBLIC ${PROJECT_NAME}_utils + PUBLIC ${PACKAGES_DIR} + PRIVATE Boost::boost +) + +# Set include directories cho thư viện chính +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +# Compile options +target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-terminate") + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +############# +## Install ## +############# + +# Cài đặt header files +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Cài đặt libraries +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils + EXPORT ${PROJECT_NAME}-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +# Export targets +install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + DESTINATION lib/cmake/${PROJECT_NAME} +) + +# Cài đặt plugins.xml nếu có +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml) + install(FILES + plugins.xml + DESTINATION share/${PROJECT_NAME} + ) +endif() + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") +message(STATUS "=================================") diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h new file mode 100644 index 0000000..1f33379 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h @@ -0,0 +1,181 @@ +#ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ +#define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ + +#include +#include + +namespace pnkx_local_planner +{ + + /** + * @class PNKXDockingLocalPlanner + * @brief Plugin-based flexible local_planner + */ + class PNKXDockingLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner + { + public: + /** + * @brief Constructor that brings up pluginlib loaders + */ + PNKXDockingLocalPlanner(); + + virtual ~PNKXDockingLocalPlanner(); + + /** + * @brief nav_core2 initialization + * @param name Namespace for this planner + * @param tf TFListener pointer + * @param costmap_robot Costmap pointer + */ + void initialize(robot::NodeHandle &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override; + + /** + * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity + * + * It is presumed that the global plan is already set. + * + * This is mostly a wrapper for the protected computeVelocityCommands + * function which has additional debugging info. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @return The best command for the robot to drive + */ + virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. + * + * The pose that it checks against is the last pose in the current global plan. + * The calculation is delegated to the goal_checker plugin. + * + * @param pose Current pose + * @param velocity Current velocity + * @return True if the robot should be considered as having reached the goal. + */ + virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Create a new instance of the PNKXDockingLocalPlanner + * @return A shared pointer to the new instance + */ + static nav_core2::LocalPlanner::Ptr create(); + + protected: + /** + * @brief nav_core2 initialization other + */ + void initializeOthers() override; + + /** + * @brief get docking maker + */ + void getMaker(); + + /** + * @brief nav_core2 reset other + */ + void reset() override; + + /** + * @brief get parameters + * @param nh NodeHandle to read parameters from + */ + void getParams(robot::NodeHandle &nh) override; + + /** + * @brief Helper method for preparing for the core scoring algorithm + * + * Runs the prepare method on all the critics with freshly transformed data + */ + virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Iterate through all the twists and find the best one + */ + virtual nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + private: + class DockingPlanner + { + public: + DockingPlanner(const std::string &name); + ~DockingPlanner(); + + /** + * @brief initialization + * @param name Namespace for maker + * @param tf TFListener pointer + * @param costmap_robot Costmap pointer + */ + void initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, + score_algorithm::TrajectoryGenerator::Ptr traj_generator); + + bool getMakerGoal(nav_2d_msgs::Pose2DStamped &maker_goal); + + bool geLocalGoal(nav_2d_msgs::Pose2DStamped &local_goal); + + bool getLocalPath(const nav_2d_msgs::Pose2DStamped &local_pose, + const nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path); + + bool initialized_; + bool is_detected_; + bool is_goal_reached_; + bool following_; + bool allow_rotate_; + + double xy_goal_tolerance_; + double yaw_goal_tolerance_; + double min_lookahead_dist_; + double max_lookahead_dist_; + double lookahead_time_; + double angle_threshold_; + + std::string name_; + std::string docking_planner_name_; + std::string docking_nav_name_; + + nav_core::BaseGlobalPlanner::Ptr docking_planner_; + score_algorithm::ScoreAlgorithm::Ptr docking_nav_; + + geometry_msgs::Vector3 linear_; + geometry_msgs::Vector3 angular_; + + private: + /** + * @brief Callback wall timer for detect timeout + */ + virtual void detectedTimeOutCb(const ::robot::WallTimerEvent &timer_event); + + /** + * @brief Callback wall timer for delay + */ + virtual void delayCb(const ::robot::WallTimerEvent &timer_event); + + robot::NodeHandle nh_, nh_priv_; + TFListenerPtr tf_; + costmap_2d::Costmap2DROBOT *costmap_robot_; + score_algorithm::TrajectoryGenerator::Ptr traj_generator_; + std::function docking_planner_loader_; + std::function docking_nav_loader_; + + robot::WallTimer detected_timeout_wt_, delayed_wt_; + bool delayed_; + bool detected_timeout_; + std::string robot_base_frame_, maker_goal_frame_; + }; + + bool start_docking_; + double original_xy_goal_tolerance_, original_yaw_goal_tolerance_; + XmlRpc::XmlRpcValue original_papams_; + std::vector dkpl_; + + bool dockingHanlde(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity); + + }; // PNKXDockingLocalPlanner + +} // namespace pnkx_local_planner + +#endif // _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ \ No newline at end of file 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 new file mode 100644 index 0000000..1bc39ab --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h @@ -0,0 +1,86 @@ +#ifndef _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_ +#define _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_ + +#include + +namespace pnkx_local_planner +{ + /** + * @class PNKXGoStraightLocalPlanner + * @brief Plugin-based flexible local_planner + */ + class PNKXGoStraightLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner + { + public: + /** + * @brief Constructor that brings up pluginlib loaders + */ + PNKXGoStraightLocalPlanner(); + + virtual ~PNKXGoStraightLocalPlanner(); + + /** + * @brief nav_core2 initialization + * @param name Namespace for this planner + * @param tf TFListener pointer + * @param costmap_robot Costmap pointer + */ + void initialize(robot::NodeHandle &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override; + + /** + * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity + * + * It is presumed that the global plan is already set. + * + * This is mostly a wrapper for the protected computeVelocityCommands + * function which has additional debugging info. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @return The best command for the robot to drive + */ + nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. + * + * The pose that it checks against is the last pose in the current global plan. + * The calculation is delegated to the goal_checker plugin. + * + * @param pose Current pose + * @param velocity Current velocity + * @return True if the robot should be considered as having reached the goal. + */ + bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Create a new instance of the PNKXGoStraightLocalPlanner + * @return A shared pointer to the new instance + */ + static nav_core2::LocalPlanner::Ptr create(); + + protected: + /** + * @brief nav_core2 reset other + */ + virtual void reset() override;; + + /** + * @brief Helper method for preparing for the core scoring algorithm + * + * Runs the prepare method on all the critics with freshly transformed data + */ + void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Iterate through all the twists and find the best one + */ + nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + bool is_ready_; + }; + +} // namespace pnkx_local_planner +#endif // _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_ \ No newline at end of file diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h new file mode 100644 index 0000000..e9fb0cc --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h @@ -0,0 +1,200 @@ +#ifndef _PNKX_LOCAL_PLANNER_H_INCLUDED_ +#define _PNKX_LOCAL_PLANNER_H_INCLUDED_ + +#include +#include +#include +#include +#include +#include +#include + +namespace pnkx_local_planner +{ + + /** + * @class PNKXLocalPlanner + * @brief Plugin-based flexible local_planner + */ + class PNKXLocalPlanner : public nav_core2::LocalPlanner + { + public: + /** + * @brief Constructor that brings up pluginlib loaders + */ + PNKXLocalPlanner(); + + virtual ~PNKXLocalPlanner(); + + /** + * @brief nav_core2 initialization + * @param name Namespace for this planner + * @param tf TFListener pointer + * @param costmap_robot Costmap pointer + */ + void initialize(robot::NodeHandle &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override; + + /** + * @brief nav_core2 setGoalPose - Sets the global goal pose + * @param goal_pose The Goal Pose + */ + void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) override; + + /** + * @brief nav_core2 setPlan - Sets the global plan + * @param path The global plan + */ + void setPlan(const nav_2d_msgs::Path2D &path) override; + + /** + * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity + * + * It is presumed that the global plan is already set. + * + * This is mostly a wrapper for the protected computeVelocityCommands + * function which has additional debugging info. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @return The best command for the robot to drive + */ + nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief set velocity for x, y asix of the robot + * @param linear the velocity command + * @return True if set is done, false otherwise + */ + bool setTwistLinear(geometry_msgs::Vector3 linear) override; + + /** + * @brief get velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override; + + /** + * @brief Set velocity theta for z asix of the robot + * @param angular the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override; + + /** + * @brief Get velocity theta for z asix of the robot + * @param direct The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override; + + /** + * @brief Check if the kinematic parameters are currently locked + * @return True if the kinematic parameters are locked, false otherwise + * + * This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL. + */ + virtual bool islock() override; + + /** + * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. + * + * The pose that it checks against is the last pose in the current global plan. + * The calculation is delegated to the goal_checker plugin. + * + * @param pose Current pose + * @param velocity Current velocity + * @return True if the robot should be considered as having reached the goal. + */ + bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Create a new PNKXLocalPlanner + * @return A shared pointer to the new PNKXLocalPlanner + */ + static nav_core2::LocalPlanner::Ptr create(); + + protected: + /** + * @brief Lock the kinematic parameters to prevent modifications + * + * This function sets the kinematic parameters to a locked state, ensuring that values such as direction (xytheta_direct), velocities, or accelerations cannot be changed until unlocked. Locking during critical operations like in-place rotation may stabilize motion commands but could cause issues if the lock persists too long, preventing necessary updates to odometry data used by AMCL. + */ + virtual void lock(); + + /** + * @brief Unlock the kinematic parameters to allow modifications + * + * This function releases the lock on kinematic parameters, allowing changes to values such as direction, velocities, or accelerations. If the parameters are unlocked prematurely during in-place rotation, sudden changes to motion parameters (e.g., theta direction) could lead to odometry discontinuities, contributing to localization jumps in AMCL. + */ + virtual void unlock(); + /** + * @brief nav_core2 initialization other + */ + virtual void initializeOthers(); + + /** + * @brief nav_core2 reset other + */ + virtual void reset(); + + /** + * @brief Get parameters + * @param nh NodeHandle to read parameters from + */ + virtual void getParams(robot::NodeHandle &nh); + + /** + * @brief Helper method for preparing for the core scoring algorithm + * + * Runs the prepare method on all the critics with freshly transformed data + */ + virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity); + + /** + * @brief Iterate through all the twists and find the best one + */ + virtual nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity); + + /** + * @brief Helper method to transform a given pose to the local costmap frame. + */ + nav_2d_msgs::Pose2DStamped transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose); + + // Plugin handling + std::function traj_gen_loader_; + score_algorithm::TrajectoryGenerator::Ptr traj_generator_; + + std::function nav_algorithm_loader_; + score_algorithm::ScoreAlgorithm::Ptr nav_algorithm_; + std::function rotate_algorithm_loader_; + score_algorithm::ScoreAlgorithm::Ptr rotate_algorithm_; + + std::function goal_checker_loader_; + score_algorithm::GoalChecker::Ptr goal_checker_; + + bool initialized_; + TFListenerPtr tf_; + std::string name_; + robot::NodeHandle parent_, planner_nh_; + nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan + nav_2d_msgs::Path2D transformed_plan_; + nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose + + costmap_2d::Costmap2D *costmap_; + costmap_2d::Costmap2DROBOT *costmap_robot_; + nav_grid::NavGridInfo info_; + boost::recursive_mutex configuration_mutex_; + bool update_costmap_before_planning_; + + bool ret_angle_, ret_nav_; + double yaw_goal_tolerance_, xy_goal_tolerance_; + bool lock_; + + }; // PNKXLocalPlanner + +} // namespace pnkx_local_planner + +#endif // _PNKX_LOCAL_PLANNER_H_INCLUDED_ \ No newline at end of file diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h new file mode 100644 index 0000000..400b025 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h @@ -0,0 +1,85 @@ +#ifndef _PNKX_ROTATE_LOCAL_PLANNER_H_INCLUDED_ +#define _PNKX_ROTATE_LOCAL_PLANNER_H_INCLUDED_ + +#include + +namespace pnkx_local_planner +{ + /** + * @class PNKXRotateLocalPlanner + * @brief Plugin-based flexible local_planner + */ + class PNKXRotateLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner + { + public: + /** + * @brief Constructor that brings up pluginlib loaders + */ + PNKXRotateLocalPlanner(); + + virtual ~PNKXRotateLocalPlanner(); + + /** + * @brief nav_core2 initialization + * @param name Namespace for this planner + * @param tf TFListener pointer + * @param costmap_robot Costmap pointer + */ + void initialize(robot::NodeHandle &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override; + + /** + * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity + * + * It is presumed that the global plan is already set. + * + * This is mostly a wrapper for the protected computeVelocityCommands + * function which has additional debugging info. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @return The best command for the robot to drive + */ + nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. + * + * The pose that it checks against is the last pose in the current global plan. + * The calculation is delegated to the goal_checker plugin. + * + * @param pose Current pose + * @param velocity Current velocity + * @return True if the robot should be considered as having reached the goal. + */ + bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Create a new instance of the PNKXRotateLocalPlanner + * @return A shared pointer to the new instance + */ + static nav_core2::LocalPlanner::Ptr create(); + + protected: + /** + * @brief nav_core2 reset other + */ + virtual void reset() override; + + /** + * @brief Helper method for preparing for the core scoring algorithm + * + * Runs the prepare method on all the critics with freshly transformed data + */ + void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Iterate through all the twists and find the best one + */ + nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + }; +} + +#endif // _PNKX_ROTATE_LOCAL_PLANNER_H_INCLUDED_ diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h new file mode 100644 index 0000000..d2ec41a --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h @@ -0,0 +1,50 @@ +#ifndef _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__ +#define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +namespace pnkx_local_planner +{ + bool getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, + nav_2d_msgs::Pose2DStamped &global_pose, double transform_tolerance = 2.0); + + bool transformGlobalPose(TFListenerPtr tf, const std::string &global_frame, + const nav_2d_msgs::Pose2DStamped &stamped_pose, nav_2d_msgs::Pose2DStamped &newer_pose); + + double getSquareDistance(const geometry_msgs::Pose2D &pose_a, const geometry_msgs::Pose2D &pose_b); + + /** + * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). + * + * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h + * such that the index of the current goal pose is returned as well as + * the transformation between the global plan and the planning frame. + * @param tf A reference to a tf buffer + * @param global_plan The plan to be transformed + * @param pose The pose of the robot + * @param costmap A reference to the costmap being used so the window size for transforming can be computed + * @param global_frame The frame to transform the plan to + * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!] + * @param[out] transformed_plan Populated with the transformed plan + * @return \c true if the global plan is transformed, \c false otherwise + */ + bool transformGlobalPlan( + TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, + const costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length, + nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false); + +} + +#endif \ No newline at end of file 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 new file mode 100644 index 0000000..ff4eeac --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -0,0 +1,772 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner() + : start_docking_(false) +{ +} + +pnkx_local_planner::PNKXDockingLocalPlanner::~PNKXDockingLocalPlanner() +{ + if (traj_generator_) + traj_generator_.reset(); + + if (nav_algorithm_) + nav_algorithm_.reset(); + + if (rotate_algorithm_) + rotate_algorithm_.reset(); + + if (goal_checker_) + goal_checker_.reset(); +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) +{ + if (!initialized_) + { + tf_ = tf; + name_ = name; + costmap_robot_ = costmap_robot; + costmap_ = costmap_robot_->getCostmap(); + + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + + nh_ = robot::NodeHandle("~"); + parent_ = parent; + planner_nh_ = robot::NodeHandle(parent_, name); + robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s", planner_nh_.getNamespace().c_str()); + + this->getParams(planner_nh_); + + std::string traj_generator_name; + planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator")); + robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + traj_gen_loader_ = boost::dll::import_alias( + path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); + traj_generator_ = traj_gen_loader_(); + if (!traj_generator_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); + exit(1); + } + robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name); + traj_generator_->initialize(nh_traj_gen); + robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); + exit(1); + } + + std::string algorithm_nav_name; + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + algorithm_loader_ = boost::dll::import_alias( + path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); + nav_algorithm_ = algorithm_loader_(); + if (!nav_algorithm_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str()); + exit(1); + } + nav_algorithm_->initialize(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); + exit(1); + } + + std::string algorithm_rotate_name; + planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + algorithm_loader_ = boost::dll::import_alias( + path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations); + rotate_algorithm_ = algorithm_loader_(); + if (!rotate_algorithm_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); + exit(1); + } + rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); + robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); + exit(1); + } + + std::string goal_checker_name; + planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + boost::dll::import_alias( + path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); + goal_checker_ = goal_checker_loader_(); + if (!goal_checker_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); + exit(1); + } + goal_checker_->initialize(nh_); + robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what()); + exit(1); + } + this->initializeOthers(); + this->getMaker(); + robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str()); + initialized_ = true; + } +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker() +{ + std::string maker_name, maker_sources; + nh_.param("maker_name", maker_name, std::string("")); + nh_.param("maker_sources", maker_sources, std::string("")); + std::stringstream ss(maker_sources); + std::string source; + + while (ss >> source) + { + if (maker_name == source) + { + robot::NodeHandle source_node(nh_, source); + + if (source_node.hasParam("plugins")) + { + XmlRpc::XmlRpcValue plugins; + source_node.getParam("plugins", plugins); + + if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (int i = 0; i < plugins.size(); ++i) + { + if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct) + { + std::stringstream name; + name << source << "/" << static_cast(plugins[i]["name"]); + std::string docking_planner_name = static_cast(plugins[i]["docking_planner"]); + std::string docking_nav_name = static_cast(plugins[i]["docking_nav"]); + + // std::shared_ptr dkpl = std::make_shared(name.str()); + DockingPlanner* dkpl = new DockingPlanner(name.str()); + dkpl->docking_planner_name_ = docking_planner_name; + dkpl->docking_nav_name_ = docking_nav_name; + dkpl_.push_back(dkpl); + } + } + } + else + { + robot::log_warning_at(__FILE__, __LINE__, "Expected 'plugins' to be an array under namespace [%s]", source.c_str()); + } + } + else + { + robot::log_warning_at(__FILE__, __LINE__, "No 'plugins' found under namespace [%s]", source.c_str()); + } + break; + } + } +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers() +{ + std::string algorithm_nav_name; + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + if (!nh_.getParam(algorithm_nav_name, original_papams_)) + robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str()); + original_xy_goal_tolerance_ = xy_goal_tolerance_; + original_yaw_goal_tolerance_ = yaw_goal_tolerance_; +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &nh) +{ + // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window + nh.param("update_costmap_before_planning", update_costmap_before_planning_, true); + if (!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) + { + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + } + if (!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) + { + yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + } +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::reset() +{ + robot::log_info_at(__FILE__, __LINE__, "New Docking Goal Received."); + this->unlock(); + if(traj_generator_) traj_generator_->reset(); + if(goal_checker_) goal_checker_->reset(); + if(nav_algorithm_) nav_algorithm_->reset(); + if(rotate_algorithm_) rotate_algorithm_->reset(); + ret_nav_ = ret_angle_ = false; + + std::string algorithm_nav_name; + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name); + nh_.setParam(algorithm_nav_name, original_papams_); + nh_algorithm.setParam("allow_rotate", false); +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + this->getParams(planner_nh_); + if (update_costmap_before_planning_) + { + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + } + + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + local_goal_pose = this->transformPoseToLocal(goal_pose_); + if (start_docking_) + { + local_goal_pose = goal_pose_; + } + + try + { + if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed"); + } + catch(const nav_core2::LocalPlannerException& e) + { + throw nav_core2::LocalPlannerException(e.what()); + } + + double x_direction, y_direction, theta_direction; + if (!ret_nav_) + { + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + } + // else + // ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction); + + if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_) + { + this->lock(); + geometry_msgs::Vector3 linear = dkpl_.front()->linear_; + traj_generator_->setTwistLinear(linear); + linear.x *= (-1); + traj_generator_->setTwistLinear(linear); + traj_generator_->setTwistAngular(dkpl_.front()->angular_); + + std::string algorithm_nav_name; + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name); + nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_); + nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_); + nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_); + nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_); + nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_); + + nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); + nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); + + if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) + { + + if (dkpl_.front()->following_) + { + nav_2d_msgs::Pose2DStamped follow_pose; + if (dkpl_.front()->geLocalGoal(local_goal_pose)) + { + local_goal_pose = follow_pose; + } + } + if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); + } + } + } + } + else + { + if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str()); + } + // else + // ROS_INFO("chieu %f %f %f", x_direction, y_direction, theta_direction); + } + + int xytheta_direct[3]; + xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0; + xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0; + xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0; + traj_generator_->setDirect(xytheta_direct); +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) +{ + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + nav_2d_msgs::Twist2DStamped cmd_vel; + try + { + if (global_plan_.poses.empty()) + return cmd_vel; + this->prepare(pose, velocity); + cmd_vel = this->ScoreAlgorithm(pose, velocity); + return cmd_vel; + } + catch (const nav_core2::PlannerException &e) + { + robot::log_warning_at(__FILE__, __LINE__, "%s", e.what()); + throw nav_core2::LocalPlannerException(e.what()); + } +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + nav_2d_msgs::Twist2D twist; + mkt_msgs::Trajectory2D traj; + nav_2d_msgs::Twist2DStamped cmd_vel; + cmd_vel.header.stamp = robot::Time::now(); + + if (ret_nav_ && ret_angle_) + return cmd_vel; + else if (!ret_nav_) + { + traj = nav_algorithm_->calculator(pose, velocity); + + if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_) + { + if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) + { + traj = dkpl_.front()->docking_nav_->calculator(pose, velocity); + } + } + } + else + traj = rotate_algorithm_->calculator(pose, velocity); + + cmd_vel.velocity = traj.velocity; + return cmd_vel; +} + +bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + if (goal_pose_.header.frame_id == "") + { + robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!"); + return false; + } + + bool dock_ok = dockingHanlde(pose, velocity); + + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); + nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_); + nav_2d_msgs::Path2D plan = transformed_plan_; + if (start_docking_) + { + local_goal = goal_pose_; + if (!dkpl_.empty() && dkpl_.front()->initialized_) + { + if (dkpl_.front()->allow_rotate_) + plan = nav_2d_msgs::Path2D(); + } + } + + if (!ret_nav_) + { + ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity); + + if (ret_nav_) + { + robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta); + robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta); + robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta); + } + } + + if (ret_nav_ && !ret_angle_ && !dock_ok) + { + double delta_orient = + fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta)); + ret_angle_ = (bool)(delta_orient <= yaw_goal_tolerance_); + if(ret_angle_) + robot::log_warning_at(__FILE__, __LINE__, "xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_); + } + + if (start_docking_) + { + double dx = local_goal.pose.x - local_pose.pose.x; + double dy = local_goal.pose.y - local_pose.pose.y; + double dtheta = local_goal.pose.theta - local_pose.pose.theta; + } + if (ret_nav_ && ret_angle_ && dock_ok) + { + // nh_.setParam("xy_goal_tolerance", original_xy_goal_tolerance_); + // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_); + std::string algorithm_nav_name; + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name); + nh_.setParam(algorithm_nav_name, original_papams_); + nh_algorithm.setParam("allow_rotate", false); + } + return ret_nav_ && ret_angle_ && dock_ok; +} + +bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + if (!dkpl_.empty()) + { + if (ret_nav_ && ret_angle_) + { + if (!dkpl_.front()->is_detected_) + { + if (!dkpl_.front()->initialized_) + { + dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_); + dkpl_.front()->initialized_ = true; + } + else + { + if (dkpl_.front()->docking_planner_ && !dkpl_.front()->docking_nav_) + { + nav_2d_msgs::Pose2DStamped local_goal; + try + { + if (dkpl_.front()->geLocalGoal(local_goal)) + { + dkpl_.front()->is_detected_ = true; + start_docking_ = true; + nav_msgs::Path path; + nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); + dkpl_.front()->getLocalPath(local_pose, local_goal, path); + this->setPlan(nav_2d_utils::pathToPath(path)); + this->setGoalPose(local_goal); + } + } + catch (const std::exception &e) + { + throw nav_core2::LocalPlannerException(e.what()); + } + } + else if (!dkpl_.front()->docking_planner_ && dkpl_.front()->docking_nav_) + { + nav_2d_msgs::Pose2DStamped local_goal; + try + { + if (dkpl_.front()->geLocalGoal(local_goal)) + { + dkpl_.front()->is_detected_ = true; + start_docking_ = true; + nav_2d_msgs::Path2D path; + nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); + path.header.stamp = robot::Time::now(); + path.header.frame_id = costmap_robot_->getGlobalFrameID(); + path.poses.push_back(local_pose); + path.poses.push_back(local_goal); + this->setPlan(path); + this->setGoalPose(local_goal); + } + } + catch (const std::exception &e) + { + throw nav_core2::LocalPlannerException(e.what()); + } + } + } + } + else + { + if (dkpl_.front()->initialized_ && !dkpl_.front()->is_goal_reached_) + { + dkpl_.front()->is_goal_reached_ = true; + robot::log_info_at(__FILE__, __LINE__, "%s is goal reached", dkpl_.front()->name_.c_str()); + } + + if (dkpl_.front()->initialized_ && dkpl_.front()->is_goal_reached_) + { + if(!dkpl_.empty()) + { + if(dkpl_.front()) delete(dkpl_.front()); + dkpl_.erase(dkpl_.begin()); + } + start_docking_ = false; + } + } + } + } + bool result = false; + if (dkpl_.empty()) + { + robot::log_info_at(__FILE__, __LINE__, "Goal reached!"); + result = true; + this->getMaker(); + } + return result; +} + +pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name) + : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false), + is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("") +{ +} + +pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner() +{ + if (docking_planner_) + docking_planner_.reset(); + if (docking_nav_) + docking_nav_.reset(); +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, + score_algorithm::TrajectoryGenerator::Ptr traj_generator) +{ + nh_ = nh; + nh_priv_ = robot::NodeHandle(nh_, name_); + tf_ = tf; + costmap_robot_ = costmap_robot; + traj_generator_ = traj_generator; + + if (docking_planner_name_ != std::string("")) + { + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + docking_planner_loader_ = boost::dll::import_alias( + path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations); + docking_planner_ = docking_planner_loader_(); + if (!docking_planner_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create global planner \"%s\": returned null pointer", docking_planner_name_.c_str()); + exit(1); + } + docking_planner_->initialize(name_, costmap_robot_); + robot::log_info_at(__FILE__, __LINE__, "Created global_planner %s", docking_planner_name_.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", + docking_planner_name_.c_str(), ex.what()); + } + } + + if (docking_nav_name_ != std::string("")) + { + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + docking_nav_loader_ = boost::dll::import_alias( + path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations); + docking_nav_ = docking_nav_loader_(); + if (!docking_nav_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str()); + exit(1); + } + robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_, docking_nav_name_); + docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_); + robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", + docking_nav_name_.c_str(), ex.what()); + } + } + + robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + if (!nh_priv_.param("maker_goal_frame", maker_goal_frame_, std::string(""))) + { + std::stringstream re; + re << "Can not get 'maker_goal_frame' in " << name_; + robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str()); + throw nav_core2::LocalPlannerException(re.str()); + } + + nh_priv_.param("vel_x", linear_.x, 0.5); + nh_priv_.param("vel_theta", angular_.z, 0.5); + nh_priv_.param("following", following_, false); + nh_priv_.param("allow_rotate", allow_rotate_, false); + + double delay_time, time_out; + nh_priv_.param("delay", delay_time, 1.0); + nh_priv_.param("timeout", time_out, 1.0); + + nh_priv_.param("xy_goal_tolerance", xy_goal_tolerance_, 0.05); + nh_priv_.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05); + + nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.4); + nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 1.0); + nh_priv_.param("lookahead_time", lookahead_time_, 1.5); + nh_priv_.param("angle_threshold", angle_threshold_, 0.4); + + detected_timeout_wt_ = + nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this); + detected_timeout_wt_.start(); + robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec()); + + delayed_wt_ = + nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this); + delayed_wt_.start(); + robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec()); + + initialized_ = true; +} + +bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(nav_2d_msgs::Pose2DStamped &maker_goal) +{ + if (!delayed_) + return false; + + bool get_pose_result = false; + try + { + if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal, 2.0)) + { + get_pose_result = true; + detected_timeout_wt_.stop(); + } + + if (get_pose_result) + return true; + } + catch (const std::exception &e) + { + if (detected_timeout_) + { + std::stringstream re; + re << "No detected " << maker_goal_frame_; + robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str()); + throw nav_core2::LocalPlannerException(re.str()); + } + } + return false; +} + +bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(nav_2d_msgs::Pose2DStamped &local_goal) +{ + if (!delayed_) + return false; + bool get_pose_result = false; + try + { + nav_2d_msgs::Pose2DStamped maker_goal_on_robot; + if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal_on_robot, 2.0)) + { + if (nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), maker_goal_on_robot, local_goal)) + { + get_pose_result = true; + detected_timeout_wt_.stop(); + } + } + + if (get_pose_result) + return true; + } + catch (const std::exception &e) + { + if (detected_timeout_) + { + std::stringstream re; + re << "No detected " << maker_goal_frame_; + robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str()); + throw nav_core2::LocalPlannerException(re.str()); + } + } + return false; +} + +bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath( + const nav_2d_msgs::Pose2DStamped &local_pose, const nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path) +{ + geometry_msgs::PoseStamped start = nav_2d_utils::pose2DToPoseStamped(local_pose); + geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(local_goal); + std::vector docking_plan; + + if (!docking_planner_->makePlan(start, goal, docking_plan)) + { + throw nav_core2::LocalPlannerException("Making plan from goal maker is failed"); + return false; + } + else + { + local_path = nav_2d_utils::posesToPath(docking_plan); + return true; + } +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb(const ::robot::WallTimerEvent &timer_event) +{ + detected_timeout_ = true; + detected_timeout_wt_.stop(); + robot::log_warning_at(__FILE__, __LINE__, "%s time_out end %f", name_.c_str(), robot::Time::now().toSec()); +} + +void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb(const ::robot::WallTimerEvent &timer_event) +{ + delayed_ = true; + delayed_wt_.stop(); + robot::log_warning_at(__FILE__, __LINE__, "%s delay end %f", name_.c_str(), robot::Time::now().toSec()); +} + +nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create() +{ + return std::make_shared(); +} + +// register this planner as a LocalPlanner plugin +BOOST_DLL_ALIAS(pnkx_local_planner::PNKXDockingLocalPlanner::create, PNKXDockingLocalPlanner) 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 new file mode 100644 index 0000000..c2f1362 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp @@ -0,0 +1,238 @@ +#include +#include +#include +#include +#include +#include + +// #include +// #include +// #include +#include +#include +#include "pnkx_local_planner/pnkx_go_straight_local_planner.h" +#include +#include + +pnkx_local_planner::PNKXGoStraightLocalPlanner::PNKXGoStraightLocalPlanner() +{ +} + +pnkx_local_planner::PNKXGoStraightLocalPlanner::~PNKXGoStraightLocalPlanner() +{ + if (traj_generator_) + traj_generator_.reset(); + + if (nav_algorithm_) + nav_algorithm_.reset(); + + if (goal_checker_) + goal_checker_.reset(); +} + +void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot) +{ + if (!initialized_) + { + robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str()); + tf_ = tf; + costmap_robot_ = costmap_robot; + costmap_ = costmap_robot_->getCostmap(); + + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + + parent_ = parent; + planner_nh_ = robot::NodeHandle(parent_, name); + + this->getParams(planner_nh_); + + // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window + std::string traj_generator_name; + planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator")); + + robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + traj_gen_loader_ = boost::dll::import_alias( + path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); + traj_generator_ = traj_gen_loader_(); + if (!traj_generator_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); + exit(1); + } + robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); + traj_generator_->initialize(nh_traj_gen); + } + catch (const std::exception& ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); + exit(1); + } + + std::string algorithm_nav_name = std::string("pnkx_local_planner::GoStraight"); + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::GoStraight")); + robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + nav_algorithm_loader_ = boost::dll::import_alias( + path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); + nav_algorithm_ = nav_algorithm_loader_(); + if (!nav_algorithm_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str()); + exit(1); + } + robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); + nav_algorithm_->initialize(nh_algorithm, algorithm_nav_name, tf, costmap_robot_, traj_generator_); + } + catch (const std::exception& ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); + exit(1); + } + + std::string goal_checker_name; + planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + goal_checker_loader_ = boost::dll::import_alias( + path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); + goal_checker_ = goal_checker_loader_(); + if (!goal_checker_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); + exit(1); + } + robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name); + goal_checker_->initialize(nh_goal_checker); + } + catch (const std::exception& ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what()); + exit(1); + } + + robot::log_info_at(__FILE__, __LINE__, "%s is initialized", name.c_str()); + initialized_ = true; + } +} + +void pnkx_local_planner::PNKXGoStraightLocalPlanner::reset() +{ + robot::log_info_at(__FILE__, __LINE__, "New Goal Received."); + this->unlock(); + traj_generator_->reset(); + goal_checker_->reset(); + nav_algorithm_->reset(); + ret_nav_ = ret_angle_ = false; +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose, + const nav_2d_msgs::Twist2D& velocity) +{ + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + nav_2d_msgs::Twist2DStamped cmd_vel; + try + { + if (global_plan_.poses.empty()) + return cmd_vel; + this->prepare(pose, velocity); + cmd_vel = this->ScoreAlgorithm(pose, velocity); + return cmd_vel; + } + catch (const nav_core2::PlannerException &e) + { + throw nav_core2::LocalPlannerException(e.what()); + } +} + +void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + this->getParams(planner_nh_); + if (update_costmap_before_planning_) + { + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + } + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + local_goal_pose = this->transformPoseToLocal(goal_pose_); + + try + { + if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + throw nav_core2::LocalPlannerException("Transform global plan is failed"); + } + catch(const nav_core2::LocalPlannerException& e) + { + throw nav_core2::LocalPlannerException(e.what()); + } + + double x_direction, y_direction, theta_direction; + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + } + + int xytheta_direct[3]; + xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0; + xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0; + xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0; + traj_generator_->setDirect(xytheta_direct); +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + nav_2d_msgs::Twist2D twist; + mkt_msgs::Trajectory2D traj; + nav_2d_msgs::Twist2DStamped cmd_vel; + if (!ret_nav_) + traj = nav_algorithm_->calculator(pose, velocity); + + cmd_vel.velocity = traj.velocity; + return cmd_vel; +} + +bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + if (goal_pose_.header.frame_id == "") + { + robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!"); + return false; + } + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_plan_, velocity); + + if (ret_nav_) + robot::log_info_at(__FILE__, __LINE__, "Goal reached!"); + return ret_nav_; +} + +nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXGoStraightLocalPlanner::create() +{ + return std::make_shared(); +} + +// register this planner as a LocalPlanner plugin +BOOST_DLL_ALIAS(pnkx_local_planner::PNKXGoStraightLocalPlanner::create, PNKXGoStraightLocalPlanner) \ No newline at end of file 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 new file mode 100644 index 0000000..de194b8 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -0,0 +1,444 @@ +#include +#include + +#include +#include +#include +#include + +// #include +// #include +// #include +#include + +#include +#include +#include + +#include +#include +#include + +pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner() + : initialized_(false) +{ +} + +pnkx_local_planner::PNKXLocalPlanner::~PNKXLocalPlanner() +{ + if (traj_generator_) + traj_generator_.reset(); + + if (nav_algorithm_) + nav_algorithm_.reset(); + + if (rotate_algorithm_) + rotate_algorithm_.reset(); + + if (goal_checker_) + goal_checker_.reset(); +} + +void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) +{ + if (!initialized_) + { + tf_ = tf; + name_ = name; + costmap_robot_ = costmap_robot; + costmap_ = costmap_robot_->getCostmap(); + + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + + parent_ = parent; + robot::log_warning_at(__FILE__, __LINE__, "Parent namespace: %s", parent_.getNamespace().c_str()); + planner_nh_ = robot::NodeHandle(parent_, name); + // planner_nh_.printRootParams(); + this->getParams(planner_nh_); + + std::string traj_generator_name; + planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("StandardTrajectoryGenerator")); + robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_standard_traj_generator.so"; + traj_gen_loader_ = boost::dll::import_alias( + path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); + traj_generator_ = traj_gen_loader_(); + if (!traj_generator_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); + exit(1); + } + robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); + traj_generator_->initialize(nh_traj_gen); + robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); + exit(1); + } + + std::string algorithm_nav_name; + planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so"; + nav_algorithm_loader_ = boost::dll::import_alias( + path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); + nav_algorithm_ = nav_algorithm_loader_(); + if (!nav_algorithm_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str()); + exit(1); + } + nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); + exit(1); + } + + std::string algorithm_rotate_name; + planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so"; + rotate_algorithm_loader_ = boost::dll::import_alias( + path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations); + rotate_algorithm_ = rotate_algorithm_loader_(); + if (!rotate_algorithm_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); + exit(1); + } + rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); + robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); + exit(1); + } + + std::string goal_checker_name; + planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_goal_checker.so"; + goal_checker_loader_ = boost::dll::import_alias( + path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); + goal_checker_ = goal_checker_loader_(); + if (!goal_checker_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); + exit(1); + } + goal_checker_->initialize(parent_); + robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str()); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what()); + exit(1); + } + this->initializeOthers(); + robot::log_info("[%s:%d]\n %s is sucessed", __FILE__, __LINE__, name.c_str()); + initialized_ = true; + } +} + +void pnkx_local_planner::PNKXLocalPlanner::initializeOthers() +{ +} + +void pnkx_local_planner::PNKXLocalPlanner::getParams(robot::NodeHandle &nh) +{ + // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window + nh.param("update_costmap_before_planning", update_costmap_before_planning_, true); + if (!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) + { + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + } + if (!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) + { + yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + } +} + +void pnkx_local_planner::PNKXLocalPlanner::reset() +{ + robot::log_info_at(__FILE__, __LINE__, "New Goal Received."); + this->unlock(); + traj_generator_->reset(); + goal_checker_->reset(); + nav_algorithm_->reset(); + rotate_algorithm_->reset(); + ret_nav_ = ret_angle_ = false; +} + +void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) +{ + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + reset(); + goal_pose_ = goal_pose; +} + +void pnkx_local_planner::PNKXLocalPlanner::setPlan(const nav_2d_msgs::Path2D &path) +{ + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + costmap_robot_->resetLayers(); + global_plan_ = path; + if((unsigned int)global_plan_.poses.size() > 2) + global_plan_.poses.erase(global_plan_.poses.begin()); + for (unsigned int i = 0; i < (unsigned int)global_plan_.poses.size(); i++) + { + global_plan_.poses[i].header.seq = i; + } +} + +void pnkx_local_planner::PNKXLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + this->getParams(planner_nh_); + if (update_costmap_before_planning_) + { + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + } + + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + local_goal_pose = this->transformPoseToLocal(goal_pose_); + + if (pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + { + robot::log_debug_at(__FILE__, __LINE__, "Transform global plan is successful"); + } + else + robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed"); + + double x_direction, y_direction, theta_direction; + if (!ret_nav_) + { + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + } + } + else + { + if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str()); + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + } + } + + int xytheta_direct[3]; + xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0; + xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0; + xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0; + traj_generator_->setDirect(xytheta_direct); +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) +{ + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + nav_2d_msgs::Twist2DStamped cmd_vel; + try + { + if (global_plan_.poses.empty()) + return cmd_vel; + this->prepare(pose, velocity); + cmd_vel = this->ScoreAlgorithm(pose, velocity); + return cmd_vel; + } + catch (const nav_core2::PlannerException &e) + { + throw nav_core2::LocalPlannerException("computing velocity commands has been broken"); + return cmd_vel; + } +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + nav_2d_msgs::Twist2D twist; + mkt_msgs::Trajectory2D traj; + nav_2d_msgs::Twist2DStamped cmd_vel; + cmd_vel.header.stamp = robot::Time::now(); + + if (ret_nav_ && ret_angle_) + return cmd_vel; + else if (!ret_nav_) + traj = nav_algorithm_->calculator(pose, velocity); + else + traj = rotate_algorithm_->calculator(pose, velocity); + + cmd_vel.velocity = traj.velocity; + return cmd_vel; +} + +bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(geometry_msgs::Vector3 linear) +{ + try + { + if (linear.x == 0.0 && linear.y == 0.0 && linear.z == 0.0) + { + if (nav_algorithm_) + nav_algorithm_->stop(); + if (rotate_algorithm_) + rotate_algorithm_->stop(); + } + else + { + if (nav_algorithm_) + nav_algorithm_->resume(); + if (rotate_algorithm_) + rotate_algorithm_->resume(); + } + + if (traj_generator_) + return traj_generator_->setTwistLinear(linear); + else + throw nav_core2::LocalPlannerException("Failed to set linear"); + } + catch (const std::exception &e) + { + throw nav_core2::LocalPlannerException(e.what()); + return false; + } +} + +geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct) +{ + try + { + if (traj_generator_) + return traj_generator_->getTwistLinear(direct); + else + throw nav_core2::LocalPlannerException("Failed to get linear"); + } + catch (const std::exception &e) + { + throw nav_core2::LocalPlannerException(e.what()); + } +} + +bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(geometry_msgs::Vector3 angular) +{ + try + { + if (traj_generator_) + return traj_generator_->setTwistAngular(angular); + else + throw nav_core2::LocalPlannerException("Failed to set angular"); + } + catch (const std::exception &e) + { + throw nav_core2::LocalPlannerException(e.what()); + return false; + } +} + +geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct) +{ + try + { + if (traj_generator_) + return traj_generator_->getTwistAngular(direct); + else + throw nav_core2::LocalPlannerException("Failed to get angular"); + } + catch (const std::exception &e) + { + throw nav_core2::LocalPlannerException(e.what()); + } +} + +bool pnkx_local_planner::PNKXLocalPlanner::islock() +{ + return lock_; +} + +void pnkx_local_planner::PNKXLocalPlanner::lock() +{ + lock_ = true; +} + +void pnkx_local_planner::PNKXLocalPlanner::unlock() +{ + lock_ = false; +} + +bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + if (goal_pose_.header.frame_id == "") + { + robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!"); + return false; + } + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); + nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_); + nav_2d_msgs::Path2D plan = transformed_plan_; + if (!ret_nav_) + { + ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity); + } + else + { + double delta_orient = fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta)); + ret_angle_ = (bool)(delta_orient <= yaw_goal_tolerance_); + } + + if (ret_nav_ && ret_angle_) + { + robot::log_info_throttle(1.0, "Goal reached!"); + } + return ret_nav_ && ret_angle_; +} + +inline double getSquareDistance(const nav_2d_msgs::Pose2DStamped &pose_a, const nav_2d_msgs::Pose2DStamped &pose_b) +{ + double x_diff = pose_a.pose.x - pose_b.pose.x; + double y_diff = pose_a.pose.y - pose_b.pose.y; + return x_diff * x_diff + y_diff * y_diff; +} + +nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose) +{ + nav_2d_msgs::Pose2DStamped local_pose; + nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), pose, local_pose); + return local_pose; +} + +nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXLocalPlanner::create() +{ + return std::make_shared(); +} + +// register this planner as a LocalPlanner plugin +BOOST_DLL_ALIAS(pnkx_local_planner::PNKXLocalPlanner::create, PNKXLocalPlanner) \ No newline at end of file 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 new file mode 100644 index 0000000..99af078 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp @@ -0,0 +1,234 @@ +#include "pnkx_local_planner/pnkx_rotate_local_planner.h" +#include "pnkx_local_planner/transforms.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner() +{ +} + +pnkx_local_planner::PNKXRotateLocalPlanner::~PNKXRotateLocalPlanner() +{ + if (traj_generator_) + traj_generator_.reset(); + + if (nav_algorithm_) + nav_algorithm_.reset(); + + if (goal_checker_) + goal_checker_.reset(); +} + +void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot) +{ + if (!initialized_) + { + robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str()); + tf_ = tf; + costmap_robot_ = costmap_robot; + costmap_ = costmap_robot_->getCostmap(); + + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + parent_ = parent; + planner_nh_ = robot::NodeHandle(parent_, name); + + this->getParams(planner_nh_); + + std::string traj_generator_name; + planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator")); + robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + traj_gen_loader_ = boost::dll::import_alias( + path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); + traj_generator_ = traj_gen_loader_(); + if (!traj_generator_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); + exit(1); + } + robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name); + traj_generator_->initialize(nh_traj_gen); + } + catch (const std::exception& ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what()); + exit(1); + } + + std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate"); + planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + rotate_algorithm_loader_ = boost::dll::import_alias( + path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations); + rotate_algorithm_ = rotate_algorithm_loader_(); + if (!nav_algorithm_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); + exit(1); + } + robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name); + rotate_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); + } + catch (const std::exception& ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); + exit(1); + } + + std::string goal_checker_name; + planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker")); + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + goal_checker_loader_ = boost::dll::import_alias( + path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); + goal_checker_ = goal_checker_loader_(); + if (!goal_checker_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); + exit(1); + } + robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name); + goal_checker_->initialize(nh_goal_checker); + } + catch (const std::exception& ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what()); + exit(1); + } + + robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str()); + initialized_ = true; + } +} + +void pnkx_local_planner::PNKXRotateLocalPlanner::reset() +{ + robot::log_info_at(__FILE__, __LINE__, "New Goal Received."); + this->unlock(); + traj_generator_->reset(); + goal_checker_->reset(); + nav_algorithm_->reset(); + ret_nav_ = false; +} + +void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + this->getParams(planner_nh_); + if (update_costmap_before_planning_) + { + info_.width = costmap_->getSizeInCellsX(); + info_.height = costmap_->getSizeInCellsY(); + info_.resolution = costmap_->getResolution(); + info_.frame_id = costmap_robot_->getGlobalFrameID(); + info_.origin_x = costmap_->getOriginX(); + info_.origin_y = costmap_->getOriginY(); + + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + } + + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + local_goal_pose = this->transformPoseToLocal(goal_pose_); + + try + { + if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + throw nav_core2::LocalPlannerException("Transform global plan is failed"); + } + catch(const nav_core2::LocalPlannerException& e) + { + throw nav_core2::LocalPlannerException(e.what()); + } + + double x_direction, y_direction, theta_direction; + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) + { + throw nav_core2::LocalPlannerException("Algorithm failed to prepare"); + } + + int xytheta_direct[3]; + xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0; + xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0; + xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0; + traj_generator_->setDirect(xytheta_direct); +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + nav_2d_msgs::Twist2DStamped cmd_vel; + try + { + if (global_plan_.poses.empty()) + return cmd_vel; + this->prepare(pose, velocity); + cmd_vel = this->ScoreAlgorithm(pose, velocity); + return cmd_vel; + } + catch (const nav_core2::PlannerException &e) + { + throw nav_core2::LocalPlannerException(e.what()); + } +} + +nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + nav_2d_msgs::Twist2D twist; + mkt_msgs::Trajectory2D traj; + nav_2d_msgs::Twist2DStamped cmd_vel; + if (ret_nav_) + return cmd_vel; + + traj = nav_algorithm_->calculator(pose, velocity); + cmd_vel.velocity = traj.velocity; + return cmd_vel; +} + +bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +{ + if (goal_pose_.header.frame_id == "") + { + robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!"); + return false; + } + // Update time stamp of goal pose + // goal_pose_.header.stamp = pose.header.stamp; + nav_2d_msgs::Path2D empty; + ret_nav_ = goal_checker_->isGoalReached( + this->transformPoseToLocal(pose), this->transformPoseToLocal(goal_pose_), empty, velocity); + + if (ret_nav_) + robot::log_info_at(__FILE__, __LINE__, "Goal reached!"); + return ret_nav_; +} + +nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create() +{ + return std::make_shared(); +} + +// register this planner as a LocalPlanner plugin +BOOST_DLL_ALIAS(pnkx_local_planner::PNKXRotateLocalPlanner::create, PNKXRotateLocalPlanner) \ No newline at end of file diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp new file mode 100644 index 0000000..7b53255 --- /dev/null +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp @@ -0,0 +1,206 @@ +#include +#include +#include +#include +#include +#include + +bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance) +{ + if (!tf) + return false; + + geometry_msgs::PoseStamped global_pose_stamped; + tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); + geometry_msgs::PoseStamped robot_pose; + tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); + robot_pose.header.frame_id = base_frame_id; + 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()); + + // get the global pose of the robot + try + { + // use current time if possible (makes sure it's not in the future) + std::string error_msg; + if (tf->canTransform(global_frame, base_frame_id, tf3_current_time, &error_msg)) + { + // Transform is available at current time + tf3::TransformStampedMsg transform = tf->lookupTransform(global_frame, base_frame_id, tf3_current_time); + tf3::doTransform(robot_pose, global_pose_stamped, transform); + } + // use the latest otherwise (tf3::Time() means latest available) + else + { + // Try to get latest transform + tf3::TransformStampedMsg transform = tf->lookupTransform(global_frame, base_frame_id, tf3_zero_time); + tf3::doTransform(robot_pose, global_pose_stamped, transform); + } + global_pose = nav_2d_utils::poseStampedToPose2D(global_pose_stamped); + } + catch (tf3::LookupException& ex) + { + robot::log_warning_throttle(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); + return false; + // throw nav_core2::LocalPlannerException(ex.what()); + } + catch (tf3::ConnectivityException& ex) + { + robot::log_warning_throttle(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); + return false; + // throw nav_core2::LocalPlannerException(ex.what()); + } + catch (tf3::ExtrapolationException& ex) + { + robot::log_warning_throttle(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); + return false; + // throw nav_core2::LocalPlannerException(ex.what()); + } + // check global_pose timeout + if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance) + { + robot::log_warning_throttle(1.0, "Local Planner transform %s %s timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", base_frame_id.c_str(), global_frame.c_str(), + current_time.toSec(), global_pose_stamped.header.stamp.toSec(), transform_tolerance); + return false; + } + return true; +} + +bool pnkx_local_planner::transformGlobalPose(TFListenerPtr tf, const std::string& global_frame, + const nav_2d_msgs::Pose2DStamped& stamped_pose, nav_2d_msgs::Pose2DStamped& newer_pose) +{ + if (tf == nullptr) + return false; + bool result = nav_2d_utils::transformPose(tf, global_frame, stamped_pose, newer_pose); + newer_pose.header.seq = stamped_pose.header.seq; + return result; +} + +double pnkx_local_planner::getSquareDistance(const geometry_msgs::Pose2D& pose_a, const geometry_msgs::Pose2D& pose_b) +{ + double x_diff = pose_a.x - pose_b.x; + double y_diff = pose_a.y - pose_b.y; + return x_diff * x_diff + y_diff * y_diff; +} + +bool pnkx_local_planner::transformGlobalPlan( + TFListenerPtr tf, const nav_2d_msgs::Path2D& global_plan, const nav_2d_msgs::Pose2DStamped& pose, + const costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length, + nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame) +{ + if (global_plan.poses.size() == 0) + { + robot::log_warning("Received plan with zero length"); + throw nav_core2::PlannerException("Received plan with zero length"); + } + + transformed_plan.poses.clear(); + if (tf == nullptr) + throw nav_core2::PlannerException("TFListenerPtr is null"); + + if (global_plan.poses.empty()) + throw nav_core2::PlannerException("Received plan with zero length"); + + try + { + // let's get the pose of the robot in the frame of the plan + nav_2d_msgs::Pose2DStamped robot_pose; + if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + { + throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + } + + transformed_plan.header.frame_id = costmap->getGlobalFrameID(); + transformed_plan.header.stamp = pose.header.stamp; + + nav_2d_msgs::Pose2DStamped new_pose; + nav_2d_msgs::Pose2DStamped stamped_pose; + stamped_pose.header.frame_id = global_plan.header.frame_id; + + for (unsigned int i = 0; i < global_plan.poses.size(); i++) + { + // now we'll transform until points are outside of our distance threshold + stamped_pose = global_plan.poses[i]; + if (pnkx_local_planner::transformGlobalPose(tf, global_frame, global_plan.poses[i], new_pose)) + { + transformed_plan.poses.push_back(new_pose); + } + } + + // Prune both plans based on robot position + // Note that this part of the algorithm assumes that the global plan starts near the robot (at one point) + // Otherwise it may take a few iterations to converge to the proper behavior + + // let's get the pose of the robot in the frame of the transformed_plan/costmap + nav_2d_msgs::Pose2DStamped costmap_pose; + if (!nav_2d_utils::transformPose(tf, transformed_plan.header.frame_id, pose, costmap_pose)) + { + throw nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame"); + } + + robot::log_assert(global_plan.poses.size() >= transformed_plan.poses.size(), "Global plan size is less than transformed plan size"); + + std::vector::iterator it = transformed_plan.poses.begin(); + + double sq_prune_dist = 0.1; + + std::vector footprint = costmap->getRobotFootprint(); + size_t n = footprint.size(); + if (n > 1) + { + double max_length = 0.0; + for (size_t i = 0; i < n; ++i) + { + const geometry_msgs::Point& p1 = footprint[i]; + const geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y); + if (len > max_length) + { + max_length = len; + } + } + sq_prune_dist += max_length; + } + // if(from_global_frame) sq_prune_dist = 0; + while (it != transformed_plan.poses.end()) + { + const nav_2d_msgs::Pose2DStamped& w = *it; + // Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution + if (pnkx_local_planner::getSquareDistance(costmap_pose.pose, w.pose) < sq_prune_dist) + { + robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n", + costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y); + break; + } + + it = transformed_plan.poses.erase(it); + } + + if (transformed_plan.poses.size() == 0) + { + throw nav_core2::PlannerTFException("Resulting plan has 0 poses in it."); + } + } + catch (tf3::LookupException& ex) + { + robot::log_error_at(__FILE__, __LINE__, "No Transform available Error: %s\n", ex.what()); + return false; + } + catch (tf3::ConnectivityException& ex) + { + robot::log_error("Connectivity Error: %s\n", ex.what()); + return false; + } + catch (tf3::ExtrapolationException& ex) + { + robot::log_error("Extrapolation Error: %s\n", ex.what()); + if (global_plan.poses.size() > 0) + robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, global_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + return false; + } + return true; +} \ No newline at end of file diff --git a/src/Libraries/common_msgs b/src/Libraries/common_msgs index 6bac684..e7dc403 160000 --- a/src/Libraries/common_msgs +++ b/src/Libraries/common_msgs @@ -1 +1 @@ -Subproject commit 6bac68429824cab4bbb1df3e259a24a8bf741b59 +Subproject commit e7dc4031c674bfa73c800e37e39531a7f828f79d diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index fdfba18..b3be5da 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit fdfba18bde0662ab5f042b938ecd011c5382ca7a +Subproject commit b3be5da393abee4ef535f68f702cd84c02f3b98b diff --git a/src/Libraries/data_convert b/src/Libraries/data_convert index 03a8fa3..8b22de3 160000 --- a/src/Libraries/data_convert +++ b/src/Libraries/data_convert @@ -1 +1 @@ -Subproject commit 03a8fa3a802db88ab01d6f7f9694050fd7e74a3a +Subproject commit 8b22de38ac16eccce348e505e12cfafdf33e2943 diff --git a/src/Libraries/nav_2d_utils/CMakeLists.txt b/src/Libraries/nav_2d_utils/CMakeLists.txt index d57f829..38548e1 100755 --- a/src/Libraries/nav_2d_utils/CMakeLists.txt +++ b/src/Libraries/nav_2d_utils/CMakeLists.txt @@ -32,8 +32,7 @@ target_link_libraries(conversions nav_msgs nav_grid nav_core2 - robot::node_handle - robot::console + robot_cpp PRIVATE console_bridge::console_bridge Boost::system @@ -50,8 +49,7 @@ target_link_libraries(path_ops PUBLIC nav_2d_msgs geometry_msgs - robot::node_handle - robot::console + robot_cpp ) add_library(polygons src/polygons.cpp src/footprint.cpp) @@ -64,8 +62,7 @@ target_link_libraries(polygons PUBLIC nav_2d_msgs geometry_msgs - robot::node_handle - robot::console + robot_cpp PRIVATE ${xmlrpcpp_LIBRARIES} ) @@ -87,8 +84,7 @@ target_link_libraries(bounds PUBLIC nav_grid nav_core2 - robot::node_handle - robot::console + robot_cpp ) add_library(tf_help src/tf_help.cpp) @@ -103,8 +99,7 @@ target_link_libraries(tf_help geometry_msgs nav_core2 tf3 - robot::node_handle - robot::console + robot_cpp ) # Create an INTERFACE library that represents all nav_2d_utils libraries @@ -121,8 +116,7 @@ target_link_libraries(nav_2d_utils polygons bounds tf_help - robot::node_handle - robot::console + robot_cpp ) # Install header files diff --git a/src/Libraries/nav_2d_utils/doc/Conversions.md b/src/Libraries/nav_2d_utils/doc/Conversions.md index d8b0060..6361563 100755 --- a/src/Libraries/nav_2d_utils/doc/Conversions.md +++ b/src/Libraries/nav_2d_utils/doc/Conversions.md @@ -17,7 +17,7 @@ | to `nav_2d_msgs` | from `nav_2d_msgs` | | -- | -- | | `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)` -||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, ros::Time)`| +||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`| ||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`| | `Pose2DStamped stampedPoseToPose2D(tf::Stamped)` | | @@ -25,7 +25,7 @@ | to `nav_2d_msgs` | from `nav_2d_msgs` | | -- | -- | | `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)` -| `Path2D posesToPath2D(std::vector)` | `nav_msgs::Path poses2DToPath(std::vector, std::string, ros::Time)` +| `Path2D posesToPath2D(std::vector)` | `nav_msgs::Path poses2DToPath(std::vector, std::string, robot::Time)` Also, `nav_msgs::Path posesToPath(std::vector)` diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h index b96a8f3..cab4803 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h @@ -35,7 +35,10 @@ #ifndef NAV_2D_UTILS_PARAMETERS_H #define NAV_2D_UTILS_PARAMETERS_H +#include +#include #include +#include namespace nav_2d_utils { @@ -55,12 +58,13 @@ template param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value) { std::string resolved_name; - // if (nh.searchParam(param_name, resolved_name)) - // { - // param_t value = default_value; - // nh.param(resolved_name, value, default_value); - // return value; - // } + if (nh.searchParam(param_name, resolved_name)) + { + param_t value = default_value; + robot::NodeHandle nh_private = robot::NodeHandle("~"); + nh_private.param(resolved_name, value, default_value); + return value; + } return default_value; } @@ -84,7 +88,7 @@ param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::str } if (nh.hasParam(old_name)) { - std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl; + robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); nh.getParam(old_name, value, default_value); return value; } @@ -103,7 +107,7 @@ void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string curr if (!nh.hasParam(old_name)) return; param_t value; - std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl; + robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); value = nh.param(old_name); nh.setParam(current_name, value); } diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h index a46a335..c9ad2e7 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h @@ -205,8 +205,8 @@ protected: std::string current_plugin_; // ROS Interface - ros::ServiceServer switch_plugin_srv_; - ros::Publisher current_plugin_pub_; + robot::ServiceServer switch_plugin_srv_; + robot::Publisher current_plugin_pub_; robot::NodeHandle private_nh_; std::string ros_name_; diff --git a/src/Libraries/nav_2d_utils/src/conversions.cpp b/src/Libraries/nav_2d_utils/src/conversions.cpp index 6e37674..4d50d9a 100755 --- a/src/Libraries/nav_2d_utils/src/conversions.cpp +++ b/src/Libraries/nav_2d_utils/src/conversions.cpp @@ -128,7 +128,7 @@ geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& } // geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d, -// const std::string& frame, const ros::Time& stamp) +// const std::string& frame, const robot::Time& stamp) // { // geometry_msgs::PoseStamped pose; // pose.header.frame_id = frame; @@ -189,7 +189,7 @@ nav_2d_msgs::Path2D posesToPath2D(const std::vector& // nav_msgs::Path poses2DToPath(const std::vector& poses, -// const std::string& frame, const ros::Time& stamp) +// const std::string& frame, const robot::Time& stamp) // { // nav_msgs::Path path; // path.poses.resize(poses.size()); diff --git a/src/Libraries/nav_2d_utils/src/nav_2d_utils/__init__.py b/src/Libraries/nav_2d_utils/src/nav_2d_utils/__init__.py deleted file mode 100755 index e69de29..0000000 diff --git a/src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py b/src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py deleted file mode 100755 index 0f05402..0000000 --- a/src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py +++ /dev/null @@ -1,131 +0,0 @@ -from geometry_msgs.msg import Pose, Pose2D, Quaternion, Point -from nav_2d_msgs.msg import Twist2D, Path2D, Pose2DStamped, Point2D -from nav_msgs.msg import Path -from geometry_msgs.msg import Twist, PoseStamped - -try: - from tf.transformations import euler_from_quaternion, quaternion_from_euler - - def get_yaw(orientation): - rpy = euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w)) - return rpy[2] - - def from_yaw(yaw): - q = Quaternion() - q.x, q.y, q.z, q.w = quaternion_from_euler(0, 0, yaw) - return q -except ImportError: - from math import sin, cos, atan2 - # From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - - def from_yaw(yaw): - q = Quaternion() - q.z = sin(yaw * 0.5) - q.w = cos(yaw * 0.5) - return q - - def get_yaw(q): - siny_cosp = +2.0 * (q.w * q.z + q.x * q.y) - cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z) - return atan2(siny_cosp, cosy_cosp) - - -def twist2Dto3D(cmd_vel_2d): - cmd_vel = Twist() - cmd_vel.linear.x = cmd_vel_2d.x - cmd_vel.linear.y = cmd_vel_2d.y - cmd_vel.angular.z = cmd_vel_2d.theta - return cmd_vel - - -def twist3Dto2D(cmd_vel): - cmd_vel_2d = Twist2D() - cmd_vel_2d.x = cmd_vel.linear.x - cmd_vel_2d.y = cmd_vel.linear.y - cmd_vel_2d.theta = cmd_vel.angular.z - return cmd_vel_2d - - -def pointToPoint3D(point_2d): - point = Point() - point.x = point_2d.x - point.y = point_2d.y - return point - - -def pointToPoint2D(point): - point_2d = Point2D() - point_2d.x = point.x - point_2d.y = point.y - return point_2d - - -def poseToPose2D(pose): - pose2d = Pose2D() - pose2d.x = pose.position.x - pose2d.y = pose.position.y - pose2d.theta = get_yaw(pose.orientation) - return pose2d - - -def poseStampedToPose2DStamped(pose): - pose2d = Pose2DStamped() - pose2d.header = pose.header - pose2d.pose = poseToPose2D(pose.pose) - return pose2d - - -def poseToPose2DStamped(pose, frame, stamp): - pose2d = Pose2DStamped() - pose2d.header.frame_id = frame - pose2d.header.stamp = stamp - pose2d.pose = poseToPose2D(pose) - return pose2d - - -def pose2DToPose(pose2d): - pose = Pose() - pose.position.x = pose2d.x - pose.position.y = pose2d.y - pose.orientation = from_yaw(pose2d.theta) - return pose - - -def pose2DStampedToPoseStamped(pose2d): - pose = PoseStamped() - pose.header = pose2d.header - pose.pose = pose2DToPose(pose2d.pose) - return pose - - -def pose2DToPoseStamped(pose2d, frame, stamp): - pose = PoseStamped() - pose.header.frame_id = frame - pose.header.stamp = stamp - pose.pose.position.x = pose2d.x - pose.pose.position.y = pose2d.y - pose.pose.orientation = from_yaw(pose2d.theta) - return pose - - -def pathToPath2D(path): - path2d = Path2D() - if len(path.poses) == 0: - return path - path2d.header.frame_id = path.poses[0].header.frame_id - path2d.header.stamp = path.poses[0].header.stamp - for pose in path.poses: - stamped = poseStampedToPose2DStamped(pose) - path2d.poses.append(stamped.pose) - return path2d - - -def path2DToPath(path2d): - path = Path() - path.header = path2d.header - for pose2d in path2d.poses: - pose = PoseStamped() - pose.header = path2d.header - pose.pose = pose2DToPose(pose2d) - path.poses.append(pose) - return path diff --git a/src/Libraries/nav_2d_utils/src/polygons.cpp b/src/Libraries/nav_2d_utils/src/polygons.cpp index fc0beec..ef8dd11 100755 --- a/src/Libraries/nav_2d_utils/src/polygons.cpp +++ b/src/Libraries/nav_2d_utils/src/polygons.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include diff --git a/src/Libraries/nav_2d_utils/test/param_tests.cpp b/src/Libraries/nav_2d_utils/test/param_tests.cpp index 5ac252e..00c90cd 100755 --- a/src/Libraries/nav_2d_utils/test/param_tests.cpp +++ b/src/Libraries/nav_2d_utils/test/param_tests.cpp @@ -140,7 +140,7 @@ TEST(Polygon2D, test_write) int main(int argc, char** argv) { - ros::init(argc, argv, "param_tests"); + robot::init(argc, argv, "param_tests"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/src/Libraries/robot_cpp b/src/Libraries/robot_cpp index 1974d0d..5b23baa 160000 --- a/src/Libraries/robot_cpp +++ b/src/Libraries/robot_cpp @@ -1 +1 @@ -Subproject commit 1974d0ddeea08a05b83363614d1a946db5dd07ee +Subproject commit 5b23baae3ace0f5acd7dc7e9cd346ffc75210630 diff --git a/src/Libraries/tf3/CHANGELOG.rst b/src/Libraries/tf3/CHANGELOG.rst index 8db46e0..fa0c4ba 100644 --- a/src/Libraries/tf3/CHANGELOG.rst +++ b/src/Libraries/tf3/CHANGELOG.rst @@ -309,7 +309,7 @@ Changelog for package tf2 ------------------ * splitting rospy dependency into tf2_py so tf2 is pure c++ library. * switching to console_bridge from rosconsole -* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 +* moving convert methods back into tf2 because it does not have any ros dependencies beyond robot::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * Cleaning up packaging of tf2 including: removing unused nodehandle diff --git a/src/Libraries/tf3/include/tf3/buffer_core.h b/src/Libraries/tf3/include/tf3/buffer_core.h index 2e6d8f3..ca8777e 100644 --- a/src/Libraries/tf3/include/tf3/buffer_core.h +++ b/src/Libraries/tf3/include/tf3/buffer_core.h @@ -166,7 +166,7 @@ public: geometry_msgs::Twist lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame, const tf::Point & reference_point, const std::string& reference_point_frame, - const ros::Time& time, const ros::Duration& averaging_interval) const; + const robot::Time& time, const robot::Duration& averaging_interval) const; */ /* \brief lookup the twist of the tracking frame with respect to the observational frame * @@ -183,7 +183,7 @@ public: /* geometry_msgs::Twist lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, - const ros::Time& time, const ros::Duration& averaging_interval) const; + const robot::Time& time, const robot::Duration& averaging_interval) const; */ /** \brief Test if a transform is possible * \param target_frame The frame into which to transform diff --git a/src/Libraries/tf3/include/tf3/compat.h b/src/Libraries/tf3/include/tf3/compat.h index 318049e..bf6ab63 100644 --- a/src/Libraries/tf3/include/tf3/compat.h +++ b/src/Libraries/tf3/include/tf3/compat.h @@ -1,4 +1,4 @@ -// Compatibility types to avoid using ros:: or geometry_msgs:: namespaces inside tf3 +// Compatibility types to avoid using robot:: or geometry_msgs:: namespaces inside tf3 #ifndef TF3_COMPAT_H #define TF3_COMPAT_H @@ -7,7 +7,7 @@ namespace tf3 { -// Minimal header/message equivalents owned by tf3 (no ros:: or geometry_msgs::) +// Minimal header/message equivalents owned by tf3 (no robot:: or geometry_msgs::) struct HeaderMsg { uint32_t seq; diff --git a/src/Libraries/tf3/include/tf3/exceptions.h b/src/Libraries/tf3/include/tf3/exceptions.h index f22734e..8ca6a52 100644 --- a/src/Libraries/tf3/include/tf3/exceptions.h +++ b/src/Libraries/tf3/include/tf3/exceptions.h @@ -37,7 +37,7 @@ namespace tf3{ /** \brief A base class for all tf3 exceptions - * This inherits from ros::exception + * This inherits from robot::exception * which inherits from std::runtime_exception */ class TransformException: public std::runtime_error diff --git a/src/Libraries/tf3/src/buffer_core.cpp b/src/Libraries/tf3/src/buffer_core.cpp index 6c5a19c..fe38f4c 100644 --- a/src/Libraries/tf3/src/buffer_core.cpp +++ b/src/Libraries/tf3/src/buffer_core.cpp @@ -674,8 +674,8 @@ tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_f /* geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, - const ros::Time& time, - const ros::Duration& averaging_interval) const + const robot::Time& time, + const robot::Duration& averaging_interval) const { try { @@ -707,8 +707,8 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, const std::string& reference_frame, const tf3::Point & reference_point, const std::string& reference_point_frame, - const ros::Time& time, - const ros::Duration& averaging_interval) const + const robot::Time& time, + const robot::Duration& averaging_interval) const { try{ geometry_msgs::Twist t; diff --git a/src/Libraries/tf3/test/cache_unittest.cpp b/src/Libraries/tf3/test/cache_unittest.cpp index c0f4bd8..98dcd46 100644 --- a/src/Libraries/tf3/test/cache_unittest.cpp +++ b/src/Libraries/tf3/test/cache_unittest.cpp @@ -82,7 +82,7 @@ TEST(TimeCache, Repeatability) for ( uint64_t i = 1; i < runs ; i++ ) { stor.frame_id_ = i; - stor.stamp_ = ros::Time().fromNSec(i); + stor.stamp_ = robot::Time().fromNSec(i); cache.insertData(stor); } @@ -90,9 +90,9 @@ TEST(TimeCache, Repeatability) for ( uint64_t i = 1; i < runs ; i++ ) { - cache.getData(ros::Time().fromNSec(i), stor); + cache.getData(robot::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); } } @@ -109,16 +109,16 @@ TEST(TimeCache, RepeatabilityReverseInsertOrder) for ( int i = runs -1; i >= 0 ; i-- ) { stor.frame_id_ = i; - stor.stamp_ = ros::Time().fromNSec(i); + stor.stamp_ = robot::Time().fromNSec(i); cache.insertData(stor); } for ( uint64_t i = 1; i < runs ; i++ ) { - cache.getData(ros::Time().fromNSec(i), stor); + cache.getData(robot::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); } } @@ -143,16 +143,16 @@ TEST(TimeCache, RepeatabilityRandomInsertOrder) ss << values[i]; stor.header.frame_id = ss.str(); stor.frame_id_ = i; - stor.stamp_ = ros::Time().fromNSec(i); + stor.stamp_ = robot::Time().fromNSec(i); cache.insertData(stor); } for ( uint64_t i = 1; i < runs ; i++ ) { - cache.getData(ros::Time().fromNSec(i), stor); + cache.getData(robot::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); std::stringstream ss; ss << values[i]; EXPECT_EQ(stor.header.frame_id, ss.str()); @@ -173,36 +173,36 @@ TEST(TimeCache, ZeroAtFront) for ( uint64_t i = 1; i < runs ; i++ ) { stor.frame_id_ = i; - stor.stamp_ = ros::Time().fromNSec(i); + stor.stamp_ = robot::Time().fromNSec(i); cache.insertData(stor); } stor.frame_id_ = runs; - stor.stamp_ = ros::Time().fromNSec(runs); + stor.stamp_ = robot::Time().fromNSec(runs); cache.insertData(stor); for ( uint64_t i = 1; i < runs ; i++ ) { - cache.getData(ros::Time().fromNSec(i), stor); + cache.getData(robot::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); } - cache.getData(ros::Time(), stor); + cache.getData(robot::Time(), stor); EXPECT_EQ(stor.frame_id_, runs); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs)); stor.frame_id_ = runs; - stor.stamp_ = ros::Time().fromNSec(runs+1); + stor.stamp_ = robot::Time().fromNSec(runs+1); cache.insertData(stor); //Make sure we get a different value now that a new values is added at the front - cache.getData(ros::Time(), stor); + cache.getData(robot::Time(), stor); EXPECT_EQ(stor.frame_id_, runs); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs+1)); } @@ -233,14 +233,14 @@ TEST(TimeCache, CartesianInterpolation) stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); stor.frame_id_ = 2; - stor.stamp_ = ros::Time().fromNSec(step * 100 + offset); + stor.stamp_ = robot::Time().fromNSec(step * 100 + offset); cache.insertData(stor); } for (int pos = 0; pos < 100 ; pos ++) { uint64_t time = offset + pos; - cache.getData(ros::Time().fromNSec(time), stor); + cache.getData(robot::Time().fromNSec(time), stor); uint64_t time_out = stor.stamp_.toNSec(); double x_out = stor.translation_.x(); double y_out = stor.translation_.y(); @@ -286,13 +286,13 @@ TEST(TimeCache, ReparentingInterpolationProtection) stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); stor.frame_id_ = step + 4; - stor.stamp_ = ros::Time().fromNSec(step * 100 + offset); + stor.stamp_ = robot::Time().fromNSec(step * 100 + offset); cache.insertData(stor); } for (int pos = 0; pos < 100 ; pos ++) { - EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor)); + EXPECT_TRUE(cache.getData(robot::Time().fromNSec(offset + pos), stor)); double x_out = stor.translation_.x(); double y_out = stor.translation_.y(); double z_out = stor.translation_.z(); @@ -354,14 +354,14 @@ TEST(TimeCache, AngularInterpolation) quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); stor.rotation_ = quats[step]; stor.frame_id_ = 3; - stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 + stor.stamp_ = robot::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 cache.insertData(stor); } for (int pos = 0; pos < 100 ; pos ++) { uint64_t time = offset + pos; - cache.getData(ros::Time().fromNSec(time), stor); + cache.getData(robot::Time().fromNSec(time), stor); uint64_t time_out = stor.stamp_.toNSec(); tf3::Quaternion quat (stor.rotation_); @@ -388,14 +388,14 @@ TEST(TimeCache, DuplicateEntries) TransformStorage stor; setIdentity(stor); stor.frame_id_ = 3; - stor.stamp_ = ros::Time().fromNSec(1); + stor.stamp_ = robot::Time().fromNSec(1); cache.insertData(stor); cache.insertData(stor); - cache.getData(ros::Time().fromNSec(1), stor); + cache.getData(robot::Time().fromNSec(1), stor); //printf(" stor is %f\n", stor.translation_.x()); EXPECT_TRUE(!std::isnan(stor.translation_.x())); diff --git a/src/Libraries/tf3/test/simple_tf3_core.cpp b/src/Libraries/tf3/test/simple_tf3_core.cpp index 5fc8cb7..fbe511e 100644 --- a/src/Libraries/tf3/test/simple_tf3_core.cpp +++ b/src/Libraries/tf3/test/simple_tf3_core.cpp @@ -46,7 +46,7 @@ TEST(tf3, setTransformValid) tf3::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; - st.header.stamp = ros::Time(1.0); + st.header.stamp = robot::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); @@ -58,7 +58,7 @@ TEST(tf3, setTransformInvalidQuaternion) tf3::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; - st.header.stamp = ros::Time(1.0); + st.header.stamp = robot::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 0; EXPECT_FALSE(tfc.setTransform(st, "authority1")); @@ -68,17 +68,17 @@ TEST(tf3, setTransformInvalidQuaternion) TEST(tf3_lookupTransform, LookupException_Nothing_Exists) { tf3::BufferCore tfc; - EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf3::LookupException); + EXPECT_THROW(tfc.lookupTransform("a", "b", robot::Time().fromSec(1.0)), tf3::LookupException); } TEST(tf3_canTransform, Nothing_Exists) { tf3::BufferCore tfc; - EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0))); + EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0))); std::string error_msg = std::string(); - EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg)); + EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0), &error_msg)); ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist."); } @@ -88,11 +88,11 @@ TEST(tf3_lookupTransform, LookupException_One_Exists) tf3::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; - st.header.stamp = ros::Time(1.0); + st.header.stamp = robot::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); - EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf3::LookupException); + EXPECT_THROW(tfc.lookupTransform("foo", "bar", robot::Time().fromSec(1.0)), tf3::LookupException); } @@ -101,11 +101,11 @@ TEST(tf3_canTransform, One_Exists) tf3::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; - st.header.stamp = ros::Time(1.0); + st.header.stamp = robot::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); - EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0))); + EXPECT_FALSE(tfc.canTransform("foo", "bar", robot::Time().fromSec(1.0))); } TEST(tf3_chainAsVector, chain_v_configuration) @@ -114,7 +114,7 @@ TEST(tf3_chainAsVector, chain_v_configuration) tf3::TestBufferCore tBC; geometry_msgs::TransformStamped st; - st.header.stamp = ros::Time(0); + st.header.stamp = robot::Time(0); st.transform.rotation.w = 1; st.header.frame_id = "a"; @@ -136,34 +136,34 @@ TEST(tf3_chainAsVector, chain_v_configuration) std::vector chain; - mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain); + mBC._chainAsVector( "c", robot::Time(), "c", robot::Time(), "c", chain); EXPECT_EQ( 0, chain.size()); - mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain); + mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "c", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain); + mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "c", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); - mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain); + mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "a", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); - mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain); + mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "a", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); - mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain); + mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "c", chain); EXPECT_EQ( 5, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); @@ -172,7 +172,7 @@ TEST(tf3_chainAsVector, chain_v_configuration) if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); - mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain); + mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "a", chain); EXPECT_EQ( 5, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); @@ -181,7 +181,7 @@ TEST(tf3_chainAsVector, chain_v_configuration) if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); - mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain); + mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "e", chain); EXPECT_EQ( 5, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); @@ -197,7 +197,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration) tf3::TestBufferCore tBC; geometry_msgs::TransformStamped st; - st.header.stamp = ros::Time(0); + st.header.stamp = robot::Time(0); st.transform.rotation.w = 1; st.header.frame_id = "a"; @@ -217,7 +217,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration) mBC.setTransform(st, "authority1"); std::vector id_chain; - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ(5, id_chain.size() ); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); @@ -227,7 +227,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration) if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4])); id_chain.clear(); - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); EXPECT_EQ(5, id_chain.size() ); if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); @@ -244,7 +244,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration) tf3::TestBufferCore tBC; geometry_msgs::TransformStamped st; - st.header.stamp = ros::Time(0); + st.header.stamp = robot::Time(0); st.transform.rotation.w = 1; // st.header.frame_id = "aaa"; @@ -272,7 +272,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration) mBC.setTransform(st, "authority1"); std::vector id_chain; - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain); EXPECT_EQ(5, id_chain.size()); if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0])); @@ -281,7 +281,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration) if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ(5, id_chain.size()); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); @@ -289,24 +289,24 @@ TEST(tf3_walkToTopParent, walk_v_configuration) if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4])); - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 3 ); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 3 ); if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2])); - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 2 ); if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1])); - tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain); + tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 2 ); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); @@ -315,6 +315,6 @@ TEST(tf3_walkToTopParent, walk_v_configuration) int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - ros::Time::init(); //needed for ros::TIme::now() + robot::Time::init(); //needed for robot::TIme::now() return RUN_ALL_TESTS(); } diff --git a/src/Libraries/tf3/test/speed_test.cpp b/src/Libraries/tf3/test/speed_test.cpp index 5dbeb2c..597d9d5 100644 --- a/src/Libraries/tf3/test/speed_test.cpp +++ b/src/Libraries/tf3/test/speed_test.cpp @@ -51,13 +51,13 @@ int main(int argc, char** argv) tf3::BufferCore bc; geometry_msgs::TransformStamped t; - t.header.stamp = ros::Time(1); + t.header.stamp = robot::Time(1); t.header.frame_id = "root"; t.child_frame_id = "0"; t.transform.translation.x = 1; t.transform.rotation.w = 1.0; bc.setTransform(t, "me"); - t.header.stamp = ros::Time(2); + t.header.stamp = robot::Time(2); bc.setTransform(t, "me"); for (uint32_t i = 1; i < num_levels / 2; ++i) @@ -69,7 +69,7 @@ int main(int argc, char** argv) std::stringstream child_ss; child_ss << i; - t.header.stamp = ros::Time(j); + t.header.stamp = robot::Time(j); t.header.frame_id = parent_ss.str(); t.child_frame_id = child_ss.str(); bc.setTransform(t, "me"); @@ -79,10 +79,10 @@ int main(int argc, char** argv) t.header.frame_id = "root"; std::stringstream ss; ss << num_levels/2; - t.header.stamp = ros::Time(1); + t.header.stamp = robot::Time(1); t.child_frame_id = ss.str(); bc.setTransform(t, "me"); - t.header.stamp = ros::Time(2); + t.header.stamp = robot::Time(2); bc.setTransform(t, "me"); for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) @@ -94,7 +94,7 @@ int main(int argc, char** argv) std::stringstream child_ss; child_ss << i; - t.header.stamp = ros::Time(j); + t.header.stamp = robot::Time(j); t.header.frame_id = parent_ss.str(); t.child_frame_id = child_ss.str(); bc.setTransform(t, "me"); @@ -113,13 +113,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0)); + out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(0)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -127,13 +127,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1)); + out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -141,13 +141,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5)); + out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1.5)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -155,13 +155,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2)); + out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(2)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -169,13 +169,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - bc.canTransform(v_frame1, v_frame0, ros::Time(0)); + bc.canTransform(v_frame1, v_frame0, robot::Time(0)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -183,13 +183,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - bc.canTransform(v_frame1, v_frame0, ros::Time(1)); + bc.canTransform(v_frame1, v_frame0, robot::Time(1)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -197,13 +197,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - bc.canTransform(v_frame1, v_frame0, ros::Time(1.5)); + bc.canTransform(v_frame1, v_frame0, robot::Time(1.5)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -211,13 +211,13 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - bc.canTransform(v_frame1, v_frame0, ros::Time(2)); + bc.canTransform(v_frame1, v_frame0, robot::Time(2)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } @@ -225,27 +225,27 @@ int main(int argc, char** argv) #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { - bc.canTransform(v_frame1, v_frame0, ros::Time(3)); + bc.canTransform(v_frame1, v_frame0, robot::Time(3)); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { - ros::WallTime start = ros::WallTime::now(); + robot::WallTime start = robot::WallTime::now(); std::string str; for (uint32_t i = 0; i < count; ++i) { - bc.canTransform(v_frame1, v_frame0, ros::Time(3), &str); + bc.canTransform(v_frame1, v_frame0, robot::Time(3), &str); } - ros::WallTime end = ros::WallTime::now(); - ros::WallDuration dur = end - start; + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration dur = end - start; CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif diff --git a/src/Libraries/tf3/test/static_cache_test.cpp b/src/Libraries/tf3/test/static_cache_test.cpp index 11e6c23..eeb27d7 100644 --- a/src/Libraries/tf3/test/static_cache_test.cpp +++ b/src/Libraries/tf3/test/static_cache_test.cpp @@ -56,14 +56,14 @@ TEST(StaticCache, Repeatability) for ( uint64_t i = 1; i < runs ; i++ ) { stor.frame_id_ = CompactFrameID(i); - stor.stamp_ = ros::Time().fromNSec(i); + stor.stamp_ = robot::Time().fromNSec(i); cache.insertData(stor); - cache.getData(ros::Time().fromNSec(i), stor); + cache.getData(robot::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i)); } } @@ -76,14 +76,14 @@ TEST(StaticCache, DuplicateEntries) TransformStorage stor; setIdentity(stor); stor.frame_id_ = CompactFrameID(3); - stor.stamp_ = ros::Time().fromNSec(1); + stor.stamp_ = robot::Time().fromNSec(1); cache.insertData(stor); cache.insertData(stor); - cache.getData(ros::Time().fromNSec(1), stor); + cache.getData(robot::Time().fromNSec(1), stor); //printf(" stor is %f\n", stor.transform.translation.x); EXPECT_TRUE(!std::isnan(stor.translation_.x())); diff --git a/src/Libraries/tf3/test/test_transform_datatypes.cpp b/src/Libraries/tf3/test/test_transform_datatypes.cpp index 55df3d4..552d6ae 100644 --- a/src/Libraries/tf3/test/test_transform_datatypes.cpp +++ b/src/Libraries/tf3/test/test_transform_datatypes.cpp @@ -35,8 +35,8 @@ TEST(Stamped, assignment) { - tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); - tf3::Stamped second("baz", ros::Time(0), "my_frame_id"); + tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); + tf3::Stamped second("baz", robot::Time(0), "my_frame_id"); EXPECT_NE(second, first); second = first; @@ -45,8 +45,8 @@ TEST(Stamped, assignment) TEST(Stamped, setData) { - tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); - tf3::Stamped second("baz", ros::Time(0), "my_frame_id"); + tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); + tf3::Stamped second("baz", robot::Time(0), "my_frame_id"); EXPECT_NE(second, first); second.setData("foobar"); @@ -55,7 +55,7 @@ TEST(Stamped, setData) TEST(Stamped, copy_constructor) { - tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); + tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); tf3::Stamped second(first); EXPECT_EQ(second, first); @@ -63,7 +63,7 @@ TEST(Stamped, copy_constructor) TEST(Stamped, default_constructor) { - tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); + tf3::Stamped first("foobar", robot::Time(0), "my_frame_id"); tf3::Stamped second; EXPECT_NE(second, first); @@ -72,6 +72,6 @@ TEST(Stamped, default_constructor) int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::Time::init(); + robot::Time::init(); return RUN_ALL_TESTS(); } diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h b/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h index 1506514..ae1319b 100755 --- a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h +++ b/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h @@ -136,7 +136,7 @@ namespace nav_core * @brief Constructs the local planner * @param name The name to give this instance of the local planner * @param tf A pointer to a transform listener - * @param costmap_ros The cost map to use for assigning costs to local plans + * @param costmap_robot The cost map to use for assigning costs to local plans */ virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0; diff --git a/src/Navigations/Cores/nav_core2/CMakeLists.txt b/src/Navigations/Cores/nav_core2/CMakeLists.txt index 287af05..c11242c 100755 --- a/src/Navigations/Cores/nav_core2/CMakeLists.txt +++ b/src/Navigations/Cores/nav_core2/CMakeLists.txt @@ -28,8 +28,7 @@ target_link_libraries(nav_core2 INTERFACE tf3 nav_grid nav_2d_msgs - robot::node_handle - robot::console + robot_cpp ) # Set include directories diff --git a/src/Navigations/Cores/nav_core2/README.md b/src/Navigations/Cores/nav_core2/README.md index 9e2ccd5..dc9250e 100755 --- a/src/Navigations/Cores/nav_core2/README.md +++ b/src/Navigations/Cores/nav_core2/README.md @@ -3,15 +3,15 @@ A replacement interface for [nav_core](https://github.com/ros-planning/navigatio There were a few key reasons for creating new interfaces rather than extending the old ones. * Use `nav_2d_msgs` to eliminate unused data fields - * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROS`. + * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`. * Provide more data in the interfaces for easier testing. * Use Exceptions rather than booleans to provide more information about the types of errors encountered. ## `Costmap` -`costmap_2d::Costmap2DROS` has been a vital part of the navigation stack for years, but it was not without its flaws. - * Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROS`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested. +`costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws. + * Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROBOT`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested. * Initialization also started an update thread, which is also not always needed in testing. - * Using `Costmap2DROS` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation. + * Using `Costmap2DROBOT` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation. The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as * a mutex @@ -20,7 +20,7 @@ The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `n The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach. -One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROS`. +One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROBOT`. Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms. @@ -29,7 +29,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan | `nav_core` | `nav_core2` | comments | |---|--|---| -|`void initialize(std::string, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener| +|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener| |`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.| ## Local Planner @@ -37,7 +37,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl | `nav_core` | `nav_core2` | comments | |---|--|---| -|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers| +|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers| |(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan` |`bool setPlan(const std::vector&)`|`setPlan(const nav_2d_msgs::Path2D&)`|| |`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.| diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h b/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h index df01ac3..f461434 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h @@ -137,7 +137,7 @@ public: * @class CostmapDataLagException * @brief Indicates costmap is out of date because data in not up to date. * - * Functions similarly to `!Costmap2DROS::isCurrent()` + * Functions similarly to `!Costmap2Drobot::isCurrent()` */ class CostmapDataLagException: public CostmapSafetyException { diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt index e34a722..de09229 100755 --- a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt +++ b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt @@ -33,6 +33,7 @@ target_link_libraries(local_planner_adapter Boost::system dl costmap_adapter + robot_cpp ${PACKAGES_DIR} ) target_include_directories(local_planner_adapter PRIVATE @@ -53,6 +54,33 @@ target_include_directories(global_planner_adapter PRIVATE $ $) +# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs. +# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib, +# which has a different ABI (Costmap2DROBOT) and causes missing symbols. +set(_NAV_CORE_ADAPTER_RPATH + "${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter" + "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d" + "${CMAKE_BINARY_DIR}/src/Libraries/tf3" + "${CMAKE_BINARY_DIR}/src/Libraries/robot_time" + "${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp" +) + +set_target_properties(costmap_adapter PROPERTIES + BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}" + INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}" + LINK_FLAGS "-Wl,--disable-new-dtags" +) +set_target_properties(local_planner_adapter PROPERTIES + BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}" + INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}" + LINK_FLAGS "-Wl,--disable-new-dtags" +) +set_target_properties(global_planner_adapter PROPERTIES + BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}" + INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}" + LINK_FLAGS "-Wl,--disable-new-dtags" +) + # Cài đặt header files install(DIRECTORY include/nav_core_adapter DESTINATION include diff --git a/src/Navigations/Cores/nav_core_adapter/README.md b/src/Navigations/Cores/nav_core_adapter/README.md index 38c067c..a0f90dd 100755 --- a/src/Navigations/Cores/nav_core_adapter/README.md +++ b/src/Navigations/Cores/nav_core_adapter/README.md @@ -3,7 +3,7 @@ This package contains adapters for using `nav_core` plugins as `nav_core2` plugi * Converting between 2d and 3d datatypes. * Converting between returning false and throwing exceptions on failure. -We also provide an adapter for using a `costmap_2d::Costmap2DROS` as a plugin for the `nav_core2::Costmap` interface. +We also provide an adapter for using a `costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface. ## Adapter Classes * Global Planner Adapters @@ -13,8 +13,8 @@ as a `nav_core2` plugin, like in `locomotor`. * Local Planner Adapter * `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity. * There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided. - * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROS` object. It is not a perfect adaptation, because - * `Costmap2DROS` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though. + * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROBOT` object. It is not a perfect adaptation, because + * `Costmap2DROBOT` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though. * `setInfo` is not implemented. ## Parameter Setup diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h index 194e8f4..29fdd44 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h @@ -48,14 +48,14 @@ class CostmapAdapter : public nav_core2::Costmap { public: /** - * @brief Deconstructor for possibly freeing the costmap_ros_ object + * @brief Deconstructor for possibly freeing the costmap_robot_ object */ virtual ~CostmapAdapter(); /** - * @brief Initialize from an existing Costmap2DROS object - * @param costmap_ros A Costmap2DROS object - * @param needs_destruction Whether to free the costmap_ros object when this class is destroyed + * @brief Initialize from an existing Costmap2DROBOT object + * @param costmap_robot A Costmap2DROBOT object + * @param needs_destruction Whether to free the costmap_robot object when this class is destroyed */ void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false); diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h index 7b2930d..cbbc3b7 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h @@ -196,11 +196,10 @@ namespace nav_core_adapter costmap_2d::Costmap2DROBOT *costmap_robot_; boost::recursive_mutex configuration_mutex_; - // std::shared_ptr> dsrv_; robot::NodeHandle private_nh_; robot::NodeHandle adapter_nh_; - // void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level); + nav_core_adapter::NavCoreAdapterConfig last_config_; nav_core_adapter::NavCoreAdapterConfig default_config_; bool setup_; diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp index 36a163a..4555aaf 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -54,20 +55,19 @@ namespace nav_core_adapter LocalPlannerAdapter::~LocalPlannerAdapter() { - std::cout << "=== LocalPlannerAdapter destructor called ===" << std::endl; + robot::log_info("=== LocalPlannerAdapter destructor called ==="); if (planner_) { - std::cout << "Planner use_count before reset: " << planner_.use_count() << std::endl; planner_.reset(); - std::cout << "Planner reset complete" << std::endl; + robot::log_info("Planner reset complete"); } else { - std::cout << "Planner already null" << std::endl; + robot::log_warning("Planner already null"); } - std::cout << "=== LocalPlannerAdapter destructor finished ===" << std::endl; + robot::log_info("=== LocalPlannerAdapter destructor finished ==="); } /** @@ -80,10 +80,9 @@ namespace nav_core_adapter costmap_adapter_ = std::make_shared(); costmap_adapter_->initialize(costmap_robot); - robot::NodeHandle nh; private_nh_ = robot::NodeHandle("~"); adapter_nh_ = robot::NodeHandle(private_nh_, name); - robot::log_info("Adapter namespace: %s", private_nh_.getNamespace().c_str()); + std::string planner_name; if (adapter_nh_.hasParam("planner_name")) { @@ -91,29 +90,35 @@ namespace nav_core_adapter } else { - planner_name = nav_2d_utils::loadParameterWithDeprecation( - adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); + planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); adapter_nh_.setParam("planner_name", planner_name); } - robot::log_info("Loading plugin %s", planner_name.c_str()); - // planner_ = planner_loader_.createInstance(planner_name); - std::string path_file_so; - planner_loader_ = boost::dll::import_alias( - path_file_so, planner_name, boost::dll::load_mode::append_decorations); - planner_ = planner_loader_(); - if (!planner_) + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + try { - robot::log_error("Failed to load planner %s", planner_name.c_str()); + planner_loader_ = boost::dll::import_alias( + path_file_so, planner_name, boost::dll::load_mode::append_decorations); + planner_ = planner_loader_(); + if (!planner_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str()); + exit(EXIT_FAILURE); + } + robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str()); + planner_->initialize(adapter_nh_, planner_name, tf_, costmap_robot_); + robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str()); + } + catch (const boost::system::system_error &ex) + { + robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what()); + exit(EXIT_FAILURE); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what()); exit(EXIT_FAILURE); } - planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_); - - // odom_sub_ = std::make_shared(nh); - // dsrv_ = std::make_shared>(configuration_mutex_, adapter_nh_); - // dynamic_reconfigure::Server::CallbackType cb = - // boost::bind(&LocalPlannerAdapter::reconfigureCB, this, _1, _2); - // dsrv_->setCallback(cb); } bool LocalPlannerAdapter::swapPlanner(std::string planner_name) @@ -140,7 +145,7 @@ namespace nav_core_adapter std::cerr << "Failed to load planner " << planner_name << std::endl; exit(EXIT_FAILURE); } - new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_); + new_planner->initialize(adapter_nh_, planner_name, tf_, costmap_robot_); if (planner_) planner_.reset(); planner_ = new_planner; diff --git a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp index 0271723..0ffb17f 100755 --- a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp +++ b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp @@ -37,11 +37,11 @@ TEST(LocalPlannerAdapter, unload_local_planner) { - tf2_ros::Buffer tf; + tf2_robot::Buffer tf; tf.setUsingDedicatedThread(true); // This empty transform is added to satisfy the constructor of - // Costmap2DROS, which waits for the transform from map to base_link + // Costmap2DROBOT, which waits for the transform from map to base_link // to become available. geometry_msgs::TransformStamped base_rel_map; base_rel_map.child_frame_id = "base_link"; @@ -51,19 +51,19 @@ TEST(LocalPlannerAdapter, unload_local_planner) nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter(); - costmap_2d::Costmap2DROS costmap_ros("local_costmap", tf); - lpa->initialize("lpa", &tf, &costmap_ros); + costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf); + lpa->initialize("lpa", &tf, &costmap_robot); delete lpa; // Simple test to make sure costmap hasn't been deleted - EXPECT_EQ("map", costmap_ros.getGlobalFrameID()); + EXPECT_EQ("map", costmap_robot.getGlobalFrameID()); } int main(int argc, char** argv) { - ros::init(argc, argv, "unload_test"); + robot::init(argc, argv, "unload_test"); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/src/Navigations/Libraries/nav_grid/README.md b/src/Navigations/Libraries/nav_grid/README.md index 7ec1957..44ed97c 100755 --- a/src/Navigations/Libraries/nav_grid/README.md +++ b/src/Navigations/Libraries/nav_grid/README.md @@ -1,6 +1,6 @@ # nav_grid -Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROS` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories. +Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROBOT` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories. ![nav_grid diagram](doc/nav_grid.png) diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index b233c9c..0ebc8ea 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -118,8 +118,7 @@ if(BUILDING_WITH_CATKIN) nav_core costmap_2d xmlrpcpp - robot::node_handle - robot::console + robot_cpp tf3_sensor_msgs tf3_geometry_msgs dl @@ -147,11 +146,12 @@ else() data_convert dl pthread + nav_2d_utils ) target_link_libraries(move_base PUBLIC ${PACKAGES_DIR} - PUBLIC robot::node_handle robot::console + PUBLIC robot_cpp PRIVATE Boost::boost ) @@ -183,7 +183,7 @@ endif() # ======================================================== add_executable(move_base_main src/move_base_main.cpp) target_link_libraries(move_base_main - PRIVATE move_base + PRIVATE move_base robot_cpp PRIVATE Boost::boost ) @@ -193,6 +193,18 @@ if(BUILDING_WITH_CATKIN) ) endif() +# Set RPATH for executable to find libraries in build directory first +# Use RPATH instead of RUNPATH for higher priority +set_target_properties(move_base_main PROPERTIES + BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils" + INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils" + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH TRUE + SKIP_BUILD_RPATH FALSE +) +# Force use of RPATH instead of RUNPATH +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags") + # ======================================================== # Installation # ======================================================== 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 a397a1e..778043a 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 @@ -295,6 +295,7 @@ namespace move_base std::string robot_base_frame_, global_frame_; std::vector recovery_behavior_names_; unsigned int recovery_index_; + bool recovery_behavior_enabled_; geometry_msgs::PoseStamped global_pose_; double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_; @@ -302,10 +303,10 @@ namespace move_base int32_t max_planning_retries_; uint32_t planning_retries_; double conservative_reset_dist_, clearing_radius_; - // ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_; - // ros::Subscriber goal_sub_; - // ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; - bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; + // robot::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_; + // robot::Subscriber goal_sub_; + // robot::ServiceServer make_plan_srv_, clear_costmaps_srv_; + bool shutdown_costmaps_, clearing_rotation_allowed_; bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_; double oscillation_timeout_, oscillation_distance_; @@ -327,9 +328,6 @@ namespace move_base geometry_msgs::PoseStamped planner_goal_; boost::thread *planner_thread_; - // boost::recursive_mutex configuration_mutex_; - // dynamic_reconfigure::Server *dsrv_; - // void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level); move_base::MoveBaseConfig last_config_; move_base::MoveBaseConfig default_config_; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index bb4b3b6..e02985a 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -32,6 +32,7 @@ move_base::MoveBase::MoveBase(TFListenerPtr tf) runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), pause_ctr_(false), paused_(false) { + initialize(tf); } move_base::MoveBase::~MoveBase() @@ -39,49 +40,66 @@ move_base::MoveBase::~MoveBase() recovery_behaviors_.clear(); if (planner_costmap_robot_ != NULL) + { delete planner_costmap_robot_; + planner_costmap_robot_ = NULL; + } if (controller_costmap_robot_ != NULL) + { delete controller_costmap_robot_; + controller_costmap_robot_ = NULL; + } - planner_thread_->interrupt(); - planner_thread_->join(); + if (planner_thread_ != NULL) + { + planner_thread_->interrupt(); + planner_thread_->join(); + delete planner_thread_; + planner_thread_ = NULL; + } + + if (planner_plan_ != NULL) + { + delete planner_plan_; + planner_plan_ = NULL; + } + + if (latest_plan_ != NULL) + { + delete latest_plan_; + latest_plan_ = NULL; + } + + if (controller_plan_ != NULL) + { + delete controller_plan_; + controller_plan_ = NULL; + } - delete planner_thread_; - delete planner_plan_; - delete latest_plan_; - delete controller_plan_; planner_.reset(); tc_.reset(); initialized_ = false; tf_.reset(); - planner_plan_ = NULL; - latest_plan_ = NULL; - controller_plan_ = NULL; - planner_thread_ = NULL; - planner_costmap_robot_ = NULL; - controller_costmap_robot_ = NULL; } void move_base::MoveBase::initialize(TFListenerPtr tf) { - printf("[%s:%d] ========== Begin: initialize() ==========\n", __FILE__, __LINE__); + printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__); if (!initialized_) { tf_ = tf; // NodeHandle("~") will automatically load YAML files from config directory - robot::NodeHandle nh("~"); - private_nh_ = nh; - private_nh_.printAllParams(); + private_nh_ = robot::NodeHandle("~"); recovery_trigger_ = PLANNING_R; // get some parameters that will be global to the move base node std::string global_planner, local_planner; private_nh_.getParam("base_global_planner", global_planner, std::string("")); - printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); + robot::log_info("[%s %d] global_planner: %s", __FILE__, __LINE__, global_planner.c_str()); private_nh_.getParam("base_local_planner", local_planner, std::string("")); - printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str()); + robot::log_info("[%s %d] local_planner: %s", __FILE__, __LINE__, local_planner.c_str()); // Handle nested YAML nodes for costmap config std::string robot_base_frame; @@ -135,7 +153,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // set up the planner's thread planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); - // we'll assume the radius of the robot to be consistent with what's specified for the costmaps // From config param double inscribed_radius; @@ -151,10 +168,11 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false); private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true); private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true); - + // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); planner_costmap_robot_->pause(); + // initialize the global planner try { @@ -166,17 +184,17 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) planner_ = planner_loader_(); if (!planner_) { - robot::log_error("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); throw std::runtime_error("Failed to load global planner " + global_planner); } if(planner_->initialize(global_planner, planner_costmap_robot_)) - robot::log_info("[%s:%d] Global planner initialized successfully", __FILE__, __LINE__); + robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__); else - robot::log_error("[%s:%d] Global planner initialized failed", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__); } catch (const std::exception &ex) { - robot::log_error("[%s:%d] EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what()); throw std::runtime_error("Failed to create the " + global_planner + " planner"); } // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map @@ -188,18 +206,18 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so"; controller_loader_ = boost::dll::import_alias( - path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations); + path_file_so, local_planner, boost::dll::load_mode::append_decorations); tc_ = controller_loader_(); if (!tc_) { - robot::log_error("[%s:%d] ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__); throw std::runtime_error("Failed to load local planner " + local_planner); } - // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_); + tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_); } catch (const std::exception &ex) { - robot::log_error("[%s:%d] EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what()); throw std::runtime_error("Failed to create the " + local_planner + " planner"); } @@ -207,7 +225,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) planner_costmap_robot_->start(); controller_costmap_robot_->start(); - try { old_linear_fwd_ = tc_->getTwistLinear(true); @@ -221,12 +238,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) std::cerr << e.what() << '\n'; } - // // advertise a service for getting a plan - // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this); - - // // advertise a service for clearing the costmaps - // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this); - // if we shutdown our costmaps when we're deactivated... we'll do that now if (shutdown_costmaps_) { @@ -237,7 +248,7 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // load any user specified recovery behaviors, and if that fails load the defaults if (!loadRecoveryBehaviors(private_nh_)) { - robot::log_warning("[%s:%d] Loading default recovery behaviors", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n Loading default recovery behaviors", __FILE__, __LINE__); loadDefaultRecoveryBehaviors(); } @@ -254,15 +265,15 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) } else { - robot::log_error("[%s:%d] ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); + robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); } initialized_ = true; - robot::log_info("[%s:%d] ========== End: initialize() - SUCCESS ==========", __FILE__, __LINE__); + robot::log_info("========== End: initialize() - SUCCESS =========="); } else { - robot::log_warning("[%s:%d] Already initialized, skipping", __FILE__, __LINE__); + robot::log_warning("[%s:%d]\n Already initialized, skipping", __FILE__, __LINE__); } } @@ -336,7 +347,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double if (cancel_ctr_) cancel_ctr_ = false; // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = ros::Time::now(); + // action_goal.header.stamp = robot::Time::now(); // action_goal.goal.target_pose = goal; // action_goal_pub_.publish(action_goal); @@ -427,7 +438,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs:: cancel_ctr_ = false; // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = ros::Time::now(); + // action_goal.header.stamp = robot::Time::now(); // action_goal.goal.target_pose = goal; // action_goal_pub_.publish(action_goal); lock.unlock(); @@ -478,7 +489,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, if (cancel_ctr_) cancel_ctr_ = false; // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = ros::Time::now(); + // action_goal.header.stamp = robot::Time::now(); // action_goal.goal.target_pose = goal; // action_goal_pub_.publish(action_goal); @@ -543,7 +554,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl } // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = ros::Time::now(); + // action_goal.header.stamp = robot::Time::now(); // action_goal.goal.target_pose = goal; // action_goal.goal.target_pose.pose.position.x = pose.pose.position.x + 0.05 * cos(tf2::getYaw(pose.pose.orientation)); // action_goal.goal.target_pose.pose.position.y = pose.pose.position.y + 0.05 * sin(tf2::getYaw(pose.pose.orientation)); @@ -687,26 +698,26 @@ bool move_base::MoveBase::setTwistAngular(const geometry_msgs::Vector3 &angular) void move_base::MoveBase::setYawGoalTolerance(double yaw_goal_tolerance) { - double yaw = 0.0; - if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0)) - { - private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance); - std::cout << private_nh_.getNamespace() << " yaw_goal_tolerance " << yaw_goal_tolerance << std::endl; - } - else - std::cerr << "yaw_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl; + // double yaw = 0.0; + // if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0)) + // { + // private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance); + // std::cout << private_nh_.getNamespace() << " yaw_goal_tolerance " << yaw_goal_tolerance << std::endl; + // } + // else + // std::cerr << "yaw_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl; } void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance) { - double xy = 0.0; - if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0)) - { - private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance); - std::cout << private_nh_.getNamespace() << " xy_goal_tolerance " << xy_goal_tolerance << std::endl; - } - else - std::cerr << "xy_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl; + // double xy = 0.0; + // if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0)) + // { + // private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance); + // std::cout << private_nh_.getNamespace() << " xy_goal_tolerance " << xy_goal_tolerance << std::endl; + // } + // else + // std::cerr << "xy_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl; } // bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) @@ -754,90 +765,89 @@ bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std:: bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) { - YAML::Node behavior_list = node.getParamValue("recovery_behaviors"); - if (behavior_list && behavior_list.IsSequence()) - { - // Validate all behaviors first - for (size_t i = 0; i < behavior_list.size(); ++i) - { - YAML::Node behavior = behavior_list[i].as(); - if (!behavior.IsMap()) - { - std::cerr << "Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead." << std::endl; - return false; - } + // YAML::Node behavior_list = node.getParamValue("recovery_behaviors"); + // if (behavior_list && behavior_list.IsSequence()) + // { + // // Validate all behaviors first + // for (size_t i = 0; i < behavior_list.size(); ++i) + // { + // YAML::Node behavior = behavior_list[i].as(); + // if (!behavior.IsMap()) + // { + // robot::log_warning("Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead."); + // return false; + // } - if (!behavior["name"] || !behavior["type"]) - { - std::cerr << "Recovery behaviors must have a name and a type. Using the default recovery behaviors instead." << std::endl; - return false; - } + // if (!behavior["name"] || !behavior["type"]) + // { + // robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead."); + // return false; + // } - // check for recovery behaviors with the same name - std::string name_i = behavior["name"].as(); - printf("[%s:%d] name_i: %s\n", __FILE__, __LINE__, name_i.c_str()); - for (size_t j = i + 1; j < behavior_list.size(); ++j) - { - YAML::Node behavior_j = behavior_list[j].as(); - if (behavior_j.IsMap() && behavior_j["name"]) - { - std::string name_j = behavior_j["name"].as(); - if (name_i == name_j) - { - std::cerr << "A recovery behavior with the name " << name_i << " already exists, this is not allowed. Using the default recovery behaviors instead." << std::endl; - return false; - } - } - } - } + // // check for recovery behaviors with the same name + // std::string name_i = behavior["name"].as(); + // for (size_t j = i + 1; j < behavior_list.size(); ++j) + // { + // YAML::Node behavior_j = behavior_list[j].as(); + // if (behavior_j.IsMap() && behavior_j["name"]) + // { + // std::string name_j = behavior_j["name"].as(); + // if (name_i == name_j) + // { + // robot::log_warning("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.", name_i.c_str()); + // return false; + // } + // } + // } + // } - // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors - for (size_t i = 0; i < behavior_list.size(); ++i) - { - try - { - YAML::Node behavior = behavior_list[i].as(); - // Load recovery behavior using boost::dll::import_alias - // behavior["type"] should be the factory function name (e.g., "create_plugin") - std::string behavior_type = behavior["type"].as(); - std::string behavior_name = behavior["name"].as(); - std::string path_file_so; - // Load the factory function from the shared library - auto recovery_loader = boost::dll::import_alias( - path_file_so, behavior_type, boost::dll::load_mode::append_decorations); + // // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors + // for (size_t i = 0; i < behavior_list.size(); ++i) + // { + // try + // { + // YAML::Node behavior = behavior_list[i].as(); + // // Load recovery behavior using boost::dll::import_alias + // // behavior["type"] should be the factory function name (e.g., "create_plugin") + // std::string behavior_type = behavior["type"].as(); + // std::string behavior_name = behavior["name"].as(); + // std::string path_file_so; + // // Load the factory function from the shared library + // auto recovery_loader = boost::dll::import_alias( + // path_file_so, behavior_type, boost::dll::load_mode::append_decorations); - // Create instance using the factory function - std::shared_ptr behavior_ptr(recovery_loader()); + // // Create instance using the factory function + // std::shared_ptr behavior_ptr(recovery_loader()); - // shouldn't be possible, but it won't hurt to check - if (behavior_ptr == nullptr) - { - std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl; - return false; - } - // initialize the recovery behavior with its name - behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_); - recovery_behavior_names_.push_back(behavior_name); - recovery_behaviors_.push_back(behavior_ptr); - } - catch (const std::exception &ex) - { - std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as() - << ". Error: " << ex.what() << std::endl; - return false; - } - } - } - else if (behavior_list) - { - std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl; - return false; - } - else - { - // if no recovery_behaviors are specified, we'll just load the defaults - return false; - } + // // shouldn't be possible, but it won't hurt to check + // if (behavior_ptr == nullptr) + // { + // std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl; + // return false; + // } + // // initialize the recovery behavior with its name + // behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_); + // recovery_behavior_names_.push_back(behavior_name); + // recovery_behaviors_.push_back(behavior_ptr); + // } + // catch (const std::exception &ex) + // { + // std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as() + // << ". Error: " << ex.what() << std::endl; + // return false; + // } + // } + // } + // else if (behavior_list) + // { + // std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl; + // return false; + // } + // else + // { + // // if no recovery_behaviors are specified, we'll just load the defaults + // return false; + // } // if we've made it here... we've constructed a recovery behavior list successfully return true; @@ -856,7 +866,7 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors() // first, we'll load a recovery behavior to clear the costmap (conservative reset) try { - std::string path_file_so; + std::string path_file_so ; auto clear_costmap_loader = boost::dll::import_alias( path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); std::shared_ptr cons_clear(clear_costmap_loader()); diff --git a/src/Navigations/Packages/move_base/src/move_base_main.cpp b/src/Navigations/Packages/move_base/src/move_base_main.cpp index a269f2e..597e051 100644 --- a/src/Navigations/Packages/move_base/src/move_base_main.cpp +++ b/src/Navigations/Packages/move_base/src/move_base_main.cpp @@ -29,12 +29,71 @@ #include #include +#include +#include +#include +#include int main(int argc, char** argv) { - std::shared_ptr tf = std::make_shared(); - move_base::MoveBase move_base; - move_base.initialize(tf); - std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl; - return(0); + try { + std::shared_ptr tf = std::make_shared(); + + robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__); + auto create_plugin = boost::dll::import_alias( + "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so", + "MoveBase", boost::dll::load_mode::append_decorations); + + robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__); + move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); + + if (nav_ptr == nullptr) { + robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__); + return 1; + } + + robot::log_info("[%s:%d]\n Navigation created, initializing...", __FILE__, __LINE__); + nav_ptr->initialize(tf); + robot::log_info("[%s:%d]\n Navigation initialized successfully", __FILE__, __LINE__); + + std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl; + + // Explicitly reset before create_plugin goes out of scope to ensure proper cleanup order + robot::log_info("[%s:%d]\n Cleaning up...", __FILE__, __LINE__); + nav_ptr.reset(); + robot::log_info("[%s:%d]\n Cleanup complete", __FILE__, __LINE__); + + return 0; + } catch (const std::exception &e) { + robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what()); + return 1; + } catch (...) { + robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__); + return 1; + } + + + // robot::NodeHandle private_nh_ = robot::NodeHandle("~"); + // robot::NodeHandle adapter_nh_ = robot::NodeHandle(private_nh_, "LocalPlannerAdapter"); + // robot::log_info_at(__FILE__, __LINE__, "\nUsing adapter namespace: %s", adapter_nh_.getNamespace().c_str()); + + + // std::string planner_name; + // if (adapter_nh_.hasParam("planner_name")) + // { + // adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner")); + // } + // else + // { + // planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); + // adapter_nh_.setParam("planner_name", planner_name); + // } + // robot::log_info_at(__FILE__, __LINE__, "Planner name: %s", planner_name.c_str()); + // robot::NodeHandle parent_; + // parent_ = adapter_nh_; + // robot::NodeHandle planner_nh_ = robot::NodeHandle(parent_, "PNKXLocalPlanner"); + // std::string algorithm_nav_name; + // planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + // robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); + return 0; } \ No newline at end of file