From 99f014e14c689772eac7775e70ae80727d759570 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 10 Feb 2026 14:39:43 +0700 Subject: [PATCH] update c_api --- examples/CSharpExample.cs | 620 ------ examples/NavigationExample/Program.cs | 610 +++-- examples/NavigationExample/libnav_c_api.so | Bin 173512 -> 148832 bytes examples/run_example.sh | 2 - src/APIs/c_api/CMakeLists.txt | 31 +- src/APIs/c_api/include/convertor.h | 153 ++ src/APIs/c_api/include/geometry_msgs/Accel.h | 22 + .../include/geometry_msgs/AccelStamped.h | 23 + .../geometry_msgs/AccelWithCovariance.h | 22 + .../AccelWithCovarianceStamped.h | 23 + .../c_api/include/geometry_msgs/Inertia.h | 28 + .../include/geometry_msgs/InertiaStamped.h | 23 + src/APIs/c_api/include/geometry_msgs/Point.h | 21 + .../c_api/include/geometry_msgs/Point32.h | 22 + .../include/geometry_msgs/PointStamped.h | 23 + .../c_api/include/geometry_msgs/Polygon.h | 22 + .../include/geometry_msgs/PolygonStamped.h | 23 + src/APIs/c_api/include/geometry_msgs/Pose.h | 23 + src/APIs/c_api/include/geometry_msgs/Pose2D.h | 22 + .../c_api/include/geometry_msgs/PoseArray.h | 24 + .../c_api/include/geometry_msgs/PoseStamped.h | 23 + .../geometry_msgs/PoseWithCovariance.h | 23 + .../geometry_msgs/PoseWithCovarianceStamped.h | 23 + .../c_api/include/geometry_msgs/Quaternion.h | 23 + .../include/geometry_msgs/QuaternionStamped.h | 23 + .../c_api/include/geometry_msgs/Transform.h | 23 + .../include/geometry_msgs/TransformStamped.h | 24 + src/APIs/c_api/include/geometry_msgs/Twist.h | 22 + .../include/geometry_msgs/TwistStamped.h | 23 + .../geometry_msgs/TwistWithCovariance.h | 23 + .../TwistWithCovarianceStamped.h | 23 + .../c_api/include/geometry_msgs/Vector3.h | 22 + .../include/geometry_msgs/Vector3Stamped.h | 23 + src/APIs/c_api/include/geometry_msgs/Wrench.h | 22 + .../include/geometry_msgs/WrenchStamped.h | 23 + .../include/map_msgs/OccupancyGridUpdate.h | 33 + .../include/nav_2d_msgs/ComplexPolygon2D.h | 23 + .../c_api/include/nav_2d_msgs/NavGridInfo.h | 25 + .../include/nav_2d_msgs/NavGridOfChars.h | 25 + .../nav_2d_msgs/NavGridOfCharsUpdate.h | 25 + .../include/nav_2d_msgs/NavGridOfDoubles.h | 25 + .../nav_2d_msgs/NavGridOfDoublesUpdate.h | 25 + src/APIs/c_api/include/nav_2d_msgs/Path2D.h | 24 + src/APIs/c_api/include/nav_2d_msgs/Point2D.h | 21 + .../c_api/include/nav_2d_msgs/Polygon2D.h | 22 + .../include/nav_2d_msgs/Polygon2DCollection.h | 27 + .../include/nav_2d_msgs/Polygon2DStamped.h | 23 + src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h | 22 + .../c_api/include/nav_2d_msgs/Pose2DStamped.h | 23 + .../c_api/include/nav_2d_msgs/SwitchPlugin.h | 23 + .../include/nav_2d_msgs/SwitchPluginRequest.h | 20 + .../nav_2d_msgs/SwitchPluginResponse.h | 21 + src/APIs/c_api/include/nav_2d_msgs/Twist2D.h | 22 + .../c_api/include/nav_2d_msgs/Twist2D32.h | 22 + .../include/nav_2d_msgs/Twist2DStamped.h | 23 + .../c_api/include/nav_2d_msgs/UIntBounds.h | 23 + src/APIs/c_api/include/nav_c_api.h | 490 +--- src/APIs/c_api/include/nav_msgs/ActionTypes.h | 29 + src/APIs/c_api/include/nav_msgs/GetMap.h | 23 + .../c_api/include/nav_msgs/GetMapAction.h | 25 + .../include/nav_msgs/GetMapActionFeedback.h | 25 + .../c_api/include/nav_msgs/GetMapActionGoal.h | 25 + .../include/nav_msgs/GetMapActionResult.h | 25 + .../c_api/include/nav_msgs/GetMapFeedback.h | 20 + src/APIs/c_api/include/nav_msgs/GetMapGoal.h | 20 + .../c_api/include/nav_msgs/GetMapRequest.h | 20 + .../c_api/include/nav_msgs/GetMapResponse.h | 21 + .../c_api/include/nav_msgs/GetMapResult.h | 21 + src/APIs/c_api/include/nav_msgs/GetPlan.h | 23 + .../c_api/include/nav_msgs/GetPlanRequest.h | 23 + .../c_api/include/nav_msgs/GetPlanResponse.h | 21 + src/APIs/c_api/include/nav_msgs/GridCells.h | 26 + src/APIs/c_api/include/nav_msgs/LoadMap.h | 23 + .../c_api/include/nav_msgs/LoadMapRequest.h | 20 + .../c_api/include/nav_msgs/LoadMapResponse.h | 23 + src/APIs/c_api/include/nav_msgs/MapMetaData.h | 26 + .../c_api/include/nav_msgs/OccupancyGrid.h | 25 + src/APIs/c_api/include/nav_msgs/Odometry.h | 35 + src/APIs/c_api/include/nav_msgs/Path.h | 24 + src/APIs/c_api/include/nav_msgs/SetMap.h | 23 + .../c_api/include/nav_msgs/SetMapRequest.h | 23 + .../c_api/include/nav_msgs/SetMapResponse.h | 21 + .../c_api/include/sensor_msgs/BatteryState.h | 38 + .../c_api/include/sensor_msgs/CameraInfo.h | 33 + .../include/sensor_msgs/ChannelFloat32.h | 22 + .../include/sensor_msgs/CompressedImage.h | 24 + .../c_api/include/sensor_msgs/FluidPressure.h | 23 + .../c_api/include/sensor_msgs/Illuminance.h | 23 + src/APIs/c_api/include/sensor_msgs/Image.h | 28 + src/APIs/c_api/include/sensor_msgs/Imu.h | 29 + .../c_api/include/sensor_msgs/JointState.h | 29 + src/APIs/c_api/include/sensor_msgs/Joy.h | 25 + .../c_api/include/sensor_msgs/JoyFeedback.h | 22 + .../include/sensor_msgs/JoyFeedbackArray.h | 22 + .../c_api/include/sensor_msgs/LaserEcho.h | 21 + .../c_api/include/sensor_msgs/LaserScan.h | 32 + .../include/sensor_msgs/MultiEchoLaserScan.h | 33 + .../c_api/include/sensor_msgs/PointCloud.h | 27 + .../c_api/include/sensor_msgs/PointCloud2.h | 33 + .../c_api/include/sensor_msgs/PointField.h | 23 + src/APIs/c_api/include/sensor_msgs/Range.h | 26 + .../include/sensor_msgs/RegionOfInterest.h | 25 + .../include/sensor_msgs/RelativeHumidity.h | 23 + .../c_api/include/sensor_msgs/Temperature.h | 23 + .../c_api/include/sensor_msgs/TimeReference.h | 24 + src/APIs/c_api/include/std_msgs/Bool.h | 20 + src/APIs/c_api/include/std_msgs/Byte.h | 20 + .../c_api/include/std_msgs/ByteMultiArray.h | 23 + src/APIs/c_api/include/std_msgs/Char.h | 20 + src/APIs/c_api/include/std_msgs/ColorRGBA.h | 23 + src/APIs/c_api/include/std_msgs/Duration.h | 21 + src/APIs/c_api/include/std_msgs/Empty.h | 20 + src/APIs/c_api/include/std_msgs/Float32.h | 20 + .../include/std_msgs/Float32MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/Float64.h | 20 + .../include/std_msgs/Float64MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/Header.h | 45 + src/APIs/c_api/include/std_msgs/Int16.h | 20 + .../c_api/include/std_msgs/Int16MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/Int32.h | 20 + .../c_api/include/std_msgs/Int32MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/Int64.h | 20 + .../c_api/include/std_msgs/Int64MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/Int8.h | 20 + .../c_api/include/std_msgs/Int8MultiArray.h | 23 + .../include/std_msgs/MultiArrayDimension.h | 22 + .../c_api/include/std_msgs/MultiArrayLayout.h | 23 + src/APIs/c_api/include/std_msgs/String.h | 20 + src/APIs/c_api/include/std_msgs/Time.h | 27 + src/APIs/c_api/include/std_msgs/UInt16.h | 20 + .../c_api/include/std_msgs/UInt16MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/UInt32.h | 20 + .../c_api/include/std_msgs/UInt32MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/UInt64.h | 20 + .../c_api/include/std_msgs/UInt64MultiArray.h | 23 + src/APIs/c_api/include/std_msgs/UInt8.h | 20 + .../c_api/include/std_msgs/UInt8MultiArray.h | 23 + src/APIs/c_api/src/convertor.cpp | 368 +++ src/APIs/c_api/src/nav_c_api.cpp | 1982 +++++++++-------- src/APIs/c_api/src/std_msgs/Header.cpp | 21 + src/APIs/c_api/src/std_msgs/Time.cpp | 12 + .../Packages/move_base/src/move_base.cpp | 69 +- 142 files changed, 5108 insertions(+), 2323 deletions(-) delete mode 100644 examples/CSharpExample.cs create mode 100644 src/APIs/c_api/include/convertor.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Accel.h create mode 100644 src/APIs/c_api/include/geometry_msgs/AccelStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h create mode 100644 src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Inertia.h create mode 100644 src/APIs/c_api/include/geometry_msgs/InertiaStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Point.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Point32.h create mode 100644 src/APIs/c_api/include/geometry_msgs/PointStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Polygon.h create mode 100644 src/APIs/c_api/include/geometry_msgs/PolygonStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Pose.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Pose2D.h create mode 100644 src/APIs/c_api/include/geometry_msgs/PoseArray.h create mode 100644 src/APIs/c_api/include/geometry_msgs/PoseStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h create mode 100644 src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Quaternion.h create mode 100644 src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Transform.h create mode 100644 src/APIs/c_api/include/geometry_msgs/TransformStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Twist.h create mode 100644 src/APIs/c_api/include/geometry_msgs/TwistStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h create mode 100644 src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Vector3.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h create mode 100644 src/APIs/c_api/include/geometry_msgs/Wrench.h create mode 100644 src/APIs/c_api/include/geometry_msgs/WrenchStamped.h create mode 100644 src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Path2D.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Point2D.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Twist2D.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h create mode 100644 src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h create mode 100644 src/APIs/c_api/include/nav_msgs/ActionTypes.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMap.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapAction.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapActionResult.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapFeedback.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapGoal.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapRequest.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapResponse.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetMapResult.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetPlan.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetPlanRequest.h create mode 100644 src/APIs/c_api/include/nav_msgs/GetPlanResponse.h create mode 100644 src/APIs/c_api/include/nav_msgs/GridCells.h create mode 100644 src/APIs/c_api/include/nav_msgs/LoadMap.h create mode 100644 src/APIs/c_api/include/nav_msgs/LoadMapRequest.h create mode 100644 src/APIs/c_api/include/nav_msgs/LoadMapResponse.h create mode 100644 src/APIs/c_api/include/nav_msgs/MapMetaData.h create mode 100644 src/APIs/c_api/include/nav_msgs/OccupancyGrid.h create mode 100644 src/APIs/c_api/include/nav_msgs/Odometry.h create mode 100644 src/APIs/c_api/include/nav_msgs/Path.h create mode 100644 src/APIs/c_api/include/nav_msgs/SetMap.h create mode 100644 src/APIs/c_api/include/nav_msgs/SetMapRequest.h create mode 100644 src/APIs/c_api/include/nav_msgs/SetMapResponse.h create mode 100644 src/APIs/c_api/include/sensor_msgs/BatteryState.h create mode 100644 src/APIs/c_api/include/sensor_msgs/CameraInfo.h create mode 100644 src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h create mode 100644 src/APIs/c_api/include/sensor_msgs/CompressedImage.h create mode 100644 src/APIs/c_api/include/sensor_msgs/FluidPressure.h create mode 100644 src/APIs/c_api/include/sensor_msgs/Illuminance.h create mode 100644 src/APIs/c_api/include/sensor_msgs/Image.h create mode 100644 src/APIs/c_api/include/sensor_msgs/Imu.h create mode 100644 src/APIs/c_api/include/sensor_msgs/JointState.h create mode 100644 src/APIs/c_api/include/sensor_msgs/Joy.h create mode 100644 src/APIs/c_api/include/sensor_msgs/JoyFeedback.h create mode 100644 src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h create mode 100644 src/APIs/c_api/include/sensor_msgs/LaserEcho.h create mode 100644 src/APIs/c_api/include/sensor_msgs/LaserScan.h create mode 100644 src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h create mode 100644 src/APIs/c_api/include/sensor_msgs/PointCloud.h create mode 100644 src/APIs/c_api/include/sensor_msgs/PointCloud2.h create mode 100644 src/APIs/c_api/include/sensor_msgs/PointField.h create mode 100644 src/APIs/c_api/include/sensor_msgs/Range.h create mode 100644 src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h create mode 100644 src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h create mode 100644 src/APIs/c_api/include/sensor_msgs/Temperature.h create mode 100644 src/APIs/c_api/include/sensor_msgs/TimeReference.h create mode 100644 src/APIs/c_api/include/std_msgs/Bool.h create mode 100644 src/APIs/c_api/include/std_msgs/Byte.h create mode 100644 src/APIs/c_api/include/std_msgs/ByteMultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/Char.h create mode 100644 src/APIs/c_api/include/std_msgs/ColorRGBA.h create mode 100644 src/APIs/c_api/include/std_msgs/Duration.h create mode 100644 src/APIs/c_api/include/std_msgs/Empty.h create mode 100644 src/APIs/c_api/include/std_msgs/Float32.h create mode 100644 src/APIs/c_api/include/std_msgs/Float32MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/Float64.h create mode 100644 src/APIs/c_api/include/std_msgs/Float64MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/Header.h create mode 100644 src/APIs/c_api/include/std_msgs/Int16.h create mode 100644 src/APIs/c_api/include/std_msgs/Int16MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/Int32.h create mode 100644 src/APIs/c_api/include/std_msgs/Int32MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/Int64.h create mode 100644 src/APIs/c_api/include/std_msgs/Int64MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/Int8.h create mode 100644 src/APIs/c_api/include/std_msgs/Int8MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/MultiArrayDimension.h create mode 100644 src/APIs/c_api/include/std_msgs/MultiArrayLayout.h create mode 100644 src/APIs/c_api/include/std_msgs/String.h create mode 100644 src/APIs/c_api/include/std_msgs/Time.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt16.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt16MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt32.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt32MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt64.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt64MultiArray.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt8.h create mode 100644 src/APIs/c_api/include/std_msgs/UInt8MultiArray.h create mode 100644 src/APIs/c_api/src/convertor.cpp create mode 100644 src/APIs/c_api/src/std_msgs/Header.cpp create mode 100644 src/APIs/c_api/src/std_msgs/Time.cpp diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs deleted file mode 100644 index 5d60a56..0000000 --- a/examples/CSharpExample.cs +++ /dev/null @@ -1,620 +0,0 @@ -using System; -using System.Runtime.InteropServices; -using System.Runtime.CompilerServices; - -namespace NavigationExample -{ - /// - /// C# P/Invoke wrapper for Navigation C API - /// - public class NavigationAPI - { - private const string DllName = "libnav_c_api.so"; // Linux - // For Windows: "nav_c_api.dll" - // For macOS: "libnav_c_api.dylib" - - // ============================================================================ - // Enums - // ============================================================================ - public enum NavigationState - { - Pending = 0, - Active = 1, - Preempted = 2, - Succeeded = 3, - Aborted = 4, - Rejected = 5, - Preempting = 6, - Recalling = 7, - Recalled = 8, - Lost = 9, - Planning = 10, - Controlling = 11, - Clearing = 12, - Paused = 13 - } - - // ============================================================================ - // Structures - // ============================================================================ - [StructLayout(LayoutKind.Sequential)] - public struct Point - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Pose2D - { - public double x; - public double y; - public double theta; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist2D - { - public double x; - public double y; - public double theta; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Quaternion - { - public double x; - public double y; - public double z; - public double w; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Position - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Pose - { - public Position position; - public Quaternion orientation; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Header - { - public uint seq; - public long sec; - public uint nsec; - public IntPtr frame_id; // char* - } - - [StructLayout(LayoutKind.Sequential)] - public struct PoseStamped - { - public Header header; - public Pose pose; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Twist2DStamped - { - public Header header; - public Twist2D velocity; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Vector3 - { - public double x; - public double y; - public double z; - } - - [StructLayout(LayoutKind.Sequential)] - public struct NavFeedback - { - public NavigationState navigation_state; - public IntPtr feed_back_str; // char* - public Pose2D current_pose; - [MarshalAs(UnmanagedType.I1)] - public bool goal_checked; - [MarshalAs(UnmanagedType.I1)] - public bool is_ready; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct NamedOccupancyGrid - { - public IntPtr name; // char* - public IntPtr map; // OccupancyGridHandle - } - - [StructLayout(LayoutKind.Sequential)] - public struct NamedLaserScan - { - public IntPtr name; // char* - public IntPtr scan; // LaserScanHandle - } - - [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud - { - public IntPtr name; // char* - public IntPtr cloud; // PointCloudHandle - } - - [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud2 - { - public IntPtr name; // char* - public IntPtr cloud; // PointCloud2Handle - } - - [StructLayout(LayoutKind.Sequential)] - public struct PlannerDataOutput - { - public IntPtr plan; // Path2DHandle - public IntPtr costmap; // OccupancyGridHandle - public IntPtr costmap_update; // OccupancyGridUpdateHandle - [MarshalAs(UnmanagedType.I1)] - public bool is_costmap_updated; - public IntPtr footprint; // PolygonStampedHandle - } - - // ============================================================================ - // String Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void nav_c_api_free_string(IntPtr str); - - - // ============================================================================ - // Complex Message Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid_update(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_laser_scan(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud2(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_odometry(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_path2d(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_polygon_stamped(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_order(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_planner_data(ref PlannerDataOutput data); - - // ============================================================================ - // State Conversion - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - public static extern IntPtr navigation_state_to_string(NavigationState state); - - // ============================================================================ - // Helper Functions - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_2d( - double pose_x, double pose_y, double pose_theta, - string frame_id, double offset_distance, - ref PoseStamped out_goal); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_stamped( - ref PoseStamped in_pose, double offset_distance, - ref PoseStamped out_goal); - - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr navigation_create(); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_destroy(IntPtr handle); - - // ============================================================================ - // TF Listener Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr tf_listener_create(); - - [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 - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_robot_footprint( - IntPtr handle, Point[] points, UIntPtr point_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_footprint( - IntPtr handle, out IntPtr out_points, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_points(IntPtr points); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to( - IntPtr handle, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to( - IntPtr handle, string marker, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_straight_to( - IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_rotate_to( - IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_pause(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_resume(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_cancel(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_linear( - IntPtr handle, double linear_x, double linear_y, double linear_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_angular( - IntPtr handle, double angular_x, double angular_y, double angular_z); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_stamped( - IntPtr handle, ref PoseStamped out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_2d( - IntPtr handle, ref Pose2D out_pose); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_twist( - IntPtr handle, ref Twist2DStamped out_twist); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_feedback( - IntPtr handle, ref NavFeedback out_feedback); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_feedback(ref NavFeedback feedback); - - - // ============================================================================ - // Navigation Data Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_static_map(IntPtr handle, string map_name, IntPtr map); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_static_map(IntPtr handle, string map_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_laser_scan(IntPtr handle, string laser_scan_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_point_cloud(IntPtr handle, string point_cloud_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_point_cloud2(IntPtr handle, string point_cloud2_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_static_maps(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_laser_scans(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_clouds(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_data(IntPtr handle); - - - // ============================================================================ - // Planner Data - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data); - - // ============================================================================ - // Helper Methods for String Conversion - // ============================================================================ - public static string MarshalString(IntPtr ptr) - { - if (ptr == IntPtr.Zero) - return string.Empty; - return Marshal.PtrToStringAnsi(ptr); - } - } - - // ============================================================================ - // Example Usage - // ============================================================================ - class Program - { - // Helper method để hiển thị file và line number tự động - static void LogError(string message, - [CallerFilePath] string filePath = "", - [CallerLineNumber] int lineNumber = 0, - [CallerMemberName] string memberName = "") - { - // Lấy tên file từ đường dẫn đầy đủ - string fileName = System.IO.Path.GetFileName(filePath); - Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}"); - } - - static void Main(string[] args) - { - // Create TF listener - IntPtr tfHandle = NavigationAPI.tf_listener_create(); - if (tfHandle == IntPtr.Zero) - { - LogError("Failed to create TF listener"); - 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_footprint", - 0, 0, 0, - 0, 0, 0, 1)) - { - LogError("Failed to inject static TF map -> base_link"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link", - 0, 0, 0, - 0, 0, 0, 1)) - { - LogError("Failed to inject static TF map -> base_link"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Create navigation instance - IntPtr navHandle = NavigationAPI.navigation_create(); - if (navHandle == IntPtr.Zero) - { - LogError("Failed to create navigation instance"); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Initialize navigation - if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) - { - LogError("Failed to initialize navigation"); - NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); - return; - } - - // Set robot footprint - NavigationAPI.Point[] footprint = new NavigationAPI.Point[] - { - new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 }, - new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 }, - new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 }, - new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 } - }; - NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); - - // Get robot pose - NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D(); - if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose)) - { - Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}"); - } - - // Get navigation feedback - NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); - if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) - { - string stateStr = NavigationAPI.MarshalString( - NavigationAPI.navigation_state_to_string(feedback.navigation_state)); - string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); - Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); - NavigationAPI.navigation_free_feedback(ref feedback); - } - - - // Get global planner data (opaque handles) - NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput(); - if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData)) - { - Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})"); - NavigationAPI.navigation_free_planner_data(ref globalData); - } - - // Get all static maps (names + opaque handles) - IntPtr mapsPtr; - UIntPtr mapsCount; - if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount)) - { - ulong count = mapsCount.ToUInt64(); - Console.WriteLine($"Static maps: {count}"); - if (mapsPtr != IntPtr.Zero && count > 0) - { - int itemSize = Marshal.SizeOf(); - for (ulong i = 0; i < count; i++) - { - IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize))); - var item = Marshal.PtrToStructure(itemPtr); - string name = NavigationAPI.MarshalString(item.name); - Console.WriteLine($"- {name}"); - } - NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count); - } - } - - // Cleanup - NavigationAPI.navigation_destroy(navHandle); - NavigationAPI.tf_listener_destroy(tfHandle); - } - } -} - diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 2c363c6..768c885 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -53,6 +53,18 @@ namespace NavigationExample public double theta; } + [StructLayout(LayoutKind.Sequential)] + public struct NavFeedback + { + public NavigationState navigation_state; + public IntPtr feed_back_str; // char*; free with nav_c_api_free_string + public Pose2D current_pose; + [MarshalAs(UnmanagedType.I1)] + public bool goal_checked; + [MarshalAs(UnmanagedType.I1)] + public bool is_ready; + } + [StructLayout(LayoutKind.Sequential)] public struct Twist2D { @@ -85,11 +97,27 @@ namespace NavigationExample public Quaternion orientation; } + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Twist + { + public Vector3 linear; + public Vector3 angular; + } + [StructLayout(LayoutKind.Sequential)] public struct Header { public uint seq; - public long sec; + public uint sec; public uint nsec; public IntPtr frame_id; // char* } @@ -108,384 +136,232 @@ namespace NavigationExample public Twist2D velocity; } + [StructLayout(LayoutKind.Sequential)] - public struct Vector3 + public struct NavigationHandle { - public double x; - public double y; - public double z; + public IntPtr ptr; } [StructLayout(LayoutKind.Sequential)] - public struct NavFeedback + public struct TFListenerHandle { - public NavigationState navigation_state; - public IntPtr feed_back_str; // char* - public Pose2D current_pose; - [MarshalAs(UnmanagedType.I1)] - public bool goal_checked; - [MarshalAs(UnmanagedType.I1)] - public bool is_ready; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct NamedOccupancyGrid - { - public IntPtr name; // char* - public IntPtr map; // OccupancyGridHandle + public IntPtr ptr; } [StructLayout(LayoutKind.Sequential)] - public struct NamedLaserScan + public struct LaserScan { - public IntPtr name; // char* - public IntPtr scan; // LaserScanHandle + public Header header; + public float angle_min; + public float angle_max; + public float angle_increment; + public float time_increment; + public float scan_time; + public float range_min; + public float range_max; + public IntPtr ranges; + public UIntPtr ranges_count; + public IntPtr intensities; + public UIntPtr intensities_count; } [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud + public struct PoseWithCovariance { - public IntPtr name; // char* - public IntPtr cloud; // PointCloudHandle + public Pose pose; + public IntPtr covariance; + public UIntPtr covariance_count; } [StructLayout(LayoutKind.Sequential)] - public struct NamedPointCloud2 - { - public IntPtr name; // char* - public IntPtr cloud; // PointCloud2Handle + public struct TwistWithCovariance { + public Twist twist; + public IntPtr covariance; + public UIntPtr covariance_count; } [StructLayout(LayoutKind.Sequential)] - public struct PlannerDataOutput + public struct Odometry { - public IntPtr plan; // Path2DHandle - public IntPtr costmap; // OccupancyGridHandle - public IntPtr costmap_update; // OccupancyGridUpdateHandle - [MarshalAs(UnmanagedType.I1)] - public bool is_costmap_updated; - public IntPtr footprint; // PolygonStampedHandle + public Header header; + public IntPtr child_frame_id; + public PoseWithCovariance pose; + public TwistWithCovariance twist; } - // ============================================================================ - // String Management - // ============================================================================ + [StructLayout(LayoutKind.Sequential)] + public struct OccupancyGrid + { + public Header header; + public MapMetaData info; + public IntPtr data; + public UIntPtr data_count; + } + + [StructLayout(LayoutKind.Sequential)] + public struct MapMetaData + { + public Time map_load_time; + public float resolution; + public uint width; + public uint height; + public Pose origin; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Time + { + public uint sec; + public uint nsec; + } + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern Header header_create(string frame_id); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern Header header_set_data( + uint seq, + uint sec, + uint nsec, + string frame_id); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern Time time_create(); + + /// Free a string allocated by the API (strdup). [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] public static extern void nav_c_api_free_string(IntPtr str); - - // ============================================================================ - // Complex Message Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_occupancy_grid_update(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_laser_scan(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_point_cloud2(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_odometry(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_path2d(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_polygon_stamped(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_order(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_planner_data(ref PlannerDataOutput data); - - // ============================================================================ - // State Conversion - // ============================================================================ + /// Convert NavigationState to string; caller must free with nav_c_api_free_string. [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] public static extern IntPtr navigation_state_to_string(NavigationState state); - // ============================================================================ - // Helper Functions - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_2d( - double pose_x, double pose_y, double pose_theta, - string frame_id, double offset_distance, - ref PoseStamped out_goal); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_offset_goal_stamped( - ref PoseStamped in_pose, double offset_distance, - ref PoseStamped out_goal); + public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr navigation_create(); + /// Helper: copy unmanaged char* to managed string; does not free the pointer. + public static string MarshalString(IntPtr p) + { + if (p == IntPtr.Zero) return string.Empty; + return Marshal.PtrToStringAnsi(p) ?? string.Empty; + } - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_destroy(IntPtr handle); + /// Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done. + public static void navigation_free_feedback(ref NavFeedback feedback) + { + if (feedback.feed_back_str != IntPtr.Zero) + { + nav_c_api_free_string(feedback.feed_back_str); + feedback.feed_back_str = IntPtr.Zero; + } + } // ============================================================================ // TF Listener Management // ============================================================================ [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr tf_listener_create(); + public static extern TFListenerHandle tf_listener_create(); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void tf_listener_destroy(IntPtr handle); + public static extern void tf_listener_destroy(TFListenerHandle handle); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool tf_listener_set_static_transform( - IntPtr tf_handle, + TFListenerHandle 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 + // Navigation Handle Management // ============================================================================ [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle); + public static extern NavigationHandle navigation_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_destroy(NavigationHandle handle); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_robot_footprint( - IntPtr handle, Point[] points, UIntPtr point_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_footprint( - IntPtr handle, out IntPtr out_points, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_points(IntPtr points); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to( - IntPtr handle, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + public static extern bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to( - IntPtr handle, string marker, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_dock_to_order( - IntPtr handle, IntPtr order, ref PoseStamped goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_move_straight_to( - IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance); + public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_rotate_to( - IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance); + public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_pause(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_resume(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_cancel(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_linear( - IntPtr handle, double linear_x, double linear_y, double linear_z); + public static extern bool navigation_move_straight_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_set_twist_angular( - IntPtr handle, double angular_x, double angular_y, double angular_z); + public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_stamped( - IntPtr handle, ref PoseStamped out_pose); + public static extern bool navigation_pause(NavigationHandle handle); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_robot_pose_2d( - IntPtr handle, ref Pose2D out_pose); + public static extern bool navigation_resume(NavigationHandle handle); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_twist( - IntPtr handle, ref Twist2DStamped out_twist); + public static extern bool navigation_cancel(NavigationHandle handle); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_feedback( - IntPtr handle, ref NavFeedback out_feedback); + public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z); - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - public static extern void navigation_free_feedback(ref NavFeedback feedback); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist); // ============================================================================ // Navigation Data Management - // ============================================================================ [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_static_map(IntPtr handle, string map_name, IntPtr map); + public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan); + public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid); [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_static_map(IntPtr handle, string map_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_laser_scan(IntPtr handle, string laser_scan_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_point_cloud(IntPtr handle, string point_cloud_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_point_cloud2(IntPtr handle, string point_cloud2_name); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_static_maps(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_laser_scans(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_clouds(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_remove_all_data(IntPtr handle); - - - // ============================================================================ - // Planner Data - // ============================================================================ - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data); - - [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.I1)] - public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data); - - - // ============================================================================ - // Navigation Commands with Order - // ============================================================================ - - - - - // ============================================================================ - // Helper Methods for String Conversion - // ============================================================================ - public static string MarshalString(IntPtr ptr) - { - if (ptr == IntPtr.Zero) - return string.Empty; - return Marshal.PtrToStringAnsi(ptr); - } + public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid); + } // ============================================================================ @@ -507,8 +383,8 @@ namespace NavigationExample static void Main(string[] args) { // Create TF listener - IntPtr tfHandle = NavigationAPI.tf_listener_create(); - if (tfHandle == IntPtr.Zero) + NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create(); + if (tfHandle.ptr == IntPtr.Zero) { LogError("Failed to create TF listener"); return; @@ -544,8 +420,8 @@ namespace NavigationExample } // Create navigation instance - IntPtr navHandle = NavigationAPI.navigation_create(); - if (navHandle == IntPtr.Zero) + NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create(); + if (navHandle.ptr == IntPtr.Zero) { LogError("Failed to create navigation instance"); NavigationAPI.tf_listener_destroy(tfHandle); @@ -571,54 +447,132 @@ namespace NavigationExample }; NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); - // Get robot pose - NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D(); - if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose)) - { - Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}"); - } - // Get navigation feedback NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) { - string stateStr = NavigationAPI.MarshalString( - NavigationAPI.navigation_state_to_string(feedback.navigation_state)); + IntPtr stateStrPtr = NavigationAPI.navigation_state_to_string(feedback.navigation_state); + string stateStr = NavigationAPI.MarshalString(stateStrPtr); + NavigationAPI.nav_c_api_free_string(stateStrPtr); string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); NavigationAPI.navigation_free_feedback(ref feedback); } + System.Threading.Thread.Sleep(1000); + IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan"); + NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId)); + NavigationAPI.LaserScan fscanHandle; + fscanHandle.header = fscanHeader; + fscanHandle.angle_min = -1.57f; + fscanHandle.angle_max = 1.57f; + fscanHandle.angle_increment = 0.785f; + fscanHandle.time_increment = 0.0f; + fscanHandle.scan_time = 0.1f; + fscanHandle.range_min = 0.05f; + fscanHandle.range_max = 10.0f; + fscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, fscanHandle.ranges, 5); + fscanHandle.ranges_count = new UIntPtr(5); + fscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, fscanHandle.intensities, 5); + fscanHandle.intensities_count = new UIntPtr(5); + NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan", fscanHandle); - // Get global planner data (opaque handles) - NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput(); - if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData)) - { - Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})"); - NavigationAPI.navigation_free_planner_data(ref globalData); + + IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan"); + NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId)); + NavigationAPI.LaserScan bscanHandle; + bscanHandle.header = bscanHeader; + bscanHandle.angle_min = 1.57f; + bscanHandle.angle_max = -1.57f; + bscanHandle.angle_increment = -0.785f; + bscanHandle.time_increment = 0.0f; + bscanHandle.scan_time = 0.1f; + bscanHandle.range_min = 0.05f; + bscanHandle.range_max = 10.0f; + bscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, bscanHandle.ranges, 5); + bscanHandle.ranges_count = new UIntPtr(5); + bscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5); + Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, bscanHandle.intensities, 5); + bscanHandle.intensities_count = new UIntPtr(5); + NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan", bscanHandle); + + System.Threading.Thread.Sleep(1000); + + IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom"); + NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId)); + NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry(); + odometryHandle.header = odometryHeader; + IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint"); + odometryHandle.child_frame_id = childFrameId; + odometryHandle.pose.pose.position.x = 0.0; + odometryHandle.pose.pose.position.y = 0.0; + odometryHandle.pose.pose.position.z = 0.0; + odometryHandle.pose.pose.orientation.x = 0.0; + odometryHandle.pose.pose.orientation.y = 0.0; + odometryHandle.pose.pose.orientation.z = 0.0; + odometryHandle.pose.pose.orientation.w = 1.0; + + double[] pose_covariance = new double[36]; + for(int i = 0; i < pose_covariance.Length; i++) { + pose_covariance[i] = 0.0; } + odometryHandle.pose.covariance = Marshal.AllocHGlobal(sizeof(double) * pose_covariance.Length); + Marshal.Copy(pose_covariance, 0, odometryHandle.pose.covariance, pose_covariance.Length); + odometryHandle.pose.covariance_count = new UIntPtr((uint)pose_covariance.Length); - // Get all static maps (names + opaque handles) - IntPtr mapsPtr; - UIntPtr mapsCount; - if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount)) - { - ulong count = mapsCount.ToUInt64(); - Console.WriteLine($"Static maps: {count}"); - if (mapsPtr != IntPtr.Zero && count > 0) - { - int itemSize = Marshal.SizeOf(); - for (ulong i = 0; i < count; i++) - { - IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize))); - var item = Marshal.PtrToStructure(itemPtr); - string name = NavigationAPI.MarshalString(item.name); - Console.WriteLine($"- {name}"); - } - NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count); - } + odometryHandle.twist.twist.linear.x = 0.0; + odometryHandle.twist.twist.linear.y = 0.0; + odometryHandle.twist.twist.linear.z = 0.0; + odometryHandle.twist.twist.angular.x = 0.0; + odometryHandle.twist.twist.angular.y = 0.0; + odometryHandle.twist.twist.angular.z = 0.0; + double[] twist_covariance = new double[36]; + for(int i = 0; i < twist_covariance.Length; i++) { + twist_covariance[i] = 0.0; } + odometryHandle.twist.covariance = Marshal.AllocHGlobal(sizeof(double) * twist_covariance.Length); + Marshal.Copy(twist_covariance, 0, odometryHandle.twist.covariance, twist_covariance.Length); + odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length); + NavigationAPI.navigation_add_odometry(navHandle, "odometry", odometryHandle); + System.Threading.Thread.Sleep(1000); + // Add static map + NavigationAPI.Time mapLoadTime = NavigationAPI.time_create(); + NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData(); + mapMetaData.map_load_time = mapLoadTime; + mapMetaData.resolution = 0.05f; + mapMetaData.width = 3; + mapMetaData.height = 10; + mapMetaData.origin = new NavigationAPI.Pose(); + mapMetaData.origin.position.x = 0.0; + mapMetaData.origin.position.y = 0.0; + mapMetaData.origin.position.z = 0.0; + mapMetaData.origin.orientation.x = 0.0; + mapMetaData.origin.orientation.y = 0.0; + mapMetaData.origin.orientation.z = 0.0; + mapMetaData.origin.orientation.w = 1.0; + NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid(); + IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map"); + occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId)); + occupancyGrid.info = mapMetaData; + byte[] data = new byte[30]; + for (int i = 0; i < data.Length; i++) { + data[i] = 100; + } + occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length); + Marshal.Copy(data, 0, occupancyGrid.data, data.Length); + occupancyGrid.data_count = new UIntPtr((uint)data.Length); + Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count); + Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}", + Marshal.SizeOf(), + Marshal.OffsetOf("data"), + Marshal.OffsetOf("data_count")); + + NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid); + // Cleanup NavigationAPI.navigation_destroy(navHandle); NavigationAPI.tf_listener_destroy(tfHandle); diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index d57761c1bd6d3c98705de3dacb6154fa923522ea..3ba853654d4629765ca13826c5179e8359b46008 100755 GIT binary patch literal 148832 zcmeFa3w#ts);`>U1R{!)tf29Rg9Z(vI1>^`RMrW}kRF-9NCKjWlaNeEBsY>70*V?) zBFv6ian+SwZ?LPc?5ZnxMMVj>1n&W5#e49AJw^pV0Ttx`oT}$!}-VZ6aa7@1|37n=p;^=WHr_J}6_c*TK<@Z>AH6#I->E4>nVf`+@*AGdZPxALV zH}|}Ij_2>G7Jk>?qgvB@*0r49`by7xp-_u{j~A~~8R@f9q?_+n!KdHN?`;w9neWNs zJ^fC3(3StGzvMs3&m-PjKA+did@rHOWRT6|PMo4ZJ*`w0-Eir7kf163#kZ2@x1V$0u<5p!5_YYBbHLQMM%a<8Y{q96 zK305=#YZ_a<)c0g83{{=Eo@R|l2{m|d(m=7-0xzS4^)<?gYuFZA5-A@(-NgfQ5uG#-=W!){IS+pNBkgnzt-jJPib&ikG*4gAH}0I#3U(- zqsfzabBq?N#12q4Db|K%o`zVZ&oQwv8Ab68iZ$*+yHIRGLek0O3RA2J)-B_d)qf~a zkZDs)PK;GiR7Ht#-n~9PJ0;6`$MixgDQ7@L7h> zU+}pbpL_7R7oYp^S%J^}`1}t(EAgT0ANV|k&%;EB>k)BZCGM-m{c+r%5YPJUN%8O$ z?oW&7HRApp?rZUR9-kNRS%=Shd^X}k_S1$BUG4b1$etACC2^R{mehxk<_zcHq1U~w8o_M|hce{jBdMfBq0!za^ zT}mGE*2QQJ61ejf_qbWwYziZb=P_RnmhNdOCCttHGayGMc<{JHgKc+ zuA$5S^0+%=^mAjcy=nK5im}=sHxHh(+LJeN`q`dcahDCh_0KOnQDAw0Tt(%A>hmA% zx}fAQe#gV5O?g)omN(U;jXUeDht|A$-ZP|E7piw;KU?%C=LyU9 zRsMCsw9LCkPk;aG+4axWHx~WR?`Exiu;ufq4fBT7wS6-3$qf%|*|nqSga4^GIY0ZF z8uKPCqPwdOp z*Vf(=?#QfR z^*iS;nDw`rSsvG6Tl=a(xkoNP_xK54d^YBxJ8JTutp7B7%BL}3?%4a-8HF^M~@$mz{d1 zXVG(Vb7x($sj}LA`rQxTF{#~s*A~yHQ*$;>Exh3RSzn#@_$yc6a`EWr ztiGliZ_QY3V81sHJh<^i&vmby@M6-!ad+J{V5B8(CK%kLd`xPHZg(-y_dcUa4Y4PB8o?VMK*|K+uZ|NQ7%4OJIjFyNn~%KCNPdunS- z`m*0Qy)b=e+M{QE+)#JU6TSm&4{W(9ane&2Z#_A`ZPVn$=Bnerdp!5OXXoBIecMw5 zo_Ob#&mUX;)@jfEdChkPPfY#ff%DEkyyThq+3}0kjv4UJ)kjiZ%D&*yK39hxFM4D3 z@;7QX{x18@{a=ilu3j;zvhn2ow);{_6 zZw|bbec5@Z5BluNzud6-xWj?YYpy7H;z;i7OCMN!@82&veBh~nb$OD8b#`gz`!0R{ zz>(xrp6vg?g=al6CpR_!q<_(sTXsk@3f(=xt^c{wJff?}+mv%Revzb65oZv_-L> zzn&0TPHYrCOiqi;Uwu;KcHa`EUFxIgKmClz@()JQ+ZQKCwujMC^fn}ly}gnWIo}hb z=(#0|-rhoatP%9TEieP|!d~=lY ze}t7sB>uOf*p(wn`>q9lr14}2>^zeESEAU}4^ixM=sA(){4qWt^_uI9TrMY-!^+>s zD#}m=nQ!4Zs>7}aMw0y;uh_olv`^rLLq&OL5r?Zu;OiVDCqc1_I97qcA2^Hiw~7Vc ze1UHe@|O$wwC1Gi7WiFM&W@=ZfO)WM6aF&%B~EAYo6>X)FJ}z}4RCFQebZGujPoz2 z1tqR>_E%A6qN*#DQy?E6-T$cOMw zfsYq*E*5s)A%YruzWp7%eOpCErwRV|M{qs2e$Ih)qWmWcdvhG-_$`9}BQyl%d;G&3 zC>Hn|%zjQ(%G!9n^!`iX^%bxyM=i(oSj!yDFRY)6@?0(CoRr1wN;8cE3&ptM5aWg& z{f(~s2J-Uk5&h*#4l5U;9xE`Obhh(+bvtSLJ(r&$o3XiutF$Lm}G*w}T`*&I)B za5?Ao<@mqQaMXW0gdOVrH+i(^k0L;2;jbxwg?~=+tz$TlA>v%3eAGfI;s65KEoPV~Ee^8WP73J6KyH51$4$-f}f`2~hLGo)} z;d1nGV5G3a4q=DKh;i%_WJ&z1-r)Sz!fqX>aJ*B@N20qc3xu9Kg`Rc!f3%5uiFvJs zi&N4uFO&QRA1~lzLjIe0k?Om27{~Q->BMmy_Xq``o3qP$I@j~^xtt$j*_9&tk@YH$ zPZR~c4dWHfgC0{q8pYO+={bcF0KdpO}PVx zO69jb#Q{AQJ2RcvR~MWpL)cHDupcxNyZZF!@*5uK^hW}}8|_Q= zS}FP?x-+{1Cv!YO;HtpA%zhG-1~EP`z0}l%M6PP|vR1!qH>*LbJunT7AqCduna(*iC*4MZk9lu-j-wx4_hKqcs z;LhX-zvVcAA1n0VAoA7quV6l+a(0M7`d*RmUi25jS0!=aF@gU>v{$QWuL(l_rD9x7 z=-@z=z$e21s61vr&<6V_e9s4*QJ;@4661qM)GJZQ*$4d*zs-cl2tBM4IMmCo+31*L z&z-`a(XH53C+xQM2~I;T?6M&vlJ9tz)0)8lh;fzl(7K5OR6e?SgxB&lBxcBibuV@TZLvb}PnVRFho~oyPNZh<1Uwvg?6!I9@ZJ z=Q~m0PoaI8K6h{+U-VPoNG`unj3?;+>?%7`$QR}RgTS{6+#?(xz1`!49Xf;^qMGcQ zGm6XU5aUKr;5nm3d8YGnel75EqW^9Y?W^0#2I1E&7k-`IU*5rdv;y`&Pppshaps4! zxcpVW=kjZW{P99>Hla7+b}5U|ugU%!9_5VR3Hv`F{G3AJ=jh|lDq(L9A%C`z|D>=V zkJ$9n_usC)Q0TLam**CKQ@$7NW%ff}7UQsW9~Uf&rPxp4e%f;3r@<`QwU_DtL?ubs zxvqzP19`q?fAUdbZ=J&4pcZyri*k{j)V$2;sRADn%lpeTF|T<9{(%_BYQ(sN>a#0Z z*zHnbw=e^C?fM-rf1xNphD~<8A^hitd0a7iz3vwLOH6+0zs0y@HMQeiQT(N`q8-it z(#`1jtiDA&-#(}(UB`&=-yz0-G!wh7v~qs4UuusR{pd-~_d35ROW1gY@mcs47#7%7 zoyGOJOpJ$mdo4h}r1EqK{iFHVWxts7H;DGt$E!sc@QJ_h3SR!d2>ySK;rs{1x_G6) z9~Aw_p>be3@}uhjUSEOnu*0;zxEc*X@>?(G@>@8&k|FGEkBPrElj~v6NY4Lnq0bi} zhx(UtJjbs?f1}HbahUL>XLB6IW>*>%Liskl!|AmGuNULhQqy>KiNG5Ua>j=Re+IH7 zIi9T?7|OFzb_x4eM0?E;{QXe?Do^M6oKauzv|q&K)C$23f`7GW-vqH993t=;VjNg% z<#P0K;7ZstmB;oTXM9}ff4|`GF!|y8G0)OGWfgV;HL|Nf)YoRx!}Az;S@~mlzV8VA zuNuVrog(5DP%pdg5c+A@#_8cgKV!wXyC%HV}bN zzBz&NxkVMeD!;RQ@`Q=`Hm^Nh@Kjfol>5u8tDL>F|4BJQj7=9);&Yb&yok}I(1Wie z605GH$dyeURpKix3RL)YB^c$VT(6uMN#FL-<#k?N<*0rDMRbk=K;?}1f(=dLEvv48 z1-RxqQ`7aA^_KFMSHV)MihWF5Md3UA(= zRMs5|ruJ;6o=nsRB^4glZ}R>2QMG|8e|e>^hlHPcKgZ7dT*PMV(FRe9BNW%O1(;>z z`$u`FrkeYY5Yw|v5rzDMqS;s_NDr00o5Wm#o^61}tSYXV4-=u`YbM6LDl)s`GJ*K2 zm>Dv9*1UP%8eeT)HH~rp`QEv<-nku}A$6N3^2d-wa`OFSs1UGNd+N-hI*gD=E3ca6 zDn`TFi_3~?z5d#wa(|tx*qL91Kh8{Vp0~KxhY{2{xsqxQ#!Ro*UshXP@2#kwr4O>6 z3B}P1`ZJc=UDBViR^A4tLEo(VpB};NnZ?yrbA7e`)a;s?NhQ^lK7Z}}$rCd4$Nx-A zb{o`G>#M0K!jM&231vW4Q>yCA(Xx|$b%9DBlDM;Tcb)MIK-%`qxD(LRR1UR5;Lo605F(|dCD5Y?@&NyWv1 z8cYZCb8E|?E~6kMiD@9M2ZteLvVByK1R7RL0yRoyQAI^H*{eOnD6YGx&R3gXTvSE! zMM>*?Rdw(mSQa*ep<8^Wfsw3k%kos0SNY-51Xy7s2t#}89Np6FwrHbElus&&Of#wr%161eA!|GDUgSj z40&jQ%6&zZ(FYhZ)Pkw`{!IQ71V`c(qy5h?rIlBe`<)&PQRzkzkon9&DP-EcJgfh@ zq>+U88a+jUN5jtG4`%Zz5c8}w0(X}YBQP&xFEM-&CSj=5Xs@@LtCRdAjA0QqH5r_v zc=ZLhtH4`e^E&fKdp#5KM|qiRlJ6Hq)2BeGQT6##D(f$pEsE;u%4bzMCr`*vgP-4H z&iO@#Ar;I!c_!v1pU>r)&~s8ZkNwVke_BmZd98~whaPN!s`5DjpSQ+e>zW7)i0B01 zC3~9g^_KW5e14xRKhujblq7ibffW1gb$(ZVj@LOcKf`NJCHECFy%q2RX`-YQX9nf~ zUoAQXv%pb$p}kerB@hS>u8mQZMKwoLk^iFbV^ErjXzmCFmGzTFb?k2J|F;!PxQ(#X zxoCjuT2~n~UFLM!$3W3~dll8xzo%!2!(H)J&GMIV zzg74v-NH*Dyfm8d9#e8njWO&`p3q|oK=0Pe8hNVDV8isNRAY!H+Zlysh+HSud%7h? zm`KvI%ZjS1d==v>s*C);KyIpCnhCn+4!9ycC!roSijY5_V8)mGDoT#JP9{Idn76ne zWRSeKWa!=KXu8b*bA3NG4K_K;=NALBS08t(>ztF(5t$*{jk>^k;Zs*4Oh9v0H%uc1 zn^ym?sypVTyq@*u<6(X}Ou`S#7NfVf9Fd+P>c})0&U8*H;Ce~T?B-z8nzsmUbFFV? zQ3ZtnW=7Ewf-95r*$i_OtNWi`6y#2g5~oP_S7U|L9VEc4#)3Mgz>m|JhC!Xm^54oC zLvkwT{TiY~k=UAZR-mZ1#9LI1@Kh~EMTTKXH?JH6ro>Q-sSfiO>ha%e#6DUN>gchY zUqL+)h5ia6MfQ$RwgNa$5hE>dX}l%#5mrSQptz`xreD+QJfh4VE+?_=I9Yii^wx|B79eh^%Tu4pKHe` z>@BWWukrEa97!_erbm{d1Z>dUWD7kqKblv1mWBF2UHLC5PUKMaPnG5;-iTb~ zo*j#cHZk_JiAb48;^<{Bq(XDJ)jXYuLAfSS>+{Ynuk{CtXw7DR?dUWgv9Y47XCf>- zQcRUaxFQ6-YY+)Ue8@Z7H($gF6& zVP4Rf5+9bk)$@7R!{9l}o8Qer>#y=E7IJg3es8yp3zU}nYO||reQZ&N6~B8Tf|awX z0@M-gsa`jV;;pH!C@-GxOqJI@zrJW=_42EWW=&C2URQ&l9al|HHD*X^EU&Do@Ya;L zJQMT1qe?0&>}hof@MNo10vFk zPEuT1L#^=}^bxzhvWZ%9$`*S9=$e&9vwdD+VE&?6>E3*=zZ?^AI#%FCvuFs*kHY#J z6+TDo82ly;+H1E;G~BP$37kqvMR5iC_;1$D^a$H;qn+PU^J=OJ3LpsZ?i znpc-FJ z-)F+q-9rz*WmWz+v?lfqe)~EdT}HpK+4V*@Y3eAAGg6U$*{lCqzC8x62zL6PDM9a< zzllW(XOj=)_Ftx?pRui9THBsQ`Auruvw@DXw$f0CV6DF1kI=mT{V$TfTagdG&>#)7xFJp7Jh&cC3 z%pm4orwn_s+3hu0gjHb=kp#|F>Wk~d0Uf;un=&0!jGckvxgrSFn~4NsXN#ZR0W&+{ zg8lk>EafJ{J^!Vre%SIv&(u|4Y`1&8l{0a`&F}O2W$(qTosy~as-4+v)NJcv={*h` z)zfmlzSm)+dhF@3b5pvr^mA>OhF!1{ENLUO8hI_5l4ats&qxZKlH*;C zT}9lO|0p?YQgwA3N@d}Xy7`qAd30dy8tyy`|WR*HL<=B>k=G(o*cg zdS_J^Rp5*dG-ubXkUE+iN*Pg0I)W87w@k_{pEL0+Q*)wb8s#9CzZB7yIzJsr z#=->Zte&sP7%7ajjTkyQa8+GtbuBw$h|T0;9J8ycX7M=v+$QFh;@Lt~&=Cc^<-ix# zG(LZUE{3%_ymbQ*RmrQH#gcycz%Wy8ZM`;D^C<)X+RtzM|U5 zX+>4D0u?pS#AwdX zU(UpE#*VVF(Ad$PS~Dik2)p^c&$zM0rBrq*4z>|s$#qaD>oyVONXO24`2t7DPII;? z-x@mNhHV2Js}lk8S|8Ixv|>a$6!%lrkI4Vi1xYm*vRWJ|m2t8L(ZQ(=rD{=%q)tqlQns)~DeF49ra}O5bmt$9Ln_2ku%t%gDJk+7 zk!_&MjFxu}$m4eX^v(c_v3GmBlJ@C{VY&GJiIATfEjAJw6-0RNmz}8j?-deZAnQGT{tJMZ|Z0<07r_5 z7kHg~LF9x}$WNKO!Sx z40dT}jno-&EBDeU0Shd1?Y-8%*x;q*skHN6UW${-7-ws1i{|6RxG;&Fnn^f2E)T}W zw0&9SF)uqZqntYguHMl;R{3MeuDMF^du;28^^H&mQ zI!lMZ?A*$qK1>%O#7wzv7dav&p_z= zGJr?gM~<TI#*L%;&4{dTOt^ zz@K5WdFQ5j>+3G4t1Z5urfT*)FRK=gOr^kX%Y+7Ex@75dGnSfB&gUwVpXDaSHy{%<+)f7yxNz`rMhzm&s3a30S% z`!Sh3Kl+Owqk#;{i@!lK`m_ItQ&B8ZU+eLZ<j?>Oavpo54(xb3!)JgUZYClU4&2Rq)?_>z`rR$10Nr&FdG(%1^&hR@6H)=1Rig~Z6e=J@%vgjf0>{)6TU{! zNv7Wm(rNv7GN~3;eDvRE(tlsejO)MiX2$j3*EQq%?*p6hBvCKIAHYYy^xtD796zyS zlt=&FMKiAdzN;D6e?Qrb>%WU`#`WJjHRJm4H=1$%ca_a}id3)X^-AGxlW_49Yb;fb zf-i}JJ0$!%N&YkmKamP0u0jdNPox=F+3TE%Mt2#nk@(M*5`MFU$4l~M+!F;?qKdS?W>=ua@|iN%$Jz_?c36 zEjQsy1U^j~Cy8IbS|$DkI*7X_=}pGBNc>$A{|*V)|1DEWZjjnV##5y6LB=~JIm0FU zSt7}iahoJZ#t%wz{wS4Gk>rSvqpzl47v;hhq0k=l2Ug#TIMKPch!ZzCJmE1aOe%MB3G zED0As3CvOxB>a%XpCsWoNca+|zE4Q_QVFM6oqjEo@Edgy_f-5%Z965c7{r%Cu82|q^C=RpZSPQv3Q`(Gx>Ns#an5}qXCv67q=2_Gf# z+az546ACOfL&C3@_#G0yR>D0J{+WaqO8D_od1@s5c8PzUgpZW?8zg*@#J@zsJ0*On zga;&ijf8(M;jI!rS;DtSxLqnwhlHOl;X5Q;|7~#+*D2v%NxmlG_el622``cGgA!gU z;Z_k}qM}Tc@DvH3F5yWXywDU&mWr=fC48>LpCaK16dIRJ!uuMiqGU)o0*S`uknmXs z3jbNc`$@P*!uv}FohIWFf1!lOOL&=tzboN25{`hQam|zP0R{@cMZ({c@Ffy{oP;lx z@PQJ(Ou~q68^q~ACz$2XJN@(yY2R5i9cS#=SX;hgs+tFBncNkna@(K5`K!r zpCaL>O1MqJPnYlv2_GWi4hcU)!c_?$D&ZapKU2b|N%&b3UMS)FCA>_+lO?=H!v7@U z^CWzjgf~d|ixR#>!q1lQr4s&#gfElub0mDZgr`XON(mn>;j1M4TnS$z;oBv=Rl?7c z@GTO4zJzy3_(%!gA>kKDc&CJ)BaK&@gxe(kJrZu0@PiVbD&fkuZu?J@@OTLyB;}hR z;pq~8l7x?zaI1vlpTsb(6ba8TP{c_jT>R5XEHy*IpOg3<68?~cs}g>pgnK0XcT&Bk zNx1kYnAlr|5`K{+r%b}fN_dTgzaZiBBz&-xZ-a#YUgBRO;fE!Bsf1r5;mag^v4k&| zaEFAil<)%*zDmNgBz%p8kCX6L2|rt^*A@w{llVI%JX^wdNO+EfcS^WZ!ZisWFX4M6 z{7VTxDB-yhuDrnu-5=Pq5*{z%FG+ZUgsT#sB;hUzw@Ub>5}qRAf0J;Vg!hs3nIYj5 zBz}j4yCqze@I4alk?=eTpC;jdm+(RfpCsXB5-$EJ6_#2f;U0;9o`kQG@CFIbm+&PL zK1ITpO88U>Unb#~OZajLzf;0jO87JhUnSwGlK$67_!Sa=tAt-E;aep9ehKf8@YNE& zL&Cq7@Jw@BsOBjJS-|3L{al5j=%^JM=sB|Kij2T1Z0B-|(Q|9|yypJmAR zYU3C2YAEhStY_8cR)3!(9ctsI_>HW}N76q6y!Xhkk8!h{p%8x=rR)vwJaXj7Qbv>h z_J+3_bRR~qH|V~MUTx5^j9y{Tag4szplLy~H@wiGsqyxP=NNQ6ql*lh79xAYml-rI zK=y{S44MKHd&72vrUl2|@KA#u$mm3arUk~{aEw7y0Ap|X>mT*{CNO%ZK_@bLt3lJk zVsCi8LDPa_Z+Nvq(?Vizc!fa^X7sHFO$!Iq-=I%o^c;gG2Y+w4$e_uwNBs?&9C6g& zpveJ8{SBG|EU3RhlOw)2oM_PGfTR8fO^!F}e^{@-mC-v5dMKl}8uXcrUT@H6F?zK@ zlS7UA8#Fo0d&9RHG&#hmzd@hF=s5;WjxOqN(B$Bv{sv8sE$VO36!1a)4VoNT)Zd`V zfkpidnjBZu-=N81Mg0%y^|vv4r$Lh=3i~%`azN4k20e<=s|}hQQPkg{$+fLnPJ_;3^j3o&$LRG2oz3Xg22BnS>Tl5G_@Mp6E*5@(9;=hH)xt@ zQGbJ;!RSPTzM9c72JL0^*Wc;&FJ$yigDzt9R)eOA7WQw@#f)BU&?SssVbCKfT%ybffoF8qLwqzk7@n~{py&W^kx(N zyor9&L_cDpA28APnCLr9bc=~@HqqCc=zxi?G|@g2J;Ov#G0}M@+G(Q4n&@;BeZGkv zW};6w(SuF&KocEjq7R$e>su53rHTI3M89vMx0~o!P4s3H{k(~O(nLREq8~8P_n7EA zOtje^n@#E0o9KXvt~Aj;6FtL3PchMXCfaGD$C~JL6Mepk9%iCXH_?Ml^gt6GXQB^d zZ&B{A-ci5_gC2b$*_?!iE>c^v(PHgjOCz9hV@B6~B)gDvodJ+?c2Le4lyeHDX7E(+niDBqFOjKvxN)ZmA)^xnhZLUvz) zPqPtl7YBk^W7CT^*iGOfQtf1V4M>uv3IPi7&!%jWvBK^)JyLcK(PA{Bkg0uxEW>YLVebyzpKPQ0h8|8%!QGYlMc`tpG7($}Ye=2C z5?Gc0rQJ`Odr%uo_0=nKx^^#X7rA%~=-tN6wyp4JM7(zL6Iy9D71;$8v5%zeJ#-b~y!Fre=}j&O!l*a3p&Z^SZ?vveTBA+dbM zO1)dVkyt*5tw73`2zp2?n*_^M#L~`L1|aUCO(5VY4m?7@g#^%e3nA|jFr0wfIdCjO zBU%yxjU2d;fc^y3a-bXl)TAe99st`G9tcNZv-tV<|bIQA&PSBtK5c zLn!%}CQ4p_!G#T2*C6-2=KcP7ckqDrB^d|OY&4!b_z3bg7-$qYNZP_EK=U(30Za2< z2CU^1Hk0))stNidwFgb*opfiD!tEpkb**65_o0@I;?OK|Gw5*oO+3)$4ni{1ud#+IUC{7cif00Zp~LEzMIvh3OA`!7Ge?=cDW-}02}gY|{u0G~5N!~?Cdez9Rp1I9T2}*cQ?N0ZRHBArhoyjG z_F`|eLl>HE*b4N~VqAX%X+xQWPID3fM``v9}aO3K5d zg})F2H$yd_a0%Q)c9ya70e*zwQ2JLxnS8ay1DAUWn$Y7>}|aEElgc1 zKpQLbXW9@^uB?J79yQco?K`hiudFsxF2)Q~F`h&sig7kC#;l(y#w{!z#i;68j7vl@ z?gVT(qXFe+@zW`4%h)GzxJ0{;ndjI-%B5xSd_Wi`?xK{W2?Ah)`;a-G)0Z46EU}Z;Ugl|QenYKxO8V*-!mvH$Cmwz&se+s4excnP>$v>@I zez{4$U6;@Hk6XrWqk_K2EO6{SqycOqqZ*G5W7T*9so@{-O6f?;8D!)5_Wl_TR)6vS zZS?-#{QVi-@7MD81N8n2z1~OF${9o4OuA(r^bdwAc2QBGmSc`|3O#QWckpL&fvI#G z{-YY)qpttz5_SE-zG_UHy7fo@APCqj1jHZdEM;*s{e3d3hOu#YR|$+j2N}EQ3_PmA zPyNTKp|Ne~&Dz@O08pe^ow&@nUct|Gr5u1BKO{#looT6VX(TW)mNTeGIRTk5S@ zQ`{|&l3lZoqAqM-if4B)JjK=WaPkghn6C!+QSWTaU8=M%u7NyKXj|?oMQzWefe`n_ z31Fnl9jtEvO!Dx2<8nN&q-RTFZYw?=_;doJaTAU9*+F&qcMI1;0m}Mt-+k&st`<0| ztde0D1|G_uE_&Y6hdSu_f*B);L_vF9~-=5iQGU-04rL;bRXvB{zFu~`_|VngHO z>5dZ!6EQZuXla@cp=$7THRz`K3CdJMc*4x*YQgYj--hMUP0U;;U;qZcUFKgP^M?ws zs>Dk+ypoSs_F59Mt!gNL+Fq0h>j9!u`13S+w(;jedd}d_W%TUe&o%U{^5=Qw|qQH$6gB@HNcRTfb7*?}~*H@;*~)NjR<#a=kg04SN*o$ZQLQ)!-Jj zvGd2qoiTn3N*23;m24C7P(jPo&~20_FE2F`p*5UbCCGW4q-H~EV}rrpDi9{kXiMTD zv*d4efQRXz1&|sFEvFl`4&0;OYEZcvG`wU|!_CBQsDU=3dZ=M3>OY9;e|s$1A)6+C zqW+v`Q<6K_IekXfRarB#uFmqJD>@V>d13>*HYiGXJK`UPemVXI;VrcPZ*S%OAGOt$ zDl(2yGq(h^uH`&E0-tCg@U0JM;Q;1e;h2TnF^tLme=6_)^$A&1Sck`eU2UZ<-rgSy zK)1Fkp?>aA6~bE83Bms8Iq0rQloZUR{sXFzQ}O7_HhdiTc$h!)DNU|n>NV&=fIcUrSV!G z@Y#4Dnu^YTyc#@+#++!wpQ#S~xthd|FAQFe#HpyM#gaG)v;5@=mc+|bEQwx}EHPKz z*h3tjKk)ML^e$dHXi0?qC*~%|%{5Bu{M(~GVzff*$aC~eiH$FH% z9!A)BW0De>vyZe(xlB`o&V*1lX(eBEhyG4RL+K1-t0iq!0~p!2j) z_QgSGawr?kJq694hr5%t&b**A#U1q3sO`=&vJB|NSr|BlIsvMkgPs$*F+uZ&v4m)1 zG`VNWXXFz)%gD9E&o;>A42QXctL!f=u6iB?Qi)ATJgiN$$)p|IK9I*TpOT7ZE|yzJ{8+)`@YC^tlvS;H7w> zPq_beR?_E^kHGjeA6IF_DicM)bl+-eqGNe*{4Y?4zoRyOf3@YN@2Ju(Q!sOW-^X%O z7bth|D>e9z8e9+0u8-Q1HkYPQHRc2KM@zz`36_L$R<&`1O>JCnvm{K0|KsYkBwmJl z)*dx;Z{S@Fm}=wBLs|Pb_r;`63#XPrE_MA!kah71ke2{k><{znhkI@;9>(KOLT)@g z!R4J37T$!(0Zc3(i8aE`VF>k0b9boSZnXD&KHV(=r zo3e4lhrgp~YpIKBh5k&eP(3nhJ&w}9AhrMjcev2nSs@r#7RDudNW;Tv^iYu1fIyxCGL6V-S(ZDT#Fat# zJf+*xDZv9PM;p7^omPm49UX~<*|tMEmjA7R8p^9IZ-hU&5G)R2$qNS5K#F9|4TQ4s z>=qbG_zF}1KA@$*o*>X6>}-<@G7G1>gL{Qb5I(8Xn2(w5AK1xWfRU5wTbjsUhoZaA zEnP}>AA%E!-3GGqpmUGqsqAl-IKK&H9}GGVLc^9O73{j@zfP8a00TtVX3iNgVayEw$SO%Bw`^Lid%%6eLsa(4#Kmfi|Pb- zK7o0eq5LGOUObdfyM44gg3pI1{OFoODM^$P%F@)}%`7i9czf(}NFj+UNgOVB-}xE~ zBDpc*2Me&5@bHIJhHNX9a1}UEg3vt35W9`2mKDh3Ta6ssKy53Q$_6DD06_}N~jCI;! zDR)Yy+z6m-R00(u@KQYPbZHAdK;L>-ZA8FTako??-{@{hPktJ|e>axT7ijvjXNehe z05olRDmH0JPV$YKAH41GtU_rNm&X(c}4~Pw7=rndMdNx`$7@$76(MWB2qjjROHtCI|?|?tfj6LoP ze5ssbmkwgq)vnDYZG@P?xM>HRHoU=v7#Zzn-yFM~3XXIuEybDnpb9|Dd*Ns?&MLna zmqK|;y+F^J`jB?r4$7L3YTd5%ICbGB8u-i$Hv|`$O0kyF7NXGM2QXohe?|sI#-epW zUicQES`lijuek0>xuOEB{5_~pj)=I{*$&*tz{>A49!(h8)M#n*VuhK+z? z{nl&k7Od~RZktN(e|xS2npChd3C)dd&sB-Q`UUqe=#%qD-d^RHjC8XfM*w?9$1Bh*@BOSOti~kSL6jGxzK#mhyg8Ir}DwpMk%LJ z0AY9sN3h1AI0?CQjn`!;VoWA5nLoox$0CtGJLvg5{+vV4HvX(ynmW)PD4E0lrHc4{ ztQwl_LHxtAcscO8<1cNA{U?^t{y3VCP$+kD4V4T=O(o$DB?2UxWzjV7tC`xEO~@g;~rCqf%kY zdK3oT6o#o6CWa`vFl4tB&afm7>Mjr4IqE#*fbVH84|HHI4+1H2dEnG^3a4hH7!SFY zElr!yB}mgX;b-xLMA9KE5ZOqXI=oFQM289A%{mlmjc^a)D&f;P?qrQ=X}$m?20;H* z`T2W`;I(oxXxoLjkKA>>!(@J_L6bmS$Oe8AMqY9gDxz z9WGTvc@)3&XVC(M&bipgOn3F+?MC{G!Loyv88E$soETaFjH3lWPE0)Pk-YqI@oMIt zz?T`jSz z5VAsl4d!ANsKv(Se*ZuGHALx zba98fC2c!MblD^RS(uEq54Uk8qOz(x_?>;fJ7%{Ux;$Qcij7-z<|s4;zRqrU3^p_- zrMN@6m~m<`Pi=7zhjO4yDDtLhM__xy+tkK(tM)LPKIo;r;Yq*|zjL>YJpn^y*D~r~ zF3eggTe_jlmxdOkSgGLJFf=DC{o-*XtkE1I?Cu{evENc1)Zu$joeg;yhMLyHj@`k1 zB95XTd74ZjyN^7L*Rlw}YM9P7?Z_jKlNu%5h_~hScI;fWC3Yc2f}1{pc~c+F2)~0T zE?)?^v+H?I4{xJ>scIiXWnKST3Ox{Q(~94q>;u;DeR#E7KGVnfeKA#p9Z1mU!{go& z@(IW~s{YfK>p>)P9!7M;A=`6pOe^iVIN1a3pwN-}68PARW`rqgh#P$ioiGERLe|H~ z-=OdkDxY&om-k%{5j;b+TX=7tge|$$!t?ZxnQ;#?w0dT?P0n1ys8Gbs>9KfU$ZO@p~Q6qY0D~eFU_G?CWFI4 zofYe!DJnW^;CHR0JXUEK)LId*&D=WOa(gQT!M_+6&z`%x4%0u&FEr73=Oi`rb-aQN*tpCS`MV8h z3A_)ZzP+_;SA_RrS9i3E{JFfiZ~MX}ZdppdwAwY0>VXkGR?Y12pL3Bdu&;T)I~121 zi@n}ES}jju;OoSEvH|vcOouzTfz_L_9O%PXy58;1l*_*nCK8q>gCjq_$1+q_p_4kyIZN#t}a zanez8ijMN7Kz~GUtZoAqz?it}*@X(#LAz!>N-VG1OtkTmyLUDt8%Gct+ z1CcKzps8kr`$Mk0|AG?_Z%)aFhHO2wlLGBvf_AFGm$8u-Tk`2}13Wuz66#9_QxXu8Iqe8+VzVEk;%;rsYh?RayLW4N5HxHPs~fVvwmwSN zd)mLi$XYPiuKn$0%rY&*uK63h{(jV7F#&LUH)2M?x1J^Ao5Es62o+J7L8<}YXGA3^ zl%zfO63)>C(~{XVjqZX$8_W64+NH>cjo&UryBZH#>fQJo6ZpO5sVPd$GJo4}Hs{0_!x&wYT4iVXdue=u*l%-oA~(y{p+U8k_72wlQ-fxiB(BWIiY5 z{fU%EW|$XBOYZuEKCZNkUAO`o_y(oaZh1ALJ^W|-_}hz-^>HEe^uMBy6|h2AFseQl zQM*La$DHNR$0sPIc4O53wf#pHN{rmUR>A0 z-60>#@YL=M0}!3p{E2_CJNPMOh|aTHE979HOUjS+UUDSffKt_GF%QypW7g$lq^{s$ zmN!(20hImf!X%8;xo~h_rPaa+Y-)U^W^Sw>j_nIX0)ww>S8YK}V>Y^iGvM@Zir2&5 z*u@s}Yvu=*MR!wPnO(GWv}A8oV>EQJ+8OBR^QHx^fFFZ{Y!K*bx%fOBgctycc{n5H z3QdW35C0y1!i|@b@8MBny7GcP#TD}&LhK26H0MZq##q&W+;9-_-{`v9_2@)6$-=H&+g!l)OP7GEpna4BE9Q>5UjZ6UKs5qP;i9yAfKh)(A61SS8UVtFa=Ufx^e|y zcaOLP>dM2Uh9!Y({h>bY;k!s}7pm(&#x~UJq`G&sq12q%1B!WD4d(T(3tzA5^-)QW zb7Ke<;8sJ%G~a@t2@MS;hz!vhXeR<$o3xjDIu6xJw8s19-;tHQ(LY>o%gajzo@R?jXJl-z&}OB6Y;zO^&N-$jzfK4r~1Y| z&kBis$Cl%8glYqp^?!sqslNRvrG31p@As^}<9U6@i~4@g>N}p-cf6H}Vm9TB$8rP1 zFdc0>4f{`_ROC&2ipQzJIp2h84+ZBOg5wz2#m{xVvKax>pUpO=VQ`8z;&aM)~qW$0g^{ z{Ou0Dl!vL(hj#3;t3kUO8kexoQhlja4Q65f9EZiQi=LsjaW*EmaXn;b9duzmZ~?ZH z5$wGds>F+_YUYoDwRm}Q0yeDK%OI{~Z(9 z0ygW!kc2Ur+CJ6=OJ09Cmc|==K6ecdBc4-V;tsa5j>gWVw9M1n-=+rV;i$7?2Q^K* zV-MZh9N*xv+X^}+w8WOc&%pY%HII)AN%9=IR38sqnO|EL&849%6K4ac{o624qElf! zI0)_BNJ7L0SG@K=8)z8*NQHN%#_VAJ#V;%u6ai(R2WB3$G~r}53`WCg54hhgUNpc# z#GDaOTF~F2&!;WpF+BIfMjpnK9hN2qQ6&#O-&OV5Hd(Gj9LEYGfHT#Ewzp@hX^WR8vwWOV=!b;tt z5*R`Y0#QtnuegGFiW-CY)rTUn39E-r#kh|!0K~d8fAIgq)wA%;*%(D&mwmB8J3b+N z2_I8h{tUYsyRN4PfPJnZ-%H~LbOX2U8h2<&9v13dr!RG9zF0dvFLTG-WauCvi|yZ0 z$YdxE;=8eL>I$jx;Hlq9D>a;0%1&}(hQi|hLkwrmrfq>UT}ZW{TLg0ORC@_z)m?}+ zQ6o818!_H^X%UQ3`yy|WALl4P)V^y)NT;&L=Hgj97nX~i*XGYydU`%E7abUcuCKvveSBU^DWW?sIxKgzb)CGlet@HC ze;^Khs`f;z@-4Go?sbHZJVIo_Z-*} zfJ0nr6HV9X?zE!GL;p*mI?@sHV!W)CK2MC;YDK`{IQH#}pk`@4ge=j{R&h3sPP1+# zXL}ykNE7X^;7bx&kN`pF(ye9isQq*sAFsO~?c=HBFz)eK^NhEu?c-C3){2oO37Zak z3pYU$pYPBD>mdlOg7aRXVR?8)<1~9=666sdh7r>?_$1H>%iN1W+7-l(I(Wh4Xa!oe&2 z#jprn%8C>^JOh%nGm(+@{xcEA8(Ltvf!<0j*j7CUs^fD$O(u?02cjSr2+h>fD2 zz^b~%Z|xafNMs(-MRAvZMfF^W*PF4j`w$TZR?W~js@^+DWHq)lpTVpU0x`-C5fNEx z92(JbGS5p!fR_DAAn76V2MX8j?AEOdQC^H)nU}>|n$N=vu25_;RE5wKy2TMsM))Lv zOOgGN4$_tyIw?#;s2C4u(Y6QXNe%kVVb&RLg?kEK z-IY7Q-Efn;l}mO!m^3OZw;2*G%}0b$or_$#8FJ%9=OWWq6V!_aUd_a;K{;}QUv*qw*`r; zHkRgBMbRefTM;N)Go3(Y-)Ye|B7TP@GubAeXd6&+*eBKnfHIkhIZzcRK8_TK|N3*v zdmkwuBMmCfXAuhDviWvGX9=ns492v-T{^ zlIGh+{?5n1Xo6%D?tFyKFyXRMm^L}OPO3kqsgC^3S@<7zgIq`KL(sbJ; z?JosCqNdv#_Q1r^FX==hX5n`K5Lf7OE1!hhY7^_H(=sh8S_8+{QKim&}6WT z&A*%3{QDWqH3>zGIRBpU1SuY+r6rv_|IWhnyPr+JpWt-0_SutkP{Ey9jGkEkrmtwcP2cQnMV%U+hikz{NxWvx&AeMDuxe;k7EWv$K2jq&Z!4|+bZz`sBiWd~iI4FTkCRD->1 ziFOM_z!%s82|w)y@U~2XAnjDtL;Lq?T|V<)aY~B@P7Iok5>@(oYX+?gZcI`9uvZ*Y zg#(shIAE6M$EaI2ZgMamTVGdd$HD~NEtOa%U_re}JA|CrA%@^i@r|3^+77Z(en29S zOTp&Q6fEiKtGHtC4F|U9%pHtiw>F&#phWZp^qF=`Q#Be{+ed1nkweVedT1Gu&Z#NanCxON6AZrP`Hb^etmkf>6c_fJg%W{pf#|1pnryUqCI#yn6$<(peIR>*f?uEGSITvLL*Ph;N%eY{Ew)` zvCra6`|s&1nGT!+&vrYrCc7r);`DM7b379GIvb_Y+E`9*wqeHoUrRH6Sq$pWfaYO0 zYn=Flg;IhiYf648W`;qKbE0B=zaVRkYocIqPRuFI(K#r)li1miCTI)a%Ae}wY+2a_ zu2O7Ha5-VWKJKpZOm;f+JOxfFXAUVM0Z%6@;mOn%OVu^`Q?j$2&YWK9sVx140$u*N zNs~(vePQx*SSi95mPrGZLtQi3d8sqIz)49$AK~4smxy#x4%agia}fv0Nj`-utrUlS zlfs+z{zP)Jv)pdiM3R#$mG^JdW+*RD?~%{q?tDn4?n(Itczci%uA+DNdVh__oi%Y{ zkM;^r>V6OHIcZ|SVq9eXmJk#AK?hf3w&mMsa7oh?;8{d@N09G1q%4jdPkc#yU5N$Fl zgmmn~xr6)hfFmaO%Z@OvWwqRExncu8FO^ybtuM7q!_h%KLv1c(BfxVvJJ?Q#lu2a( z?q{dCEs1w0XHa<(???CyI%U7ulS&F@qh8As>`1v*!$za(3K|HxLbBF^9q)~REPt}q zl2}KPQXFrxBwlC5(^WWpo;V%HG^S^Dmo%k4UT&e~I0U>KCd*r9WSt#%;Z4cCHB!zBgl@1SPeE`SlJ_*l9 zcP2MwAi_G~!(_%EpkNq^yIv4uZTKn-ruy>&Jad1Z_9JPZ>qpq{DwLLNg^I%M*xhn} zGBs-tdtM0u_Dth5GgMlnlv+~mHVmf$@6%-&epPaf=37NYU(0{h3epSlIyVmXpnr*( zHB=DQB%>w$bV34IE14V};ihuXg-2>xy2x7fWa?#f!CEN?_R4aBk@3{vneGkZz7%&Y z^C4*J&4;5}YYs8p4<>G{+*GMh)I_?;qB}>nSISargLD|p{oG#32;px5cv#r${mDzH zM2Qb4FC%ucC!|uSTtuqyFZM~xZ;B`7E$)CnXo!Jx5# zqJRWLxM(C1CW^NjO}NAmO)V-a)mXJsMWvN0TB?zY;H{=!d8^i_t)6L6(t5+|{GVs- zea@VjOvJYD_kREH`+9!AnRU)O`?B`hYp=cb+Lr@b;9jl705cx%E($&cEs3=r(Zi=h zkkr~3?DLF))lmAXf3)o!j@@YMmWzXSq$`CSt`>R1d7szy8}{Se`IXgD9xsCW&+59> zNsss(jnC=Ys^nk$Ptgjl-jxLgD7nW!w#nch9SHts<4vBci&R%dj1qH3a|hcJf_04HV2pPKo*gPQy{rt6*avUY$VUw}C9v_If7T0-2z3cB2loRstzg zlY!b8Xg&gA2NlLSWi8N?WzB|^=mF3TM36{u;HLa`ghvy?!bUbr>KqAJ zK!-{>c@XIj9N%?H$o}dy9qX?m#8dJkbAvTzWI5SXjGS=#2s_rtz))bO8(9=yaAH--kxr8 z)B8Wtp4gwjH}7g3sC(TqvMKE7Y?@CJV@Q0;SOMSkD%Q2x3jrg|m@(wYgA=Bltm3@H z=ETNLti|UXZg-?*0#+03j9`TW>>+H$eT0?NGM+03Ji%oY&Qi`V{~a6iF`H8QM+@6( zzb*4%U+z2pi02FN7vdJl#a}MQiJbi8#`kb{us?U@PV7|E>pSYOf}>l`w|L(JA7RU| zFc!-DA}#)tu&Eajuru$6km4|v#rRxhM6l9K>-WE;^q}+!`S>vg5g~04Y-vTm-Nk*u zLgAkyo-eik38{uaQeLFtX{0{aV)A>K{2usJ>fY(PRfl8zRn}+7MkJ+ad+c;{9*Ok` z#1>6KsHxCxY|==>pN~HrX_8bwOQe3H6WL@j$@dL=-!_D2Yo8#ky@V|l(d6)@-MIJX z$6Gl$$|e*lf=!AMqh3O}?H*O`C5dte&@M;-iZ0#DScm3qu?NuQ+w^Z__wSt1BlNrV z^A02d@}b?O5V@{Gs6Bj2&Q@)vcv0nkNR|6rmISJy93{gv84PZg&wzMY=SvmAy9M18 zMrkoCic&!T6h@lsEfxZ$7mr%ILJ<9zUuOek)$ z(&!i3f2V?k*b*7rp;XvQ$MuhK>({JBP=><6!bC~rP!WdI@y=NT_KG_vD z7x}olL_z;^`4#Lnzen#(3iA zrRs@u96h1W9uoetzj#^tn-4&gb)j&&CMQ4GKN~{`R8z43WHrFRP(u$e3Y`H4h8h=y zyBe)!$dNSxM?~xcnit9XTJ`^77ptHTRii&$WP*b`94A5^fdPdm8?@|4!GZ8N##=`? zg*=9hWkf>t;b4!gVY};8ej%3Uzf^g+h`VJC2~*`^WJuk(+^&t*t9K-7_#C%A#8ODz zt^f^51ZypIF$gBH(3&zrraB5!qSTW697(vwpU?UTQ+bzTE)66=_Xqo*N>*YrA55hy z6$Zh&+v9N=V^Pmi59k)~1Km5yvJSWw(GK3S4t1#B5_<&N+inw2;ohZhsShrRG@n9P zBcckDaf)hdMCjZ`&VLw{MJHG08pX|3M%{?uBRH(*k*P(nGbUj~lUp*Pxao)$rxvW3 zjJ3bxvV=s^kG4B5B;!HFZ%#E>_oBV%{a{1e9gQ;@RP`KBtp*J)zG4sq1S$HgD7cEN z5YR~%Wo->Ey@77A=94fH5^S7;dwR#3H{^$T+cIW#9b;L?-Ud-8li7WDNb$C_8_TmM zgIA?IrAqqHHR9_kYp#aZ3ESqCKE~`7f@VQ1%A53v824}hDvwlI(;h6zi}u0V*?hLN@p%9Pra?u(}J=5QDpdZuYgpCrV30MGY}2aM7^zJeuAQqfQZNZqCYWS9Y6NiFD2JA${5dKm zxD+!*bkb2wlbv)4{qhX-3pWZbeN#x^8S5)Pw|YFGm~iG`I&>56>2^CXuIvi2yCN;h ztq2d_FyW;>P{OgllnjXp_N~?bR*bD7u`t0@xY#ce^LZ{iF7|W9acCr$9Y=ivZTFG& zEGq;I)s|e7&??()pOH`SS+YKos%6YQ3t<80AqKsKn_%q4YES^BXST#HcZ`uO zcFn~ua&X&YmpS*_Vplr%TVhjj@6t~_{NqXbU>Z1|P9Kc*Q9_kI$ils9_D~8Om{2BA zdTIFwP|95`p91Ez{7%*pH;A}ezDSGR@?VjPbG3X<(z{yz$4<+C40pQaD;buSZy~<4 zde&O2A=sa2A(SQynt0< z?NkAz$<7DsV50%7$}emB()t4Op%ZCGWG1o^u*~y?cxBQ03rHD=1kEFG%K}CKd4XVu z&h-K%Io=~Rk0}v2W3AH|a#V%6Rb$#5%nHFI z>#!(r2_42Z z0{zvA7tmi(QSK`L&}5Bt10_9jJSZUi4RAB;(Rw3zYtv z)UCuD z4s`Cf#5}J1{)u}kK8i$4S$cGbZ;L$$ZLa8`s!Y;v6xJmDM)?r^1~;%Wcd|C4I~)*P z8ZuEG&4<$05P}~s*!U+u5G@!mls0`vHCK$M0OI{WMMlBc53qgk#|4`*9%2T8@`6Ea{wN32V3WQ z(8Gn7uywj6E2L1?bay^=p6CK+Lp(HK2&AlXCgj=>gv!NW5uhKa^muVO?!d1ggxvfC zAKp`NSVRX!9=A6SamCQ`8DtDrLf`Q-7CAXlWz2Z6{j9p3rX3S@G8*}MbX0`fRi3uz z56YUca2b158lKK6Ya0JaadXkv#ZBX}W98YpzeF1TmJ=EHCH&wxAFsF-8(JF0kfZu+ z2dz>>=YT^po~@ReC{HQ|D{g7GpmYUx=>}pG9E&DfOi;97<8;>PEC|)~j z6iG;j&kXG>RJy3dg4EJ7Bh0}#G@M+Wk2ONv8xuPauW%lpRNQB(t3u6e z3nR6J-Gv2ZgQ`MU#9xSbsQFIX_^gj$<8P_5AB(FCeanvE^0m^pd|>S()IOzqk>^z} zQi-u4;&O2b&VImfuyo)jI1@yx=y$CLMEk;odSgi#&7hl2#^{)3u%5Y2RYSy?R-Z>MNdNFDqM*;;9}TvG>+vxaDh&*^hQqPo z_JqY{Td;*7L0PcnO4ql8mPoV8msKlLgJUU2aJ=e#lH^ti#l2D`x#IL*fIO{6V#%(N zU3D)zkui*eoxIzcD{+)@sd|wq8q&Mc2xA9nBU#c z2`__7eUORRAp9x)t$7?=dI9>G7(WW8@mzBFU&xTYR8#t(dgd=e=;{||NUN}ws@UTY zN4sCd4xW#!k&rJfk}6zm&CgSW1R+ z(|=Mx!eY;Z71j&Q2-Y&@59gr@ix9Z;#D(-AA4KZ82APyOqQ)bKY->9qin1iQ{F_pAgJN(slH!gTMcOnwL~d(SM;rMeb-H=FJ~?ZfNVsi6V-8Kv&k zP=L*6Jt;ze!)C+_U4iN=?0&~Dd)wIWTtIHv-73y4LbrMoQP7@|m8^Z;??{qRP8^N& z-tRC&-R}rZ((g1r#5mwMBpwEK*D26zs^}RgI@D%{bU&5Y<{K($E)-Q5%vdlXu;iLqlic7_wN4S;#(rkSRA4Hf1IG`y4bf;v=)t8Yi2U=M=ISwQIOohTB_ov=JM=5J*Q!SD));f?uR4-y%) zMmmCE<3X(D6=D37wI3o&Y?oKvHU>@|!d0Azxki$(b71Eggmomj4*ZziB_@rwTR&kd zZ+ePE(#FEpaakwC{tCmcY0Q}Y76_crdrp#DeOIX3Omts`PWSZ$LZKc8xVkTq@bA@q zc~}-Gc_C7i{O%@V6w}zDh$x^#8Jh6_qF)1zx+mlLbJpXGL;*lg_Kp+F?#b3jlDa3; z{niSN6Xr?A7we3ZgcTVVv?k?;b2traqLbfSDnF5IJHNp??Rv5|sh`>hZ;E~oiXjJ$ z;^1<1R1?W=%wGZ|&%0rMQO;wKe&-a1l74Al8Tk~e{;F{go?`Q$va$YAaU!_%W%f58 zCi)v2KZyNPNuqyZKcvQOe~OJx(+^P}VSM(AX!IoilJrBlc&(R@{d&369JGH)^`2w} zc%hA}-Byz%MIWmg=Sl0X5c}#|s~U;8`l0)J_CuS{4<+Pb=o{#n2O|`Er4chwJ@+p| z|1+L4+cM^8B5oyOzoq!g#h>bkCZhc8pRQFs(6(3w`bOOwv3H>m$IeaQV&}N-&v)*( z$8K}(x5O@W?zhKI!oAoHOf+fyHVvEQllq?s5NZ8WXl1s4DuXQ!iV!Rl|5PjD=&mQv z8i~TX{ZqA6WwvKpzRcEKFQSfCnFZ;gbGn@R1oz^cp27%j=d>vZ$2rY2ft7Qb>p{`p z>6~UR7>m%+Gat85y>v`(%CDEohJnU3(1us=xyg_HfsOg~lCH}a*2*#_yx_Z$yvnF5 zkr5M$G;?(p62tvtgOH-~6~led28`q_ZaaAcSMtY^duT)LVTXiq?jEG}*fDrXd=rku zq+|K`2FgG&oto^b#Ia0$rVR715jw?jBRoUc5qohg4-_#2CrJ3oqw`@Y@2^o(8{((& z+kuczq{VH|{;g3-X$331R@u0(0hjR3P-0xVV?2qzdMgDh**nAePR?IFXZvR+FeLml z6B6xQ{LZvz$>P{*m%dPb=M21}-{vcc5(cf>Z^NQMFKl9aMb%2lTz(shhxXgt0!BN2 zXNHSymsVYmq9@X;SLsRn*P^2JLjltlapF>fZ>>^ydW;r(CnY`Q2RckhOI?rP__4jL ze>4G%EV&}#KormlkoPTPcAo{kao#m@{~qtp#66uoo4ImGx_W4j*ugGyHXHVzZO$H) zxTklAaueI%!EKN2f<<8~o+ohcnttv%zr}t!Nng-{u1Ke0 zaP$Wnm$VmDgp5nEBCdJgeDYCFJ>rN}pal_KL`9tsh~vapw|Jhn9y5XF@N z6ueZB3a*sF=;hSVQUX{eP zigO1FlCJ)6qMK8H&ml~&^@mOX+)mWrf}f<;-;0Q@>W}M^{-yex1Gd25!TNhfYHO3S z;Kf{;AUsnGlvRxm7ff=E9tK=OX|dRe8s)ia?>$9|Hed0)+-s^MFRY~>P z7tyVgNS(VrUxEo@*XOIw{kGUz-0!76H%d8N^|=;^boKdTSAF7;_}=StT1tK1az$!= zo`mSCK26;HOZB-5LZa*QSHdBE4!qq%!`L0k{vzQwI*(%Iwzm0w46BUT2KqX*-)04{ zoc_iqYx$VN_Ws4U zs=W(MUBRU%qrzjA_|twCF7?!UAQvj8S3U4Cxt`Di+)ZS~uVe&gf!g-i4%qOv+&t&p zZ;RcIdzIh6-u}nB?4@80TVvm2OQ$Fg_%11ZFwYPraW#NL;PyB}R@iysq%g1XlIc)j(FXw(+Y#r{? z%16VlX|B#4o(|C@GLwUj{i&>H>z0%?4{7S)=@|=g<__$oynEo1$EQi5`gj*z~KjLhW&R@9DA5`ryW>PoFB9T(Z&1S<%BaXtqVpequ|H{czw>Ykb;bZgfGTi?! zs9|Kk5H{=FmO)C>Q%vZ_<~*MY2)^irHAeUYY~`9SE}#JwUuW_*6+2yDaRDKsHzl zEBo=(adA!v*^w7;LgdkIwM_;nAD2D3iUc<#w!_SAeT*X_T~gSRDD0n3MVOZ1(@|zC zfWme?&h$uetP~{ac_fq&yZV}@W~t-Dob||$ZDtlN`>j%`%tR_f)hFz&r)pqnoG(d) zF)P7cflX6$Vy6IO-Lu$k8JJ*sHFg^YD5{}g%~?n6PCUBiuckiP23~W^Q=YYJ5!=|d zC;BqmjX={s3r?{D%vrxAEjUdWhmtn%^G79y)r=yk!$f|KsGK;E*@G9r|89)#BP~-g zbzl7yH?m=?S8aEL*Tb6ukAY0qN8hGrary$jI^D1pr{rzH8L%xA<2Y{?$ht0^1cAI+ z>8L7xK;sSSgskYu$93$fISjWYIUI)C2-Nj}@VNw%vGk3))~+AQxy4#*y+P~`WM6YeY13a(5wMEl zHUY5Fsk2JEqJmP*+jY3QoA4q|(0 z+E#`SmtbC zNp`pOJR$05U!Dy8EBO17=thOB+m&bpu0`z!qUOS`(UjW9!Ew}U)(@c)Y}S4Q>(^%a zJGgh*tN#>#52s#8@b{x0{H+!KiVne%+LT?*V{)8~#yXBIS@yaL^804A7B|07viZHG z!n%K98h&F@z2f(U!pPrC-=dG%HmwzCn33k+)8td*)Jp{6u7E0qaVh}ozli~}PbEYJ z{D2j(TKGRgq3S+N%l~6~=Rf*GeAN|Xz(E+MTDT4=+4Zp!=CfTNwYdLR>*GEa)2@#w z*6~BuvFi!4{&~E7Nun-f?476!=;^YNA4Z3%xJ**DVM?R$fpRJPM>Jo$9=0w;bekzR!6vquavARbh4!D!kE@dS z(OOStvH6R5a{THNPtB5%iY1C(;mhZgG8bPSy@0%cK7f2?SNF=7_MUt>JXKi8k2hR$ z()BxQ0XS{*;~v z%NjsPj$zar)@BN0v$+Kau>%((QCQrQj|vnYKPP&cM~E2{LbmAfw~PbXGo@a@d7&|nFu*wf178(hnR~F7)*0bmT|TV<an(FF zgEMdO1 z5Q`EAOP3NyF@TgEK`A>5JX7W8L@gC>E-Nwz37>?N6f)9Wj`JS(JA4g}qMU}q{$dBq zEBjmukvYa#iQ?$vD0GzFI1I3EIHS}`QHvqUj=ebsj7;!1{!(lk!hjgX4?K^?GbX*h z91TJ*oQGa03+zL=ehNZldC2fX$Ep(cNh+aLi#8tXw*-NCE&}WF3Q%mTqXRI)w(lQw z-T&Hk&r!Io$`&~HTVnUP?mxt+GC@B>yXMn-@&7d5LkZ(ynjhno47IT9C3|646Gc;R z{Fusc8>gzvlvPu<^5Aa4T-p6_9s{-be;jEUybTj(Z|a6R{L&d)<4JuqJt*GAT$@D5>1UN&~*jl;3M$VT?MLxDV9w}CTYSS%a2GU1N1lSAZ7Tpt+`QN`K& z5MiVSvhAt;Gsx51g|s0AFt4DVs`$`;igxW46O>n5_GWz47_K?J{q^Ipls zw%DMgx>tO~pa9#FD==!|x=e<2kth8hzc^CXf*+^*jCiMuTk;;h1xNlRrGiRvV&uFzWCj@ODK;eje?Q-nssiW1{^m6XF0*sK2bG5*4u(TaXJ2YMD{g4Zb*qN(sgu2R{=WaPi9FAKH39#D^n6zNcfSff2S!!?Z3&8j$)Y3c#V6 zv9)s&`jqrALiC5iBPT{NVmv)A&#=EsX7jgbYFSH7u52R3aMop*U_=P3q-h9ewAJ@d z!E%|pwpPq$?);FD@@oG`yxK!$?IRA1Z>9qR=J>cQd!;mv1F%2fR0<~Q)7`hgri=cu z>mWEHR3BoX(jrYbfF1{nA`P|38*{?!-TrbOJOOt@Cf_>=GI=JRtiPj3TE5IW9QJPW z3;YHL4??G^2Hg|FU|fXESLa`&7c!4Tc$$j#u-qQlnbucG8myfeyN2~7>0=~oi<_Cl zh%%O+(2jI|>pUV)d<88jllB{#$>p7qmJ8Z37KQxK2QaQf&B))tj3AoGZ2HXDzR1mP zPgC)Mxs^Xf+?JYlC+nsO1<0?3NhY#C@w>7wva|1z^#D~y>y88oPt-z`K*tQATr;k! z{M-`lTe1^meTqR^q-nx@km6_%E!}pV(<4qz?SP!kJ5=v&qUIw;R zx2G2KO5`i`tMXpP#Dop3>ca2LcvP<$=jx1CE2uL*2TzIi$@VnWE`PImBNIcRO&Zyn z5nuewP0U>mY3bzd0ftR0cMTOHcUE>1F)sk?stzrWNrKUA8qCtBh*C1op-6-@*C8p5 zkRBuyK}dT@$=_!wiY_H(N>FVnIgwoup*DD$Coh#g!86!QZ^5Jf%&1NkVu^GYo zc%6+o(Q5161f?pCW`?EG=uscDNr9sklFJg|WJtUWR50yfqcz3$9O)cI!hD}?%Yg9f~-$XH=n6H%5cD!m$lDDO{K5&%4 zASc&!#Tzc!rYl~y&h}>tN|rNYQ}N`;x6}Ti|C&L*)_+VgN&gX{*pNoRAHJRiPe(!1 z49(zNWUBdVu4G1THf#OQR1&N{(aADR%Px`}(@D|*=IBV$EqKDmU)A`7`9sh5L-sCx zyT6kyC8^zcO2_JFuj{R#ZPRr@k4BDedrqCVlE(g^;%%O0MLb_USqo9Hej+Kr$qNg? z97rGt86WsJI<>j`Ln2IDy=8L7ZFUK~`I3Ep%qv3@8{Cp13 zG8E-e^i&q{? z4vL4pmZQ(G|2sRngv}=8?EV(U*x}MXV7!6RZ~|Ckr2*j>Yg+j<-%8-6l0A9_XLD?p z53BIXQ0!sEh5thlJiS~o=SMAM&dl#(4hlx=is#Aj*tqQ_5fEm?#>Q1Ypwg>MFrZ2p zur!YBVul2&X9TiAuThfcjZ`tRaS*oU@?$yPISaPsjUZW?LerM6ee9be`{rmD=U|&g z5Ez0eJ(G2iJiKikA<`%n741+&ckSrvU=m%wu@iEV{(+6bJi=~k9@6M~fOp?a!`)qA zdfm#dRd#=;&nLp%A{zy#p;GO3YbxT&YzP~Qu`;=#Oe2ZS=3!{#|6D`qlhja#Lbg;x z8Gt)iLuq5e-`P-(v&Aw^LpdI?zehtkNRmixC`XbeNFUC1b2pTLonNmFMYTJM*1y_N z_DEeggBz{iq`g?_+RHxYu!_>PmpxNm?PXoM_7cG4z`xdBPTos<*~9jt*6>ODkoNKq zp+TNid-(y|lm>;!1ro?h_AR75U+g?sWf~vi| zfYf@lm+pp|EKk&w^RT2ZZ~BG~zlIBQJkd;kRttU$r2)m^Hx#ygT@VCfZFbg79v{#5 z7!tAbC ze+_aIW4ohk?sR(#j(vSRVo}dVG1o)yWeDaYNwQ&xe<`6Jhe6ZXCAg=4{LD(WIZ;&Y40KWG$|ogi82=n1 z!Al=(JRi!D&i`68j2qA}I3>f*1?6II2yZx?(Y1I|<+F}w#FoKJ8Kku7qtd6o^_Gcu zu;E)@aCtS#S=#U|c=|4Q8u7eU+Vp2|fE=l2nwlG^L1TLf{=kgclp>t5g4vh~@tS;Y zJ#jiU$p$G=>Bv9xOO2mf2S~;|;7P3~|AH+Lo_s{wZLQ=3W<7BFlRB-O$zhggLw-H- z!1qvsjTez1h*LRF;uh@E4>=FjiU1H2ZY_!MFy1gZ5CIY_SNo2@fN~s2Pmesle5d$; z)21L_wq#92+AhiZ7CcpwwH0@Ib1iwGcRH~eP)%yXQL}w;ggOMry81L0URS5CGe^i# z)}$>~+rV#E#{Go-6zNK2%q>=>&`k-k*5a%ZSAwuv%VIrgf-};z)&o;NI^T$53}_z~ zrv(2d37dT4%QZTF8{$V=^xj_{iu?4}P_T3GHns_Ox?f3D&lnnNl!q0F$jh!Qv=UZH zLgLyX4fy1u7Y;Uj6&2E^S}>#yZY0fTIf<=hFr092?M2-FafTMRRr&Rji}>lvCXwmt zIJ44`Z&BiQTlX?!AQ=wT5357(L*weF_Si??uvm?F$w@UdrOjWVu;)Xzv7Q2gi9I81 zi+3^=$XPprC3cReHi4~8nAxQ}Y(L|{7-ioK@ub<+93x&`U*(|9%yOQDA z5?6-yHruM-)^(+gxr&+zDk_L+-{j`{0pd$T#z(4>8|@2xb47m8h#tspq76lIwO#0d zb<7%(LhKrJx3^lz*=-qU(@vgUw>$lebYzQl#0tG-u@cX0J3?R#D;1<9Xv$rE>;gc| zxE0b+#9%e51pr6jiEXi2+M+vLAEwM=2Ol{YW3)~A^|}<)9!g)mkJ;bGH-byw0|R5U z2JI{l&^O6BKZ@XXLQsXU3)s^5XUB7B?!z+0?qIr@xU}ELyovinO&#ULCZ+Oay;w{W zBO4<5zUx(+OGYe$mx-q2_;YOg=>_!()uva)AiH|kMwYkvlq5T9EpW020Mlv4L=Q-8+B(~{Z}yLTh=C8`3h7R=1D|{VQ6Pqk46|@ zoiOJzdUmp-R=amx8wHdfjp7&vy-fHj99fOIPg3##TN9-p*&OD|<^lAAR=H&3ZW`BG z&tpwyS3`oy7+1!wM2`tQ+XtjQwxo(|`L^ik62B+Xg2p3X_-!`VT_IE0g$RSTkF`jV zjYDb|)l?3_0vD)i5j|-!O`9Q`!;pruK+q|zMwIHgL#pQDhy+ZDJAGdO_vrJmh=WPW z3~ezE3hg_dMbZ@{{c;{?<{nclEoqzqCMPxOV54j$`2=N+t%dyP{??j2fytOTEF24C zmyqR&aWz7MueC)Rk0T8%t{k4unnf8a#3V$9hI}8Y);d($q&BTk|7hz7iVtvs+w{ZK zat3%l`BBgGI44aJ07Ga;u2fuVQlD<%%(BV=K_Oh-!N#dbrWE}eX3akjHeQ1#&S-Du z-!r91Og@mx#iscBn+P9VN>OHw&rH-KkqCG$72z^2p3VdvZe~6we4$pvX%3)$A{4sy z2#~Qb?pj#_^hUD&>;vmt>JcpNxfc9Fc?)^i!B21x#@=?Bk{aYI42Dfyl4ZSBf-kUr zG&F9lLY--uUs|SOTzz>$&ZJ7DDu&=C#$8WP@Q`dVl9h^pEh@q`7loqKKq&u3xwCDp zUgYi8ajEjQRwNLWM?!EoyY)GQ3ksybbMgMk{P{B!L1R14G)w7b=1K~#mKSX7X6JVr z@Ui!C2jNrp6NqyoMxfC((cfrYN64Cjll3s;01jLZ<{vL6H!i39k^R4Vay$dU z>9JdYa_TFce}J>`_>K<7S6@Y%Ya&?j5gneWUpTTQckuh`%@btbz*(}Z;ctm`W{h)$9j8Qg zG(4~0!VM&ANT$OV)atC`>Uz_$C*Zh`UZs37*!wYAt#$sudZ&nj@cVX1Z^T*ayaoJl<0Ok;D2{91RT0+o;`2wyjm;8vIrto$0JqRwHDK-pyJ%>mz zmu$d`YV@S(L&+><{dJZ?imDgU`dlb?6Zuz#afr;bUM2>_JV%(k{(7TA)qSXhCp~>W z^n>$(<5rH>h9t%_3j0{J0dQBSlFSKAUkIraA*>kkY4OPjt?W~$Af z-Bupzs}!AZ8-h$?*Q!cVBvh}-uLrsmY?EEUM{D~9GUsrp(%K(E*$&B}zpAh{d%ikF zmEFyc4>T^pk3#EZ;8?il0J#!&(4?kMaACmg=t~MzbgSM%lb+0D)fbT21+@p|V zmfyC|UH(mWEiSuUeN&_bqQPw;Vaj0xi`0Bb*&_1f$`D;?g&!9MYf5tA$i(VT&H>6e zm&e`6mk0F8?|uC=2H>hx&di_DXcA}UY~0cF17BvCj!=ZbQ8RE*ex&_PX8a;FZ5S5Q zK%Gp>;=VSHOR_>&{AV4YLIU9Jd>wKGLzE-L9MH>oL=M8|TTg&PJkg%6WgLx$3{@}5 z17^%3eHRHmzXRJYW>tbB(n>FOZHx}S`MfqXq!0Yohk?*T!h^d=CY%)~8S_(r2_ ziKA3eZtGA~EqWm;ydjJaq1Zl#@NFyjBh)9p11TR;vHdn%urLr|en7S(P8Y%w`taPf znPIzD+4&u2-=Jp@X*dZa$`so*j$YbRyp)}G;H!@B%7rX!qC-+r`-nTgg69r|vkpbQV3|u8E7#F|#73fNaLW?YfZwCI z`Gn%&BNzBtqs`N^tUt0fG@JK9EdW)Fad6W%aMp zxLVbHER{&Rtq(yQGse>DuKtRGZMKAVy+}Rr#weEN18a*8$%d -!G7_m#xSlk9y zILW?x)m8}&yOz*lmi1%-D#WE0pu6!6o<2ew7m{4EfWlLfi%_gWk)0AQa4$*O`)qv* z>Y(KdRY-hIB%CE~JoZ|7MO2b#rf6p4q=8Rr9+r<6@UEMy=F@(g!+j6>XfxPcu5)3z z@rs(HKrPmK0U4+@D0krwhE4|eW`xlm%DK!70oic(rL(8y@+{aqA><4_1U+T_T310> zRihV`kLE5uDW3Q9aquUgm7Qo7ofa=V?eo|hvs+5Cx*H5ncS8Z(wAYdu2sPj3d1dR6 zm>700v8~;rS=fKT^&m?h!E2I7Qe$T&)sf)XS?iDZ?Gn|8)}_1Z0x?GPzN!~bfGmJn zaQw6V^+QUD5v{Cm2riwSK?1v#!RU^Z}&$G6YuAwp}^sDY3GOi8lvWQ-&hOlVB;^f#z@<$c1NwJtbPjZ@L&wxGUH{yJT!0y>(uQ`^>U6qB?c{r~-(9(zUKNWe*-c7{Q z?HlFD)}~KOo4zd@_>uH<`=G8%H|;@ra9tH?8I3uQJ;7yfNFBl2r~eXziCoJg-$coISlCbd5X~=cwfpPY36=g@F{YIO*j>1DDsG%Bk)<2?opa77F9_t?q*-Z1H zim?6>er&d~gH3SodT`J>2K7j}XS)%tG6mFC& zZTcGfb5Psp_di1j+O)aWE@VNA-EX~%JBSfJ+)9aU>V#iU`_IHpgw0t+5C>E&D!*x1 zK0l=GFM?5&Wv#VSQ?c9)Qu#lvWlXzii}e$o2UZz?51X5IPLK5IP#=t=d#(Ye$uRx@ zT>s&~e>m{}BnReIEv#C!pwd`;>EdYBg~p_+#fxXps|rP{mPA8!3uj+EdqMT=^Qx#D0m3u~gGi)U9am}AsbR@T+dURZhQ zta*zT%sFn(?C9)Kp(7U?WjHHg(d5e63q$j(X3wcwblluUvoEZgwP4N|yfjX?V@*J$ z(BcJ`Rn^QLbL3@pA;zCoGuQP{SyQ(#YNPB*>yi56=4i=;4fs^-qFtB%f^U0YkVa862LHPX&08J;%$yqcQD(GwN_h7PM*w5Vp$tjgKZ zs(CewE-f*V-=8?ADhiGsf4pYN4OXGzQ@}1-}FE^W(QaeuvmrSKZ#`1QkY0)8{``yqZu;Kz4|s4JejD(61;4NHJNV*wycoYZ_%-6U2ER@C?TW_ZgYiC1i$n87BSE{HW7y}E zIP$)z3JvjMG78s_`85|-4OvukUQKjC<>Dbl6V4n`ykPF!S!d0!s;Zt>C|cqehz8xFB}R>! zQ(Zl3)Z+QG7gf!fRlVT6MY9)OI%?Fss%XK$&|IMa9I64O@+Nf2?8Tw#8YoMsBx;%O z5#JbSgbL z0H%L0BJIe4I{%SXODd~sDYj0^Dv*9Z^h75Bh@*}lT3A_Kv#?4sOo`)4-cDb43`2*b zd~Q;|Bj(=nAF2A51qp5U09I;Z@-mv6y^wbe+raMsX&HbcqM z=3n6@i)Pm<9{y8ArpU(#7yn4vtlBxH<-JLXP@N)ClTVlh=}EACa`gBU-11W!T?B5{ zR?n`iDn0Fl=-d;A7LKi(JGW}lIH<#t$+Jr*51m!Aw?>f6;-n`>dltZ0flBrqYR=$L*FSccBkJO+5c0f5!$yuyH4=!};DmIK}ixEkWhK_nD;4Z*%fHTlpUI6$e;1z&N&?)W!^oP*r4@S8Fy8u_h zfY^L2(np7yb6h;W6>t<_1v=XKfL{aN1UMgt!d5_b^dImUurEfv9e~FIjyTFN$^mNu zX9KPUTm;w&*Z`OZ!($a-Ip95jI{_o`4-})Lz8nzx)VLL}74R{@!eb0$CtwHQ=YX|? z;U9%NrQlff^?;)XfzN>Tc>XcQ>30Km0&aj|6NX1(2cFviL&pIRIE8RHo>70DaMEr7 z?0`#oH0m(~*LFEz7;p|?1>h3EcEGyxxl>RtfT5|N8?f+9gafPv{2H(muo&`M4%cfG za6MofVCOWH3-srn1NuOJ{S4$!crMC`a&%Upej(3o@Tpb0v<2~xCF3saXh{++CkyP;1^&g;10n0OA!w3p%Bj1HGu7xBOKaA z{S~mo01JPDdP6(t1e_1pdKLHq*bbNl`R@h{1M*PLC4jAf9e~Ewc#rl_cn#znuoKXa z_R!XV@>&f`5SF6-W>5Bh&&pLA$6}g?MNeowtIIfVp=deYA^?Hpl^B+b@t0+C|6R zpcCz)`*DbhfVF__fa?MG0Cocw9*y@8 zpgsUY4?&Iq!|OqJ0rLAb_=$E=_%PA~3~xX>$AI6zg6h02VqkR-^ipO(d?1usK z0m}hL0M-JQ0oDUn0JZ`~0owpq0Cobd2Q2(Otskxlf^-z_$T*0v2wL$FBf>Jzx&P6>fq41Y8f83w&rR zJHqJ0C_zXSe*-}Ud(ZHfFo z03X5k&MxR_z_yRTNAkZLass`x{_h9}*bV51-YNVC_y)*}#o*<+?8k;@Nv`LhoXo6N zj~a(jWcLKY3$#@ukUKt*H!+xfNmjja%7LSfIUygCr{Wg_b`C%|BJ{;{XW+LJcp2bn zJi|osE1U?PyKpOTKiGwX_!?{QD*&#D;UaPD^*{<#Oa9k}ttj1T0l^iBwb7WNI~BFiG^ zSEe-rVRj)5)Qeoil>xWMg@Y?b6I(r)KuneGk=UBvt3`q@QyrVdg}oFL(q_g_ToU^GG9?3)Zi};lzUN( z5HLl7+_8bI;%s9C!+Iezh{xGB#<|WrHjvli8yg6%^q(3iSZ0<5Iy~Ns`vwY%10lQ` zE0HH;Gj14hqhYxm0*3-tp@%R@$l;LzJHqd^(w=9r%oo{>~)Z#A<6s$B)3luKPC=HBg$ea);_Z{u) z8yHa>C@cyTpm%EliLj1lyI35R=4gXu@ zJa~cMTF{ga``}&N`(FTF_&L$xXEFI%5-4oQ@U8e{k|{!X$2825_%9nUM^X@=(L0^cKhAJh;Wnx*2S+5jK_I!qyKi$ zfQ#vLf~Hl_NwbMKmAXvyfvTTzHftwrdHQ)Jd$}2q2ZTR|;ic}SUPOO*PqV`-9X=Xi zv!FK)BOlLZ+XJt!quyxom$1y>knNqF4qpO_Uiia{kw7@P zuu){S0r|F5M>5~3$@vPeCEsG5uXkL!^h?vGPxY9F$Vj&MAsfNVtWoj!PpEIk9;Rh) zVshIS+3_v$ricp4$@cD6pwix1cb_1=v8cP@xc7e&!oT7#ZS5@RR&GO5{1x4nkf}sD zCj`RY*V2)GYT9!84)mlgXmK|AyDvLdj2XUi>NO=_w6A3u!rx-#du6(Oi__Gf_g0&9gm+({(b_(Nc1`Qt7S^+)}42`Zy@-bMxRg2CSPAceI0rL+Bxq14B~1k%Ez(NiXSPx$`VoW4#X zQYX{EtGmyP$460j{}zL?M0zQDOwyYWsPG+=KE0xB_Mzoa;Q5Eg_1E8*FwFyk~Z2hKQdSh6AemNSy@m$a8oiEgcjJ= zfY_d82v>k`6PQ|3xJ6Dl=F72uIl=`P4i|ANfGY#ew&7_Xt^sZdaO9h^59f(}$afp@ zZVlcI`G}t0Meg*G1)_s!iz{VCYhAx`Fkg*1nT5F!_-5o|l8?*lAgD1o8 zkH^o&z5jI#>cs~4PGZIdz^%ahzu-fBCo5Z-@{_gC`$3HEy@ zt@ES;(AHT7)>Avi^p`Shc8{=UyYpRwu#M~C?_${Z;QWyCdgt2tuEzUU@ct6M-)_Hm z_%YSam;Bg@up_`bq45qotamZf;`_hg{aVm<0n=*0d&T>6?Du{Q(pIC4HGDtMdC#UL z@$>P12WVNq_oq7VWdOx;7vud#_(H1reu$kvBtX?&60NleyKWu)0}Pw#q;-Lv7yWCy zAY)P=U9hMXtyO$K8gbS$K4=-tzU)Q3M|*I$b6XC5w?Ugw$GHTN@w*X!4g4z-|M_0x zmj%MUm8tO~*=(~r;U`=9NIVX^(J;TY{dss_D(}k?Wi=Y{CwM;;3d28J(b>ZeO!TJ{0_(lLqp3jCb!tej z48L7SvlBj?N$B7Fmwi{7Fzsktc6+?9BS~}}DgESxZ0d@?pbYdePG^4si_&|(utM|> zC!SZGofyZnOx56B8*uDj{73Z6KVf5vjW8k5=JV~7-Vo`{p1*T!!k%yQC7Za8JwK?w z@pf#Sx$ng0<)|njq zNowFbf^GuMx#z~?H<0E5ie|Tr^x$W^*H@CBpE3p{eY{*{Iio1S&vtJw{9Is{=^^ED zVVUSp+_&PND-+|jLT*+f-ZJ*%i;-SRJ)n#69mHyIoGoLeT{24--M54jbcb!GvEODH zJ)jpXhYTuuiOT}w?%DD9Bg7wM=PP46$f3u3H4Hp^fS#t#2Zj^*wRvkJ~kKFm@yU2t|YTS&46?I8ebsQKGp|$QC{zxSLnMLV4^uuSjk0W00@3_m{~H zr33aLt5=ZkV|eyI2fWx{+Gas}Z*T^5w6O85yGT zXJ26<|6$0#k01FT4gB8p4Z6E~)Atpq0hJGcyAF2ai;w2X#5 zxe2t)z`g%oMGNWM2wG-4bMC{UluSJKS21rgBPg8Lpc!>rUapPHRQ^Vdkf9CNjo z(;gi3J!o$C`F@_>zbdv6WsjFj7xy2Tkkj^5+F1{z|16a8bwK~;Y^92yt5FZbFkhU{ zIwyW_b>7;~cXoPtg?3FVuXX76Y@N~CuNQf}0klm-eE!bv2ZN2r~VbFFQX&aZItpl{}x-uT$OxjNQ9<;T3mD954xS=;2 zdyI>=*4}7)sjsH(4Qe7)r_~q|hpvjpkDhjF`_w%VnJ|AXjz710O zpel`GxLo<^bF>Tet-xdM1okN$R;sVz+M=C^M+xu`NW{AW@!ITo!JhHB7U|b^yf_BW zxR~Dt#9M2}`yBX`{PsoTxYmyMK_cER#9MF2d%I^mOuHGCcDxr8@$%tP-)P6%-ZNe! z;*GZBJ*whSulo!S?YNq*t&!9zT@nT@Jj^f^j^3_l4e{ zI0T><3n4r&S06U%_(Xduq^U>QrhWG{@;U*~KQ58i9+dfc_-&ei?=dfUf!!6WdBI`? zsSm-Q1=`LbZ9!Gm-hB79z5yt8s_%Y$x;ge+h9+pPO*gl_7_@^ehW}wO4Hx;e;c&0Gm!W3n$lEtTU$_Z-2}xh}Ug+~~>d9J+tVv%5;$NMi`Sk`I zh`31KUqD|WuvgOM!Mg{KkwF_^g&)ppMgHh zu<4tp=qm?`YuH*r-)WqKsZ{B^eGD>}?DiqHdwg5d+fZT)gpg(-%6ts!suK79DJspC zSVJQIszTL%ry$JyP4T#ww%h2Q`6lMQ^c;4(SNvHk5ii^hy9(v;pV~8C3cbaF^&a1J z_Q5?nMzQnBgHEJ513Z3>v>e?t%^p6o4zKT$^gQTo+;i-4iMH0!n>}=uAM2wihg~_K z3idjF`A4C@-Hb6I@J9J}`hPGC_Wdw@QH!%#&K>#K)6)`<|ATqlg*^7&cl+*Rf<5g% zDJSJ>2F6H-m{^B^dw->(n{q$wNc7*JdkOQpO6BGD8=!4An4CF(F~tRsltYLgMf{0K zw;uQYDJs5}A&mK9BLcq~VSb4)%Nb_09Y*}35{CWYoTK4ig51x-z0&E4IYA633X^$<=MTQyObMh>IW1muxoK?jnpX=xrzOL@uCK8v!(0JV z1Z6qI(|>X38n36#V>WwzfAi$w9zt*>ODZK{YX8n}95PyrlOTpNU!74Srv@Y5dgW{Z3PrXyj3VFG%E{N#y$^^4k*mAA5cQ z`I~c%H@xO2{f&>j<~RL~Tm5-22aK2e=Cc9gZNK?cz_>5NT(ytU)aM|$s=w@GF7I!= z+t+*>_u1x-{>J7U^U40kHT}$0xyG7+`SX2@wE^C52$)^{jpu@w37ZbD<8eBF_nG(R z7%NTl_8jA#Ec3N&qp^?q0xC5Jq$qywHx0L|ymRk0joAQY%sc!NV2C6VzwH1|zX!c7o-h5ruYKk+ljjoJ^>tf+kN-yqmOv?a z9pn2_>XkG=e*IZ0jL>x3exAO?=6xQ|xf!>65{bo`)}g6sL0)E?Klgcddb~gN`JVNf zOMQtbNE>mEOO1o{Pchp(o+%lt_L}}b6Bgyj%{WXqLshN}ce(x>-Ve<}`%dNmoB7WIW*UoMPUY%0C{;;H0kV);uVq zAt|jSnumJ^o_pwDGd(MP=HD}YzxR8$W%_Q;Fu%$4y_;eFG1GT%Ch-qv5|c<+=*~~g ztH?9_JoBbLo)5j|wS9ay`OQD{@x5SrAME4Xo@xG~kMD*o^YK2u53-2gmBlc7vWQPf zcVue1XL}|sFk}5ZZ+gu?_VfMDZ@$&f7c{khx4-Sj6x{jk=jn46@~iilE>=QzCV7hHnY(j5AA8OBa(qwu z&DV20e=yDGa(vfidhf{bZSQM7nd7@5+q^%=*PYF`S3@u4_?G1`d~*&%Cg;<4F!H&^ zV?OH2D1ICGf1bHA%k#d^yDQW8wrO@|`X0(KS7-UY%rN(4`X0;V{kBZv-RYrS9i7Uj zLp=NZ&g1>1&-aqYyw&G>*=t_sODJ!tvjOSjJR))K^ZDNMnzw(?IJf(m&kcUxb5J_| z@0ZVY66YGf53267z2JUc+;x-HjN-U}MK`JIoUpWnOMXO_;OSdf>iI3X>~g z4)6qn=5ntmY<}hOO*GL<+ftie{`T`^pW)l=^)!0SKcs}+FI840>_5DoRUY%(?-6#A z?{kl5rN?aeK4CKu7U}LSY}`}S;`80u*Lc_GyQh!wrO)?ZALCKK@AkgNbAI0=eT};_ z%=h|WJ*K&Ng(pHGBT-H+K2W+fCyZ#(cn~^%=gzGwcNOH+_6d%qRMoUqD~L&hdGF z*#|1ayrz%&foVS5$GkDqysnS=L>BM6vUu;R@z7yR=KxQ}BVO|ckNXYmd3o30leygU zw)g#nMUYko;-2rcO2MN3zx4QC^cqdx5l?%K_q>?cBM*-?7Z`IC53`1uwZ;V;F_5P3 zwqYgEfByc%fnGRJ`;dB3|A2y>8ipU#cnu46xL%U^5BmQw4)lBbCr4h^|Nh_DWZ%}g z?Z_X#^w9P8^Zx{-o6o-7_r!U_`0M3zhjrgGxX+5q)l1a>W6!$j-<9(GK+5y)^fP8f3pIf7hSPpkLJ3d8zBjsNspS$1x8dldr4s9Z~Q0$7@&p6P(Z zrGfE~rVmqwa?wA_%clS~Z>#)tvg!aTyp#D7z)T%(fQAJP>c+h=@Xuw26m z4Qn;5*RWN?HVxNn*sfuRhMgLAYiRsb<(sQvNW(%6!y1-rSfOF9hV>e@YS^aXdJWq( z?9i}N!)^_Y8+85}hBPeHFsxy@h7}stYFMvftA=eFuGg?#!wwBQHSE^V_?gaM!;prB z8iqA2*RVpvS`F(pY}K$$!}S`rYuKS-r-t1c8mn~v8iq71)G(}JxrP-Q)@oR;d%|*HSEx^Q^Rfzjnz7T4MQ5{y^>xpOc*zARH$I;c{p9LE;RDEVaF97 zJG4%ohW_OE!s7~u9e4adg;(i**S`hTXp=;<_@XueMZo*?Hu=8A9py__N-4IHa)dnnhw8@kyW1#{{thJ2V10EK3FNX zJ>`Do!|9y19hHpV-^kVWRWg2nk*+`TVK%$|59#uS*H+t2$>|(uq?2DCP8JQPk!zo` zD2gO{#}hrSZoB??HKsba{2Qh7Sf~x^i)CC(~&? zNrik;r^B@##J{NX9k2164<>%UyA&d#@h@ro%oO|{jlW0Z_tD`G_NsIq)p+ue7r%GI z^gA_vghCr-8lQK!LfGLi)cB$l{Bn&yU*m22`{x>8mxAA*@%1VA=QO@qY z6nwy^=zl=t^L6;)8vjR)xA}0E#=ocWdN^TRr19DJs08eMuhaO0HNH%z|A5BN*Z3J4 z|GdU;)%fiizf0qrf2k0!YkWVyqW`>m6=I^sAEWW>H2ye^FV*-zrQqjleAZeO&Rb?6Lag0N@Y93FGD3KFQPgFz|;Xe0_&P%+~k- zip1~*J5-Is(voW=@aghhDENTU@wp1OPKUo%(#bX2E>`J`)cEbdGvDx!6yCN62B0G& z{jD@qaIyd9H4J#tvqRG#QD|eHgx3!)@Cfj>u5+P{81Q#Op7V4&0vqMZfP*bvxy}Th zbcXmrA6%EnzhV3o_`_g-wq2y~w*KED=>&}2qZIxm9sYfd?>tuFxfYsNCLDPzS48vK zwg=AC`1%z5M&LB->hOhcs&G8}hFAZfqNj6%dba(I5y2~*`@gCe{8He_pS9Z+Jq0?_y*mAN zojzKKT<`Ug4$o7)6X|s7bZmLP3^(b@bt~{9pKqviEuH=~5GwLH{Eb3fukp76pH|+0 zWI7!uEBdd{;dwqN<)=c+&k&8j3ix#N+#`6{jdCcMVQ`-UudTg=kNiN(iEcobs^G@$ zz%$=Y-5!T&{A<9c%l8vWM+b5NZs<-IKED_IXy93oxtjlW`&|gU@MnQ45Z5yE+T2Sz z9|#`$YF`zQ`x1HaFrakw9J^n7{29IA=K@bUcWONxlGJflaxdYx^@2ZWf0a)8Gm1{T zyypYY@`h$8Jog0f>el#<6h0IWApL0XTU9uo9mcC#@ao?EU*8MFTCtiGi2 z6?%MR>w)3uAf#Tj0X0@w4hHH$&?yJj%8G|7-3_fFrrebGtBy1DGXF z5ho;+IAgFqnI7F>Z0%}icV+F$iX@ixa`cX7TGFs{G1HP(5Er5#b^@uCbGSlCLM=xH zNJv-;Y;Z!kmWo3G8$jePCr~C-D3Zdk3Bg{+Ujdx*z4%oI+$#L=Q|RDC$NNi$OET|OfD``M5+j;9 z{6P7N!55;|?K?u};VT%Q)-u!ebPSNFOkfEA&jU{MTNn8KxcvBfz_ku-{Cfd+@!>xj zINI?EoG-0Urt6}>H=fV%VFr6YjR73W_uPva{w9In0XX&F^;L#HTj1{k+?DU=1pi@? zGtU?N?+E>Akq=FQpM;=x(fNJAUEllZ4E%2aZrX8!37-`Dv@f)a{+|O*_|vpl6W5C1 zUyHSJF8mt-C;ScCtOD0>2>v%S@Na>mVdVDpjQjnQFz^Oc~XK{wK{+6x} z88|=N{~rXL^4&=F_rB){e~KW_OQJczDc^H{&j7RMBOd=m@Xt&AObY(D0e7|gRnHYV zpJhbp#@?+0KPCNC<i7X<&00PmA$`}QHgi672i$N02nldcC1oS*IgUlsZrDSh!XV0e;O=Y=0mNTL?t zuJ59?LX_{u4;hj6$)M{Ufa|v~g#SOH_+t0|rlIR(Lll^~I`bA#g zuNM5fzsGORj_+cc~?)Lyr^|>MaeZSy;(a@0>>vmD-n?NYwuae(h0)gw9`Q;D1T*_lbOXqTt^SN5hrx zeSp*Vu8Z8$c6>bN|efA+3| zAR>6{Zy8bL*bRWY`1W?dUFAOwIFtW;B$?}{sa6z(eQ3m{X05|O@6?wCyxfgKapM9zL*gpvV^cNUWviE)@@U&fV<4c&n z_gTglvFp7OaB9caUoo8al&9-02F}m+|Mz6TKb-;pBH+)l3CQyg4LPRSK|DV{hGkf{ujTiZ2UvOsU6QBV)_tk=6X=z=f2GEw1kMRtI^PguZ9^P zY-z63fKxd)zRB;x*4`ToT#&5WdotiZ6gvB!%k;IsJo6T&KP`3|x~I8r5%|UpjIVLg zK;UP;&v=r|dn4ec{Gtbaq4S`?&wq{qS1>v631e=%aUO8`-myw|D5Qf zpEXeUK>|kj|E~j1`Ho3^@-6b?%MLI2cq?Ve+K^d0H^Zzy`1a$9?9f77(Y2RbIUYCLYTY|s-cZ~N!&ffc2 z2L6Q%_@jWo6ZN){vfoCJP<`(6_KBVLEY8Mz1>mOM{)qwREC)P(Sm5g+7ligOq3h-e z`d;vZ$WOITP5`dF%9#9r1#mbXziFxLqFl}DMMM^bWN z2ym*;_3tvGbSG~TaN^I6lwa^JgKw9RpT7k-m2*SB7aVM^sbdV^x54iXfxi`SSHCz5 zxUnZC&eX=@~8cmZy5TLnRR>0 zZSL~>fD@g(#CumXO>oj5#hK(QIH0yL-fG=e!MVm!du?WNL?3hIX074{Ckq7&Z@SqD zXX&W$urf30`_^lWK478WW*<|I1pNs>oFs20iBBeBAnyP8G&8f`6$=T@WNDx$!V>LpZOwC5a zfjP6+_Y3w}?-OM@;kS$vi-V|Lsz#lOGM#c-E*8fN6Xvkg>B;iUB%Y8hPK0l@o2^{l zvyOSj+3lS$3KpBCT2QEP;WB@> zK1tZhRW|y0ld{pzR^CqP*6+P#Ylp&KbkHwO#O|#f!Puk_f??&;Q;uGg%%b3!Z>vH& zS)>T9Tc%CXtb%P9udpd%r4*HV%GEuyoqw`rM;WJ=N4-6cT5qe*Rh-SfRdF``PzRFZ zD>F(;ecUP6f~1I#kB(*$pZxJih-2g-KUys>m29p`@(A#9JL*I@%Q{!~fEq56@P*W` zQ`c*R9WMwf&0w+CoQEK+)66}CIgMyXbf)`FG1XNnPHoSd7Ojk@Q$dnRWuIC%Ti{$ms4 zpgMPKTr)j6H@?Gz%JTtE9pCcQ^nr|1(`O5O3b{7bZHErc7UzPw{RfT~sSb>CTL{Wa zr^m()ADuWb9u#u@xdB@<81)0qJ~yF9q<7ZPpmjZIzR-y(K^Z+`vDscLvI#H}%*;c< zhT+8YWHOaMz^BQ_31*{#R;k*an4BF5U?Jp}8`ak?hXIa9$MNlc;LZhz1C?;0v|NjV zN=RHYF*{0MN)&=wK+2Kdi6&+bqT**q0>6O#h-Od=I~{YnI({jR1jmA~4W&_>M#6qA zbkJy4KoA)AXJ@wo%$!g@e5kzCZZ?|z!SRA$;CdpiFQ4vjG*^o&QpqPy1gPYkKWf)= z+CrebXSMPwVN|Nt{L~rqS?AFgO=$+_o6Sz-DE25fLA9~a1h2jZb=9QG4u-hEals8* zv3M|FT(Jw97WxNBZjH=MKr$TLf1x7L?vmMksiygl(9s5FsU;+-u3K+WJWN4}+G77Rt!KYK# zD5k^>$fRncRV$Ul3B!(l{s2^G*dA}TLnEmC zpm=m@6ndz!Z2YQ1aFl$ER|3(-2g~~dQrGxKs_0?guo+Wr&VH>a&Z-})WWC|=c zW6WHN8A8es{4->WP_hXoHW!hwbjxQ-!90AC2HC3RrLnQ`6DRf`mVO*a))DS0G)h)gJ3XQ3ES)FTI%(XB~=kY;L>TWjeWL|+yI zHNP!eh&4(fuU!L;vIurI`U;ir5S2TPLmCO+=~NdRT#*Tq{IzlOI%LKU+1beZOcUJ? z8>RVL7|b`LrA-V(s%k%3fBB#=Bvv2m@}!wR7ZeAgwN?m|3EVOSZi!{Z?m4ia9C)&@ zop7{Q59^3YBsl}Vce~x(rWKQ60uR^W<&_r0Vs*M~+45@ojC8v87m4_wRhb~my}wec z`2!tx> z0+BQuu7v0{%{4=rq6<40cuOWsW8&S$rj6j_1RNKlCQsUgN#kl@b*U7YtP9=Yhpz35 zQ3B`3x-9MD42^`S)m&idGPEKdr_)*+GWc3w#scn#s}7xb46dP(YGPPS+-imkz{xAN zeT;73KvW;GrP#KZMuWlffT`Rf1EW)Il$)I-+oj}TH&BN%`>HKzJVdR4hCRlDz)U~Iz zl6p@?b%r-K(7@ON>tGGUI_y4?4y3n>>Cf*vW%e(QaTsogJr4~*U>pltW64cKa`|=X(7gKQ2WN3jDS6TY2Kt^Q3z3>M} zSZeT6O{_h#`KW4Qjgqpwv*PE{$u>88MvU3hAIB(@hL1+)BmhM=ZMPJ=w%d!|YPsWb za^1bf?aQjoeP9q8%J)s+Fl}b0#(sttFs2b!5~$f7Y!|_pK@-j~Jz0xnM@CHSeuU!u z-3}k~&$i<>G0Sy_0;T2myjz<9HhB?72ATm39$x}qK%76;R5=?zX(KY}wq|u6j-zoz z19H8Z@)oHQ%rMs;_j9S?Zjhq4lkMIrH0z$%NT)?d1 zk4)E=5%W6Qq(S>5VGRS>2++`=lGBHCIIJ{QVo94lu4k%(N^Lr6N7&*%IqS!idxSMV zRal`CEJxKEhIz;lFC3g}l2?W(#5P9)EWOg5`C(@-jyUp-87(0Y{B0}50aA!IX`ZP? zbsNi?ODfVh)v13q|rb%KrA2YB5GKae!E4eY+{3$Oi z;jK9g@1U{f27m^qkTt4fC*e5!VGH2ggjviXuo`j$@ng@4c6znfC2awG-$-FwDzkjHwm-0q9B+8Z}dCQfP;* zCg$-OVaIlu8m#toI7FarZA_xWm?bt)7E;(Cil)&D35ZOr$yh`Ytd%+@elpH$UShM= zo(bmSmKl3>4Cuw-v1}#CaX!;OQ^I|mhCgj{Flp~P1YP#jluLA$*|k%nCZ#pD?W*|k z(0^=1nyRsfA7~)HQ{8F=qE`c&N=nV%B-_{aV#!RUwuSNbkfUd9$HZ|>thlF669!aFMffM;N%iF zd^osc&m|d0yi>#Hd!7rmn$GmG!+jWah7FXI@|Jxs1FV*bPebCC5OXd!|RrAb4CX6FYd$?)84Hv4R(;}1*FI!iL4!ZEI}-3~1VkV+seTgnVl=R{}X6t=$x zRwgxwkLvbfdZPF`9}#s@sWmZMH(AjdKIJh&*9c)^%8Rw4*Bwx#>E$s}Y=hc13)wat zucQbi0+!w8v&M7HEf%w8)$}BjAzCVOhBqrz}_JvcRIaMhE&Z`gJQi7 z8@xBR+AFy%U1c|#HC4#MD0FNbVtwdCJEGO|yVW(c?I8@4QI@o8m1sAzxUpcwRyJgP zrASEYWi3ZiDU}$z^j+f&qtuIMnM--Fp>U9Tt8*$_hID{!*qv9`YFRxu(IItUD>>ef z2>qzNnzq4udxebo$f!_$3L5)}S?8!E*c{kq-#3Xc?oMIUq=1bL3+*rI>05|7n(1kz zw9Kd;7F0N9WLu1A0!t*_%))N8E!mBj*wh>09SmlJsEQR_gP8S%fUj%qW=fCz1bhnC z&)q>D2~Q=IxCu^e;oj^t9xY9t>`UA12&%5L!)2FL8q?q=k$_Q8-=4 z5DK5Ix9Ny7^R5*Esm_nk$FFS~_ zi@6lZL4)L+!~&Lptkk!P3QWvlPEx!s9E+oARrnlDclUyqHCC$-2s37L3y7BSajcZZ zf@^D#G&{UI1r_d!#Tm^fQ^I2rpU|kAw1p#Vq&9{%^0&=U&z3`*>X%(|=w`Fwzw{m# zq$gGGr@Xt$acDIxzfR07E|oFH+=j$=4Jd5oNseG_v57SyW^|rbEu?ZZ3q>l+PR~dt zj4!}*S;+ESDm5&~?NLS=A4%s%i=o;WD-X=FT{9md5$sj&iN5(_WsT=0B(g(+}`#>YC9fDsS?O-yPHluHidD4VR=ZXeQo2T*dbd?n0y!$;K zJ*MK4PjAgks4ifBz3i-jVHsIYLNn|0iTMq-U2oi;{anb zsdyN##F$+rE~Xl4t>tzYtW?|4atXt4wg@1BuG@Sh#d#`byOSwj5KYE8a7|(I2eDSK za+Fu-Ow+=9V(FdF>ma?NGkfhYUaz`QyU9KGF){Bq-pC2mcRw8HjKynrx4N)$syz>g zeIt(6VPW7Vn-ie&_B=+aE8dd^Xxf?EY(30sdTOr>nrV}%m`%l;OtkSbe2}lEUq%PE zj5EtUhD?!7naoNWTFjLcNohftoKn<{=3>MvV=++nFp|m%m(QV|ALK4a9y8($$zTMf zN|!ih$89a-G<@OZpsl0QJnm7O?@Rgw?Gom&+d+=DfWxjJYhEq}%`aDLl^d%S&mb(7 zI!j&-lN~^e?@^n7!R~z>tUgV=1o#c!XNeNXQ>zwvIf|TnIXvbToAS^J%U%xCP4P-Q z4gtGqR?+3cCD~(WsRC;HhHT2%;>2v{0nd{xSwoe#A7UE%Y_^?3F+kE3ndWIAeKVpG z074s3bY}sh zEr&IKJ$%T*Ul+n!e$7YkOGy7CEB*T#Ok3}H$&kuV ze_DRV;&bqbw$jscr1d^68;ENDnqI&EjYvzs9T0-oy@?<6?mdOy3FqhVmrnmyz^I6_ z&29Jm#k&E-Mf_!7S`LNXi3dBqo?or^eVU;p(0z9G{@bbadd{`p6;JcicfG$om0r)o z)_eTH8Tr3Eo&GE%j>-Ka7x-QOYrgvaemvOUujgy){i%v5_jLWy{@|2e+dn>syG2m! zLw%LG#sB)^Cw}-SZgzS-w^Q$xxM3MTo&VpY((Ae2@%=hZr~gyU>3u$xUeEv5`#I{| z)|Jlx%lPeT{81mKOV0^^_+I}0RQ7RmYB?T3ny2AU)9ZQSPr8rOD<09NOW*ZQ+gcN* zrjPfVmh}5l^_On{Cm=DQXnH-Lyzk?j|0!u$>icwQzDkGoskZaibIjKt;PmT!GimMg zN{9A&x6{YxouB7~*QP35)9L-%RC+xZ{n`g5eVj8tXgUScF;@iB?;n%>iu02GYl-@Fd`iUHSR%M4Wg2SEg3;OB!Oz_KqLW!h@FrHBH2tj5M0JENtCvY z6CD@aqvL|xh@uWUh(XpFoiXBu3mROjjmV}fD&&99y|=1QSGrjgzwi5>$Aa#v`_`#* z&pr2?bJyxzm*F1UEh?(JOB7uce3?OY}Sr^46W*G*mFo-NsSB);QprwZC0=AEt= zZ4XP#INM@mn)2vLmy2cEdhWMN;>MXjC-PfN9B^39b(s=2&iuJvxV53XEXRiaq32w? zd~T7F&v=fg=(*-2C4KXWp=YN|+Kh9wWMd;4>9a`Px1Qq^opH84w@y80Jv-EM#+mY< zBl}Bl;V;QAOFg&sWvNf=x$p?d-#8ojU5w{QF9&-VYMrF=i&PKseI!TYtZG^4l6a8K zjGHjo2KBU(EQaCI^#GNo`>r2SU;6s=r(34?{`tD8Pb_WwWb$!0;I{1uFr0>S96m?l zV>@kNYqvk25ch{^^KEUdJ`@SLXmxeY9!IZgiE3CF-z}5YjkwX zI}Oe==f8HLJ7c7+hkM2PV_GLTby_}1pVr_yz#g+W} zhq_*>&M)Eovbz4KIxolhReW0Uc@3Xc_|UOhp2YWBe6PdjU--N(kl@#U)a9EvugB*d zm3~j1H{kpMJ|E%pF+Lmd`2?SKd^X|p89ra&L&sP61n~KW5;!{W`4*qel)&+wJel8H z)zvl=30`bhm*3;O6QAAq{D{xb`0T}JA3ny>iHmMgh?DsA!lySr#&IOBkHV)PKK=1I z79To}!}$b!PE={aPR8}ADjmmgx;N00c39G9A|0==K{%g{&tQDc#pgVH=oq5Ti8v>z z>!COg!)G`?$@q-M=R$l^@u4F_on^{q8>`YToX4wcw>oFyJV9L({X}+sF|H@!lcTUp zaL!fNL^}=F)A6|+9}mN4;JOH(nGBnyuCGw%**KT7`(?N;$EQMJV}?A_t^YrUZg@5K z^}I{Y?mK?x6`x!(ZuUiwJp1u=@{Kh#gWs&mKseh)>*j~?Cp;wQ;R9Xrn%H|zfAW+Vr8|Frk- zr+;2Ih-TCULFV0AQXxkNkowx6VZ6^)*`qFKuZFq9Y@F_o@^U3&(btUbM&!3V! zb@Wx&Z&`QpcZ2_RlqV+pxF-sxrra}P_(gxewBh|nqtEXD^!7l)kBMK6o!lAa-BR{O z>S>#YFMn^s+o`=gUq)@-aKWzj_tLKDyJu~`#?-Ig9Q*1?LwlV$(;46E;))x4*UXuD z;ayELUO#5*jrZrC^~TV7ysVv_RQOsOnv$C zk~69=+5X(5!S_6{>d|pW4>+ag#e;5XYm0ey(?>IhEuXe&*ucj#2Cw<((dfVfqi%K9 zZYg`X$BNJQo-t~|^RGR!?x?Y+ZT?fiobQfk{QICc9vJjlV|m{__HlEYE+5(J%zu18 zXJqPQ?>syEsk3jsrTWI+8|vq4e?9Aij=S!>@v$dwtt|WSmKm>a8hG{9r5CQh^!07G z&-#8&$AD92Y+v;0W&Or{`nl_bJC{G7GxyGE>!;3Mzv#+;E$M#dxAykT3<&B zbI0)c&n}y(=I5_>}E~7R>Jb ze%i(7TwPo@`R^S=3J31Ie)+chE?9f=k~c2D|HRg&>pnbf)X3XPKYFBdb;Z!L-kblo z8L3(Cl-+T|n6{{b_eV@R>#DhRKkajz@WS@DmUQp+$RAIi`_H-c6CODC=fIlL-z*+5 zuJg;R|339{`SjSEx}EZM?EEqJOw7IfzYk77_0{Ow$4>ciYOlcDA99}mWXcEk^jul{ z(_dB}bLITi+Khf%7r8r*U6}jEH_ttL-=682*>SvEQ`j*pgxuWNuZAqR7_RTzFZrhu#y#rhB zTC(i^zn#Cc&(5K)$NzYG{OhB?dHnv%KN$bTJ@;sP6V|@o`|ItO+;rwOFYU>BKL639 zXU_dHZSw7v*)QBNB7WSCvb&9DpTbv{Bjqfc?dUEj+W!;6<1H!S@y7Ar^F@SwZ#_PIzMCW9gCgi5Ygl+bw?YZw>U~rM`b#2|_r>_|{ClQ_=YKrx zB%D4kjZnXT0dY9`FEYc|<7elDr$0JEz8^)vH$>3$kO=zGweaPA6LQ9dvA1WB3@_)T z2=x+;@m@GRJRQN`{~3XPL2`IL|A`O>{(NqD`b81qK;Q1+>CYJ*9`6&O-j_$f?cfv6 zZoi38kMknb%ROn~`J53!pQ$H?m;Wf#SGamfis0Ap^$kydcZBkejo{aNBG~^*XL$Zo zBFJ+=1peD2#H(%*;^7I9C!8MY0fxgzM&O?mfzQMU^$0d${p;Zf`F`ULFVBCE4UfMx zG(3J~g!s^BLU_B1iXi8N2=bg0f&OYVVBze)EkfM*If7rj8o_QqieNu|k?%mv`Lw)1 zC9&Dg5Z@T5A|9S*>uXzly~JC)%NvhC$bq(FZQ8Nj1Q4g_tAro*wN=t!8pn8f+m=08 z^05_4K27qr?d@I?Pf&xv0Sf=}WQn(^L7qYX@_C9+rR1|r(Jz5QNuHKinIX(c9P{x_ z@@HKn>H8?$qvVYFQUX)tZCmq6WDk99d0QnuP|+Wu@Vaj$P^9o>BPIXlX34;i$B73i z--M|WU)obX@D%h%c)J?>D%dt4PUdSnTGAWi<~tG;e>G^Oxez*LLs7)%;a6n(C3&)4 za<0@vu0;;>(GC>IUlaTqrI(jSQrlUXP}+%e?I7V*EYtxXC3jG{kT*75$PUB%Y_rg`_y1$dK~nDSwAsiQ}aaQlBl# zKny?HIbPxk%HIt={|E7t{7$#fzY@WJ-xqp7+)(vp*cHtkiF#Ll3NsVOMDQp3X+B-n zuR%X7TGCfq%Jn!1gdWsH(o>30egwbEML;0>IAyo6NO7F1^xXWjO!rp!i^?8a_ekJD zg=eU8#aqht3=~ED>y$kwDf*vOeTm5_U`E_{zQ3e*s>v_7k2v}(eJ-YnFC1gUH|A_{ zA?#|2#D^%{iS|+G;Yx|WqwoZW#I1hxiIUUmcil%x{pTtFHS|9;PWc5*O5!l`eHVsJ z`7X|rxKG@Lzm8LKz9?}tH{!St@sr|h8%-eM_*|ZB6El@vsW@ij>o`gI-M50=HbT+A zkt*e^RdT{S#L@RcDNjO+By5moWGiz@KCL4qJ`?^=N3G(&c#BLwDNnW=;RvMXxPM4` zG-u*?4e^%B+pZ=U4SV|>6-{Cv$zFW$(Q29HGFODNs ze5+ITd4a;0DSwISE9Ere(&RYFzs_POX$Uw}uC1yaG4z&=_(OVIZ1MLi5+r>~spP*_ zd{p+f_h7%ozfRQ`9v8<_OQk%QOYvZpFLD)>3&7jgBH-tdd7RC{eKQr)ijeBzLD<|+Hh zS9m!TLUFHE=|5iKW5+1}vee7UK9Wy@CH@bDgODDYE&Zak7gD*7wdGp+Y4wFxU_{Ac+j3R=;#6@UM#+D51iyId49Ta>;zxI);1myAXb}>} zF)Cjj6-xQGS>$|4*wwK%ry@l76~{ zJg=$p4i@$kV{2a`0hoa}YE=Kt=9Tz63V#$yl0Qb-?G%L%j^IbhM@afCOLTkr%k@B1-Pqqyz{#5=b1u^Q;q4XJT(Px2*ldURFex>C9 zuS?pMuISM$isLKPBbC>gFEhML;Y(FJyH%CzY=w_;O8PcZ6b^_jj+<0@+i39@haqRp zd6K@eOyX>oSkuTL_>tKnmRQPq^Px{Q8AaRh1 z<0R!@HlW{P`zK-lhy$u!Lw4er zqUtxsCi$GB>T%3)CI592H|EnS6}^)-f#A3U?HnE5l>J-%J!!DClja0TKU>$SLu- zbcvtPOR%vWr{<|_f04L4*&bJZ-Mm%OrOK1-IPf9;SpD%6H6E#~l6;JIf3fOUw5zyh z#F^_6Z^xg~!Fw0sT-r*rM9IXoWw43zDbKVkaXnkht@HDbKg^ zWV<^~>Y?4z?!SV15O$*Y82&O@`Il4q7n(V7OcnNwcw({JjW9Lh)2iAF!>@O%c%G*^ zo*ybc&!v+dVr;FdyaN^fptAG0ZzQ0aRom0&OFd*+`qiggB>6a1|Kw7|Cr6D(5~fH6 zOi;KB4oLdfmHwRyAEoTAT$7Z_l@>$o${l0 zDhLik{;6k4{s}5>82yv0U@%lKvs7F%W!&AY@VGsa@D@d1i#Sbk##!PL>H9$%Hz_@gQso_M)Qj4*XIFTKp`ULh z<08fX`#4puV;bsE3GIm%E_NmS|sm;@EWLc)3NlKboR9;k7Qs617s;a2+6y$r0 zW>!?q8&;lQRR9lEN1@X=q_laehsy*HD5fH|1(pPKdq} zN0d~143(q!H4rglC;*Z(Tm>a2ph`TmDoSB3t~nV=!;OcHn(~yC!>Y;)iiEZfhQ%;d zb>TULHIu}oYH#5|8%HSh$g-m9>in5SV}=*h)+Q#Bp2-giW_t=|&GwM;Ssf*gD5@rdP>SFOGoAx7J90Rilv>(qQkI+ArrZ4LbfMqq&I)2 zr=qH4Cb^9#$lW`yvM3{Ke6}}*uAs%@ij4H6!^bL-vobt%B=b-zfv1vW>{@8lIt2t# zN>C(u5{J#mFJ#hZ;0TrWx5?r#mm+~mkzZO`QQ-M4N?>LG>!jy~N8$8WYdj1?C1JKV zX{aYV7`-dKu<-KA()@xV*MylHps){nQ^Ih4Ao(%Vd6f`U4HE0!T z6O%md3b<5dMQKUFyo{uvtiJ{8-%jep1P>afDx#?-?yx#gV&cpqFTzY| z5!&F4Ng=U-#pB;3>yeZ-`~WfOe@B265&(S#1iwO4gu|+Fi#PsHY1^9Z|EQ*|nICp7 zca0&xkzT`O^jm5&Om>lV_Z!4C8F*NKxv(^gcbk7zzhby(%|$#bpIe5|k~2kh_I?#x z-sAf%f>e5f5ScVVLL2?lU!9f3NmMq!lzmsr31#@JRBW*N4J__AWSuw+t>}K+wo4~u zdlOTr%Tbm;yU0^LE58cO0J_S_NyeOv;T}qot!76cwPA2sP*zEn5q8j!6n@xb zj0ZAGvI@$GhwQLdMdlM>j2g-)XrFR;gF%QG=*q#F>MNUmHzFL7@Fx^@XcgMUlCq+Z zM-B4~_h5QxM^lm*9rBQ&Qv^A}(kgL=dZr{f(bea31~GbBzYj; zOz$k&g5Wk~p(dz?nz6-$VSZ>NPqf@r!CN)2T`O`wlVg}v^kPgaVp4)R#pNNVG7LjC zf~xU}G!<+_d^UggOY~0aDyS+#bk3MmR%RQCWWZOw8U>P=$(Nm*o{I*j|c0znEN5h{r1k5@EU0 zR?JB;8gNi=+d>Upl~v3ETM@!lqet}^Iycf~`o9V@8#Hev#xaS*sw!qwcs=F$b3A3$ zGpiGaO)My=sl?3jym3_}g&r5&V@`0|=wQi>?uB5UIE>^JYY&*Jttv_!nN^x!j_KO; zd~g258gFHd*R=P8et>cyp$p(`B_7deNgpS)WM;mYE5?K}ntgADsvhcA7S>d9ii+am zYRq`dtiX&*QXy&Y&|`;i`ReMDndLNFGz{xFr~^@uVL9k84lNgbk?NB2f{N-$c|+1N zOu6ve(k+NtimAWkOlWcpYA;Qd-zceC)3JGrJ*BjGM3dp+Fe)r*1rCJ>SYBORQKbUi zhyp~Y@(R(fG&;!`pcc$l@*ROWkUg;u-rh|+m8FX2ku9Dl8;+VMgs+ zp(nI9L)=O{E2)BY5Q%~*?;uE$y3(L?My-Yi)|Ag>rS(1))5`KgqC4FQ3TAz#B8(Er zNy77@JK=d%6{)}>!m33Cb#uQ@GR<%q)uZfgh#{!k2!TxE5UnwaVE&OPST+Yi5?Vbh zBWvZ5+Gdtk%%BwwOT7t=%C!#59Tl#zKx;8e6|~h2L=>YV*JTL4pFVI>QejE{%yMjz zKr2Wqyj5kQyFeXw3Lx3usUk9M*vCjGAI_#@ZhVG3D0@K}8M6z00^sNjyTV zR>}T|HTR&9(0~-O3!7l^k?#nPhxCOa(f);^DsMQ2WlRF)P+5LO?aJ}wBzQ8iM|iMw zKhz_(4jg(vZdmo){7P__J-BSIDw;9uXJ(-SJLE|AOqzk76sAnjy@MNI;|lE_V0d{& zq3rw08a4-}hh`r^Ax6w8D)3fRxxzK3ssS5YQdC-KtOuJL0#Kkse$>)1k;k$)5fY9l z)`r}Zo@D69l{MZNn5Y>J%if=iZ11qj{E{jcb)libgqre_D{G2qG>j>0Mlyz>MOElV z2oI~X*L z!Qxvm?uEdXuy;5WvcEw@S2e&>tZE?V*}unwBAgm{FUk_$etkW6Efyb-9L9qEPe{&( zH-}Ml77h&4iG1=i=x>-Fr`4JSweo4SJ;okXvD*+m4{ZACTBu}Z zXi3uF03E>4KtVG9l5Q;K(`-R-}Hzd@_=XXvfk2SWaTeZDvh=6&j8Lys}bNgxt-R(HN>8)@{jXtVFJuO0O-! zS|Jlbbp%4N{23Khw4x}&YtpboTK4Z^{cNY=FX0zrgN#??ZjZ4AC%FCPaPZh)xgrLK zqyzgxh-@?*{0;ozzM6l4|9qNI%K{kypHurfsY^@U3uVZ8fjyz+P$N*b2wVUTsWockt&6mMr94pGDK>Ry&VEXYDYY?z-dF2$6jDkJ(a~5enxK`kpcC$o}%)5NEF@7D=9PJ4=S8 z&7%DerDIDg^1Va%ySc$ly?vR`B*aJ`L$rCW_bK4;Sk$ z!wAWKzr~6;yf~ynMacQ^Fb|_K@sdxXC$k1)PSLG7z~UY1FNbRc(yiw zM#-E++5uaDm6mFc8E{40Krb8}>T04P&@Xu|R4UrTU0o7hu!#CkV*87Ozfz!Dx`@Fp z@!)ri1HV2dH>!vvUBU!jhA*g)9ReAkWYlbL-kjh!b=*2qS0(rw^?% ziD@R&*o`oC&=a;@TtV_PMh}OJpJ612%r8`XSa0BlLW|eD3V$2!hfnsdb$7VskCX$t z9KXdTBI+ak27JYeKxIu8-bpH{^48?jHpt+}WIv6p(d9mb1~kGWk=wQj7<`9&XIbp` z>iSzy7~b;$WxV1?!~QOjBzO}$a3l4qx`u>Mmmpmo%F|s<22&K3cyp@=n+awY%~Nl+ z*$k*m^r36Y>5Uo-C?MH$4))+|r6n`OHfZXjomX8!X^fiGm7I;Hx6u`VAL1$)JQ#`d zMu;cPtC#hy!M6&00wvZ}5a;!RFS@XSf_%&i_$T~#oovV3-}c#nX z@O$0(myaK#rQaWo!zb&^c}J3c`P2{8()~-={hAX>dfQ;_x0v^juxaXk9iI1HHweGH zd6exwh0S~G8Sp#O_M}SZ@4gfFd)t<(^xMzOh7%oOTdUGniu*ln>s8u2=vMr)a1Yxi zl`awYyW2J+O-B^&Rj7NE9#B$(Uq0<)`&nVP-Es$hx$_UUUK3>*rTo2w{8VP|@l>8} zwtl!z#|HH~bM(r(gNRPpb;O(-X$uIk4i! z-z&1>#^0;3;+pzBSu36!0XKes)=Ixym5cC&B%pCHKKLnOQ~p^IaO3yHi4UhYeh=4* z8^7ah#WzIYzcm7G{NBBlkMVo!R^0eI7FOK&ds;W6}(I^q~UoZ$%! zf6E}o*~#$h7@o!O3mKn0h8w??OOI7DT>U8r!L63z|6=@`Bj7FScaSMdPQQTBH!?mg z44=mEMGPOo@aR+J5B9=YZ5&_9=+#g1ilk)!;IWB6Yg zo*MzrV)e-B6Bxbm8`#87WB6l?|15@|#qc_YtDk5VcbggBiwdERJoS6f1aZ7B0^Y*- z7=OEiSZ#=aSF-YQ`lXD19Fu1q!*?qg>9mN+!|`?(#Zv3`4-Oh`E*G9nWBH)W6;Oipbiy5w(ULo!7 z2>4Qlt3M$jOQ(JZoK(PZEdsvulwe%q^sS7)Uvd`T(Toqrvm)TREbejoHpc&E#(x9D z>lhx#_;Y+#1iX%wi_`0j|5C<(E5i#J9>e(eVRjYE@M=b%!0=NUeKNz1zfD1pIT_x` z_~bHtJHzuBKAQ2bWq2;b+fEMFFURW`{rQYOkJT5)7clzc7=1It&tUjMhO4fuC`Ajy zKVjus#PG3<&u%6^#}_mDGa3C-hPN~R>lkkQhCSVGWBB`wegngezfnN9+Zp~EquX+kjj9&ez0`XJ=!_}V# z7I-qlr!oEOtiCwzWb}Vv^vO&fj%$qm3MPLR!>2Po3m6}c&tmi&8J|jq|AXNR82$mn zn;HHB!xu7q9K*BJ-}4|5Ilh?D-_GclGQ5=GZ4Cbp!#6P8_*)ZnyPe?|G5Tor2Ny^l zj_Zt`{&Z2SdSMg4R^Cfky(CA#Ya`&hnLTqpF^qnnl7&vWjGp7M zjQ&GLznIZ;JdV*{#^~c2zJcLRh9|M|Y79S$;dKnx8J`6VFJpK!!{221LWV!ZaO3aC zP?nYW81=Y_(GzaeqsHoyaN}rY^nIE9TO;5J%&s_nGSf51*D*eK8JXgo#pL058>6pc z^s^X$j&ES}XEXWR8Gb3lcQbqj<8Nc-Qh(A&+)ZZqT1M|=_@5ZAF}#T7o5gVTCmqF8 zc?^$c^s^ZLHsfE(@TVDG%kVoGUdQmy8NMz89;5zl5Q)O+olO56U%>dB#N=;gcpSqQ zGJF@4XA#5IpEwtH7c)GT@mb37&5Tbg!_}WQ6L;GfK9cd-!0_)FpLT|CW%yQxZ({gv zhQGvc8`Ebq!=o8){Ou#U9n0`2CTASO8yKH>hBq-hnc+7u+{y6SjK9Wkjp11g&trUY z8U9a(=P~>zh8us!j#xYJ6{GIeG5T2y-@y76w8CH*DGdXJ+ z-ofY>FuZ`_%?w}9@P!PIXLt+4&t~`{hI<*_%J5Vs=Q@T@Vt51KVp1h8D7fh;}|}f;qeUL!T2OFd^*FO3}46aSqy)f;gt+m zeli+n;cX0mhT$6+ zuKx6-xZBR~7a6_I@JkrJmEo5%JWj25km!>b9?$Tx43CYI<8F#Qthlx~hJVB8;~6fW zls6L?-ks4WGn`gwjl;=snrk)=jp5M-h;tUhdow(j;eTLw9>Z^C_$-DW$?!^sFK2iy z!}~D2j^R%*d;!CcVt6yd?F?VYaHDOZ+bs<5$LJR^T>a@oad$Dpk7o2s8Q!1atqdQ) z@O2D7hT&}tAHob}1H+GH^z95kj^R4PjlMtK-pcS382xUBpU80Az@XjEVR$sdPhxls z!%ty&EW=M_cpSsy7#`2?`x&0V@PQ0ZX1IspPKE~d`92G@WBjU#PD+%zL??X zGJGk+&trHi!xI?3j^T+6Z)128!#6N|D8t(semc{?&hTN3ek;R=GkiD0M>5=YTG0O0 zpT-tVw^?qu|N3{PeFEQTjD{Zuks{V7}VR4v2DFg|q*Phnc+QIz6%+i&gfeh zp2p}GF+79OFJ|~$hA(Bf#_(2#&tmvGhPxQv#_-!2zJcMH3~y(+`ctjquFmj@jD9P_ zvlzac;TJR9rus9~Z^>nNG{dJdJci+XewC5@e)+owe%HY78u(oUziZ%k4g9Ww-!<^N z27cFoMFTtRr~IhZe;cj&dp=tcWz!m4z1=$7wE9)iD+Oie@Xr8m?>yskob0FCh+d2@ zwg)zKc6K(4G>vDr2i`K%G?d*QSYf7V0&#obSu;%wDcb`NnrWIy+#a~aOw$D7_P~5I zO@o5%fh)~4O(bp)rinR}-%QiQ z8_I8{X~GTVH`6rHhVq+fT6jVE%`{E8q5Ni=CfZPbGffj|D8HFLS)}`!X_`<&`OP#< zq@nzKjq=Bd^d>VsP^8~7(=^eA@|$Uz$U^zeG)+{Y{AQXas8D`0O%qcnznMNuq^~s7 z@gkjXrfFgdDKt3FSA_LqvMp&qn!af(i9+ zrfFgc^>3zWLJ8$J(=?HU@|$UzC_?$oG))kp{APNDNY6LZG*N{5H`B=?oo}X7MEYVg zO%p$;e=|)JJ}AGLrimVu-%Qg456W+*X<`TEH`6qsgZlr;D8EyrH<{^Fk$%fe)5HqO zZ>DKN1?4x>G%5w|SdpG@rpJl&m1bHK>3lOy6C0?1GkuXrr<&>UBAsZa z-6B2EOw&XM%5SD8h;)>hril%d|3{n6X}6wdb&vWGt-xg zbd;Hc5|NDw*oCQG`} zlD^iGuCb)cEa@Ukdb%Y&*^erw4^s$((hW*|FNW3Thgys(l1!jPg~NDSkm`d(zjdEO_p?{C4H?WU1Ld? zS<*$8^mI#lvL&5qNoQEnqb=#-mh`!n^cj})$(Hmnmh@4UbWclquf<<~u%tIz(pLZ8 zXu1EcCH)^udbK6}iY5JmCH=G|{fH%fpCx^}CEa96H(JuyTGBO^beSbxWJyoAq$gX_ znU-{hB|X}b9&SmWYe}DBNuO*tpd_8@&YW`zx1YNPc^fi0G zjClQN^jS6k5uI7F9gC=Ga`{$)uU?F%-0k~X|1WM693pW_h{Q*Bl{k985^w+` z+w+*jKcSZyM&joo6iIv%m)L)V=Fe@{qLixZ&>#m@JqdDHR6UWPh^pQLF{G+*0Nuf; z`YbN^nDB!C1ffXq0^BxLZ4rF!ZZ7y;;A#>44uT>IUWXShrQp|t?qCEj=7RgsuMMN| zF-q_!aNFg-#u1~X?Dh84sXUr*BTRF}z+I4030NoruW`fz$dxQli=zm528!9Y~Mh?0$z ztiZUF*v857B&w_j5kVmc5?cc<3`K>=GUvV`A5#OJN|U>OAYpgvLx}557_HJHN=qLJ znom1EWI3&snEooB(|?2-k<*PB#4?hH70JCs^1dWli*c>)2ap?B4MeXKmoL+0jktV< zF0Y^`9+6KpVx+52CEzv*d;vh@B5U*lAv?hxHzmypWPdVKl5*Qss0&du~|Q+c=Yzee^vDk0ru8G3+G5EbaVJ4p6QT6rXl{ zY+9Kx-zNxWWOqw4{Z?Z1A5=hKImV+b$9W*29Ony_ZRvQI?j^_yn=Y6wa|pvYA5M#W zT`Ge-4L~%2WRb%bJ%MtVO*!0z5i4;qNsbXDpLTpC@|8vxB{6-^4m>uD9{T|U9+KpJ zC`a>s+VO?vzcx!cn9Ns_)83+ubK14xoUA_l|yR|E`{Ko$Y-5)dPS3IbLU(23$g*qaGhLcn(tcp8A) zw@d#EGNK4`C+?AK*I}Yh|2Jp?8x=*FqNq_6(}`jcQGA3!w~$>rcMdd+D$(7P%?&ae z9pi3BlBh@q5lNvW8IGYhkvNtA3z98*FCs~$Y;M3ASv$7=p$(XxqHd_U157RM%ZyeFgm zO8wu0^4eswtDE&}K+$o7m}b>{EA^+v&1H^76bOYoO%rkiJs=*kjguni7eWC2Dm)S> z#uR|oB%jl3fpomC4@`tNaa`C1hpy%A4#tTGzNJ5{x9!P&s- z6-M!iDrl&bO&^R*^kRT8%>FZw1@1jYnn0C(run^Sy?mW|hD7weBw;eB1KkTjYr6ZF zLU-53xJ2{xzDNyk!3#UYu9nIHt)RE-b>!)ftDK)DUf)8UfiGrK(O^{Hk#a@UkS+Pm zlF=oK(GbNbOY=W3LY#iG;#EYvT8P(`CD7ayB;Ec(N3=WTa~Rv#pmO;~Ib2QY)QzF~ z8V1g;`c(-X8x1>~A@e3Xvp3AeT(=$rnm`$@Ncb38r9X>0*`j~{9SDX{h98#MY^FV) z48-NXR+GeUf*2{)Pgx;T5I}m&v!XP^ufle8iiMLPGi4O7M~gf@)tf1g*CBfVe*+%M zK3~S+dEz$WA8wPq#*)4E)Vm2)Y=IFb>r)_khfds-BWK8GI-WEottN6)(ldlChNQ>I zlqu;EB0anf8}q3?7m19?GmL83NwOo_e6kr5A5RkBP$MK(P2U=-4>-?q+^2epb)|d^ zktae-z^I30{cp&FL{^2oO>o?z--IWqkcj;oYrOi&c!CwHT=9L6B!Xm@Q~q~RwLNl` zoQtC%u|Pb;gTx5pZuIp;*l-Bs8b>8HlGKUpF9i6s<4N!pI_(Al19M;v#pl^iZGijQ zjP;+%TGQyB_>m5*8^siA{vKzb-vULUOPScJKPhxK`Zm-uy!QnPk4?kh$4S3MsPWu? zL#y@|Gi(j5-Vqq)eU4;Zvd!Lb12OUUT#_x)-U~$6wNM)>+1nd$qxbokvQ|I(B6?dz^L^>;~6M32Dk?g6%3gGR_-KA~s*v(cPv_9A+| zLaLIX#VhWheV#B#zjW09+H{8#PohzN*EQ$|H=6tvySr)j zXI7Azp02&D9{q!s(q_Lw2O-7$Phg4$A2EO9idv1in^wxxDH&@i+YuLh+t3lPpX#)^ z{ZU%|iauKX+CFaIxbM^bsXwRt#{I0N{JZ80t^S)n+Mw;)if_7UP3g^jsLT*C#LY{| zzumreStF^J8-A17xVh$1ls0gbuzi@rSFR?sdWq}xf1!Sf6~Y4gB0U^*9q0Ca; zn*tQjp+w{oBUXIjz`NUF_MhwUvdhWU9{(DJ^lj;QLv=+0tq85E)eSs^>*8QUZRYha z%u?0=gmCpQ1e5ju2{h{O4qu7MZ1T$b&-8gM^^fw2`iFhn8wQ8e|2tH5qW)jT6~$vw z|F9Ud{#C7~iMpmGFI2{CjkE#ROiLEk<)mb?d+=p{jR4A21iEnx>nk zv8v9}d@JPR+MpfYzMAhts9JxPjKjA=i~7Ljb2|{9Ln`uWYeh;wqI$dnQU@}+)MFH! zmc+BxBSr7ksH0##_729s#E%mH6_|%K>NW0D)a#L;)UT(?ed|kVy?1piGwQW3{L#4H zf#!hhmDi^TPO?55v1huAKR2VmhCiQz+BvlPq_<{+{yYFFS)V^rHAJk>9rlXx3HZu( z;%i!#Yi)y=0Kzj^%Bv{I!`JLe)QubcVE>Z~03(qjobxhA7Oo2&dH61M5K)7p4g~JO zPtwWW&_>@2z90w&u>C7CfF*RxaCNF(|2@Q1I{i~3Z<7%>hKI>~lrh}ophphT9(s1M zhizaP$sS&yx;6d!UkKBxJrwppy&%{_%tqM5PhY?k=Adl)iq9?f;DCP`_Rt&G!X7T+ z7NJo&>c2&h8eV;o#UkR-dHRzOQF{1wrbSS24~grt`GjdB<_r5~Zce`|;6^x&lW~5T zX&3bC1EyV&GGk3o*IrJ!`SWD@FMQtZzZw$Gwnq9)-(J_C%`(z=N2GsEG(`7+t@iTT zD^H^TQ6kze0YnzC6{<3LiSD81$K)m%!%A1wMoZjxrEIY`Tu=HDeFG|2M?!HkwM+}r z;@rO3vDB1m^(!^VIE0KfQ@8gUj~;Z!Zq1jr8_#<$@@L@jtI-;k#rhB&GkR#gt7F{$ z8iatf(AIDtax&F4cPhD#_pb;h-rGA$soq?^%Vb*;Z+02&jTGAuhLTx(lIlT=g8mpD zwKu*>_ery2Z;!xf2#8M0)zq+5l1KQ`WIPg$Efq|(E5pf_M7Q66bxfddFdk_X-Hn!M z2p9w{7#t`;EGVRH|4gxvZPTg3hS51e6opu}z;ys%#N$7N5sv^Zt}mqPvvI9Ig8cQZ z?LaYxitTtxn(^AeEVOrqN#Dh;ufesso=knMBLdHXMRD^=*0_-##lx`pkl+VXukSp{ zeuEvhiZ1mRAK>>*JGFuyFKbb47f=`3Y15XbxieBHxh9NT1AU@-hCK+J8fkqj+*R|t z9aK4e#PGx3sFNY0a%6oh%|NA~>@iuXle3HQ^Z@WV*4FW=^6S*4F3fBRf{Y32#pwnG zWp|vdW1-QXe{~8rrx00cT8^vOca7o`@QNxTIkP5ZWMpRLWRRTcLQY&AZwpKUMRBvX zG<$Md8vcM`m-|WL{_-4y|CouBiqV1#In#v{0XwsUrIRu)%1Fz}pj%2Gfi1x}Wy;}t zX~H=4MbjN8NTn5{EsYJV7L-tl20m%2ZntX!@fpYDeF!8Zue=|U&kezRNEPmh**SQ6 zfGto?&qzC8n&nQNFd?MA0uzJJp*|;0$eA?J?S7dQPx8ski+wqcSR_xh1>!)iEuB1J z{Dg^@OzAgOIWH6CjYOd>d zqyPLdJygFN{pU+@Wq)z9Z9$HsQn+HeV-|Iv(OuMu!if z)aI`u>rD?Ct6=ooYHxf7I7};@r!yz zZ!KEy508P&xWu=tZb5=dk4K3y;se8kz=?vP6a02j)02y4>m?W!8GoJ_55Wm20aO2s zTAgb?rbtYnKgW?5*hlM_xhOB>>G+s}KRhOIJDLU+cL`$tYJ)0;$e5X7-nknEChvqt z4kkzQ{UAJ*ej~wugxg;s#;0WBPPeZR!x7(45GhXckBg@EE*VeT066^PVh}T&!hz}t z!~s0sJ}BLhMPGUJRY`4k!wyISp~+Jti%y5U? zmGnGWxG3#9T-X~Q#<#XMLpTv!avkQhQo$KwGzfpiJyC5~&4-HnWh9V-G+#y*uAQ!w zjoxVTq;k{?1(k%piOTK^dKE%Zj(0}PR31Lwm@^eSX)bd5H`xr}Qa*TDh4u=ku-Y3)0k zX43SJP$zICxP{(IrKw+qlC@7uy=YDXTLT&3kmm-6lW6`zX9m z$0|{tR5eC&`QE`sqWRxOqpCzDK<8-0$+pl(!1rY;rp4nLbCCGCXpjt$Pf_DY4z^!iXf;h&%X4YmLW^i!Ra*`(60EVx8`fpc727bBcU9x zR5q~TBjBU0ujqN#22v9uDtw2SSH1+D0zrFV?}zbFjw5T0GiDIA5*pFvQg;Z= zhu;1kalyJus&9>4f4x~CQmb@p+`QFyB(Bnrr*K}r~W%8mn{O35_Dc{)}NIGq)F}dm<^qmV`Mc*zL{;$~A-EDtn ztNsENo921%r*b!C4RH_pMyvnva{KjIH?_H%rXXg2a@7`&@?X~E);8ig7S`>bO8j%L@IE<0Fs0D%IW`8fY zzgTr&5W>V11PwlXJK!0Z`5!BKF!6YVdMX3_W2py|AmUM_0@3`VQ$t^}4@2SoB#H`o zZ=nJ(K8|zyHYKh{+`yKlb5Kg?C@l+$%Z?-WMD1-sVj5WjIwqJh!F`ktWkg@dJ_wV2 z0D3s=gAfC#f05VO3=fAR6KCp(4?yd&poc?mUyp9xi#;5H?$jR6c-v6=%E0wz+qg1t z6|RH+YVj|)$^38EfxF0E#e1gb2GP$qB08ELYee^+M-gBtIG9#vZ+xB{Z2l_Z8yW?m ze_As!^^aaGhIi1v%lAFhorQUAq5oNUlJuYBPb2+TittbE0v*6}DJ0h6n$FY~(76R? zth7*F?9$(PQ`SYE+qccAixy}|RD)1IYD{!KL^X&iqVOy#gu-uveb8-=Ic3dRXKHg~|S*UP4(xDD0A@zC4DfbLY{}w59-)X9FAf1t5 z7-=-qS`$KaYUlg1XJy3VZe&fUbK2)-Z@2?JBz;R8*064(zLL2ltkq+_$X0U^a_G31 z{23M&pSYe>FdxMwkC%gBaKp**n><3s7as{<7q z45L~d$T6>12Cl|+aQrWqDouYyk(ddEb5%4(p{`0%HPlsAfAY8V*Tw%qX)XSG4KyH( z0cJ6t`)fA&>on!By57ZJqcF#tMp6bh%own2X)ExJNdsso%T{a4{Xf{{78*X z(0hXY`QvDCKz;gV(L1Qe`?tcr%~8SUcoP04Dh=&dke>b;4viu#^vfgB$A+iBi_wSe z^>liNLOzV$Fm^mA_CY-yoy~DVBk*do>8m-4+IYY_Y;34sRTnl$V7P$x9K(e#qF<{i6(z<%vWTh-87GemCGz5R6=45|CjLv$Z% zu7X9>slF2?$kccYaTI&Gv1`hP`6}cuCaVzpF;i7D)R?6bb2(0YBP`$>aX|!&N}9dG z^i+bq-!!!G{nD~J=4+VJj@EZlBtopmE{(LJ3}=NLvy|zq#2<#G8b4)XLB-ifU=(`+Jd0fU~XYm+%##&5W5e$T{Psj_dAk= zjp=C9_x(Fqu_+{%96#oeM7bdnX{vjss*{F(52A8J?|et9@10=pyx9q9{E$qvWVBl= zMyM8RN0uO)t$xTTChgZHm}O1 z8pUAWs84#Jmf{pC{-NS#>KCumk7ebYfnHXq{?UI;&5>h}os(g!*%*A(6hq;%BiY%k zk&R&+#w6<9p{ZAhZ?zwh;@TLPPR%$JD4NU_tx9G*)&-qXWE2N|{Fsd71!;t)QqkfT zqzy6*5%L+AB0gcSO|$*B zek9FaX#Q90AR;~AQ?FV_Hp+JZ9(V|;@cnsuUh%umDVd$og;{^aELSj_ zjFgPOEOt;l69ojnaZllp*$dPS-U;>Q^U&zUF<}fragv7^5M7 z{aoB)uacailGsE<^QU4%)*paPgIP{ro<|~F7H|PD3gc#hG)v^|MV;*{XAQAT?&A-_j z>UsG+SWmB@((v+{<}qHsgBa8DHa2~V@?s~NsLb6z@^IJzmKAr+y4lKXKZV>HxOxarvG^{HJ0gL z=BNXZiFdEgN59DBzYfdffT9Mmc%*~hTLdqBeHv6V8Q*?_J$hDu$AEXt0+8C0UDif8C4(} zvx~;!UeIX%S41C5)P!FGq8BKDt0X}B(wZ9E=_E3pA@Ri$Z#D6Y1b%HLi0e#nxCFCc z-Ffoy(n9Pz4ebpJVCMQyuR*~1tFbp0`C|3R-blYYV$_^rrSF0YmAVue zX8IoyGEt?vQ`+qfv{A{7A1?pBVzln^KP4wxQ#$MoYw?(h18)Bd3y^Zly`NOOqlDds z)^D-ME?B>JAPfCs0!97qs%bMA7}YzBis8RD55-lLdzwU?KvdPnNF)x3s@Q!i#8W|) zWKc#rq4QT$y~1JbjhYCBsyv29R)WX{BW(tit4YodNIgF;?qKUR!F7TOssC05$CF{H zD*2PZxM2+=0cH{`=QrFbg;XbxpiY$HP18yh55Gd{gf~iQDWgFGzjl_ZDWg_Obeu#q zAh{ChDG|tGSjUc5D$ljiKw>4$CLrk1U@*UBxea0kXIlfu;&JK6%Hiyd=R?9kG0h!_ z`ZV>|VQ-A4TmQrt`LT3Rd*fiNBnGD7%cN%}_{mQsj0hfq+P$DL`={LB+CUZXg9Y&o zQP&(iqtq36gJd$FkI+7-cs^OgHWAdYM|r1r8qKnzW8-deT%hFipRX?>#f06({P$Od zJ(-cer+zOGa~Jccbi00K486tiB2-J$s#2?g9PkwFZ210&YHzLb6^E5Cn#p%QoI>?s z`bptbAEtX3QMWCJsKQ>+kr>8AeKJ(k$8ihFQ0qsLr9=3>F?CZE@%?X zM$|rN7d=H94b$cfS~sL}<3$NQ3yj_V0$R_qH~a}VXghkA=!DQ(dy=Fx`bA?PE(&M( zuD3={AO!s)H+H65`bEd!7V8)FQc2#)*!~KNpNCwj%RXGNqMF)aZ_Mi?zEoSC%m*F< z3HH+53Mi?EzEU5r4TxP0Z7gNCl zATj!P$c47}iGE|pZKgco{4Fq-CaZ$|Nn?EwmNEZZ9SUG0y#}$#V?V1h-r*Rs&>Zmw z@u2xS$x~?DFylw;)PN>+dg|p^pfLOx`wXtaBnPI3e{lP&FuD4+=67Oc49%Lq8@@3W zHwM0+?ntL`ASRJ%a4S~9cDtjt(x!XtUdTc&Zl4z$HS70b*U;Y)CUJ#bLtb2AdiDrx zy3f=6w6==P7PRJ#-Hi$8MThLtr3FT6|LF+s(xs`tO1FOr1$bk!*g8vWA0(S4nxAe3 zoqMp@kPE*;{`Ljb-=eA7vHv7r;d^!0gznXK`<@pP7<~9v-94}hxmA}YuoGxfJC2qD zeK(0MIW(Car?0|_F{uYTR@c%Lwf%3mI z_?vVabK$E&)ak+a+d3%Iocpjh7NJKBe;BPR8L-M!2$c6da6u6)Cx07(y#ovUl zVa*4wT%Cpa_5^aY7Dz}3ZF;Grvp+3&2UgHD;cqVAnjN(ALQdxzw39p*^-pd=9X)Nu zkKODImB>p&<%r`{E#)=)4OappN83pChhiKG(*YrAaaRbo7Vd5l>}^0K zSh^{g;ElOzxVTj?;%Vlg1cSfmyM!gun;Tss01OqHqzLfZ^CUfw0K4g)hyZus>K8?T z=`VsQB7i8h7%q)hh^P*MqzEJ!69yrzVnPnZ1koQAabXh)L~-F0h9fSl5{39MP!u69 z+^K?F*tl@%3n6i#9;`#-!c_Hi*tl>Ko@R03b0aReeLL`?$}}XE{JvdCKL_Ue2Y5tx zEWy{Q_(HVEeG1VcO{o9Vj>kl_pe~CMAu5&|*D6B%B?tt(=JndZRP%aeARgD$-y$X< z@gf%<7}7p#01hTz5FLvb?~xPDUq$Q=K3>$diJF3ui6$DeX0nOSp(eUg#1i_-bNg5u zeXa0KEL+Mp`ZIWhj2SfZpkfA`LbN|#$1D*v@LEHH)+C)C3(CpFHO7b-A>8|ia;Nc| z2C36gX^Magu7jb2Ml*}!huP8ywqT<3QK#VMPb?6VlYg<{ITsh>1%Z) zm-?-ZjRK>dr~0jpHvlN-I%^;1$An+ez8Imvz{|jtzrnA>tOwfLYl40y=;`GQqK^r5 z81!OYnMzE#qbYWfUlS?r9Y@J)R>z7eApePW6R=ID51bQ+lt!eZM1TFok z#p3z}x^AIs^lqX%8t4=g7z(}6a`+OXVef~2nsL1o{Q-HsHZah{R|Xuo4vuHL_@|D- zhxjM8$Oq$}M92JdB-QDD`_Je*yP;sH1^biGXG2F?cA~{DFzh;@DQ6${ku3T<6;ref zYh>S0?0OQW+JIaQQx$zBvu}vq$8sYXel~1?LztK99Lk+zW1vlVgMI}~?9;o~ny7BBO&1;oz3bh8i=8cUzM`?U#*I2veM6Y%D2M9X=?keoA-^?%!+gHe zr#*w4FaXuD8%3-~3eh^{25DGLU?Ey|S(hsV_ps}7T>lUC&l(YE_uoH@Sf5!`cvhbq zp0d{G%@oWC>U<)NX-S8Xic*Ix=ThvJH z>Da5*0~30}(r}Fs%R>KSDmbg**;z=+;Jg~vErK)s6djGuYGPGq(dZODFL&c$(|jfN zu;sx_(H3Ge^8#ETA14K!g}BCsX1s6NFN0o-Xcc|5j4a+K+d+O5+F9%)q}rt~ewy@% z&Z6X``DLErI*m1X$O6-tTg}d50ok2=nEGJoEV@-cNp>1D@h;06NM4JZw#-5M@cb;Z zqgdWms0Dziqj+LaD8W}Tb@^6h4*EH?!&p;GOkJ2ZwQ+)su(E$LgCoD$IyejmLC6tp+QpI3me|Xcvu0Qp&C?*!5U;U59HHVM6v}i+RrQ%j)JrzxPafsgoADPg1)cHfry!rCCzFIJ~%f&lG-7!hF4ot!K1FD0kZA6$gOAd+B;U^O)ZbEi?rsL6mIid}4Y zQ1n#Ttbp3IM$-ZIhH5AS(|tF`$E3f>c@+j zU+MRf>Vwr!Jr%tc{Ha+M@3&C+&PNYGDt&c;`q@mOmi9N&Hig5iPjVjp;>Qk?K6j&H z4ndzj@|Z~abWJ}DI1|?2pL(RLJ{O3JXZrjQ z{Wn9O%W!ScCrw2L_vfK!Pw`Og&r57Icl14iCuQ6Yp{G_gEdBN2>6b>LpBbLMH4?oR zo_<{<`auT0dY=!I^9#{Q2pL>ZuYm>^*c)c<#PSYRFKN+DHlm=(ar8im2VT??zn~NR zy%ubQ5$(ngslIphOE6hQFC4jiUm-@LSJK&x-^&r*lbe>4QK9dLk%y+2iz(L(^i7vJ z+EEU@iROpV3nYMs_xd&Hdf8vR&5=x**I$b-{IcwV$+7kZ8lMA+#t&N@AunTa`7>#Q zUd#~dHR3Ha>aA1~g_v!6{cmJI%N!(-*3{t0qqA6Fe^%WQqh3M#fI_wsQ_$K>+D3Ip zC|}T43*;KSXo+6XEXmOWj5e$#(=Uc$dF5L#daJ=5Qfc}H^bq!y;8x9-g2>!ga=PLN zRZwW5Vb3h;Ya9tYa5=72JE@+2tfN(qgD|4nfIdT@7qx`tAeN4>HZTIcAi73RFmNia z<@1F6kK4yc2xzg7T&R}1ucgu=lzmK)<`A@xGBFp&?Bg;_79Ljnc=EwW_Hm246SR-( z6w=i`iqxH;ePjzH)IKf*wqN@=g&qoTAKeu{t9^8|9MC?NqRE!_F#${r`$$GN%dn4u zxb9*fSm%5jfv*l*4kN#DL+{#zyauu&u{HYNUQDmx#p(sM-1lWE$`om_dgEw{*8L)qy0W;6LANffAgJ>r=S7sr&RCXAX;Hh6>a2K zZ{J{&4VfOyD>xLsXjVwRf#x>p9;(Q7cS-=PjMTTeBA zbL{_beOKPwRo|mvo`*%>!hrrCd+!1sRdqFB&ovAQW`csodK)!r@J3Kk!bKBEUyi}tBf{GfcR;pM~tDZ3^QBkS4`QNqo zIcH9i31a(w|Mx%t(>xsZnSEJ%?X}lhd+oI^rwhG5p^H=Wp7-k%jc>w;wdj2-a1;yi#}SoK01iwtKtVRDD4a_y1(?KU| z|3zxvg_lOnyNcAji`@Y%XHN;oV50nG&=G@ukU8;Uom!kh%3re+y=hIrfS@K|`0teK zP0h!dZ%bJg#&Ldf9H3R0v`Bl5pOp@Z4_xB5TsFUkLBq`MWB~n+Ixt+P$h9{tV^7ap zWV&poGX$G18)fv0&xQ+*w#kzEC&9YEQBF?%4~o=&$1?H=e)7gL^1HZAK(*AhM4UDh zEsIsElV0H`t+ST+a6(9t<+s0Id1!Eggdw1e`~{M>oOc`-`1HAH65mh1_rtNx+-~Qy z?>q_$d8`}mJnGJ)ArckaYJ6wX5_g}5R2DaD0hC~sD?XB@u$z4bbDF}j$q(u1Afvymm=Clo!`;U4o836?@3`Ab;`wxMoRvJ-}GdWYYYDG4oCtcVvfee2DrCTgo3Dd`zEO zqvae>VDS|4GzqeCee(yMQN>UCuv{p(npdbZg}xb2N9Y}*!6p9?LkSeFry?Pxkkp2R z(qWl>f zZ99>jmudIJXcQ@7E~lWC^aUy0jg=!)K|vOFYNZkH_6BqFvoQNl>4#yxc_ zpVqMJ5@F_!mbRU2zqcfoINndT-~>y!m??n!&F9; ztWwdOxKJAU`JSzMViRZ;w^Q^aPW}Xyc9EcVpyxi%WbuI9W`pTm|AGW zg?7L4R4ZR_XZ2WG04{^*{IYCAR-DD{YvN~0fd=&B!|H1*>a)vdB~8gNpL zQgp(R1sxTm-X^S+qu$gYJ6&^vb?*x*Xh`%X0?;{OX2p)ws8sJ>;YOep+)@Z#&`On*pg!rMfsY)uGerY8K#n0$-YgkjNyvteZ=zmJPv zrOEk;sM4&+pBmc2WN2Hbw6j>Pn@-`EiQj!)J$CaE1BZ*2&ibmw1uerm6 z;!CyKWHqiFRKzS1pKiIaAAtjdN1L22+_t3)8TUR zPr#-?d8wPSpbV?g?`czlMGaYej@}Ghrqv3Dnh>9_Vod6wkyi)~XykmY4;?!{#~3F| zUZqJ*eXKHD`Z(8wT_X@!$CfMfeZ+}8yA!ezIi=4ulur8iGu8aJ635ej)_8zE4u>1; z^=hTx=pa>x)~oP|wO*}s>wufIZiQELKDGVX2~W6fhtbazAp3qpDPW>S_bFNaZOP|U857Md&QYUF476s-6u61V%>*9 zKQaoXHm#D#6ggz>h-seD_yo(LR?B3gYeU~nR7>N=(MIFE>rp6A3SHx+avM+Vtbn?? zFvZW(io`&0H2Lg_14LXK`Yr{EA{d~`1=K!(O3Wdn=@+sZlB>SEYmcY#Qn}qHc2+MX z493`BS$xsLtv87eFd0pjVXRF&$4`gv)aw7Uo?Q+S>g15IQg|-5?wL$!`5L0dM+OP; z>Bap=eI>X~rjNE-UyCx9f#4O7vhToI6o&??x}zmImDMvjPpHliZO-YBk@8fH@RX(2wv(J>QIhMWRRG zOHy5ZRxio-WXF6A?PO}ZKYms(1*?~J$3aVCc8Vy8xsLadymwuXnyCFSI^%)30mVLyAk z8ZE>AGRPc`|D{P~p$C*oepN;CNgVr>(O+~-h!~7vtlWQ6!1$fSqRH-&P$cC60TX+t z?cPS|%I7}v8LLGsp>nPg%!{F{U@|D&z@?vIE=uCO*vrAXr&PIS{K;)cn0$$(4giJOo^#m#E@;2;oFbYuH%yNAz&~fP|X(w80f^-EnccC zRn4#Wf`VGdWUR6!l5@eeYiFh1pGNbVqWe%ztT=NX11l72)Kfg*qn1MY1{2m@2W5`CJKwiyD-% z^G;i%(AVUnIf?~jz9R}=)`JD3S#sqGCdYUdn+snL)_p4_tz+smIdK!ruH;|al~`Ivk?JA31B)dWKIdrH8>Mx3|ogZV(so~gT^p#>H$OaMigJ7)N+d$M+`R1k(*&o9E zv^-chK#)*M1T5J$%)R;75G8w<9*9d*pA&7yM&XymTmuQU=RI`!m2snbuEI}_=_lW3?j_B+nO3>LaEjoI)2j~cB zK}SI2QtE`xi>liM9r^N6jwyL#A<~5i>3CJeM@oEaP*P$>BEGE0$agK|KuX96D>fQ9 zUcxaDuwJ`BJbHX9v>Elz7|s+K#d3f+BIYz6jPo$xmx8 zDBb3m3u?O+@-6|QlCb^&li}N(AJkt@0r)R*nX06QUoJL{e8?|*{N;D0;I2vAG9n;oYIl%=;WV>9aL7L1Yy}m-}ox z=rdul-RQIaoICVU7=2b{=`$uiXgB2-eKpxgL(oNcX@5@r+tuf!G+O_ZMq8by^RWrZ zxN`|4fXk(7N5>vSJ<_rLndA$?9Xhti)<;{UGzd%(lkx%afe3@hu(n8sHO`7{4A#9V znjnT93f{9sFBOo77FBtRIS!8k-{ zNdnTRtJ810P2Q7S{g%|uzgQ~Gv%u=iOw0ggwj1(z)1f5t;& z3YA);NZ9?7j3VMG9q_jJM@3;T7q}*Chx@e$>(>*ZFi;}M@eb5V0ka;{M+%454g&Nn z6t}F0%NYMsEtS;zxSv65?m?W7MnTy!MmH7-i6)?G`U48%phP4)H*pK9hW+B4m3$9a zio+6jC{2g{Q&h!hll^x2C?4ToQyD{7TN!^s?V^mKE6R9cN>8!@QC+~ZYM^;{wk zB4f`YT>+|dC68e6YHV=lXz|8AB4itAL;i<0zbl9ZvD7y%kuW40uYV5)6t&oAwnQ*&aM<9)5; z{W<%+Mdd-yOL50}jI{gBzDj?8rR>Z2r|J!dTAMGtQ>(A7F}GyM8Ap&L4q>+p0P$eOR?w5V$YYcQQ|Pg z_Z)NlS?Zq0bB%C({;qKBm%GA+Z&trQ7Tvjb8AGus2IE{sOItfMoRlx>v|zv|n!)8X zB6AQ z4p)QsqZ(GLg`*4CSFe^*oKa-IW(~b-f}`+42fZR(C@0geulYo^G~6_TZ-GJdL?Ce~ z>}T|qHZ=@;=`wgFah1gt>k{Y7oYsMT=lS8)+c(n}ou`O(sY83+Ij>K#4VDc6a4^7h zzaH0ujB5hB^SEa3>%^vVtS>T9m&F%NFg9LELm*;mZSi~=kk#6vocKYzsLh>}PPexB zxs=UE(9NBm=EC9(A#i+P{|~SLr+}1EitNeir5O2-QvrN6MJ6J8VX@d+S&YO=eGxVK z$xy0Ph+bJ#^Od_P=vZ0&7wGAg#oKt3g~eiPWpS*vvgjQ7LMW-5wIP9{Qsy&}meL{u z`s1&Blk|(QGY0_uMh`S{b%~TGc@Pm7h(BKgDm_edP+F-^(f;B$kZW>uPH7#wm7=X>Jv8+V7`+}ExiL@v>GjZ2kW3n~-Rysu-CX!HK`VH4Q-}3Xnl*vxm#(Dn z*gS17Ct4U`?QvX|dQV29z&YB_&wOkeJ{@&*Is^eK^UIE@3~|MjvYNPDTduIubDB@3 zGnBgrt;-Z^L-5)KYV`id7@-igrdS*G6l)q&Gs?zG^O?fJFl!_IcBZCQ?-@TxH=()M z%wF3(2$3HYj*V##3+8oypsq5OU_l6_Fodtrz+5aRsC8@x`6O*faoggUZJ%`fo(hvR?bBGVW`n=NQlW0ofkU)cV7V zs_I>dW>k&kUpA+NVv$PEi@IY7pIokH1ZA;&*T;6kMB^2S zs$Gi}ySRxzDdLN8_vHEKyp%;62x;cFABd4ItD9oy4x zA{n(MhdzCo;3WP5QTd5Ws4!JNqV0_}0MN=pP5w}2 zYCTjwe{coGRcA!R-uOV3t^ph6O~{p(PV1qO*!IM7$ZphLBKG^1{-(5=U*!M7m{BFk5HnTzLhzcOgmwl#F6~AtcjIO2CVmI?e}$w`yB5t z*zc^7I^Ihh?_u6;dRz8|)8Dj4+WYU$&t4LZ*U;VktbCDeI5Ii=#g9(TCM(KxnVkKZ zO1hn#)krr%Jmq&BD*OO{sQKA_srlLEGO=Lx94Y)RSXZNzo0u-v+{~tro}WFS`jUBw z8l4!a>i;2Cc#OLx!5pXF2Fe~z&_W$1Xxl01n4k^dr=FmR9#(46-T7Y?o)$HNv_15g2%(p2Y< zJ*L_*0JWzEAX$&&7is@jzY8a!uB%? ze7Xs2-NmB#)CAV6HXoYf2;(iP@1_UIgNp2#c)Nmbu^t!sbYfe%_iI0I`>vz1sYCgGH zu}9kZzPvh{s);#{WpqMk8R62COx#h2a3 zes4)!V81sfGVS-4#0mC$bHc;Bt#21)hMj!6^W@*pFYLZQii>ZF)35) z+8lmr!(ROQYR8~i93H_J##Th7b~+pP^8MS(xR>2@$gm@0Id#e(i<#kmT;jjmkKmq7 zwKDPjyY#mDnC@lPo)`5IT=~?<)wWqWPnuq9)ayknc1RQ?lkc<<2>T!!U zrtepGl33M-hlqLq2<=VC-h6967FxB`rMB<<1hf8C>4GN^1^Lrv{qv?-KWDxmXKB?v zH0zHNLF>k>mwQ<|nDrMy1jqK&(^bWo=durLVUgmqIx)+XY7&$Pf1fLpTSD99*ObO&ue<^*WeGJwnq6W6;gd5cD zO57D;^q;4D4vW^@tTRkBwVIs&RcMNO&C#26WPV=OFkLq7C@sroov*FUI_xGhH|vHcx~0!wmq!qs^&?r30};Tj#0f(!!JEwkj?v5I2cc1AcnNu(_C0=xlH+FyRFs?SC6m^()$N(TyIb*mGpH4@q$jsv&x=q$; zEPq>irM)drRUv`@K2wcl({2VCd^aqaD$bxWRsJPCU=M9>*1U^(iXH?*{*G zE7hl?^!+g52LIh1e#d*rS&w9SK)zAZR8r`T1}56~u8mSqinUdL>iSQ&%j_2}e3c?~O;La0zPhY*96Q5C1^C(aVVd%|+;3?z>(}u7-^2dl zhQ^_YL)%`;&s*E%V@Wo8)R2PeFY3Q4i~BEq9j$dsQ}D5-@N?TjIG1DX;hwEHF+sP; zrLz2PlgiTecB)bh|0?k9kqlY>TDc~fElt>x7dwvy;L>DmE1sb~AjA*bD_5O~OBN>D z#zo@B$Mk(i__d=ZxUKUpQlG>9sQiWh>2JQXms7^oy5fP2yOP^dwd?ge&Nz0kAO5d@ zCr+6ovZeSy=ph#>>McU2lMs&gok>68Y|c>&pX0EV%WZy?{LhG?ml#f7-9G*MgigN? zOa0D~{ClT<%XX%O$NpW+ef2SY#pRHJU!aWm@%6eqdSYeIZ&Jj=dDF{g*pBk=y*a+N{HOwvJ`dbR5d>Bhk~+ zUhysD6t6EiFbQSX>08i`k_MrA^HUPl2jUh8aarULeY4)XUkjo&d4wVaX*@WA_jAt7a#l zgFYSUzi76iztOJ4XNq92$%#k!rTH=Z#9mvkJmS*c4?Y54eZvZiQYE9qfTmn8WH0qpeZ^NFp5{QhUxA zGLy!M@PfL)P1D`p*>YOA-}CY*cOyfsTkci$_EH)@-B)xg3xSkctA6&F^L-r^)wH8d>1$^S9=w z1kTH4Yf1FNaNJYYaB#6WAiD}D~~RZZctloCr(!5om|o?@)<7tQp%lrboHGfb(}Sp&Dc3iT*a~N&?tI#1kXycam_Sr^w>#KWw7qIf|@Ka zt}`;HOFzWV_qwPA&Z{INNl1Xspbg(qG zk|jk8Tff--|C8u3|NQjy_=foZSLjiUAnk@89_bgMQ)hb2o(Mh0)0@VtWJyO4^E{?V zgklXDY8VUbOHP@IiQzRe5N?Q zZQD`y_#%G0e+V~uQB@A=DK6FA#Cgq6jkVH?P!H!I+;oj`KUizcwoY{42mBPY@9xA= zYP@T&orjk4BZfr`T518<(ofMI=Dul%n)*A}`&S2nW~(WgmuWqvSLZ@3%fP`s_!{rP{p53UBghKKs=>yVW^!V(^_TA9IN9)5}|gR z9-t%Ir8^sHWp}yG=+*J!s?r%hmmZ8?#{CN;%crI1lZCGSF>CcA{D7~-LuLpcVo%Gv?79^P zgz)BorWw3#lbEbR%bO}0Vsd-?KJt}2xrLl1t^)N0}4fo$q zb$nNrT`x^V7H~k06)f=D@u_0jAPolcYeTp}A2Gv`$M#9J?}m>KYCot9@AAGJ3?y7- zhE0UVUlcp~uM|vaaKmclkQAIr71b$K@I7S_Kh;R(ucS7KFkMah;F~nmkX-y34^%)i z)YIHa63W3b=HEt+#G=#K|8rMf4f)FAQ$kXK0Ltb2Txy+BeuRSc%|b&el6(@CWA;1c z1tE!6`hAglAw!jn?oARa2ExtsY8esI+HicSa&49sHU$??lkV=fL99Yf2dXbK8^5y= zlh|-W)gWW7V9qb)yUM|Ayh*b`uVy)udZjEtvQoZHpm*${6Q$$KercsYp11lzoM2%9 zjn;r_pBKdBD83?qHcX7W&(kC-5dSQSiw&*P`;DpN#DH#T>>XzmTGL zDb~io(bv6ca^sZ`*54rny?U+4gUGizL}_Q@!KA4R-t|6B8BC$*B1RhVAsP!6%B&xX z6<o^D9B>&UsSk=oNXhM@yxj-T=PAq&|1~5t?Klb9_O3edRi~% zBl&Mjf2UXunK2g+Ot$($b-(}ECS7+&wi>CpnZ1!}&exHnmRcM&o9(!o<4leeg=S2g zAQanboJbo?1Ej6D^}=Ed=Q}`Mc;dBlBrBwX>xiSHQjcD9nN46D)_hb{LBVGE=a9@CTSoo7@i5fVoSrlXcuUR2Gl1NKN4@gzm7tkz4MEjP- zK9CAgi8~CJj;wBT)Faa4fW5s25W4RD68F+NJ2J3L2X zt3OW7W~I#GR+&;c`dGcuOl1gTSd!w&ZH@MM!nQ4nzN(j@=T%+l`77a%&PURQ3VX$! zhl!5XF{t5E<=;BU$dwq{n6G| z5j1z6HBM9~0`|3$PkGr^E(pmM`W*Q<*Ajk+Xq;k=i`Kf$$C`^6JWF%LRIWBQOAL0L zND1H7G0R%E;FF~4eIcr?51K#ABlu^gXj}U>GcNT_wLwNLaia1sBa~0fpE#DCQxUYS ziI1+&`mPRiun&_g*4Qubjwc~NHC`xKUmvi^FVgFZWq6$-tl^OloE zP7f*J@f+{5%wivNP$v%{SGw;RPBR`Ot3!{-DFur>%M=vGVFx{~HRDqBC@>ZfCyl$1 zkV68|W4JLkfquTK-%n5s;?b@Ew{n?z{ z)V2z}Bl1)`Z-P6TUCPQ(OxAvCAUgwxzu!}xeWV-*Ee&fH8ZT&|Si=o`kMwur_`z4n zrclthj+EjxaKCG^z)RD9J-euF;rpzj)BIH+hi%M!rKtduIUGA@vk?`LasT&{B%k(D zLl~#VKjEfF<&i52VSk*>@viDgwHFEgrUp#mKbu5U5!*z?BE^emBY&F$ZX*BjLZ zXEk8rU+~xUXK*756m)G43zcxNp<-kJv28@9RMR4$`AII#Lx# zL#n@^=)R2Irj`=LSjYp57ml+%3KTrOKv5 z`Y0#Tm!8>$-sh0Mhx8tr2KPZ4_tzd*xOYqMeiZG7-lukp`W(U5;`3+AJJS0e64TI| z4!oP2QHsZxHO$<6mc`j>N^NrYtpFm|AwPogLej-1fkX7?M+=ZD456c_SMf>$AlE1$ z>BzoH)nf9Bmmi8v0B3y2kG7Q#e~wyRumy7KpSv>k`)4SW=g=_OVQmSO7%}@t;<5on zN#t76t1l?789+TxPKTIb*14it5yFXS=4BnRvn;Qb-(0@U)Zg%d`Ef*){{2T zgDWX+vtAD&URn)1P}O3x-jcBn*1M3Dwv`+i*Y<20r9{ zjZ&RE%E_AjfKWZtWKHf1)~spu)n5Uk{E7Yns-mk*aYy6IP>wXNvDmv<_E z8o1SiTbll|M!So33xd+_-E_)K(mQmDsSoy}xZNr96_9lLU^&>@y6lTFj!yX-DZ9}J zEi5Hk{b8Zptbq9^k+eIauO_{FeQ+K?Y<=(qS-YeA2Vm=<`^gsaq7U{LPI-38NidGlZ13 zmAlt(M=A8v>bC<`Qd&*m)-AhpRo5PL>bK8H??%7f1ELQ7wu_wIbzmM@9UWNB%)-`h z(q5;2vsSz9@n{r)E&j8*Y=p$6@3MU0(Phv5v1`Me`e1(`bgvJZ!BFdiLwD1xvE*_z z>(^mNv#usBtv+ztkI{#_HZ26=-85}770|Q~rPOY?@l+smW9p0(;AwUC)e5%BgRhV3 zXzHvc(Xto7-yB}!6J&T@^-!t}VfVLI51lJW#`Xy}T$bNu92${t+(-=+K0r6cY2iDK zWT8(tEs3d;>7?m#1=?&$W~Y{X%SXRrm$>wO5>d7J-{Zhzr-nGE z#zO#*p5M+?6iC-2SAn|Sj!h%F@Y^3r-vfTL?Qi804c0BkbTsF^0VeHJe=my$Z>vau zlppvIy4d9hO6y|W{MFemFU>b52xf8ryAyBQ>QNls5D~lcUno8Qg zM=!^I61hM2cEw7^C4MJtcF=A!6`N$^5jTG&i^}`}E~kIa{p<4;IRcZ?(byn7mU?7)-rP zumH>=w(4qr1^e~N3-;#v89C2Z$I9k6M3%*m+ecx4+~ow^#UOjmN&{*am+Uz4>)Q_n zE9S$C%&JG|<1E2_L0xV|SY(_^TjUu21Pj*36O;0$@PA%jacm?FEXC8B0dddE;lCL)Z)CxQ1TDpNB4L%aR zMtGA;$Db=ww^i1^%i5yK#A>H<>~6nLTKg`D;@nLm)OxmCw1zf zxza-y=O@kurR1i>d*89NIvdzBjIuD@wx7JPL!zzJiaEph*J+x*zZ@#Wm^YLi%BA?J z)zUj68|;k)8GXnkaUtVp@S02`49b}GuZR$V)JAfw`7{;2nM+1k=H29m>LEK*SANS^4{xn?v{+>iR-D?M% zW12AcL%*TB>u!ND)qo;!*fc7iug{6e4q*R3B}4DA=n2?E#)F1N5k}xC&DJF9ZIjx@ z(7RM_TMyP94KP(h>?7`KPnPGG#q;u1iXrR|6PCpX4g{>M6P9sTRNbMe>W?MoC&4-v zCCOQe9vjxIbB4+ackzWAm6IOVNKS;OFA9KYu=nH~Rq!unLGd1{5?5O#gec-%JcZgH zzt=<&o-Zr>JXnXttO&sEO!ja;T9M}0W=gB6^PsySi{x^cLvM_(#yKeWjoMo#)VhpD za9Rq%N{wK3QhICHCt#4kyUzPXp!G?0+Zq`g+U^xJ)o1UFO|VWe_Z3ZWCuYN~`gLsV zgM@rT+DyQ2YfhMh^{lcZg#KpLw)9@m9fO4%SvAyN&?0`?ljg&z&T3;~KLuA5di{wS z>>LGF3qSWKxX_oTYxPbAv>KSK+UE)Qe?e^t(X1Vg3{xeis$g_mWRYweStKT#U(W`90y!kTCYa7;Z5W7s?v%k3sb9C>#6j+-)i_9TNb&+Xg7sM{TDkwn zSp3;V27~qYkS(1Yn(`H_p9~*s8GBCbWcct7g+Y2rS$9eYN!gd5wxs-v9~LI&-ERtB zM8QiZA`2BXQ$$`o*@m^lc!-q52k^BuK8hN4YN54vLxj3-$)U$#ZTElS_$}k#eU9I( zUkDc_o|G}Mk#_zG5E0Wu(@}J{f(rK%Xwf~viC16n>PCgt|BT9%ni{5FsK4!X+n=cL zFJq^TQpc^}APkX^L-vLXpq6af@_Vus6R}%Q;eTp)Qf_2D>LSbk)_8h1z<*>sz2+o)*!(Hu z=>pZZ^jhK`21mzu+J1s9*&i8yCCzGMn(_2N1vZ_s7_ESsLV0%z4Nn za9g+I>DiS0X;S#3LXaE#cmN#jpP{}1@fo^F)Zz6(Ql@s|}U znrZzN?yz5QCK%ZMvL``9%`Ff?<8D$q`^!EP^wX}e9^!;E-wEd|f%CuQFDnGLsGaWo zWiQZwLf_(kyYZLp6x5Z!>;kx)S%dL8#f-7}qCqWx*;X>y{<3RmUwVI8>Uz8mmOhTB zs{&>5{&HVhKe^dSE;;BGo1ky3-igLxo4#Tf-0~PVlHH%|eE^%tc(=86r z3}(vt9XwW?0Z3LKDXKF7M7|*-fCs#tU~4RQx(GRG08J|1@AN z7%cY|=<^EUh6?eK#@-4KsmP5cWF$Km!j#1))6H*{iDx0!EVxKTZI5z(QMPSu9DGuJ)k`T{_JwGQtQKmZ8v0u*mo^b8<5Qj8zZ8|*8 z>LfLDY3*MgPVm?8lc_5yT7z}H@ehTcw63s_@1E4%O1GB9kEmD*uBS8>2$otavWTHjZp&i-|2o1ahsIY(aoX> zb2UZkUJ>Dn4}1;?0Q!vt4Sf&4L4uKimXS(Q!!09o6u-nqqJmhAY8k1_E8~5t6ksob zW1lG%3 zV8SMN*<)*5rs~;YOp|(YDQ4GmDA}xf{M0G+d<`OPw{jax3E7Pul$DXBEkpf}B88St zLYg$ZAqEYvkrEFbN(GJ+5a?0u-_ZwSVsEIuUP}AqhvVmUW#z)H!MZS^f>@?1ghEw_N&*PdeA#rV{&>FOkH7SCePJ3&si3$3nYH zRO*BP7e+fIh0oE&kXWvuI=q9hh#+`z$jFs0e*|E#{_m7mS6W55RA@;_o=*-BBIhjo zsJm&VQlI`1m+U8teOMCv7#CcbraPT>FX>X${?1vvNByQ)AGAeLZtxa?H^hGsd8(Vk zYf@6HuGcCqe1p&L9f}9s$*aM7bqv4rm{Ftn&c3@Q9Ipt(Pex;d>OQKe8=|3(x7YMW z2&J7Z*GfCDrhNTZXlQP*QO%4G7f<*#LRD#Tu>KImg#bj$9&FwKCaOdOjapL`5_O_w^qtz1Z7pOodkkOeBP4 zJPr&sua)`|ham>WXQb=%@p^qB_NCsa$j1J^vcQ1(a();7Ei3#eSeFZf7~>A1Nrh{J zb-#g(tZ6la2-21z^f|+;)Neo`d>P~EN71VkMLOzwI*m?e783j z-wAK(fwl(B;m_f6e_et#BFzVSnIs?Z0~m>e^o+k_0aZ(74G_d|_lxcy~d!B!b=+uCgVA{={9y6&}v9PUOgBb>ot6mE&) zB|j&m2jTVedoP?Q!|>VR!Z#M2sq9NV{$6=7l;m?XB1&Aw!ub0UQMy~UFckBhwb<*4vlV-k2)}(~))}92V`%x#N>)udx`TJ5ygl|Zh1OLNl3aZ{ARa*o9H45<8 z#svcS*A!FzNLA>!@4mI_fdlualvD?U{=lo40rSY-w#3*02yB5tK-j$*p!;&k)%4|M zWHuhy--2(A|8npX{i$72YB#Z(dIalU65(S;$<0>9a^2K{i3Va){2Cu87A_`azNlSh zAj;;Oh1`8=972E9H(l~JxBcd*6NqA0ZjQvZA`_R&omcvzDJ~9>x%+3dhTF9UY^rPG z)}Xf{!O}M)+ZQFIcJQtKmTt#Z9Vgv-kHU0R$zlws;?<8BR;`d*<# zZretg-*B>$xlE)GWf)z~00u%v6tYCrh|e@?KHSSJrW8ks` z$12VH=Zen>H;fAC!?W6eGLMHBt`F9YQF_V4hks;1;8LYfY4EY3?BNi4CZWK`kP`|B zkBK}&uM6e-BFw-FC+9}Xz|VN|XJTvNPGa_@?<1@3B^U7vKFOn_SqO{qC}q{%WWl;M zK#bI0nd_1l7qo1S1WW!Q5TRuULKe9M0L7sdA4O_0E=ob!BxMUHhk|vF${T33W1iY* zR0T@wlEM1dNfXphvI_F`(b~(bs-beQbUn}C+GDT6cYEQR4FM1d3)ctC~dYCwwz<5^Uj+U3QhO2VSZ;o~l8#bj?FnQuzoZE$F z+%XsXo;kDQemPF9TldSIXf*)r^>>fUV(ZIfP&YnAKFeg})lGTPx^3>|2-n5wd+oV# zuDE89YId;hQ`HMC#>13TMXDFF?h#-XeI>gI>$;h2(2`2=w&%%&q_3fMBgM~s7YmrI?6Z_0(`Xi-)O^w*cma%64f7-47@d_F}&Hj<)9wuUP9~aH)#ReQaIY_pOmGvd=4GdFR^$hVh?nRpBOFmCi zAY)#DVsp9Oi~Ig|&{xKH`vQEG(#|y?MW4#Hu0;s)@Q}rI+=s#Kkj4PSLtS%14+J0E zl_&yuBb($_P}Dv#t8f#M7urW#41gaPKkpRIpy`Y3(IqI)z2B-gpLm5w0Ck;D&m~ z!?M}r5bbj$ZBvs`_KODVZ1@-8nuBH-9&CJiTJf{q6tdmMI7qCglERaXTg$qL4c?PbQLJD71fvp zNaaaH4wUgjL)6s}@>}d;&HtMH-vN>tX);;l5+TLmG)NfjFN(LJmmXUjd>yRAL;@He zQr23jTmHYMX(&2)H77WOoXsDj%$8*-S~MiPxVN30}Wf zjYNQPJyR@^r34|RpM&)m5Ytv?jR)e`Z(uK-?5$Kc+>PExurv?p+zrFnivu+(MVtlb z=`y%+i2oDn49E5l)>UHJF~(tgRq&RQFhov$7}45ar0A_Z4!Lbl*e%o<|y(nYC zGnVt-_Dl*_p|_-|lkIYYF($p=7%VaqAHrRNTvxI+nmsX*PmI-`iOsTJ2gYLa)vgWj z^S^)prNDnF@Lvl2mjeH#z<(+5Ukd#HngTN`=T^?2J&^f=T3JG4t0vJ7+W!I-du83 zPm6};R?nFenmaEVx_IiG+0&uo(j$|})jY5B?BwagV&PDpNj!Bt_w%gf*~0T3&#~m0 z!y_Hgf7ZN}d=77mqr-f643_Gb+xRRarTwqI~`Z{iA~l3Mwu>p<>~J z{tM<$>t8kZ!bKG{0EFgOs+i*P$b$aUDyCM=RzFpm{5pPGRh4Vv5eo(%F@17oXn5t! z*>eYnNS!@%YIOFzxfQeL&W_He371x8a>`%Cm_?bid2bt=U-4cEt=miG;!hV z=&Vqw1u8VGdqC$;o$i1znf|U`Fu(uoxzpxUPp|B+>hEvY&|g=dil_j|<7d%}|EKs* zoik_Nw2H~Di)lTqF&JT(U#TN!&s|VCKWeAUujJ>&mBJ_Eq5}q0jIBoADiE>CNZILy z6%{k*R#!}0v}nKy6=n0LU06{yZ_ey#mz17RTB`CFR*b8vxM=Eyl@$wSO`Tsky&^hw zrhrQO2mF&I|Fn74bED+lqe>i*P9La(gvfP!gr=FP2C2zM&utiddQvPNpazy%$fx|@<3idA0gzX2!dMopUVCX<`uYc)upm0UB88-*){ga&QR#JEQ95Fn z`MdV-fPs$w6|PBF+GXL_wfqUr@-}}|%)Y2R;S`vF4cScCTFw0x=af(Aeq~?k$p?25a>)QJ^Mjc80@51lc`V%_0-mdL`41QPY zs5-_)PdL%39~ntts;W6tr&UHyKQTJv!~q4vt7pupoIe6HqIBHU$hZL&r9Y;mJCevX zmEY&{OyjwLrw7lWJlQ-wc}DYuctnfO=b6Vdi)U}1b9ux~eH2fCr#H_5Jb64v^8AwL z7@qMw`}6pC_T@Q<$IsK3=NCL;f6d^jsv8MR*O*UOc0C7|LA-^Bm42X@~LD@|?=^GoHCT z`|;%R9Kw^&Glpj(&yW9Y6IA(YH$(j^^?&7-!fbys z8KR?3AzVecknlh{^A18EM17a=cEWbT0tmZLMl!j9@E3$3I_wIy8zl_G7;6c)5rzW%rgKgw zTt&D-e$xpr$#23>33Eg69E(degeMWMAuJ_qCp?=lA4Z-@IE8Q#;q8Pm!UO5(2MMEu zFB1v_v=dGt+$R%z9|nGma6948@o@)W<}V4Oga@-cT67eAN!Sa9h!N)XgI8Iuat&~~ zMiSPX1U$lBgg0a-lT`!h$6zwKgfP@QnS7dlYQ)c*OaC3QG(=`lpJ8s_BH~ zEUPX;xT*-361K9^xq`5U4NZgSx2in)g|Lb6B>Ha~VVJP-24a6|`3)a6oyBj$oO37--%ObRpAa?@wh}fG2H=}^!Xm=Zx!_M&PPmG&Y9jQ9 zj~XY_Poh97=vVltY6|@c|K!X-P6?M^NdKYGmS04B;iFY^;RC|fD)<6^T3!twz)#yQ z1#SWHmxB-dQ&dYi!h#s(;GgmvlF7~JfhmOB39AT2`PLBjA#5caN|@7_WSIth38zas z;UdCD!li`U2v-rd6Rst6{R+5*IfUB@LxeH_6cF|yEF#P&oI*I1u!^vpu!e9tVI$!p z!sUcZ30Dy=Cu|~IP1s7ffp8n)7Q%MIc0$*U&VvK6=4-& z4PhN&E8$AQcETpYoST@t6P6RU6IK!W(2vUriwMhaCLduV;p>F0gzbbyw*X({@z!MW zX2Nzt86dXZM*9fcZ>K#nVB87*ghk8XC*-q{FeGwGSVY)#7xf80-_38rRfMYui+&AV z;0M?J$b;|&VITNl8(|4y(Qj!NVH4p}_@Vp(Z9_+7LznHrJ%ilnr2EYe@ zV;mzae-pYA=DbBe5VmfCjwkT@J^GKZ{C)HqVbKTZkrPS(hp3$sJ1{Cs|6iJoxGFnrGvQM5K^FBV^={``L!7f-7j0cl+}S#tD%*!R*OFv%FEcJg zm`~hyPxA2vk4EisQy&@O?b{Ymq){M#`Pgi;5p;u>|w7? zaYKmoJBu|O4oAYb`poUVbDl_Pq+K{Y0{*A zOa}EQZEPh@>UYNdlQ^m08Mli#soxpb%d6|R;)LHq#7X_5qz2XR#o4*FUOIn%c5X>_ z&Tz1idc)+C{_8FI_>;Iv#0fuGanhbD;!Yw?@nuSg!)tMCvG}% zs53TINd5^bzsRQixi2j*Q>ZN=t(M9Ph@%F%AKoaCwDIgg9iRGt&N=27kUL3ZjNOkVj@ z?I&(Eai>azYR^TgJt2NBC2cclb0zHz>FY>#D2j}Y&JK)5#w5=!p4+MS2}0kiP{ryK z2W|(#=f`mZMdf+6YaXFZ5qY*y*JwiDA9bDv&+zQrr9zvStY71MdX`6zDd4hayIZ?W3qFcu#2AU>J{^IO{+6>Q?04d21 zk)~0N2r>e;)QX{B2LWO`@RnScOu7Z9PVmMEyt3?8x98$c;6`Oh*?e^Pr1(!SEA3c` ztQ9OxCQlSvPh$Y5?i-{n*GpR(e8aQzm-JgNT7rEqzI8{bvVzMJ>0u3&&2N*m!GA{38i{ zwJG=p@W=OHTm%o_f**tbi*Dh!1NTr1ezArxeG^9Jd_9xNw*~%*8h)3)fzK9a6ieTf zWmkEhrIIwAGcp}be>ovV)2iAYi!+w`8+_M$Z}3<&eRSs!$pzBp_od}p#%^X1FM`)j zmxwbh85@~(ZsvL!>Pz@v`9Gc}-z@TB;+SK@`AA_TtC)0=@hzk;A^j6c@1!R@&u3a% zZ%o#i)~nXrV&k`FcGTNq?{6V)B6Py;DDr55cZK=66uAm9$NfEh}awA4g>` z_jsT2bY)058yVw~KL<|^I44EF3M}^2Ee-0WP=cqGO>s_jNM`$&Ux(*}s_0V<6nA4ic$ph-L#$4fpe-bA? zhClKyx_GHajb+qb>zmn}of}z?ir<(WI9nnXQvun^dBpz|*oUp|LdU1j{?M_DY$&~@ zWMfozlgB$8Vx|!eX_`xDA%Bjxoq1M@o=qO7fRIo6SnzN7{D-9Ut8W}Q@F#LB^W5+H zB$IE5-Z`Ti+){FjPW2Y2m)o?u@QLv$yrwwO5}XQnioq#J=zITg{?~F-)(g?tH%0rQ z#IFF)@x+}ic(#xi8m1WwSGheSJB@`S(&>{!9eAyB*ZLRxmU9c#RUg_R)9= zZPtR9k(*486WRndUg>F5+=(_}HD-h;cY@X4w!?s^{k{jn@9q1Wa6clxTbieby9l`7 zx0`fq)^Nq%i~@HT=N6dCx_r+PKQ=pOEE+i*!IO0PbMSWc&r@{Ck^T|x-2hyX1>X${ zuYi=>2;5c+?zJ7^O1Viq%dBz>Q{{XB+F+HN*R|Zul$&dnt4Ng_O1UP=Rg&&Ihxl-I zfoB9mM`>2Nc06=usvY76R_!i%7yn);`|u4^dp`!Hc&LR}9^T}Kfg z5juut-lBB9XQn;o7iW#K-)W_$2Z6Jle?BACPJ3Xm=y|PG|EnP>rsg(fFvJ?_R~U_! z`O8^22bxvdEYdcTRxFuGTkM%9W{gU!6Gpf`sX4T^>k(9 zQllVsv&&iZhkSg(4#>9aTEFfe8QB=fMP zp46PtvmlcZvt)gC&L~FA)6MycE>}jmUDVf*uDMU8ncn<$IDE}UcuIQH9IgS%d<*P^@sU&EU+1FTx)=R`4K5!QJFLaeqTh~`CjCI6bl{jyt9wgJTo=Ui!tswBkedp zD&08$+c7D9fGzHr%xE$yTkT42$`Rj&?^}^xZL6&Wukw6+(D04#YvO+lug0H**Io|1 z8oR?Q54_N&==l_0B_cn^B$Kxay&l~Yyn6f;dR323(W|jXH}u+)_#rt-@S2&zYYTX_ zAD>JAwf{?51C&9`V=SNbWXa&q@+y?Nhb?vj4muxuf$H!)(u}R6TuI ztKEFESUyMCFQ=ZSQ1@WEAi4$O~Rqp*%x$Tr&WtDroYq`rQccfMBrBu1ReX+l+a?M@Kt)^V^ zBNNX@Q{^U6Zn;(N!LH@DQ*N_WZkaAOl=j_Bx!WoCGi*ZNO~k`bUHo#)lD(diPCmx8 zbI^e!Q+(azO}XRjxn3nWU@g0RBT_iz;ygd8(8S@ipN50*@DiT>7Tg0g+!ErdfcxEG z?a%e?*9}}Gwl-r_Hj~-0xOfoWAN4t&S(<7i^VyWsQE(~XS@)sI=g;oXFJzYIJ8;=H z#Kh$(;?vRjM{p7NjXX;%_%~?yGDb|wg}*Ol4~ZYz#x?xij-#zUZ;lu{T{V{Sdz=K! zSqQ+fHG0k^_&ROMOEC?#2dLC(iYgr?`z59iO(t&>T!!ocE~`8qJW6R?Fr(9r>937& z(7(H}^I9&?xQ{mYdT1Jn?yTW;4*M*`_ow2!)t#n&)8z3?N=u^QYA)9byyvOsE~&@M zk7B$@opibA{#BSY;>6C-peGfTH64&^3hdjD#41lho$s)(1ApfG>Fv?K{qu(;sAETgg9&FR z5$#tdlXpl&YJR7AXkpj3%t3aGBFSX4vXeTrMcMLQ?CMsxcSU-Apyt}rpYrGE`$$o$ zFIu~6OF!CDMmgU{3^e@7n9~$u{zF@O!voTm>t){5b&c%(j=Fe^tg*c!Q zF74gXWinriQF`MTO_wluZKhotX_uOdtM$iD^GVhpt2=oWOR}o;lvKtJ)3quRAUi#B z(A`sccTExB#TE8C=Mr?rbkd$C$=8gbqGS*qQ|ld*9g=ZSybLIgaMpa@v5|N$%Zgah zMU>kJjK-@?7_~nghVWAkJpJ@F=34g%U|?G<6<#oP5R0TT#*7jtpX4thf3Lb^@-5zd z`;kAT8<7K91QlFa-Nz>=`K!o3XR!&t4Xv~r_%d!U7kt<^&?4}k?V8_nt^*7@?kx^0t`+eW#etQo8NZtGC4 z+xm(qmkZxd2hZ1e_f6|iPWUlJ!_w~gaS|{ZsawG~{HMc^K3NWo?eLeP@%v0@Q+*+m5yARxPr6(h6CfGK-w19zD4KFd)w z?t28{j)v`ELD%vW9wa`u3Kn{530?2ksZ>X~M) zGDfZMZHGk;+VX>1Pw>4O6l~R;a5XsB+^==2mV3)i_sr9>p$zqCb}mB4>lDh#e%M~W zWnZiSA-W5nD7vY+N(tur*|1%ti~lJsvNvc%ns_xYz4=Jy|J3oQhxVTczSPrY{!jaB z@%ff}qmZ)mzMSGL>64p}W;|cPH$8#lz%}JJRx~tmo#GvrzE8rcPx6n#pZ^E;pox9< z$80A&wf9K*p;PeK_T@b~Jv@zD!7=!D*f)Ht@JlB;Dp)E06FN3PFJe0R@vwp$DSkPy zX02jxunmj3i@7JN6J7%Amb9v%G|O`{37B)8Dk;l?83v8)5(CZoT8UnU+6@8a}HGHy+pQ8&>fxH zQkr!j^jZmS8yMNoGC4DY`n*|;61bXhvXlC?aKmQadsE~y41Ure2tu`_(*u{+g?t-=yne9aN2f;RlQq)*G|3n zNWHQnx;VRw?JxEEkdwf8d|lGs9UbaT*WS@!-%WdmD}5t=pZ)&+L-Kxs{Wb9TJC-L8 z$N4Ja_Y*!mhmVF76gdyxZ;ljoY=_%GV`s*+K5!3qsd;+{@kmxX1gkJC}EmyTYtL>CQ6T{<}P` zhdn@(y7vCW?Pn|9jou+!ysk}Nc_-GFr!m=kv)j+0_m*eOYOm`LUU`?)L(SBf$6w=f z-RT{()9d=!EALjSw6&l6;BVdjSN*QI&;PjJz0r4iv)^^EU)~?|OT17`{Uw+6b_aU- zd%Mm6uo(-atRs7b^8ISj7PsWQL2RO|GW|N2 z(RDoKVY6UH%O2G6Q}ffNoQy-wCaO03qz9~ikv9LQ=>sy(?Et?64S~Pg^xkg&RT+P9 zHeAu^XPxPUtc}RH#+J1%eMNe4##_5hKRoxI?$Zaj^FHwSpUChw`aBP1c$fP9f6H*c z==ZP7@Lr!G@pooOOp2v64;<1N&tdL@e|Y?DJ-lCg{mTR1-}pUC0^Zdb{+k2t&olgA z_wcs#kob)~B+up^65ko-(Vby}+dsUX&ojJV`~27S@UHdyf5>oOli~j`!~1N8#JrRt zF`eNI?FkK= zzdhi-sfT}6z`L`DiVw(peLzxw6c2d#VYl}QkN0!8{~@PJ>qf5rzm{Lg%;CG``J9aZ z!*uAi*qw2W&wYb?;Pnm-BsvnkCAzaiAN?Eox4QkWdAz^#WW3`P-R}GEr{qt%2R6C= zFZXoSd;M%4T;}z^lj&OR^M9D>`Y^+PXO`>N0snnjt}g=~;&1MGF!7r+WtCFe7jX~8 z_xNh2cZb)X%=EtH_kWb>eka5KcBc39fd7k3_oF@i@hop+ro{g)Q}R5TDKTp@B~J&s z^mX?X9Y5WFt=o~H>e%7&Kbz@Y>+`>v>Hg5~e<{X_ztRzW+CbN@_?`6~Zkz24Ps&wF0~%QDK}?v)6~$cO(0 zJ1V*CP4tE$bKF z@4fD=o=_~sMk;UL&Up*n{pWh`>FNH!>wmnbcc;&PZBH+~^`390r2Bv8S$2f`AV_d+!1bQb-xzSW_VC;h zaQ!_13fp0gp60+y{YMTU?sCuC3>WkFTyJ_jzt3=e@9{j9;rhxnmG(U6 zQ8IL_cYAkcxW4dt-pg>^=l8T_xE}R;{?7jyp4JT4iXLR4`aR7mytRjILbvqzz58L$ zn_ipLs&6~nNlACF29N(*zw2p_|7vCQ^=ioQU!UoEI>WytQ}KRYzneVw_i(-K@h|J) zUhehZ*uz!p`&^>_Ciw5@;osRKWy7514jXRo^^t)7rel$&5 zD$-C$-FqXad60(hRwY0}LW@F@LLU84ZwR3@-;Hi4!A(LUNLoUY!u{7?YtQVn&zaG~ zA~*M5pYN0Qod2x7_S$=|z4qFVvrp|KP`(W{-&j}o!3{MZT3`2P8)}|H3*S)t>3YJy zP*3<%^+b8Ho?`vDo}OyOQMLr_|1eU!sm9khUp{_C&7Mr{qxChDYwJE=Uwcno-6!g6 z|M^vQAFQu=c3s`4>TAAsX5GKk*M1p^*4I9^p78nggg?EWD8IX&V*L!l)R&SYpZ3fF zr1i&kTFBe~SW|b)hT0FUt$Vt@_QATk&(+twXI{nKe^Zz%DXsTLY=5NB*U=F&{Wzw3ziGHQ=YEQnbuCO%CG|b;bTPlIx~O>a)}S1DXk;)wKrtK2Ki@Bgdu^oLV*{`41- zF*H=ymTnI9{iW7@yLG?Uy1&}GPgwU^xj#$Nn~?i0a=+zr{=utt_&vk-Sglmo+K;hh znD~2geenjqf6;m_WT%7ECGdZw1g0)1wGdzW5ozz8681>gFJVr? zNeQPUoR)A#!dVIDB%GIUK|<$mIoSpY8zqcL*ePL;g#8lcB%G9RO2TOgXC$1Ja8AN` z2^S=E?w0f=Y?Lq}VW)&W681}&lWNY5;jU0k+4(39try;%t<&Y;gp2a63$3CE8(1k^Aavd=-emiOV}vk z=8MAH#h#rz-`aS|l><0Uf2^^isii4$X>?58MQ?~jnj$Stu{SWze5?Jh#YFI(A<5KY zDd~|=ytdR&ntW@WwT|u|{dk?D`%ypsDo6K6eteyy`!PR$hNJsKKYnJZ-}K|?rFHu@ z9@OFl=*CcdgA3~D)s>NyA3!e$Y zH#&zN3d7HJ?pg@L&vUf@V$XmLe$cU-{R-^ z`TPp@D~Hiv;B6MD1}B#{$o!HzhYr}=1fTpz=0Rtu(RDli zhXx>}s_O;(Cw%fzzNdGo>7sM<2!E&0W9z57uy0G@J^#e_bZ!}4F9_~g^jF|H z@#+5>)6qF(be$0Vy+U7L*!d5^U$W#pAA6RG&+BKIuv6$e1RoN7tKdC?|B>K(1s@mu zp9DW!@V^jzUhsf1Tj( z5dLoxJSBLK;Fk%0kKn&8_>kZYpJ$*+@S6pHhv0_=|A^ps2!4^^4-5WT3;#F4|EGm- zz`%y&ulWKq(0n3-Uo5yT=c@&8weTAR-)-R^75r)oe^l^c3;&_u6M}D*e4UK}5y^Sn z!kYyDJHfU5`vsr3@aqJx{US5|E8+iM!9ObaUkZMg;5+`oPfwrqC3&6}T-%dx34YeY zO#kb`rxtdM=r0v~zu>PId_r)Y?g7Eie}oAy6#DB0Zxj5l1;17BdBHy@_JaC;qK8*oZj z_X|4RJqi!@Um4-!i2WBaop&kvU_V$%{tqbnjn17R{b^W!KL&dG`i0$Je@5{M_ID2p zeuWgVaze7VBzuNxHpD^A4fXX)&;nvniF z27IH)W(NBej{fm`j9w}ae+T%5_`=5>beJ+OJ@0Wd@U75?ewS-7A@qGP_=M+185ebT zJ^-BfY!g24kks`B;9>H-sPMC#*^^91XUoxbKIUaMf`0>jiW(Q4#X(mLIPss~#B%Os z*zpv-+!(<O%N+{r7T$+HSW2r~EeTXB_5NU4ICi zH3QBS=i_2(}W2#;OvrAP-)7JVJgiPzeSHcY^i{EaU% z{jD50*D73YjNrx!`0W+&hk#SLZMOQ67b?&nzybl4+pN?(nytFt16=8+)Yt0;zZW>k zA9;>}D+HfY^qZU+QT%O!Z~Rr}-$?^WT<8|n^$y@9r(^Y-be$A4(Dkwo1y1EKDfO<~Zwxrm_lu+Sr11GKz)8;eb<9Ukx7{xITr1;GiJbQd zpNQC1bZ6?i;B_L8=*ew@?*JZF&cndT4&=nn_Xz#1z{w8GiXG7Y#y5q3M9Tknh5kZh zkmT>bfCaq0jvu@oIOTWNWBi2B(-He&^4u)+k!zTq&c>qaNx|oP8L#2Mxd4Kx@|Sk4 z?b~kPB+ul}IKHO84mjm^dh;6fU@t#*KA`C3#t7~a`eDmXJ|p-gmYshIILTxCnb*J( z4a;u^IMLhfV%-}B|0riPCNjl@--=TNz4B^;Eg7s{C-^M=cQlJ2@};Jkvkt z_;dy;U55pqdL0u!C-gbNXT;FzvbqU4$vyEjL5^MfGa!mJqD0Xb^W{WZ_Kjbx?gBPMAC=7R=r;XoaD)gJ=`vQP6%%I zQ(qE3otA&{6X0RxUms;TXDm6lC|nXVg7;Ox?*y*OS^V37kT^dAPUX{MjibAusLF19 zp9wvo9~8Xjc?M1hev`uG#t3GCQ@Z_DfAIov>KA5Y+yVDkUFSA)IUln8t0Zuer{QU) zljh`%11J8|mj0X+d{V|2+Rv%O0EhU@wsZZ`yZdxC0zZd;`u^vDQ@uxIoT&ZDM}Skh zb7F7LlvwLq8SlKD`M*{01n`YM&U|yD(C38DtA%Dp(W4(+U_k5D;}!V)yaK+tjpdmX z{=0>L4{*xYF-s3`5&R4bzaMy*{ErELC&~GJPWb<6JM-Tx^E|qKp93DoryB)J^4wv~ z3*~{6ex9`a@CSg0@%brmsuxG*J)qv|+R_nTzn1|g`l(@-|6!qj7jUKjn;F;hIe(^b zAu@uG04MqN`dx!OdqUy3yPx^=2_AuBP`>)DaoL|J9P?K~|2B^1JP4fR=@)&tQ1cgj z_6H1nNAPC_&j~=^qEOdw3ZKXM_O}GT0657%Eae{&^4-9xd^!^>$Oi;}N#hyDpAc%$%n5O|oJUsE{re3zT`isAd~OC#`Td>0XS(}2 zaGt0@zh*nr=klDO6o<11xYAp3ShPGTg$q+7I0l^b@TAzE&GPJ#3iRIruIk0BQItB} zO>poOF6FHCVNCD^!QuX?>x;li&d&3gkDkw&7y3!D=epc#ce38*tbTJVa3#O^)moqX zfs;J8ANZ$&=LY@qe^&4*v18iL`7ZFVdSAPX^=hn z-zg6t5k8L0x4?~5*H085l+S--{@>@oc{I-D77_b%i_m{x@cwTx0I}8elHj>-^8I%> zd1nhcCMq9${N4>*)r;6GZ9`I;{&p6GUeTuOzYG1Gr297vI}Zu{)b&jNnBaAL!prlG z74VyYE4})!%txKe1YWIsSWGD-I!GpG?hDZx8M4>3)uu3Lapy*NMO z`|W~%Oz_-K8PI&bEPQN#>>1$7|Frt|9s5|G&g)o^hlT%pfm6B*R=S@6PWHA_^#8EX zpShpg<0aA_b-&gNJgglJ1E+F}h=bWJeBNJyen$8&SmVT>DtgS%oXPUjng4X1cYyiZ z{gf;C^oW&T;3tr;Y1xmY?a6(>!{q;l@UholHXrcP*k8c%TJAjALXMmHO4OTmPQ1RL1I95Nt8d zhA@g6+bN_^85@q>GMgM*XzW`V5NaTTgpsp)^i~00e{eA&>m_Gb@1^nBJk1#cY+TZ4M?Q!nwfQQxl zt_pZs_}KoZ;~wsg#@s#oFW<3!zw36p(df41h?mH_xsgOBp zA1mg@iXl;(lOx%T=Z<=XLSo44>W+5gvjf?pJLF|Yy<&dC9W4wMqOCpIk%^&frni_F z&3P%et2fa#9;d6P&po)Wx7B?GSvMuKBdMkEZ(-3#6FHW;hZ^DT&*A62;-zt`$nU9DXb$ zl1>jLis@{|%?=J0JZNY(F#>6-J}$7n256Fb54yl`anKz}7m6OW9O8>O#dOjw<`bF1 zU^YJ*h&%)$E|0-%wwTMOGsOVSXm;Fli`f94$|jG5;flV1G}A-F#V~?=7Gj06%q7MO zk`<$n@?K$VGzcfr8oZH!Sd{PLQIrCTA_G%Wg&>g`8XF0rD3?kuTkxn9$x;LuS!5yg zM-)ny(71!1ml{YUj|3hjQYl?^FaV*CM-m09W0XPY<7|peUf>}~Bw7*1njTkSo%f`2 zm0E={5Q@clBLC-T_I-2G)G1o zrbHL3D@{|aqBM;uU$FGa1}tXQtYQeQA(GfgDVUH|RLrF0&lJW6+%A`Tnxvb^rJItu z9Qyv=;{I)}n}+_Ui%v3MEELBE2b+=(YSJ4b>z#Ltqtr4oUcqtQRJNoj#caOdCdQ6A z$?Rxu#4ExkZHu%;!-?EMB;Y3U`NV|lW#IlegZad$=cdL+M<)=)xN}JqAF1o^KDhlr z+>Lkda^YOKd%CZ5<9h|%yXzn&S;)G>iA-w5b9P8*6HAYHyo(c$CQr=)JgU5|-VPTo9C2`afh3F3Lb0oN7xLe`&5gz& zAn|lZ;Lpb4q|%f4Hur$*g?2O96c~bnvOSz>iMW!9Xj`eYk*>;qg#F|+ zbO-`R+-PJVk)qa_O(KO|k@&b-c9j_nkQiKgZOP$$Hj{03uZp2`Rbi6`Cyz8|vPa|N zQd5Txxu~h8Xop!@w!2_H3-mW6*j4`Ok>ZZUhRE%Kw`0{7qVyJH9i@^=XV7y(+cPOQ z@4^0(7coGES<(Pim@efGg>$gi^+~TJfDYp^(n@CwT}i}^CWkR-A)luRARf1)9PVx| z4mP*+rANJOeO+i+2eu!IkH=%hQR3Gdo};J~WH3XTUMjS`kaLq+ycCMz4ArWeL%q5o z71zqF(wV`m${%VwhT1loj?t}}hR-AkHNnWnN`>;PW6X`VkUEyN7HuRTrmu%v&9r?@ zAs%l}r3>T(swUCBkF80tU}I*%CbJndGjhSHE?QM%^T&^rtAQ#i)?Q2up|eg8rC~RX zoEIl@Xy^N&ZS)Jaaxfc@wr9wSk0(Z8a^PjBGebLClgExlqZKQX91JR>-eSA!D$=N^ zFH9pbz<)0>8m7oFJJ9A`;O*1srWiV%QiWGBu3}7(bgbF!Eo&*+wR8r9{3OQd)HI@z z9q0wU{LXCNQ^qdp#`kx3pi9Y&DVbyLej0D&vLor_L_EeqIV8C<&Uuj!ok6Ur6~ZC2 znQReO>8MX~5?iXVTBi_e*0x$2UpAA1&y&r>(_E5mUoke1tX`EBs{_4RBA4?rDX4jJ)F)^)9g9%kIE<+jFORvK zSc~p)q)UftVT=R29yjgQAUbzAW1}#VhKFgD7uiFf81h_qAHUq*L9`78DjghqCG0g| zQVV`pv|Z=IO+$MUq`%}c48$+*bIp<>3$F*K$bY5&scMgl#9Y<t?|oO+*6jrA1s?C&P41OcL(^Z)RILh&Mkz%ePv)Vo zrmH9oLyhZbzO`hKeC@+1E1JyE6Ne;oZ&8is+VsGxG-CD}a_XoJE~=YV^HSw}2b*4V zdr2HM$gOH&eVL9{?A5b+iNqbk8VOnUfNQxkO2&EHt}*m}?Q_(HKO}3 zw7{X_Fb^x?rP$sxN=6w*T(s^=E}s8pP#P|vnPGijG~vUs{D=^KRx z^WmEy2-J1inL>$IDUImj?OnN%{s!7=jBUwkwx>MQS$2Zm0rF~A(3|ej1|^u+WhrLB z29R>Ds(}uq0QZ6v+h5au55DE-aH7a&r?*%hsD!PCwyBXyY2}M%nVK;N1n2DlW=D8R zi2cc~%b<1iEIw`y%i!f@u&{(-YssbT0T~=lbZX`Z0I2AZc#c%b6T(}~bvR0ihYE6g(bij>Ops%}gPLkW&02GZkE z8c8Klg$1|dheBa|N{i$$Rl>uqk?fG31P$x0bRmRPlARfdLOuYN`Ia*3J%xGjb{LE9 z-nOtgZ*!`BJ6RuH`^IF}5+@s43P%&U_`%SwlYMZSASk&dRuQ8lN;L{^xIC4%#MCj* zuU8FP-R|g8_cx$seX9dl9RVX+66hiuO9mUzBFh4x(9JZ~vaDumt3# zSTVICQowvZe0aU46AcdDf-5)MjJk<&-2hsoZa_hGQ>9so+oXn!x*C_az7wd$uz8X4 zoQTDtfMVPn|&(=VRLxpF|%(788rT-0ivI- zwV1^$kg;kga|8C;V7wjX(j7gf{2oZQT<27kf^_=OW~&()U44vHVUYp0Rqv|6M!iwr zcqiII^|_LjjaV#|P7GzT1=?anYZGt-)u0O9j=|GvzTsnbJs8nT8ETf@@^P)AZX3i> zu9w1uyirKT9Ishc%I&G_^9Lp#WaymoMzKX`#P5Bf*u$5VD_JJhzIBxH+Bd<_-BoW# zx`Sl*NG&tlh%n(xIXEmbMDo+=Ghs;RXj=#*7iS@h{uFlMmJ zf&$R?3(H#C-se(%6E?r-A-N|%;!U``_lNY%KZalO`m}$BAbcs zHlv$Uv%T00;p_P^DH+o9ukO5XAk&um$&jM5jb6gAq~x}_hiT{>sPolzZi6i$YV_Md z#|e(T?bMR5Uwy9&C``*w!~Ad;ro3Ctq?c)=Q&85RVyxIun8hRZd$GRVP!drPxH!hJ zWO*z6{uv0KO)RzI6?#G~|K}|NYsn<)4{R;_P%Zc|pQ`+{<2)L_W zu}|;j7K_zU0(yC)@`1vTdbIp8>HrcY`vBR>^9xNKKv%GD!SX&)eOyxIrANPO7+tcD z9c`5lL-?=~MQ|IqXdku~&)rST$`QkFMYW2u?mj27OD$$jUpxUX^!p>QqwpgLh6s&Z^LxNyxucGY02pbt|$*sod+as$L z*QhF9=JTz%xnt_JBq|N^GV@pn#ePR#I15Rcx@+v{!bV<~hH~ZUA9F-nO{%a#V`*%* z?9OE|ciL2Sj1N;2m~g_tH<>5|mJnWXZ_4a{R?(fJ@xiK@hJg9!l`AzT;%2dihxHKI zs}#B>a*#(Vw98hgyRZv90#~uk#h%D!S8I&yDptKz$#ilDVfXq}$EV)?uw!F&9OX%V zEW#Td3T@0*JMjWOx3qVWRPUNrd+@XmNE6L!4c?mj2y@q@;zIP^stJ4Y&-X3~UJ~#y z-=R0w^Il3dYK*!E)MPEqLoQ{%(ko|h9p=F^XE(mcNlEZcGdX)m&fbFOR>Fs>|Vrv%fpHkl?Z^+VCRvfQT+VF?BRn@i{(+&|U-t?i4P>7|> zhq6{PFo1rg?8v*`MaOAy-&d{^k+VS%-Er(t0axw$QzfZzGAX4-Req_Cz$#ZptAlroN~L z{1Ty|Q-;deGu)hs)!K0G^JiqU`EvcofV$WdLMo4V(Uwlhq7j-{<5Cx=OK|1gv@aX% z)R(*9QyI<19qezS@uThUYCo*PiXnV=wa=Oy8|A6smDJFe$x0KXr{p{OmSw1I8vc68 zXFR18w5MCyvoQQQ_CwXIeb|~*tJ~`q(|Cok6?=?ni7Mor56h)Pk5+#1AUK^Q>)qBH z((Ig<6(KlUF!e_H2kLZ>Q!6N$LaMlav&fosX&7LRslw)Tl{XH*0t@r98WT1k#A2&q zw%H8%7Foz&UVLJ(?5&zb7WV4M676Z}EurO%Mgo=wWuRJ@+NmB?nerUwFZCHL*hB5} zm3r30IAY7rgR)R!2*X-K7F z4yjH{5-!CnGe^_tZVswr?w}t#cjA~FoSdX54}yGRi@%lb*ORa}w8@f`misENm(fgC zm@=!ug}g&p^0q>>8(qOlj)mYcV*KE!f1iwhYto8J#;OCjA7P~<@9S4qvntyOO>g3` zu0?Ald+BQm)i%4(1_Hwm#r&$Wukw0Z5?h?|aD~cydCE$4>J2tnj`6B<{}_*_#)1=( zv;?%ww^Dq%ZFIoe$@4GYS%-zR>ZLsWIx|gq$Y~+wTvT`;ZgkG(oCW4Ls5PN?H{Kso zi>SOh9q80^bmS-^xkIDuWaCIG<6X&DrgoOJk9i4$B{il|ik2J8<1C_dzBrb^a=f8u zMmh0t!Z^>@amjk>!|p8GS*0_q(&EUuD#a|sVg%7KI2i@^am8Ud|s)k zYZY=%iieJG)mP%8+V!k(tfBKXZ-_BA+T{G8+REu$%3KOHn z!~nu#p2K1N3$L4dO*oRQDVNXYaE#!D(^P^6#?m9HOVcSw5ez2^!%h=+iGdl1#XLV5 z#{rpi_N5||8yC+o1(Bc#@+&t|bed=#(`mxbrlG9-D0oSy3Gct)mQMHtbygkl*5nP# znV!QbaMO24CW$i<)rp%Jy_18gxbktI%tQZHC(%L-uv8+`1~+EDJ66}e*MSG zFK2c73-UvUe;^+)(DCf_@mUzB9)CLiu?@^xhj(n{d-|*debGV3-^{Rs&s#Y3DF+?D z;Vt|?he>>Y#Ju>c5M>0%_d72e@)vL5XFB}0FxK+xbaXfj@|TSGhkl>q>rg&7Mz%lA5c9bf04lA~ukn9P|w&OdbM80Al_?D+2mMrHCUj^;Qs z@FZ0rJXzXVEFyl8$JRLTa7;_AKp+Q|br0;5&@gw)~a~*0rMjiUXgSvG17q~Oy zcS?L6mNVv$r{n*X9sdEQ)8Bh}NWO=`XgPg-|8Xn++K-Vms1pfat?tVI?DG2?Jfax7 z{+$_)ufv7)<+${#(SPfF3ioFG1^Et<4pY>)jmu8|i+EUQ6TrP1(66^FAB>sZLpSH@+mgiN7Oe(J9Ps#Uars`S#PN`V=EKaHYX+F1E>CbK9 z2y?II@Dhm+GIi +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +/** + * @brief Convert C LaserScan to C++ LaserScan + * @param laser_scan C LaserScan + * @return C++ LaserScan + */ +robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan); + +/** + * @brief Convert C OccupancyGrid to C++ OccupancyGrid + * @param occupancy_grid C OccupancyGrid + * @return C++ OccupancyGrid + */ +robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid); + +/** + * @brief Convert C++ OccupancyGrid to C OccupancyGrid + * @param cpp C++ OccupancyGrid + * @param out C OccupancyGrid + */ +void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out); + +/** + * @brief Convert C++ LaserScan to C LaserScan + * @param cpp C++ LaserScan + * @param out C LaserScan + */ +void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out); + +/** + * @brief Convert C++ PointCloud to C PointCloud + * @param cpp C++ PointCloud + * @param out C PointCloud + */ +void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out); + +/** + * @brief Convert C++ PointCloud2 to C PointCloud2 + * @param cpp C++ PointCloud2 + * @param out C PointCloud2 + */ +void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out); + +/** + * @brief Convert C PointCloud to C++ PointCloud + * @param c C PointCloud + * @return C++ PointCloud + */ +robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c); + +/** + * @brief Convert C PointCloud2 to C++ PointCloud2 + * @param c C PointCloud2 + * @return C++ PointCloud2 + */ +robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c); + +/** + * @brief Convert C Odometry to C++ Odometry + * @param odometry C Odometry + * @return C++ Odometry + */ +robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry); + +/** + * @brief Convert C PoseStamped to C++ PoseStamped + * @param pose_stamped C PoseStamped + * @return C++ PoseStamped + */ +robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& pose_stamped); + +/** + * @brief Convert C++ PoseStamped to C PoseStamped + * @param cpp_pose_stamped C++ PoseStamped + * @return C PoseStamped + */ +PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose_stamped); + + +/** + * @brief Convert C Pose2D to C++ Pose2D + * @param pose_2d C Pose2D + * @return C++ Pose2D + */ +robot_geometry_msgs::Pose2D convert2CppPose2D(const Pose2D& pose_2d); + +/** + * @brief Convert C++ Pose2D to C Pose2D + * @param cpp_pose_2d C++ Pose2D + * @return C Pose2D + */ + Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose_2d); + +/** + * @brief Convert C Twist2D to C++ Twist2D + * @param twist_2d C Twist2D + * @return C++ Twist2D + */ +robot_nav_2d_msgs::Twist2D convert2CppTwist2D(const Twist2D& twist_2d); + + +/** + * @brief Convert C++ Twist2D to C Twist2D + * @param cpp_twist_2d C++ Twist2D + * @return C Twist2D + */ +Twist2D convert2CTwist2D(const robot_nav_2d_msgs::Twist2D& cpp_twist_2d); + +/** + * @brief Convert C Twist2DStamped to C++ Twist2DStamped + * @param twist_2d_stamped C Twist2DStamped + * @return C++ Twist2DStamped + */ +robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped& twist_2d_stamped); + + +/** + * @brief Convert C++ Twist2DStamped to C Twist2DStamped + * @param cpp_twist_2d_stamped C++ Twist2DStamped + * @return C Twist2DStamped + */ +Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped); + +#endif // C_API_CONVERTOR_H \ No newline at end of file diff --git a/src/APIs/c_api/include/geometry_msgs/Accel.h b/src/APIs/c_api/include/geometry_msgs/Accel.h new file mode 100644 index 0000000..03397f8 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Accel.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCEL_H +#define C_API_GEOMETRY_MSGS_ACCEL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Vector3 linear; + Vector3 angular; +} Accel; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCEL_H diff --git a/src/APIs/c_api/include/geometry_msgs/AccelStamped.h b/src/APIs/c_api/include/geometry_msgs/AccelStamped.h new file mode 100644 index 0000000..5ce2455 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/AccelStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCELSTAMPED_H +#define C_API_GEOMETRY_MSGS_ACCELSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Accel.h" + +typedef struct +{ + Header header; + Accel accel; +} AccelStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCELSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h b/src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h new file mode 100644 index 0000000..c66b1ac --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H +#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Accel.h" + +typedef struct +{ + Accel accel; + double covariance[36]; +} AccelWithCovariance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H diff --git a/src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h b/src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h new file mode 100644 index 0000000..5a48066 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H +#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/AccelWithCovariance.h" + +typedef struct +{ + Header header; + AccelWithCovariance accel; +} AccelWithCovarianceStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Inertia.h b/src/APIs/c_api/include/geometry_msgs/Inertia.h new file mode 100644 index 0000000..645d9ae --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Inertia.h @@ -0,0 +1,28 @@ +#ifndef C_API_GEOMETRY_MSGS_INERTIA_H +#define C_API_GEOMETRY_MSGS_INERTIA_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + double m; + Vector3 com; + double ixx; + double ixy; + double ixz; + double iyy; + double iyz; + double izz; +} Inertia; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_INERTIA_H diff --git a/src/APIs/c_api/include/geometry_msgs/InertiaStamped.h b/src/APIs/c_api/include/geometry_msgs/InertiaStamped.h new file mode 100644 index 0000000..d077204 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/InertiaStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_INERTIASTAMPED_H +#define C_API_GEOMETRY_MSGS_INERTIASTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Inertia.h" + +typedef struct +{ + Header header; + Inertia inertia; +} InertiaStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_INERTIASTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Point.h b/src/APIs/c_api/include/geometry_msgs/Point.h new file mode 100644 index 0000000..12346f5 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Point.h @@ -0,0 +1,21 @@ +#ifndef C_API_GEOMETRY_MSGS_POINT_H +#define C_API_GEOMETRY_MSGS_POINT_H + +#ifdef __cplusplus +extern "C" +{ +#endif +#include +#include + + typedef struct + { + double x; + double y; + double z; + } Point; + +#ifdef __cplusplus +} +#endif +#endif // C_API_GEOMETRY_MSGS_POINT_H \ No newline at end of file diff --git a/src/APIs/c_api/include/geometry_msgs/Point32.h b/src/APIs/c_api/include/geometry_msgs/Point32.h new file mode 100644 index 0000000..9721822 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Point32.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_POINT32_H +#define C_API_GEOMETRY_MSGS_POINT32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float x; + float y; + float z; +} Point32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POINT32_H diff --git a/src/APIs/c_api/include/geometry_msgs/PointStamped.h b/src/APIs/c_api/include/geometry_msgs/PointStamped.h new file mode 100644 index 0000000..81f89bc --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PointStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POINTSTAMPED_H +#define C_API_GEOMETRY_MSGS_POINTSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +typedef struct +{ + Header header; + Point point; +} PointStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POINTSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Polygon.h b/src/APIs/c_api/include/geometry_msgs/Polygon.h new file mode 100644 index 0000000..d34d157 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Polygon.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_POLYGON_H +#define C_API_GEOMETRY_MSGS_POLYGON_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Point32.h" + +typedef struct +{ + Point32 *points; + size_t points_count; +} Polygon; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POLYGON_H diff --git a/src/APIs/c_api/include/geometry_msgs/PolygonStamped.h b/src/APIs/c_api/include/geometry_msgs/PolygonStamped.h new file mode 100644 index 0000000..f4957e6 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PolygonStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H +#define C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Polygon.h" + +typedef struct +{ + Header header; + Polygon polygon; +} PolygonStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Pose.h b/src/APIs/c_api/include/geometry_msgs/Pose.h new file mode 100644 index 0000000..595ab8c --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Pose.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSE_H +#define C_API_GEOMETRY_MSGS_POSE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Quaternion.h" + +typedef struct +{ + Point position; + Quaternion orientation; +} Pose; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSE_H diff --git a/src/APIs/c_api/include/geometry_msgs/Pose2D.h b/src/APIs/c_api/include/geometry_msgs/Pose2D.h new file mode 100644 index 0000000..cc282d3 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Pose2D.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_POSE2D_H +#define C_API_GEOMETRY_MSGS_POSE2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double theta; +} Pose2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSE2D_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseArray.h b/src/APIs/c_api/include/geometry_msgs/PoseArray.h new file mode 100644 index 0000000..60876a7 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseArray.h @@ -0,0 +1,24 @@ +#ifndef C_API_GEOMETRY_MSGS_POSEARRAY_H +#define C_API_GEOMETRY_MSGS_POSEARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Header header; + Pose *poses; + size_t poses_count; +} PoseArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSEARRAY_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseStamped.h b/src/APIs/c_api/include/geometry_msgs/PoseStamped.h new file mode 100644 index 0000000..6207d60 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSESTAMPED_H +#define C_API_GEOMETRY_MSGS_POSESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Header header; + Pose pose; +} PoseStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h b/src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h new file mode 100644 index 0000000..cc1d7e9 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H +#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Pose pose; + double *covariance; + size_t covariance_count; +} PoseWithCovariance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H diff --git a/src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h b/src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h new file mode 100644 index 0000000..e243833 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H +#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseWithCovariance.h" + +typedef struct +{ + Header header; + PoseWithCovariance pose; +} PoseWithCovarianceStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Quaternion.h b/src/APIs/c_api/include/geometry_msgs/Quaternion.h new file mode 100644 index 0000000..e843fc1 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Quaternion.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_QUATERNION_H +#define C_API_GEOMETRY_MSGS_QUATERNION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double z; + double w; +} Quaternion; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_QUATERNION_H diff --git a/src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h b/src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h new file mode 100644 index 0000000..89f21d1 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H +#define C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Quaternion.h" + +typedef struct +{ + Header header; + Quaternion quaternion; +} QuaternionStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Transform.h b/src/APIs/c_api/include/geometry_msgs/Transform.h new file mode 100644 index 0000000..c318393 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Transform.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TRANSFORM_H +#define C_API_GEOMETRY_MSGS_TRANSFORM_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" +#include "geometry_msgs/Quaternion.h" + +typedef struct +{ + Vector3 translation; + Quaternion rotation; +} Transform; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TRANSFORM_H diff --git a/src/APIs/c_api/include/geometry_msgs/TransformStamped.h b/src/APIs/c_api/include/geometry_msgs/TransformStamped.h new file mode 100644 index 0000000..c983d97 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TransformStamped.h @@ -0,0 +1,24 @@ +#ifndef C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H +#define C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Transform.h" + +typedef struct +{ + Header header; + char *child_frame_id; + Transform transform; +} TransformStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Twist.h b/src/APIs/c_api/include/geometry_msgs/Twist.h new file mode 100644 index 0000000..93cc3df --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Twist.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_TWIST_H +#define C_API_GEOMETRY_MSGS_TWIST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Vector3 linear; + Vector3 angular; +} Twist; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWIST_H diff --git a/src/APIs/c_api/include/geometry_msgs/TwistStamped.h b/src/APIs/c_api/include/geometry_msgs/TwistStamped.h new file mode 100644 index 0000000..4161520 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TwistStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TWISTSTAMPED_H +#define C_API_GEOMETRY_MSGS_TWISTSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Twist.h" + +typedef struct +{ + Header header; + Twist twist; +} TwistStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWISTSTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h b/src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h new file mode 100644 index 0000000..6711fae --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H +#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Twist.h" + +typedef struct +{ + Twist twist; + double *covariance; + size_t covariance_count; +} TwistWithCovariance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H diff --git a/src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h b/src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h new file mode 100644 index 0000000..c70a1ee --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H +#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/TwistWithCovariance.h" + +typedef struct +{ + Header header; + TwistWithCovariance twist; +} TwistWithCovarianceStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Vector3.h b/src/APIs/c_api/include/geometry_msgs/Vector3.h new file mode 100644 index 0000000..65e14e9 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Vector3.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_VECTOR3_H +#define C_API_GEOMETRY_MSGS_VECTOR3_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double z; +} Vector3; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_VECTOR3_H diff --git a/src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h b/src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h new file mode 100644 index 0000000..4f3b729 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H +#define C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Header header; + Vector3 vector; +} Vector3Stamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H diff --git a/src/APIs/c_api/include/geometry_msgs/Wrench.h b/src/APIs/c_api/include/geometry_msgs/Wrench.h new file mode 100644 index 0000000..f8bd8e3 --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/Wrench.h @@ -0,0 +1,22 @@ +#ifndef C_API_GEOMETRY_MSGS_WRENCH_H +#define C_API_GEOMETRY_MSGS_WRENCH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Vector3 force; + Vector3 torque; +} Wrench; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_WRENCH_H diff --git a/src/APIs/c_api/include/geometry_msgs/WrenchStamped.h b/src/APIs/c_api/include/geometry_msgs/WrenchStamped.h new file mode 100644 index 0000000..70bdf1f --- /dev/null +++ b/src/APIs/c_api/include/geometry_msgs/WrenchStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H +#define C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Wrench.h" + +typedef struct +{ + Header header; + Wrench wrench; +} WrenchStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H diff --git a/src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h b/src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h new file mode 100644 index 0000000..61e0947 --- /dev/null +++ b/src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h @@ -0,0 +1,33 @@ +#ifndef C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H +#define C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +/** + * C representation of robot_map_msgs::OccupancyGridUpdate. + * Note: This header is intentionally named *_c.h to avoid + * shadowing the C++ header `robot_map_msgs/OccupancyGridUpdate.h`. + */ +typedef struct +{ + Header header; + int32_t x; + int32_t y; + uint32_t width; + uint32_t height; + int8_t *data; + size_t data_count; +} OccupancyGridUpdate; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H + diff --git a/src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h b/src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h new file mode 100644 index 0000000..6494317 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H +#define C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "nav_2d_msgs/Polygon2D.h" + +typedef struct +{ + Polygon2D outer; + Polygon2D *inner; + size_t inner_count; +} ComplexPolygon2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h new file mode 100644 index 0000000..570a695 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDINFO_H +#define C_API_NAV_2D_MSGS_NAVGRIDINFO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint32_t width; + uint32_t height; + double resolution; + char *frame_id; + double origin_x; + double origin_y; +} NavGridInfo; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDINFO_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h new file mode 100644 index 0000000..00b5d4d --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/NavGridInfo.h" + +typedef struct +{ + Time stamp; + NavGridInfo info; + uint8_t *data; + size_t data_count; +} NavGridOfChars; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h new file mode 100644 index 0000000..2080ab7 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/UIntBounds.h" + +typedef struct +{ + Time stamp; + UIntBounds bounds; + uint8_t *data; + size_t data_count; +} NavGridOfCharsUpdate; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h new file mode 100644 index 0000000..0d46911 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/NavGridInfo.h" + +typedef struct +{ + Time stamp; + NavGridInfo info; + double *data; + size_t data_count; +} NavGridOfDoubles; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h new file mode 100644 index 0000000..6b0facc --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h @@ -0,0 +1,25 @@ +#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H +#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Time.h" +#include "nav_2d_msgs/UIntBounds.h" + +typedef struct +{ + Time stamp; + UIntBounds bounds; + double *data; + size_t data_count; +} NavGridOfDoublesUpdate; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Path2D.h b/src/APIs/c_api/include/nav_2d_msgs/Path2D.h new file mode 100644 index 0000000..57648ba --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Path2D.h @@ -0,0 +1,24 @@ +#ifndef C_API_NAV_2D_MSGS_PATH2D_H +#define C_API_NAV_2D_MSGS_PATH2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_2d_msgs/Pose2DStamped.h" + +typedef struct +{ + Header header; + Pose2DStamped *poses; + size_t poses_count; +} Path2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_PATH2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Point2D.h b/src/APIs/c_api/include/nav_2d_msgs/Point2D.h new file mode 100644 index 0000000..4359592 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Point2D.h @@ -0,0 +1,21 @@ +#ifndef C_API_NAV_2D_MSGS_POINT2D_H +#define C_API_NAV_2D_MSGS_POINT2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; +} Point2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POINT2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h b/src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h new file mode 100644 index 0000000..8cb0602 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_POLYGON2D_H +#define C_API_NAV_2D_MSGS_POLYGON2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "nav_2d_msgs/Point2D.h" + +typedef struct +{ + Point2D *points; + size_t points_count; +} Polygon2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POLYGON2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h new file mode 100644 index 0000000..d4f0e93 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h @@ -0,0 +1,27 @@ +#ifndef C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H +#define C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "std_msgs/ColorRGBA.h" +#include "nav_2d_msgs/ComplexPolygon2D.h" + +typedef struct +{ + Header header; + ComplexPolygon2D *polygons; + size_t polygons_count; + ColorRGBA *colors; + size_t colors_count; +} Polygon2DCollection; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h new file mode 100644 index 0000000..de7e011 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H +#define C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_2d_msgs/Polygon2D.h" + +typedef struct +{ + Header header; + Polygon2D polygon; +} Polygon2DStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h b/src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h new file mode 100644 index 0000000..c4b8107 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_POSE2D32_H +#define C_API_NAV_2D_MSGS_POSE2D32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float x; + float y; + float theta; +} Pose2D32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POSE2D32_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h b/src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h new file mode 100644 index 0000000..6ffbef3 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_POSE2DSTAMPED_H +#define C_API_NAV_2D_MSGS_POSE2DSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Pose2D.h" + +typedef struct +{ + Header header; + Pose2D pose; +} Pose2DStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_POSE2DSTAMPED_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h b/src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h new file mode 100644 index 0000000..69258c4 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGIN_H +#define C_API_NAV_2D_MSGS_SWITCHPLUGIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "nav_2d_msgs/SwitchPluginRequest.h" +#include "nav_2d_msgs/SwitchPluginResponse.h" + +typedef struct +{ + SwitchPluginRequest request; + SwitchPluginResponse response; +} SwitchPlugin; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_SWITCHPLUGIN_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h new file mode 100644 index 0000000..8ac217f --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h @@ -0,0 +1,20 @@ +#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H +#define C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *new_plugin; +} SwitchPluginRequest; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h new file mode 100644 index 0000000..deaace6 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h @@ -0,0 +1,21 @@ +#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H +#define C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint8_t success; + char *message; +} SwitchPluginResponse; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Twist2D.h b/src/APIs/c_api/include/nav_2d_msgs/Twist2D.h new file mode 100644 index 0000000..9125feb --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Twist2D.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_TWIST2D_H +#define C_API_NAV_2D_MSGS_TWIST2D_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + double x; + double y; + double theta; +} Twist2D; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_TWIST2D_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h b/src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h new file mode 100644 index 0000000..e42a448 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h @@ -0,0 +1,22 @@ +#ifndef C_API_NAV_2D_MSGS_TWIST2D32_H +#define C_API_NAV_2D_MSGS_TWIST2D32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float x; + float y; + float theta; +} Twist2D32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_TWIST2D32_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h b/src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h new file mode 100644 index 0000000..a74c638 --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/Twist2DStamped.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_TWIST2DSTAMPED_H +#define C_API_NAV_2D_MSGS_TWIST2DSTAMPED_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_2d_msgs/Twist2D.h" + +typedef struct +{ + Header header; + Twist2D velocity; +} Twist2DStamped; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_TWIST2DSTAMPED_H diff --git a/src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h b/src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h new file mode 100644 index 0000000..ea094fd --- /dev/null +++ b/src/APIs/c_api/include/nav_2d_msgs/UIntBounds.h @@ -0,0 +1,23 @@ +#ifndef C_API_NAV_2D_MSGS_UINTBOUNDS_H +#define C_API_NAV_2D_MSGS_UINTBOUNDS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint32_t min_x; + uint32_t min_y; + uint32_t max_x; + uint32_t max_y; +} UIntBounds; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_NAV_2D_MSGS_UINTBOUNDS_H diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 2fdc4fa..0b4c51b 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -6,154 +6,58 @@ extern "C" { #endif -#include -#include -#include + #include + #include + #include + /* C struct types only - do not include convertor.h here (it pulls C++ code into extern "C") */ + #include "std_msgs/Header.h" + #include "geometry_msgs/Point.h" + #include "geometry_msgs/Pose2D.h" + #include "geometry_msgs/PoseStamped.h" + #include "nav_msgs/OccupancyGrid.h" + #include "nav_msgs/Odometry.h" + #include "map_msgs/OccupancyGridUpdate.h" + #include "nav_2d_msgs/Twist2D.h" + #include "nav_2d_msgs/Twist2DStamped.h" + #include "nav_2d_msgs/Path2D.h" + #include "sensor_msgs/LaserScan.h" + #include "sensor_msgs/PointCloud.h" + #include "sensor_msgs/PointCloud2.h" + #include "geometry_msgs/PolygonStamped.h" - // Forward declarations - typedef void *NavigationHandle; - typedef void *TFListenerHandle; - typedef void *OccupancyGridHandle; - typedef void *OccupancyGridUpdateHandle; - typedef void *LaserScanHandle; - typedef void *PointCloudHandle; - typedef void *PointCloud2Handle; - typedef void *OdometryHandle; - typedef void *Path2DHandle; - typedef void *PolygonStampedHandle; - typedef void *OrderHandle; + typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid; + typedef struct { char *name; LaserScan scan; } NamedLaserScan; + typedef struct { char *name; PointCloud cloud; } NamedPointCloud; + typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2; - // ============================================================================ - // Enums - // ============================================================================ - - /** - * @brief Navigation states, including planning and controller status - */ + typedef struct + { + void *ptr; + } NavigationHandle; + + typedef struct + { + void *ptr; + } TFListenerHandle; + typedef enum { - NAV_STATE_PENDING = 0, - NAV_STATE_ACTIVE = 1, - NAV_STATE_PREEMPTED = 2, - NAV_STATE_SUCCEEDED = 3, - NAV_STATE_ABORTED = 4, - NAV_STATE_REJECTED = 5, - NAV_STATE_PREEMPTING = 6, - NAV_STATE_RECALLING = 7, - NAV_STATE_RECALLED = 8, - NAV_STATE_LOST = 9, - NAV_STATE_PLANNING = 10, - NAV_STATE_CONTROLLING = 11, - NAV_STATE_CLEARING = 12, - NAV_STATE_PAUSED = 13 + PENDING = 0, + ACTIVE = 1, + PREEMPTED = 2, + SUCCEEDED = 3, + ABORTED = 4, + REJECTED = 5, + PREEMPTING = 6, + RECALLING = 7, + RECALLED = 8, + LOST = 9, + PLANNING = 10, + CONTROLLING = 11, + CLEARING = 12, + PAUSED = 13 } NavigationState; - - // ============================================================================ - // Structures - // ============================================================================ - - /** - * @brief Point structure (x, y, z) - */ - typedef struct - { - double x; - double y; - double z; - } Point; - - /** - * @brief Pose2D structure (x, y, theta) - */ - typedef struct - { - double x; - double y; - double theta; - } Pose2D; - - /** - * @brief Twist2D structure (x, y, theta velocities) - */ - typedef struct - { - double x; - double y; - double theta; - } Twist2D; - - /** - * @brief Quaternion structure - */ - typedef struct - { - double x; - double y; - double z; - double w; - } Quaternion; - - /** - * @brief Position structure - */ - typedef struct - { - double x; - double y; - double z; - } Position; - - /** - * @brief Pose structure - */ - typedef struct - { - Position position; - Quaternion orientation; - } Pose; - - /** - * @brief Header structure - */ - typedef struct - { - uint32_t seq; - int64_t sec; - uint32_t nsec; - char *frame_id; - } Header; - - /** - * @brief PoseStamped structure - */ - typedef struct - { - Header header; - Pose pose; - } PoseStamped; - - /** - * @brief Twist2DStamped structure - */ - typedef struct - { - Header header; - Twist2D velocity; - } Twist2DStamped; - - /** - * @brief Vector3 structure - */ - typedef struct - { - double x; - double y; - double z; - } Vector3; - - /** - * @brief Navigation feedback structure - */ + typedef struct { NavigationState navigation_state; @@ -164,201 +68,24 @@ extern "C" } NavFeedback; /** - * @brief Named OccupancyGrid structure - * @note map is an opaque handle to a C++ robot_nav_msgs::OccupancyGrid + * @brief Planner data output structure (C version). + * Mirrors robot::move_base_core::PlannerDataOutput in `move_base_core/navigation.h`. */ typedef struct { - char *name; - OccupancyGridHandle map; - } NamedOccupancyGrid; - - /** - * @brief Named LaserScan structure - * @note scan is an opaque handle to a C++ robot_sensor_msgs::LaserScan - */ - typedef struct - { - char *name; - LaserScanHandle scan; - } NamedLaserScan; - - /** - * @brief Named PointCloud structure - * @note cloud is an opaque handle to a C++ robot_sensor_msgs::PointCloud - */ - typedef struct - { - char *name; - PointCloudHandle cloud; - } NamedPointCloud; - - /** - * @brief Named PointCloud2 structure - * @note cloud is an opaque handle to a C++ robot_sensor_msgs::PointCloud2 - */ - typedef struct - { - char *name; - PointCloud2Handle cloud; - } NamedPointCloud2; - - /** - * @brief Planner data output structure (opaque message handles) - */ - typedef struct - { - Path2DHandle plan; - OccupancyGridHandle costmap; - OccupancyGridUpdateHandle costmap_update; + Path2D plan; + OccupancyGrid costmap; + OccupancyGridUpdate costmap_update; bool is_costmap_updated; - PolygonStampedHandle footprint; + PolygonStamped footprint; } PlannerDataOutput; - // ============================================================================ - // String Management - // ============================================================================ - /** - * @brief Free a string allocated by the library - * @param str String to free + * @brief Free a string allocated by the API (e.g. feed_back_str, navigation_state_to_string result). + * @param str Pointer from strdup; no-op if NULL. */ void nav_c_api_free_string(char *str); - // ============================================================================ - // Complex Message Handle Management - // ============================================================================ - - /** - * @brief Free an occupancy grid handle - * @param handle Occupancy grid handle to free - */ - void navigation_free_occupancy_grid(OccupancyGridHandle handle); - /** - * @brief Free an occupancy grid update handle - * @param handle Occupancy grid update handle to free - */ - void navigation_free_occupancy_grid_update(OccupancyGridUpdateHandle handle); - - /** - * @brief Free a laser scan handle - * @param handle Laser scan handle to free - */ - void navigation_free_laser_scan(LaserScanHandle handle); - - /** - * @brief Free a point cloud handle - * @param handle Point cloud handle to free - */ - void navigation_free_point_cloud(PointCloudHandle handle); - - /** - * @brief Free a point cloud2 handle - * @param handle Point cloud2 handle to free - */ - void navigation_free_point_cloud2(PointCloud2Handle handle); - - /** - * @brief Free an odometry handle - * @param handle Odometry handle to free - */ - void navigation_free_odometry(OdometryHandle handle); - - /** - * @brief Free a path2d handle - * @param handle Path2d handle to free - */ - void navigation_free_path2d(Path2DHandle handle); - - /** - * @brief Free a polygon stamped handle - * @param handle Polygon stamped handle to free - */ - void navigation_free_polygon_stamped(PolygonStampedHandle handle); - - /** - * @brief Free an order handle - * @param handle Order handle to free - */ - void navigation_free_order(OrderHandle handle); - - /** - * @brief Free an array of named occupancy grids - * @param maps Array of named occupancy grids to free - * @param count Number of named occupancy grids in the array - */ - void navigation_free_named_occupancy_grids(NamedOccupancyGrid *maps, size_t count); - - /** - * @brief Free an array of named laser scans - * @param scans Array of named laser scans to free - * @param count Number of named laser scans in the array - */ - void navigation_free_named_laser_scans(NamedLaserScan *scans, size_t count); - - /** - * @brief Free an array of named point clouds - * @param clouds Array of named point clouds to free - * @param count Number of named point clouds in the array - */ - void navigation_free_named_point_clouds(NamedPointCloud *clouds, size_t count); - - /** - * @brief Free an array of named point cloud2s - * @param clouds Array of named point cloud2s to free - * @param count Number of named point cloud2s in the array - */ - void navigation_free_named_point_cloud2s(NamedPointCloud2 *clouds, size_t count); - - /** - * @brief Free a planner data output - * @param data Planner data output to free - */ - void navigation_free_planner_data(PlannerDataOutput *data); - - // ============================================================================ - // State Conversion - // ============================================================================ - - /** - * @brief Convert a State enum to its string representation - * @param state Enum value of NavigationState - * @return String representation (caller must free with nav_c_api_free_string) - */ - char *navigation_state_to_string(NavigationState state); - - // ============================================================================ - // Helper Functions - // ============================================================================ - - /** - * @brief Creates a target pose by offsetting a given 2D pose along its heading direction - * @param pose_x X coordinate of the original pose - * @param pose_y Y coordinate of the original pose - * @param pose_theta Heading angle in radians - * @param frame_id The coordinate frame ID (null-terminated string) - * @param offset_distance Distance to offset along heading (positive = forward, negative = backward) - * @param out_goal Output parameter for the offset pose - * @return true on success, false on failure - */ - bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta, - const char *frame_id, double offset_distance, - PoseStamped *out_goal); - - /** - * @brief Creates an offset target pose from a given PoseStamped - * @param in_pose Input pose - * @param offset_distance Distance to offset along heading direction - * @param out_goal Output parameter for the offset pose - * @return true on success, false on failure - */ - bool navigation_offset_goal_stamped(const PoseStamped *in_pose, double offset_distance, - PoseStamped *out_goal); - - // ============================================================================ - // Navigation Handle Management - // ============================================================================ - /** * @brief Create a new navigation instance * @return Navigation handle, or NULL on failure @@ -371,10 +98,6 @@ extern "C" */ void navigation_destroy(NavigationHandle handle); - // ============================================================================ - // TF Listener Management - // ============================================================================ - /** * @brief Create a TF listener instance * @return TF listener handle, or NULL on failure @@ -439,13 +162,8 @@ extern "C" * @param out_count Output number of points in the array * @return true on success, false on failure */ - bool navigation_get_robot_footprint(NavigationHandle handle, Point **out_points, size_t *out_count); + bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count); - /** - * @brief Free a points array allocated by navigation_get_robot_footprint - * @param points Pointer to point array - */ - void navigation_free_points(Point *points); /** * @brief Send a goal for the robot to navigate to @@ -455,21 +173,21 @@ extern "C" * @param yaw_goal_tolerance Acceptable angular error (radians) * @return true if goal was accepted and sent successfully */ - bool navigation_move_to(NavigationHandle handle, const PoseStamped *goal, + bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - /** - * @brief Send a goal for the robot to navigate to - * @param handle Navigation handle - * @param order Order message - * @param goal Target pose in the global frame - * @param xy_goal_tolerance Acceptable error in X/Y (meters) - * @param yaw_goal_tolerance Acceptable angular error (radians) - * @return true if goal was accepted and sent successfully - */ - bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, - const PoseStamped *goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + // /** + // * @brief Send a goal for the robot to navigate to + // * @param handle Navigation handle + // * @param order Order message + // * @param goal Target pose in the global frame + // * @param xy_goal_tolerance Acceptable error in X/Y (meters) + // * @param yaw_goal_tolerance Acceptable angular error (radians) + // * @return true if goal was accepted and sent successfully + // */ + // bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, + // const PoseStamped &goal, + // double xy_goal_tolerance, double yaw_goal_tolerance); /** * @brief Send a docking goal to a predefined marker @@ -481,21 +199,21 @@ extern "C" * @return true if docking command succeeded */ bool navigation_dock_to(NavigationHandle handle, const char *marker, - const PoseStamped *goal, + const PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance); - /** - * @brief Send a docking goal to a predefined marker - * @param handle Navigation handle - * @param order Order message - * @param goal Target pose for docking - * @param xy_goal_tolerance Acceptable XY error (meters) - * @param yaw_goal_tolerance Acceptable heading error (radians) - * @return true if docking command succeeded - */ - bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, - const PoseStamped *goal, - double xy_goal_tolerance, double yaw_goal_tolerance); + // /** + // * @brief Send a docking goal to a predefined marker + // * @param handle Navigation handle + // * @param order Order message + // * @param goal Target pose for docking + // * @param xy_goal_tolerance Acceptable XY error (meters) + // * @param yaw_goal_tolerance Acceptable heading error (radians) + // * @return true if docking command succeeded + // */ + // bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, + // const PoseStamped &goal, + // double xy_goal_tolerance, double yaw_goal_tolerance); /** * @brief Move straight toward the target position @@ -504,7 +222,7 @@ extern "C" * @param xy_goal_tolerance Acceptable positional error (meters) * @return true if command issued successfully */ - bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped *goal, + bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal, double xy_goal_tolerance); /** @@ -514,7 +232,7 @@ extern "C" * @param yaw_goal_tolerance Acceptable angular error (radians) * @return true if rotation command was sent successfully */ - bool navigation_rotate_to(NavigationHandle handle, const PoseStamped *goal, + bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, double yaw_goal_tolerance); /** @@ -563,7 +281,7 @@ extern "C" * @param out_pose Output parameter with the robot's current pose * @return true if pose was successfully retrieved */ - bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped *out_pose); + bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose); /** * @brief Get the robot's pose as a 2D pose @@ -571,7 +289,7 @@ extern "C" * @param out_pose Output parameter with the robot's current 2D pose * @return true if pose was successfully retrieved */ - bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D *out_pose); + bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose); /** * @brief Get the robot's current twist @@ -580,7 +298,7 @@ extern "C" * @return true if twist was successfully retrieved * @note out_twist->header.frame_id must be freed using nav_c_api_free_string */ - bool navigation_get_twist(NavigationHandle handle, Twist2DStamped *out_twist); + bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist); /** * @brief Get navigation feedback @@ -589,13 +307,7 @@ extern "C" * @return true if feedback was successfully retrieved * @note The feed_back_str field must be freed using nav_c_api_free_string */ - bool navigation_get_feedback(NavigationHandle handle, NavFeedback *out_feedback); - - /** - * @brief Free navigation feedback structure - * @param feedback Feedback structure to free - */ - void navigation_free_feedback(NavFeedback *feedback); + bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback); /** * @brief Add a static map to the navigation system @@ -604,7 +316,7 @@ extern "C" * @param map Occupancy grid handle * @return true if the map was added successfully */ - bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGridHandle map); + bool navigation_add_static_map(NavigationHandle handle, const char *map_name, const OccupancyGrid occupancy_grid); /** * @brief Add a laser scan to the navigation system @@ -613,7 +325,7 @@ extern "C" * @param laser_scan Laser scan handle * @return true if the laser scan was added successfully */ - bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScanHandle laser_scan); + bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, const LaserScan laser_scan); /** * @brief Add a point cloud to the navigation system @@ -622,7 +334,7 @@ extern "C" * @param point_cloud Point cloud handle * @return true if the point cloud was added successfully */ - bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloudHandle point_cloud); + bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud); /** * @brief Add a point cloud2 to the navigation system @@ -631,7 +343,7 @@ extern "C" * @param point_cloud2 Point cloud2 handle * @return true if the point cloud2 was added successfully */ - bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2Handle point_cloud2); + bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2); /** * @brief Add an odometry to the navigation system @@ -640,7 +352,7 @@ extern "C" * @param odometry Odometry handle * @return true if the odometry was added successfully */ - bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, OdometryHandle odometry); + bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry); /** * @brief Get a static map from the navigation system @@ -649,7 +361,7 @@ extern "C" * @param out_map Output parameter for the map handle * @return true if the map was retrieved successfully */ - bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGridHandle *out_map); + bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map); /** * @brief Get a laser scan from the navigation system @@ -658,7 +370,7 @@ extern "C" * @param out_scan Output parameter for the laser scan handle * @return true if the laser scan was retrieved successfully */ - bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScanHandle *out_scan); + bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan); /** * @brief Get a point cloud from the navigation system @@ -667,7 +379,7 @@ extern "C" * @param out_cloud Output parameter for the point cloud handle * @return true if the point cloud was retrieved successfully */ - bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloudHandle *out_cloud); + bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud); /** * @brief Get a point cloud2 from the navigation system @@ -676,7 +388,7 @@ extern "C" * @param out_cloud Output parameter for the point cloud2 handle * @return true if the point cloud2 was retrieved successfully */ - bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2Handle *out_cloud); + bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud); /** * @brief Get all static maps from the navigation system @@ -685,7 +397,7 @@ extern "C" * @param out_count Output parameter for the number of maps * @return true if the maps were retrieved successfully */ - bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid **out_maps, size_t *out_count); + bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count); /** * @brief Get all laser scans from the navigation system @@ -694,7 +406,7 @@ extern "C" * @param out_count Output parameter for the number of scans * @return true if the scans were retrieved successfully */ - bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan **out_scans, size_t *out_count); + bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count); /** * @brief Get all point clouds from the navigation system @@ -703,7 +415,7 @@ extern "C" * @param out_count Output parameter for the number of clouds * @return true if the clouds were retrieved successfully */ - bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud **out_clouds, size_t *out_count); + bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count); /** * @brief Get all point cloud2s from the navigation system @@ -712,7 +424,7 @@ extern "C" * @param out_count Output parameter for the number of clouds * @return true if the clouds were retrieved successfully */ - bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 **out_clouds, size_t *out_count); + bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count); /** * @brief Remove a static map from the navigation system diff --git a/src/APIs/c_api/include/nav_msgs/ActionTypes.h b/src/APIs/c_api/include/nav_msgs/ActionTypes.h new file mode 100644 index 0000000..325800b --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/ActionTypes.h @@ -0,0 +1,29 @@ + #ifndef C_API_NAV_MSGS_ACTIONTYPES_H + #define C_API_NAV_MSGS_ACTIONTYPES_H + + #ifdef __cplusplus + extern "C" { + #endif + + #include + #include + #include "std_msgs/Time.h" + + typedef struct + { + Time stamp; + char *id; + } GoalID; + + typedef struct + { + GoalID goal_id; + uint8_t status; + char *text; + } GoalStatus; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_ACTIONTYPES_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMap.h b/src/APIs/c_api/include/nav_msgs/GetMap.h new file mode 100644 index 0000000..dc9fc25 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMap.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_GETMAP_H + #define C_API_NAV_MSGS_GETMAP_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/GetMapRequest.h" +#include "nav_msgs/GetMapResponse.h" + +typedef struct +{ + GetMapRequest request; + GetMapResponse response; +} GetMap; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAP_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapAction.h b/src/APIs/c_api/include/nav_msgs/GetMapAction.h new file mode 100644 index 0000000..8a7321e --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapAction.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTION_H + #define C_API_NAV_MSGS_GETMAPACTION_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/GetMapActionGoal.h" +#include "nav_msgs/GetMapActionResult.h" +#include "nav_msgs/GetMapActionFeedback.h" + +typedef struct +{ + GetMapActionGoal action_goal; + GetMapActionResult action_result; + GetMapActionFeedback action_feedback; +} GetMapAction; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTION_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h b/src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h new file mode 100644 index 0000000..cfb8091 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapActionFeedback.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H + #define C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/ActionTypes.h" +#include "nav_msgs/GetMapFeedback.h" + +typedef struct +{ + Header header; + GoalStatus status; + GetMapFeedback feedback; +} GetMapActionFeedback; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTIONFEEDBACK_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h b/src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h new file mode 100644 index 0000000..27e6926 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapActionGoal.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTIONGOAL_H + #define C_API_NAV_MSGS_GETMAPACTIONGOAL_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/ActionTypes.h" +#include "nav_msgs/GetMapGoal.h" + +typedef struct +{ + Header header; + GoalID goal_id; + GetMapGoal goal; +} GetMapActionGoal; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTIONGOAL_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapActionResult.h b/src/APIs/c_api/include/nav_msgs/GetMapActionResult.h new file mode 100644 index 0000000..6068a0d --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapActionResult.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_GETMAPACTIONRESULT_H + #define C_API_NAV_MSGS_GETMAPACTIONRESULT_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/ActionTypes.h" +#include "nav_msgs/GetMapResult.h" + +typedef struct +{ + Header header; + GoalStatus status; + GetMapResult result; +} GetMapActionResult; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPACTIONRESULT_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapFeedback.h b/src/APIs/c_api/include/nav_msgs/GetMapFeedback.h new file mode 100644 index 0000000..818d346 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapFeedback.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_GETMAPFEEDBACK_H + #define C_API_NAV_MSGS_GETMAPFEEDBACK_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + uint8_t _dummy; +} GetMapFeedback; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPFEEDBACK_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapGoal.h b/src/APIs/c_api/include/nav_msgs/GetMapGoal.h new file mode 100644 index 0000000..4af2dc8 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapGoal.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_GETMAPGOAL_H + #define C_API_NAV_MSGS_GETMAPGOAL_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + uint8_t _dummy; +} GetMapGoal; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPGOAL_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapRequest.h b/src/APIs/c_api/include/nav_msgs/GetMapRequest.h new file mode 100644 index 0000000..cc53cf1 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapRequest.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_GETMAPREQUEST_H + #define C_API_NAV_MSGS_GETMAPREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + uint8_t _dummy; +} GetMapRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapResponse.h b/src/APIs/c_api/include/nav_msgs/GetMapResponse.h new file mode 100644 index 0000000..e00c11f --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapResponse.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_GETMAPRESPONSE_H + #define C_API_NAV_MSGS_GETMAPRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/OccupancyGrid.h" + +typedef struct +{ + OccupancyGrid map; +} GetMapResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPRESPONSE_H diff --git a/src/APIs/c_api/include/nav_msgs/GetMapResult.h b/src/APIs/c_api/include/nav_msgs/GetMapResult.h new file mode 100644 index 0000000..011ff51 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetMapResult.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_GETMAPRESULT_H + #define C_API_NAV_MSGS_GETMAPRESULT_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/OccupancyGrid.h" + +typedef struct +{ + OccupancyGrid map; +} GetMapResult; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETMAPRESULT_H diff --git a/src/APIs/c_api/include/nav_msgs/GetPlan.h b/src/APIs/c_api/include/nav_msgs/GetPlan.h new file mode 100644 index 0000000..a1830c4 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetPlan.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_GETPLAN_H + #define C_API_NAV_MSGS_GETPLAN_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/GetPlanRequest.h" +#include "nav_msgs/GetPlanResponse.h" + +typedef struct +{ + GetPlanRequest request; + GetPlanResponse response; +} GetPlan; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETPLAN_H diff --git a/src/APIs/c_api/include/nav_msgs/GetPlanRequest.h b/src/APIs/c_api/include/nav_msgs/GetPlanRequest.h new file mode 100644 index 0000000..0ba5658 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetPlanRequest.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_GETPLANREQUEST_H + #define C_API_NAV_MSGS_GETPLANREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "geometry_msgs/PoseStamped.h" + +typedef struct +{ + PoseStamped start; + PoseStamped goal; + float tolerance; +} GetPlanRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETPLANREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/GetPlanResponse.h b/src/APIs/c_api/include/nav_msgs/GetPlanResponse.h new file mode 100644 index 0000000..2215632 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GetPlanResponse.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_GETPLANRESPONSE_H + #define C_API_NAV_MSGS_GETPLANRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/Path.h" + +typedef struct +{ + Path plan; +} GetPlanResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GETPLANRESPONSE_H diff --git a/src/APIs/c_api/include/nav_msgs/GridCells.h b/src/APIs/c_api/include/nav_msgs/GridCells.h new file mode 100644 index 0000000..ed7f331 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/GridCells.h @@ -0,0 +1,26 @@ + #ifndef C_API_NAV_MSGS_GRIDCELLS_H + #define C_API_NAV_MSGS_GRIDCELLS_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point.h" + +typedef struct +{ + Header header; + float cell_width; + float cell_height; + Point *cells; + size_t cells_count; +} GridCells; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_GRIDCELLS_H diff --git a/src/APIs/c_api/include/nav_msgs/LoadMap.h b/src/APIs/c_api/include/nav_msgs/LoadMap.h new file mode 100644 index 0000000..8cb1b90 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/LoadMap.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_LOADMAP_H + #define C_API_NAV_MSGS_LOADMAP_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/LoadMapRequest.h" +#include "nav_msgs/LoadMapResponse.h" + +typedef struct +{ + LoadMapRequest request; + LoadMapResponse response; +} LoadMap; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_LOADMAP_H diff --git a/src/APIs/c_api/include/nav_msgs/LoadMapRequest.h b/src/APIs/c_api/include/nav_msgs/LoadMapRequest.h new file mode 100644 index 0000000..f3735c8 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/LoadMapRequest.h @@ -0,0 +1,20 @@ + #ifndef C_API_NAV_MSGS_LOADMAPREQUEST_H + #define C_API_NAV_MSGS_LOADMAPREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include + +typedef struct +{ + char *map_url; +} LoadMapRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_LOADMAPREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/LoadMapResponse.h b/src/APIs/c_api/include/nav_msgs/LoadMapResponse.h new file mode 100644 index 0000000..699a780 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/LoadMapResponse.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_LOADMAPRESPONSE_H + #define C_API_NAV_MSGS_LOADMAPRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include +#include "nav_msgs/OccupancyGrid.h" + +typedef struct +{ + OccupancyGrid map; + bool result; +} LoadMapResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_LOADMAPRESPONSE_H diff --git a/src/APIs/c_api/include/nav_msgs/MapMetaData.h b/src/APIs/c_api/include/nav_msgs/MapMetaData.h new file mode 100644 index 0000000..90ba28a --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/MapMetaData.h @@ -0,0 +1,26 @@ + #ifndef C_API_NAV_MSGS_MAPMETADATA_H + #define C_API_NAV_MSGS_MAPMETADATA_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Time.h" +#include "geometry_msgs/Pose.h" + +typedef struct +{ + Time map_load_time; + float resolution; + uint32_t width; + uint32_t height; + Pose origin; +} MapMetaData; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_MAPMETADATA_H diff --git a/src/APIs/c_api/include/nav_msgs/OccupancyGrid.h b/src/APIs/c_api/include/nav_msgs/OccupancyGrid.h new file mode 100644 index 0000000..eae5454 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/OccupancyGrid.h @@ -0,0 +1,25 @@ + #ifndef C_API_NAV_MSGS_OCCUPANCYGRID_H + #define C_API_NAV_MSGS_OCCUPANCYGRID_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "nav_msgs/MapMetaData.h" + +typedef struct +{ + Header header; + MapMetaData info; + int8_t *data; + size_t data_count; +} OccupancyGrid; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_OCCUPANCYGRID_H diff --git a/src/APIs/c_api/include/nav_msgs/Odometry.h b/src/APIs/c_api/include/nav_msgs/Odometry.h new file mode 100644 index 0000000..972a1d9 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/Odometry.h @@ -0,0 +1,35 @@ + #ifndef C_API_NAV_MSGS_ODOMETRY_H + #define C_API_NAV_MSGS_ODOMETRY_H + + #ifdef __cplusplus + extern "C" { + #endif + + #include + #include + #include "std_msgs/Header.h" + #include "geometry_msgs/PoseWithCovariance.h" + #include "geometry_msgs/TwistWithCovariance.h" + + typedef struct + { + Header header; + char *child_frame_id; + PoseWithCovariance pose; + TwistWithCovariance twist; + } Odometry; + + + extern "C" Odometry odometry_create(void); + + extern "C" Odometry odometry_set_data( + Header header, + char *child_frame_id, + PoseWithCovariance pose, + TwistWithCovariance twist); + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_ODOMETRY_H diff --git a/src/APIs/c_api/include/nav_msgs/Path.h b/src/APIs/c_api/include/nav_msgs/Path.h new file mode 100644 index 0000000..53b6a45 --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/Path.h @@ -0,0 +1,24 @@ + #ifndef C_API_NAV_MSGS_PATH_H + #define C_API_NAV_MSGS_PATH_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/PoseStamped.h" + +typedef struct +{ + Header header; + PoseStamped *poses; + size_t poses_count; +} Path; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_PATH_H diff --git a/src/APIs/c_api/include/nav_msgs/SetMap.h b/src/APIs/c_api/include/nav_msgs/SetMap.h new file mode 100644 index 0000000..aa76a7e --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/SetMap.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_SETMAP_H + #define C_API_NAV_MSGS_SETMAP_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/SetMapRequest.h" +#include "nav_msgs/SetMapResponse.h" + +typedef struct +{ + SetMapRequest request; + SetMapResponse response; +} SetMap; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_SETMAP_H diff --git a/src/APIs/c_api/include/nav_msgs/SetMapRequest.h b/src/APIs/c_api/include/nav_msgs/SetMapRequest.h new file mode 100644 index 0000000..284372c --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/SetMapRequest.h @@ -0,0 +1,23 @@ + #ifndef C_API_NAV_MSGS_SETMAPREQUEST_H + #define C_API_NAV_MSGS_SETMAPREQUEST_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include "nav_msgs/OccupancyGrid.h" +#include "geometry_msgs/PoseWithCovariance.h" + +typedef struct +{ + OccupancyGrid map; + PoseWithCovariance initial_pose; +} SetMapRequest; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_SETMAPREQUEST_H diff --git a/src/APIs/c_api/include/nav_msgs/SetMapResponse.h b/src/APIs/c_api/include/nav_msgs/SetMapResponse.h new file mode 100644 index 0000000..b5ac1ed --- /dev/null +++ b/src/APIs/c_api/include/nav_msgs/SetMapResponse.h @@ -0,0 +1,21 @@ + #ifndef C_API_NAV_MSGS_SETMAPRESPONSE_H + #define C_API_NAV_MSGS_SETMAPRESPONSE_H + + #ifdef __cplusplus + extern "C" { + #endif + +#include +#include +#include + +typedef struct +{ + bool success; +} SetMapResponse; + + #ifdef __cplusplus + } + #endif + + #endif // C_API_NAV_MSGS_SETMAPRESPONSE_H diff --git a/src/APIs/c_api/include/sensor_msgs/BatteryState.h b/src/APIs/c_api/include/sensor_msgs/BatteryState.h new file mode 100644 index 0000000..258b05a --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/BatteryState.h @@ -0,0 +1,38 @@ +#ifndef C_API_SENSOR_MSGS_BATTERYSTATE_H +#define C_API_SENSOR_MSGS_BATTERYSTATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + float voltage; + float current; + float charge; + float capacity; + float design_capacity; + float percentage; + uint8_t power_supply_status; + uint8_t power_supply_health; + uint8_t power_supply_technology; + bool present; + float *cell_voltage; + size_t cell_voltage_count; + float *cell_temperature; + size_t cell_temperature_count; + char *location; + char *serial_number; +} BatteryState; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_BATTERYSTATE_H diff --git a/src/APIs/c_api/include/sensor_msgs/CameraInfo.h b/src/APIs/c_api/include/sensor_msgs/CameraInfo.h new file mode 100644 index 0000000..621e55d --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/CameraInfo.h @@ -0,0 +1,33 @@ +#ifndef C_API_SENSOR_MSGS_CAMERAINFO_H +#define C_API_SENSOR_MSGS_CAMERAINFO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "sensor_msgs/RegionOfInterest.h" + +typedef struct +{ + Header header; + uint32_t height; + uint32_t width; + char *distortion_model; + double *D; + size_t D_count; + double K[9]; + double R[9]; + double P[12]; + uint32_t binning_x; + uint32_t binning_y; + RegionOfInterest roi; +} CameraInfo; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_CAMERAINFO_H diff --git a/src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h b/src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h new file mode 100644 index 0000000..8a8a782 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/ChannelFloat32.h @@ -0,0 +1,22 @@ +#ifndef C_API_SENSOR_MSGS_CHANNELFLOAT32_H +#define C_API_SENSOR_MSGS_CHANNELFLOAT32_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *name; + float *values; + size_t values_count; +} ChannelFloat32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_CHANNELFLOAT32_H diff --git a/src/APIs/c_api/include/sensor_msgs/CompressedImage.h b/src/APIs/c_api/include/sensor_msgs/CompressedImage.h new file mode 100644 index 0000000..897cdff --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/CompressedImage.h @@ -0,0 +1,24 @@ +#ifndef C_API_SENSOR_MSGS_COMPRESSEDIMAGE_H +#define C_API_SENSOR_MSGS_COMPRESSEDIMAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + char *format; + uint8_t *data; + size_t data_count; +} CompressedImage; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_COMPRESSEDIMAGE_H diff --git a/src/APIs/c_api/include/sensor_msgs/FluidPressure.h b/src/APIs/c_api/include/sensor_msgs/FluidPressure.h new file mode 100644 index 0000000..bbf8d20 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/FluidPressure.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_FLUIDPRESSURE_H +#define C_API_SENSOR_MSGS_FLUIDPRESSURE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double fluid_pressure; + double variance; +} FluidPressure; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_FLUIDPRESSURE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Illuminance.h b/src/APIs/c_api/include/sensor_msgs/Illuminance.h new file mode 100644 index 0000000..f77f525 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Illuminance.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_ILLUMINANCE_H +#define C_API_SENSOR_MSGS_ILLUMINANCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double illuminance; + double variance; +} Illuminance; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_ILLUMINANCE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Image.h b/src/APIs/c_api/include/sensor_msgs/Image.h new file mode 100644 index 0000000..e3f1bc5 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Image.h @@ -0,0 +1,28 @@ +#ifndef C_API_SENSOR_MSGS_IMAGE_H +#define C_API_SENSOR_MSGS_IMAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + uint32_t height; + uint32_t width; + char *encoding; + uint8_t is_bigendian; + uint32_t step; + uint8_t *data; + size_t data_count; +} Image; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_IMAGE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Imu.h b/src/APIs/c_api/include/sensor_msgs/Imu.h new file mode 100644 index 0000000..78f6a41 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Imu.h @@ -0,0 +1,29 @@ +#ifndef C_API_SENSOR_MSGS_IMU_H +#define C_API_SENSOR_MSGS_IMU_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Vector3.h" + +typedef struct +{ + Header header; + Quaternion orientation; + double orientation_covariance[9]; + Vector3 angular_velocity; + double angular_velocity_covariance[9]; + Vector3 linear_acceleration; + double linear_acceleration_covariance[9]; +} Imu; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_IMU_H diff --git a/src/APIs/c_api/include/sensor_msgs/JointState.h b/src/APIs/c_api/include/sensor_msgs/JointState.h new file mode 100644 index 0000000..0007c17 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/JointState.h @@ -0,0 +1,29 @@ +#ifndef C_API_SENSOR_MSGS_JOINTSTATE_H +#define C_API_SENSOR_MSGS_JOINTSTATE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + char **name; + size_t name_count; + double *position; + size_t position_count; + double *velocity; + size_t velocity_count; + double *effort; + size_t effort_count; +} JointState; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOINTSTATE_H diff --git a/src/APIs/c_api/include/sensor_msgs/Joy.h b/src/APIs/c_api/include/sensor_msgs/Joy.h new file mode 100644 index 0000000..d2cf043 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Joy.h @@ -0,0 +1,25 @@ +#ifndef C_API_SENSOR_MSGS_JOY_H +#define C_API_SENSOR_MSGS_JOY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + float *axes; + size_t axes_count; + int32_t *buttons; + size_t buttons_count; +} Joy; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOY_H diff --git a/src/APIs/c_api/include/sensor_msgs/JoyFeedback.h b/src/APIs/c_api/include/sensor_msgs/JoyFeedback.h new file mode 100644 index 0000000..717081c --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/JoyFeedback.h @@ -0,0 +1,22 @@ +#ifndef C_API_SENSOR_MSGS_JOYFEEDBACK_H +#define C_API_SENSOR_MSGS_JOYFEEDBACK_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + uint8_t type; + uint8_t id; + float intensity; +} JoyFeedback; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOYFEEDBACK_H diff --git a/src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h b/src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h new file mode 100644 index 0000000..6435943 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/JoyFeedbackArray.h @@ -0,0 +1,22 @@ +#ifndef C_API_SENSOR_MSGS_JOYFEEDBACKARRAY_H +#define C_API_SENSOR_MSGS_JOYFEEDBACKARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "sensor_msgs/JoyFeedback.h" + +typedef struct +{ + JoyFeedback *array; + size_t array_count; +} JoyFeedbackArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_JOYFEEDBACKARRAY_H diff --git a/src/APIs/c_api/include/sensor_msgs/LaserEcho.h b/src/APIs/c_api/include/sensor_msgs/LaserEcho.h new file mode 100644 index 0000000..35ce456 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/LaserEcho.h @@ -0,0 +1,21 @@ +#ifndef C_API_SENSOR_MSGS_LASERECHO_H +#define C_API_SENSOR_MSGS_LASERECHO_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + float *echoes; + size_t echoes_count; +} LaserEcho; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_LASERECHO_H diff --git a/src/APIs/c_api/include/sensor_msgs/LaserScan.h b/src/APIs/c_api/include/sensor_msgs/LaserScan.h new file mode 100644 index 0000000..71613b1 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/LaserScan.h @@ -0,0 +1,32 @@ +#ifndef C_API_SENSOR_MSGS_LASER_SCAN_H +#define C_API_SENSOR_MSGS_LASER_SCAN_H + +#ifdef __cplusplus +extern "C" +{ +#endif +#include +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + float angle_min; + float angle_max; + float angle_increment; + float time_increment; + float scan_time; + float range_min; + float range_max; + float *ranges; + size_t ranges_count; + float *intensities; + size_t intensities_count; +} LaserScan; + +#ifdef __cplusplus +} +#endif +#endif // C_API_SENSOR_MSGS_LASER_SCAN_H \ No newline at end of file diff --git a/src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h b/src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h new file mode 100644 index 0000000..774e93e --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/MultiEchoLaserScan.h @@ -0,0 +1,33 @@ +#ifndef C_API_SENSOR_MSGS_MULTIECHOLASERSCAN_H +#define C_API_SENSOR_MSGS_MULTIECHOLASERSCAN_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "sensor_msgs/LaserEcho.h" + +typedef struct +{ + Header header; + float angle_min; + float angle_max; + float angle_increment; + float time_increment; + float scan_time; + float range_min; + float range_max; + LaserEcho *ranges; + size_t ranges_count; + LaserEcho *intensities; + size_t intensities_count; +} MultiEchoLaserScan; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_MULTIECHOLASERSCAN_H diff --git a/src/APIs/c_api/include/sensor_msgs/PointCloud.h b/src/APIs/c_api/include/sensor_msgs/PointCloud.h new file mode 100644 index 0000000..1c2a8d0 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/PointCloud.h @@ -0,0 +1,27 @@ +#ifndef C_API_SENSOR_MSGS_POINTCLOUD_H +#define C_API_SENSOR_MSGS_POINTCLOUD_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "geometry_msgs/Point32.h" +#include "sensor_msgs/ChannelFloat32.h" + +typedef struct +{ + Header header; + Point32 *points; + size_t points_count; + ChannelFloat32 *channels; + size_t channels_count; +} PointCloud; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_POINTCLOUD_H diff --git a/src/APIs/c_api/include/sensor_msgs/PointCloud2.h b/src/APIs/c_api/include/sensor_msgs/PointCloud2.h new file mode 100644 index 0000000..f275eae --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/PointCloud2.h @@ -0,0 +1,33 @@ +#ifndef C_API_SENSOR_MSGS_POINTCLOUD2_H +#define C_API_SENSOR_MSGS_POINTCLOUD2_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include "std_msgs/Header.h" +#include "sensor_msgs/PointField.h" + +typedef struct +{ + Header header; + uint32_t height; + uint32_t width; + PointField *fields; + size_t fields_count; + bool is_bigendian; + uint32_t point_step; + uint32_t row_step; + uint8_t *data; + size_t data_count; + bool is_dense; +} PointCloud2; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_POINTCLOUD2_H diff --git a/src/APIs/c_api/include/sensor_msgs/PointField.h b/src/APIs/c_api/include/sensor_msgs/PointField.h new file mode 100644 index 0000000..569c8ac --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/PointField.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_POINTFIELD_H +#define C_API_SENSOR_MSGS_POINTFIELD_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +typedef struct +{ + char *name; + uint32_t offset; + uint8_t datatype; + uint32_t count; +} PointField; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_POINTFIELD_H diff --git a/src/APIs/c_api/include/sensor_msgs/Range.h b/src/APIs/c_api/include/sensor_msgs/Range.h new file mode 100644 index 0000000..bfbaa6d --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Range.h @@ -0,0 +1,26 @@ +#ifndef C_API_SENSOR_MSGS_RANGE_H +#define C_API_SENSOR_MSGS_RANGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + uint8_t radiation_type; + float field_of_view; + float min_range; + float max_range; + float range; +} Range; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_RANGE_H diff --git a/src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h b/src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h new file mode 100644 index 0000000..62cdd67 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/RegionOfInterest.h @@ -0,0 +1,25 @@ +#ifndef C_API_SENSOR_MSGS_REGIONOFINTEREST_H +#define C_API_SENSOR_MSGS_REGIONOFINTEREST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +typedef struct +{ + uint32_t x_offset; + uint32_t y_offset; + uint32_t height; + uint32_t width; + bool do_rectify; +} RegionOfInterest; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_REGIONOFINTEREST_H diff --git a/src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h b/src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h new file mode 100644 index 0000000..7892098 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/RelativeHumidity.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_RELATIVEHUMIDITY_H +#define C_API_SENSOR_MSGS_RELATIVEHUMIDITY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double relative_humidity; + double variance; +} RelativeHumidity; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_RELATIVEHUMIDITY_H diff --git a/src/APIs/c_api/include/sensor_msgs/Temperature.h b/src/APIs/c_api/include/sensor_msgs/Temperature.h new file mode 100644 index 0000000..d1120d2 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/Temperature.h @@ -0,0 +1,23 @@ +#ifndef C_API_SENSOR_MSGS_TEMPERATURE_H +#define C_API_SENSOR_MSGS_TEMPERATURE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" + +typedef struct +{ + Header header; + double temperature; + double variance; +} Temperature; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_TEMPERATURE_H diff --git a/src/APIs/c_api/include/sensor_msgs/TimeReference.h b/src/APIs/c_api/include/sensor_msgs/TimeReference.h new file mode 100644 index 0000000..0ca62a0 --- /dev/null +++ b/src/APIs/c_api/include/sensor_msgs/TimeReference.h @@ -0,0 +1,24 @@ +#ifndef C_API_SENSOR_MSGS_TIMEREFERENCE_H +#define C_API_SENSOR_MSGS_TIMEREFERENCE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "std_msgs/Header.h" +#include "std_msgs/Time.h" + +typedef struct +{ + Header header; + Time time_ref; + char *source; +} TimeReference; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_SENSOR_MSGS_TIMEREFERENCE_H diff --git a/src/APIs/c_api/include/std_msgs/Bool.h b/src/APIs/c_api/include/std_msgs/Bool.h new file mode 100644 index 0000000..9a0d4dd --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Bool.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_BOOL_H +#define C_API_STD_MSGS_BOOL_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t data; +} Bool; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_BOOL_H diff --git a/src/APIs/c_api/include/std_msgs/Byte.h b/src/APIs/c_api/include/std_msgs/Byte.h new file mode 100644 index 0000000..b7554b1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Byte.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_BYTE_H +#define C_API_STD_MSGS_BYTE_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int8_t data; +} Byte; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_BYTE_H diff --git a/src/APIs/c_api/include/std_msgs/ByteMultiArray.h b/src/APIs/c_api/include/std_msgs/ByteMultiArray.h new file mode 100644 index 0000000..7ed0c51 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/ByteMultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_BYTEMULTIARRAY_H +#define C_API_STD_MSGS_BYTEMULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int8_t *data; + size_t data_count; +} ByteMultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_BYTEMULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Char.h b/src/APIs/c_api/include/std_msgs/Char.h new file mode 100644 index 0000000..8b4ba99 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Char.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_CHAR_H +#define C_API_STD_MSGS_CHAR_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t data; +} Char; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_CHAR_H diff --git a/src/APIs/c_api/include/std_msgs/ColorRGBA.h b/src/APIs/c_api/include/std_msgs/ColorRGBA.h new file mode 100644 index 0000000..561ae9a --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/ColorRGBA.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_COLORRGBA_H +#define C_API_STD_MSGS_COLORRGBA_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + float r; + float g; + float b; + float a; +} ColorRGBA; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_COLORRGBA_H diff --git a/src/APIs/c_api/include/std_msgs/Duration.h b/src/APIs/c_api/include/std_msgs/Duration.h new file mode 100644 index 0000000..0e7da17 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Duration.h @@ -0,0 +1,21 @@ +#ifndef C_API_STD_MSGS_DURATION_H +#define C_API_STD_MSGS_DURATION_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int32_t sec; + int32_t nsec; +} Duration; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_DURATION_H diff --git a/src/APIs/c_api/include/std_msgs/Empty.h b/src/APIs/c_api/include/std_msgs/Empty.h new file mode 100644 index 0000000..9e3779f --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Empty.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_EMPTY_H +#define C_API_STD_MSGS_EMPTY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t _dummy; +} Empty; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_EMPTY_H diff --git a/src/APIs/c_api/include/std_msgs/Float32.h b/src/APIs/c_api/include/std_msgs/Float32.h new file mode 100644 index 0000000..e2f72c2 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float32.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_FLOAT32_H +#define C_API_STD_MSGS_FLOAT32_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + float data; +} Float32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT32_H diff --git a/src/APIs/c_api/include/std_msgs/Float32MultiArray.h b/src/APIs/c_api/include/std_msgs/Float32MultiArray.h new file mode 100644 index 0000000..de400fa --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float32MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_FLOAT32MULTIARRAY_H +#define C_API_STD_MSGS_FLOAT32MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + float *data; + size_t data_count; +} Float32MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT32MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Float64.h b/src/APIs/c_api/include/std_msgs/Float64.h new file mode 100644 index 0000000..b7ef266 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float64.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_FLOAT64_H +#define C_API_STD_MSGS_FLOAT64_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + double data; +} Float64; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT64_H diff --git a/src/APIs/c_api/include/std_msgs/Float64MultiArray.h b/src/APIs/c_api/include/std_msgs/Float64MultiArray.h new file mode 100644 index 0000000..41272f2 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Float64MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_FLOAT64MULTIARRAY_H +#define C_API_STD_MSGS_FLOAT64MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + double *data; + size_t data_count; +} Float64MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_FLOAT64MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Header.h b/src/APIs/c_api/include/std_msgs/Header.h new file mode 100644 index 0000000..8f181ff --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Header.h @@ -0,0 +1,45 @@ +#ifndef C_API_STD_MSGS_HEADER_H +#define C_API_STD_MSGS_HEADER_H + +#ifdef __cplusplus +extern "C" +{ +#endif +#include +#include +#include + + typedef struct + { + uint32_t seq; + uint32_t sec; + uint32_t nsec; + char *frame_id; + } Header; + + /** + * @brief Create a new header + * @param frame_id Frame id + * @return Header + */ + Header header_create(const char *frame_id); + + /** + * @brief Set data for a header + * @param header Header + * @param seq Sequence + * @param sec Second + * @param nsec Nanosecond + * @param frame_id Frame id + * @return Header + */ + Header header_set_data( + uint32_t seq, + uint32_t sec, + uint32_t nsec, + char *frame_id); +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_HEADER_H \ No newline at end of file diff --git a/src/APIs/c_api/include/std_msgs/Int16.h b/src/APIs/c_api/include/std_msgs/Int16.h new file mode 100644 index 0000000..6b26619 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int16.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT16_H +#define C_API_STD_MSGS_INT16_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int16_t data; +} Int16; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT16_H diff --git a/src/APIs/c_api/include/std_msgs/Int16MultiArray.h b/src/APIs/c_api/include/std_msgs/Int16MultiArray.h new file mode 100644 index 0000000..53dbf90 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int16MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT16MULTIARRAY_H +#define C_API_STD_MSGS_INT16MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int16_t *data; + size_t data_count; +} Int16MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT16MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Int32.h b/src/APIs/c_api/include/std_msgs/Int32.h new file mode 100644 index 0000000..cac37e6 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int32.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT32_H +#define C_API_STD_MSGS_INT32_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int32_t data; +} Int32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT32_H diff --git a/src/APIs/c_api/include/std_msgs/Int32MultiArray.h b/src/APIs/c_api/include/std_msgs/Int32MultiArray.h new file mode 100644 index 0000000..5178f2d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int32MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT32MULTIARRAY_H +#define C_API_STD_MSGS_INT32MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int32_t *data; + size_t data_count; +} Int32MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT32MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Int64.h b/src/APIs/c_api/include/std_msgs/Int64.h new file mode 100644 index 0000000..01142c1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int64.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT64_H +#define C_API_STD_MSGS_INT64_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int64_t data; +} Int64; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT64_H diff --git a/src/APIs/c_api/include/std_msgs/Int64MultiArray.h b/src/APIs/c_api/include/std_msgs/Int64MultiArray.h new file mode 100644 index 0000000..f5154d0 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int64MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT64MULTIARRAY_H +#define C_API_STD_MSGS_INT64MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int64_t *data; + size_t data_count; +} Int64MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT64MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/Int8.h b/src/APIs/c_api/include/std_msgs/Int8.h new file mode 100644 index 0000000..10747b7 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int8.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_INT8_H +#define C_API_STD_MSGS_INT8_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + int8_t data; +} Int8; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT8_H diff --git a/src/APIs/c_api/include/std_msgs/Int8MultiArray.h b/src/APIs/c_api/include/std_msgs/Int8MultiArray.h new file mode 100644 index 0000000..13f3495 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Int8MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_INT8MULTIARRAY_H +#define C_API_STD_MSGS_INT8MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + int8_t *data; + size_t data_count; +} Int8MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_INT8MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/MultiArrayDimension.h b/src/APIs/c_api/include/std_msgs/MultiArrayDimension.h new file mode 100644 index 0000000..b7b1b9c --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/MultiArrayDimension.h @@ -0,0 +1,22 @@ +#ifndef C_API_STD_MSGS_MULTIARRAYDIMENSION_H +#define C_API_STD_MSGS_MULTIARRAYDIMENSION_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + char *label; + uint32_t size; + uint32_t stride; +} MultiArrayDimension; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_MULTIARRAYDIMENSION_H diff --git a/src/APIs/c_api/include/std_msgs/MultiArrayLayout.h b/src/APIs/c_api/include/std_msgs/MultiArrayLayout.h new file mode 100644 index 0000000..1928b7d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/MultiArrayLayout.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_MULTIARRAYLAYOUT_H +#define C_API_STD_MSGS_MULTIARRAYLAYOUT_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayDimension.h" + + +typedef struct +{ + MultiArrayDimension *dim; + size_t dim_count; + uint32_t data_offset; +} MultiArrayLayout; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_MULTIARRAYLAYOUT_H diff --git a/src/APIs/c_api/include/std_msgs/String.h b/src/APIs/c_api/include/std_msgs/String.h new file mode 100644 index 0000000..ce6193d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/String.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_STRING_H +#define C_API_STD_MSGS_STRING_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + char *data; +} String; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_STRING_H diff --git a/src/APIs/c_api/include/std_msgs/Time.h b/src/APIs/c_api/include/std_msgs/Time.h new file mode 100644 index 0000000..cb70ce1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/Time.h @@ -0,0 +1,27 @@ +#ifndef C_API_STD_MSGS_TIME_H +#define C_API_STD_MSGS_TIME_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint32_t sec; + uint32_t nsec; +} Time; + +/** + * @brief Create a new time + * @return Time + */ +Time time_create(); + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_TIME_H diff --git a/src/APIs/c_api/include/std_msgs/UInt16.h b/src/APIs/c_api/include/std_msgs/UInt16.h new file mode 100644 index 0000000..205ef45 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt16.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT16_H +#define C_API_STD_MSGS_UINT16_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint16_t data; +} UInt16; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT16_H diff --git a/src/APIs/c_api/include/std_msgs/UInt16MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt16MultiArray.h new file mode 100644 index 0000000..2d6885d --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt16MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT16MULTIARRAY_H +#define C_API_STD_MSGS_UINT16MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint16_t *data; + size_t data_count; +} UInt16MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT16MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/UInt32.h b/src/APIs/c_api/include/std_msgs/UInt32.h new file mode 100644 index 0000000..5cd3152 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt32.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT32_H +#define C_API_STD_MSGS_UINT32_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint32_t data; +} UInt32; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT32_H diff --git a/src/APIs/c_api/include/std_msgs/UInt32MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt32MultiArray.h new file mode 100644 index 0000000..1ad24f1 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt32MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT32MULTIARRAY_H +#define C_API_STD_MSGS_UINT32MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint32_t *data; + size_t data_count; +} UInt32MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT32MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/UInt64.h b/src/APIs/c_api/include/std_msgs/UInt64.h new file mode 100644 index 0000000..5a94d2a --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt64.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT64_H +#define C_API_STD_MSGS_UINT64_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint64_t data; +} UInt64; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT64_H diff --git a/src/APIs/c_api/include/std_msgs/UInt64MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt64MultiArray.h new file mode 100644 index 0000000..32507e7 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt64MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT64MULTIARRAY_H +#define C_API_STD_MSGS_UINT64MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint64_t *data; + size_t data_count; +} UInt64MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT64MULTIARRAY_H diff --git a/src/APIs/c_api/include/std_msgs/UInt8.h b/src/APIs/c_api/include/std_msgs/UInt8.h new file mode 100644 index 0000000..d097ad0 --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt8.h @@ -0,0 +1,20 @@ +#ifndef C_API_STD_MSGS_UINT8_H +#define C_API_STD_MSGS_UINT8_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include + + +typedef struct +{ + uint8_t data; +} UInt8; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT8_H diff --git a/src/APIs/c_api/include/std_msgs/UInt8MultiArray.h b/src/APIs/c_api/include/std_msgs/UInt8MultiArray.h new file mode 100644 index 0000000..316bcaa --- /dev/null +++ b/src/APIs/c_api/include/std_msgs/UInt8MultiArray.h @@ -0,0 +1,23 @@ +#ifndef C_API_STD_MSGS_UINT8MULTIARRAY_H +#define C_API_STD_MSGS_UINT8MULTIARRAY_H + +#ifdef __cplusplus +extern "C" { +#endif +#include +#include +#include "MultiArrayLayout.h" + + +typedef struct +{ + MultiArrayLayout layout; + uint8_t *data; + size_t data_count; +} UInt8MultiArray; + +#ifdef __cplusplus +} +#endif + +#endif // C_API_STD_MSGS_UINT8MULTIARRAY_H diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp new file mode 100644 index 0000000..1960f3a --- /dev/null +++ b/src/APIs/c_api/src/convertor.cpp @@ -0,0 +1,368 @@ +#include "convertor.h" +#include +#include +#include + + +robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan) +{ + robot_sensor_msgs::LaserScan robot_laser_scan; + robot_laser_scan.header.seq = laser_scan.header.seq; + robot_laser_scan.header.stamp.sec = laser_scan.header.sec; + robot_laser_scan.header.stamp.nsec = laser_scan.header.nsec; + robot::log_info("LaserScan header.stamp.sec=%d header.stamp.nsec=%d", laser_scan.header.sec, laser_scan.header.nsec); + robot::log_info("LaserScan header.frame_id=%s", laser_scan.header.frame_id); + robot_laser_scan.header.frame_id = laser_scan.header.frame_id; + robot_laser_scan.angle_min = laser_scan.angle_min; + robot_laser_scan.angle_max = laser_scan.angle_max; + robot_laser_scan.angle_increment = laser_scan.angle_increment; + robot_laser_scan.time_increment = laser_scan.time_increment; + robot_laser_scan.scan_time = laser_scan.scan_time; + robot_laser_scan.range_min = laser_scan.range_min; + robot_laser_scan.range_max = laser_scan.range_max; + size_t ranges_size = laser_scan.ranges ? laser_scan.ranges_count : 0; + size_t intensities_size = laser_scan.intensities ? laser_scan.intensities_count : 0; + robot_laser_scan.ranges.resize(ranges_size); + robot_laser_scan.intensities.resize(intensities_size); + for (size_t i = 0; i < ranges_size; i++) { + robot_laser_scan.ranges[i] = laser_scan.ranges[i]; + } + for (size_t i = 0; i < intensities_size; i++) { + robot_laser_scan.intensities[i] = laser_scan.intensities[i]; + } + return robot_laser_scan; +} + + +robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid) { + robot::log_info("OccupancyGrid sizeof=%zu data_off=%zu data_count_off=%zu", + sizeof(OccupancyGrid), + offsetof(OccupancyGrid, data), + offsetof(OccupancyGrid, data_count)); + robot_nav_msgs::OccupancyGrid robot_occupancy_grid; + robot_occupancy_grid.header.seq = occupancy_grid.header.seq; + robot_occupancy_grid.header.stamp.sec = occupancy_grid.header.sec; + robot_occupancy_grid.header.stamp.nsec = occupancy_grid.header.nsec; + robot_occupancy_grid.header.frame_id = occupancy_grid.header.frame_id; + robot_occupancy_grid.info.map_load_time.sec = occupancy_grid.info.map_load_time.sec; + robot_occupancy_grid.info.map_load_time.nsec = occupancy_grid.info.map_load_time.nsec; + robot_occupancy_grid.info.resolution = occupancy_grid.info.resolution; + robot_occupancy_grid.info.width = occupancy_grid.info.width; + robot_occupancy_grid.info.height = occupancy_grid.info.height; + robot_occupancy_grid.info.origin.position.x = occupancy_grid.info.origin.position.x; + robot_occupancy_grid.info.origin.position.y = occupancy_grid.info.origin.position.y; + robot_occupancy_grid.info.origin.position.z = occupancy_grid.info.origin.position.z; + robot_occupancy_grid.info.origin.orientation.x = occupancy_grid.info.origin.orientation.x; + robot_occupancy_grid.info.origin.orientation.y = occupancy_grid.info.origin.orientation.y; + robot_occupancy_grid.info.origin.orientation.z = occupancy_grid.info.origin.orientation.z; + robot_occupancy_grid.info.origin.orientation.w = occupancy_grid.info.origin.orientation.w; + + size_t data_size = occupancy_grid.data ? occupancy_grid.data_count : 0; + + // robot_occupancy_grid.data.resize(data_size); + // robot::log_info("convertOccupancyGrid: 2"); + std::stringstream ss; + for (size_t i = 0; i < data_size; i++) { + ss << occupancy_grid.data[i] << " "; + } + robot::log_info("occupancy_grid.data: %s", ss.str().c_str()); + return robot_occupancy_grid; +} + +robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry) +{ + robot_nav_msgs::Odometry robot_odometry; + robot_odometry.header.seq = odometry.header.seq; + robot_odometry.header.stamp.sec = odometry.header.sec; + robot_odometry.header.stamp.nsec = odometry.header.nsec; + robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : ""; + + robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : ""; + robot_odometry.pose.pose.position.x = odometry.pose.pose.position.x; + robot_odometry.pose.pose.position.y = odometry.pose.pose.position.y; + robot_odometry.pose.pose.position.z = odometry.pose.pose.position.z; + robot_odometry.pose.pose.orientation.x = odometry.pose.pose.orientation.x; + robot_odometry.pose.pose.orientation.y = odometry.pose.pose.orientation.y; + robot_odometry.pose.pose.orientation.z = odometry.pose.pose.orientation.z; + robot_odometry.pose.pose.orientation.w = odometry.pose.pose.orientation.w; + + if (odometry.pose.covariance && odometry.pose.covariance_count > 0) + { + const size_t n = std::min(static_cast(36), odometry.pose.covariance_count); + for (size_t i = 0; i < n; ++i) + robot_odometry.pose.covariance[i] = odometry.pose.covariance[i]; + } + + robot_odometry.twist.twist.linear.x = odometry.twist.twist.linear.x; + robot_odometry.twist.twist.linear.y = odometry.twist.twist.linear.y; + robot_odometry.twist.twist.linear.z = odometry.twist.twist.linear.z; + robot_odometry.twist.twist.angular.x = odometry.twist.twist.angular.x; + robot_odometry.twist.twist.angular.y = odometry.twist.twist.angular.y; + robot_odometry.twist.twist.angular.z = odometry.twist.twist.angular.z; + + if (odometry.twist.covariance && odometry.twist.covariance_count > 0) + { + const size_t n = std::min(static_cast(36), odometry.twist.covariance_count); + for (size_t i = 0; i < n; ++i) + robot_odometry.twist.covariance[i] = odometry.twist.covariance[i]; + } + + return robot_odometry; +} + +// Convert C PoseStamped to C++ PoseStamped +robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& c_pose) +{ + robot_geometry_msgs::PoseStamped cpp_pose; + cpp_pose.header.seq = c_pose.header.seq; + cpp_pose.header.stamp.sec = c_pose.header.sec; + cpp_pose.header.stamp.nsec = c_pose.header.nsec; + cpp_pose.header.frame_id = c_pose.header.frame_id; + + cpp_pose.pose.position.x = c_pose.pose.position.x; + cpp_pose.pose.position.y = c_pose.pose.position.y; + cpp_pose.pose.position.z = c_pose.pose.position.z; + cpp_pose.pose.orientation.x = c_pose.pose.orientation.x; + cpp_pose.pose.orientation.y = c_pose.pose.orientation.y; + cpp_pose.pose.orientation.z = c_pose.pose.orientation.z; + cpp_pose.pose.orientation.w = c_pose.pose.orientation.w; + return cpp_pose; +} + +// Convert C++ PoseStamped to C PoseStamped +PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose) +{ + PoseStamped c_pose; + c_pose.header.seq = cpp_pose.header.seq; + c_pose.header.sec = cpp_pose.header.stamp.sec; + c_pose.header.nsec = cpp_pose.header.stamp.nsec; + c_pose.header.frame_id = (char*)cpp_pose.header.frame_id.c_str(); + c_pose.pose.position.x = cpp_pose.pose.position.x; + c_pose.pose.position.y = cpp_pose.pose.position.y; + c_pose.pose.position.z = cpp_pose.pose.position.z; + c_pose.pose.orientation.x = cpp_pose.pose.orientation.x; + c_pose.pose.orientation.y = cpp_pose.pose.orientation.y; + c_pose.pose.orientation.z = cpp_pose.pose.orientation.z; + c_pose.pose.orientation.w = cpp_pose.pose.orientation.w; + return c_pose; +} + +// Convert C++ Pose2D to C Pose2D +Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose) +{ + Pose2D c_pose; + c_pose.x = cpp_pose.x; + c_pose.y = cpp_pose.y; + c_pose.theta = cpp_pose.theta; + return c_pose; +} +Header convert2CHeader(const robot_std_msgs::Header& cpp_header) +{ + Header c_header; + c_header.seq = cpp_header.seq; + c_header.sec = cpp_header.stamp.sec; + c_header.nsec = cpp_header.stamp.nsec; + c_header.frame_id = (char*)cpp_header.frame_id.c_str(); + return c_header; +} + +Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist) +{ + Twist2DStamped c_twist; + c_twist.header = convert2CHeader(cpp_twist.header); + c_twist.velocity.x = cpp_twist.velocity.x; + c_twist.velocity.y = cpp_twist.velocity.y; + c_twist.velocity.theta = cpp_twist.velocity.theta; + return c_twist; +} + +// ========== C++ to C converters (for get_* API) ========== + +void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.info.map_load_time.sec = cpp.info.map_load_time.sec; + out.info.map_load_time.nsec = cpp.info.map_load_time.nsec; + out.info.resolution = cpp.info.resolution; + out.info.width = cpp.info.width; + out.info.height = cpp.info.height; + out.info.origin.position.x = cpp.info.origin.position.x; + out.info.origin.position.y = cpp.info.origin.position.y; + out.info.origin.position.z = cpp.info.origin.position.z; + out.info.origin.orientation.x = cpp.info.origin.orientation.x; + out.info.origin.orientation.y = cpp.info.origin.orientation.y; + out.info.origin.orientation.z = cpp.info.origin.orientation.z; + out.info.origin.orientation.w = cpp.info.origin.orientation.w; + out.data_count = cpp.data.size(); + out.data = nullptr; + if (out.data_count > 0) + { + out.data = static_cast(malloc(out.data_count * sizeof(int8_t))); + if (out.data) + memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t)); + } +} + +void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.angle_min = cpp.angle_min; + out.angle_max = cpp.angle_max; + out.angle_increment = cpp.angle_increment; + out.time_increment = cpp.time_increment; + out.scan_time = cpp.scan_time; + out.range_min = cpp.range_min; + out.range_max = cpp.range_max; + out.ranges_count = cpp.ranges.size(); + out.ranges = nullptr; + if (out.ranges_count > 0) + { + out.ranges = static_cast(malloc(out.ranges_count * sizeof(float))); + if (out.ranges) + memcpy(out.ranges, cpp.ranges.data(), out.ranges_count * sizeof(float)); + } + out.intensities_count = cpp.intensities.size(); + out.intensities = nullptr; + if (out.intensities_count > 0) + { + out.intensities = static_cast(malloc(out.intensities_count * sizeof(float))); + if (out.intensities) + memcpy(out.intensities, cpp.intensities.data(), out.intensities_count * sizeof(float)); + } +} + +void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.points_count = cpp.points.size(); + out.points = nullptr; + if (out.points_count > 0) + { + out.points = static_cast(malloc(out.points_count * sizeof(Point32))); + if (out.points) + for (size_t i = 0; i < out.points_count; ++i) + { + out.points[i].x = cpp.points[i].x; + out.points[i].y = cpp.points[i].y; + out.points[i].z = cpp.points[i].z; + } + } + out.channels_count = cpp.channels.size(); + out.channels = nullptr; + if (out.channels_count > 0) + { + out.channels = static_cast(malloc(out.channels_count * sizeof(ChannelFloat32))); + if (out.channels) + for (size_t i = 0; i < out.channels_count; ++i) + { + out.channels[i].name = cpp.channels[i].name.empty() ? nullptr : strdup(cpp.channels[i].name.c_str()); + out.channels[i].values_count = cpp.channels[i].values.size(); + out.channels[i].values = nullptr; + if (out.channels[i].values_count > 0) + { + out.channels[i].values = static_cast(malloc(out.channels[i].values_count * sizeof(float))); + if (out.channels[i].values) + memcpy(out.channels[i].values, cpp.channels[i].values.data(), out.channels[i].values_count * sizeof(float)); + } + } + } +} + +void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out) +{ + out.header.seq = cpp.header.seq; + out.header.sec = cpp.header.stamp.sec; + out.header.nsec = cpp.header.stamp.nsec; + out.header.frame_id = cpp.header.frame_id.empty() ? nullptr : strdup(cpp.header.frame_id.c_str()); + out.height = cpp.height; + out.width = cpp.width; + out.is_bigendian = cpp.is_bigendian; + out.point_step = cpp.point_step; + out.row_step = cpp.row_step; + out.is_dense = cpp.is_dense; + out.fields_count = cpp.fields.size(); + out.fields = nullptr; + if (out.fields_count > 0) + { + out.fields = static_cast(malloc(out.fields_count * sizeof(PointField))); + if (out.fields) + for (size_t i = 0; i < out.fields_count; ++i) + { + out.fields[i].name = cpp.fields[i].name.empty() ? nullptr : strdup(cpp.fields[i].name.c_str()); + out.fields[i].offset = cpp.fields[i].offset; + out.fields[i].datatype = cpp.fields[i].datatype; + out.fields[i].count = cpp.fields[i].count; + } + } + out.data_count = cpp.data.size(); + out.data = nullptr; + if (out.data_count > 0) + { + out.data = static_cast(malloc(out.data_count)); + if (out.data) + memcpy(out.data, cpp.data.data(), out.data_count); + } +} + +// ========== C to C++ for PointCloud / PointCloud2 (for add_* API) ========== + +robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c) +{ + robot_sensor_msgs::PointCloud cpp; + cpp.header.seq = c.header.seq; + cpp.header.stamp.sec = c.header.sec; + cpp.header.stamp.nsec = c.header.nsec; + cpp.header.frame_id = c.header.frame_id ? c.header.frame_id : ""; + cpp.points.resize(c.points_count); + for (size_t i = 0; i < c.points_count && c.points; ++i) + { + cpp.points[i].x = c.points[i].x; + cpp.points[i].y = c.points[i].y; + cpp.points[i].z = c.points[i].z; + } + cpp.channels.resize(c.channels_count); + for (size_t i = 0; i < c.channels_count && c.channels; ++i) + { + cpp.channels[i].name = c.channels[i].name ? c.channels[i].name : ""; + cpp.channels[i].values.resize(c.channels[i].values_count); + if (c.channels[i].values && c.channels[i].values_count > 0) + memcpy(cpp.channels[i].values.data(), c.channels[i].values, c.channels[i].values_count * sizeof(float)); + } + return cpp; +} + +robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c) +{ + robot_sensor_msgs::PointCloud2 cpp; + cpp.header.seq = c.header.seq; + cpp.header.stamp.sec = c.header.sec; + cpp.header.stamp.nsec = c.header.nsec; + cpp.header.frame_id = c.header.frame_id ? c.header.frame_id : ""; + cpp.height = c.height; + cpp.width = c.width; + cpp.is_bigendian = c.is_bigendian; + cpp.point_step = c.point_step; + cpp.row_step = c.row_step; + cpp.is_dense = c.is_dense; + cpp.fields.resize(c.fields_count); + for (size_t i = 0; i < c.fields_count && c.fields; ++i) + { + cpp.fields[i].name = c.fields[i].name ? c.fields[i].name : ""; + cpp.fields[i].offset = c.fields[i].offset; + cpp.fields[i].datatype = c.fields[i].datatype; + cpp.fields[i].count = c.fields[i].count; + } + cpp.data.resize(c.data_count); + if (c.data && c.data_count > 0) + memcpy(cpp.data.data(), c.data, c.data_count); + return cpp; +} diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 0d836b5..1b1967c 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -1,1145 +1,1171 @@ +#include #include "nav_c_api.h" +#include "convertor.h" +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include #include #include #include -#include - -// ============================================================================ -// Internal Helper Functions -// ============================================================================ - -namespace { - // Convert C PoseStamped to C++ PoseStamped - robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) { - robot_geometry_msgs::PoseStamped cpp_pose; - cpp_pose.header.seq = c_pose->header.seq; - cpp_pose.header.stamp.sec = c_pose->header.sec; - cpp_pose.header.stamp.nsec = c_pose->header.nsec; - if (c_pose->header.frame_id) { - cpp_pose.header.frame_id = c_pose->header.frame_id; - } - cpp_pose.pose.position.x = c_pose->pose.position.x; - cpp_pose.pose.position.y = c_pose->pose.position.y; - cpp_pose.pose.position.z = c_pose->pose.position.z; - cpp_pose.pose.orientation.x = c_pose->pose.orientation.x; - cpp_pose.pose.orientation.y = c_pose->pose.orientation.y; - cpp_pose.pose.orientation.z = c_pose->pose.orientation.z; - cpp_pose.pose.orientation.w = c_pose->pose.orientation.w; - return cpp_pose; - } - - // Convert C++ PoseStamped to C PoseStamped - void cpp_to_c_pose_stamped(const robot_geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) { - c_pose->header.seq = cpp_pose.header.seq; - c_pose->header.sec = cpp_pose.header.stamp.sec; - c_pose->header.nsec = cpp_pose.header.stamp.nsec; - if (!cpp_pose.header.frame_id.empty()) { - c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str()); - } else { - c_pose->header.frame_id = nullptr; - } - c_pose->pose.position.x = cpp_pose.pose.position.x; - c_pose->pose.position.y = cpp_pose.pose.position.y; - c_pose->pose.position.z = cpp_pose.pose.position.z; - c_pose->pose.orientation.x = cpp_pose.pose.orientation.x; - c_pose->pose.orientation.y = cpp_pose.pose.orientation.y; - c_pose->pose.orientation.z = cpp_pose.pose.orientation.z; - c_pose->pose.orientation.w = cpp_pose.pose.orientation.w; - } - - // Convert C++ Pose2D to C Pose2D - void cpp_to_c_pose_2d(const robot_geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) { - c_pose->x = cpp_pose.x; - c_pose->y = cpp_pose.y; - c_pose->theta = cpp_pose.theta; - } - - void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) { - c_header->seq = cpp_header.seq; - c_header->sec = cpp_header.stamp.sec; - c_header->nsec = cpp_header.stamp.nsec; - if (!cpp_header.frame_id.empty()) { - c_header->frame_id = strdup(cpp_header.frame_id.c_str()); - } else { - c_header->frame_id = nullptr; - } - } - - void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist, - Twist2DStamped* c_twist) { - cpp_to_c_header(cpp_twist.header, &c_twist->header); - c_twist->velocity.x = cpp_twist.velocity.x; - c_twist->velocity.y = cpp_twist.velocity.y; - c_twist->velocity.theta = cpp_twist.velocity.theta; - } - - // Convert C++ NavFeedback to C NavFeedback - void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) { - c_feedback->navigation_state = static_cast(static_cast(cpp_feedback.navigation_state)); - if (!cpp_feedback.feed_back_str.empty()) { - c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str()); - } else { - c_feedback->feed_back_str = nullptr; - } - c_feedback->current_pose.x = cpp_feedback.current_pose.x; - c_feedback->current_pose.y = cpp_feedback.current_pose.y; - c_feedback->current_pose.theta = cpp_feedback.current_pose.theta; - c_feedback->goal_checked = cpp_feedback.goal_checked; - c_feedback->is_ready = cpp_feedback.is_ready; - } - - template - T* clone_message(const T& src) { - return new (std::nothrow) T(src); - } - - void clear_planner_data(PlannerDataOutput* data) { - if (!data) { - return; - } - delete static_cast(data->plan); - delete static_cast(data->costmap); - delete static_cast(data->costmap_update); - delete static_cast(data->footprint); - data->plan = nullptr; - data->costmap = nullptr; - data->costmap_update = nullptr; - data->footprint = nullptr; - data->is_costmap_updated = false; - } +static NavFeedback convert2CNavFeedback(const robot::move_base_core::NavFeedback &cpp) +{ + NavFeedback out; + out.navigation_state = static_cast(static_cast(cpp.navigation_state)); + out.feed_back_str = cpp.feed_back_str.empty() ? nullptr : strdup(cpp.feed_back_str.c_str()); + out.current_pose.x = cpp.current_pose.x; + out.current_pose.y = cpp.current_pose.y; + out.current_pose.theta = cpp.current_pose.theta; + out.goal_checked = cpp.goal_checked; + out.is_ready = cpp.is_ready; + return out; } -// ============================================================================ -// String Management -// ============================================================================ - -extern "C" void nav_c_api_free_string(char* str) { - if (str) { - free(str); - } +extern "C" char *navigation_state_to_string(NavigationState state) +{ + robot::move_base_core::State cpp_state = static_cast(static_cast(state)); + std::string str = robot::move_base_core::toString(cpp_state); + return strdup(str.c_str()); } -// ============================================================================ -// State Conversion -// ============================================================================ - -extern "C" char* navigation_state_to_string(NavigationState state) { - robot::move_base_core::State cpp_state = static_cast(static_cast(state)); - std::string str = robot::move_base_core::toString(cpp_state); - return strdup(str.c_str()); +extern "C" void nav_c_api_free_string(char *str) +{ + if (str) + free(str); } // ============================================================================ // Helper Functions // ============================================================================ -extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta, - const char* frame_id, double offset_distance, - PoseStamped* out_goal) { - if (!out_goal || !frame_id) { - return false; - } +extern "C" bool navigation_offset_goal_2d(const Pose2D &in_pose, + const char *frame_id, double offset_distance, + PoseStamped &out_goal) +{ + if (!frame_id) + { + return false; + } - robot_geometry_msgs::Pose2D pose; - pose.x = pose_x; - pose.y = pose_y; - pose.theta = pose_theta; - - robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); - cpp_to_c_pose_stamped(result, out_goal); - return true; + robot_geometry_msgs::Pose2D pose = convert2CppPose2D(in_pose); + robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); + out_goal = convert2CPoseStamped(result); + return true; } -extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance, - PoseStamped* out_goal) { - if (!in_pose || !out_goal) { - return false; - } - - robot_geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose); - robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance); - cpp_to_c_pose_stamped(result, out_goal); - return true; -} - -// ============================================================================ -// Navigation Handle Management -// ============================================================================ - -extern "C" NavigationHandle navigation_create(void) { - try { - // Create a concrete MoveBase instance - // Using default constructor - initialization will be done via navigation_initialize() - move_base::MoveBase* move_base = new move_base::MoveBase(); - return static_cast(move_base); - } catch (const std::exception& e) { - // Log error if possible (in production, use proper logging) - return nullptr; - } catch (...) { - return nullptr; - } -} - -extern "C" void navigation_destroy(NavigationHandle handle) { - if (handle) { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - delete nav; - } +extern "C" bool navigation_offset_goal_stamped(const PoseStamped &in_pose, double offset_distance, + PoseStamped &out_goal) +{ + robot_geometry_msgs::PoseStamped cpp_pose = convert2CppPoseStamped(in_pose); + robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance); + out_goal = convert2CPoseStamped(result); + return true; } // ============================================================================ // TF Listener Management // ============================================================================ -extern "C" TFListenerHandle tf_listener_create(void) { - try { - auto tf = std::make_shared(); - return new std::shared_ptr(tf); - } catch (...) { - return nullptr; - } +extern "C" TFListenerHandle tf_listener_create(void) +{ + try + { + auto tf = std::make_shared(); + return TFListenerHandle{new std::shared_ptr(tf)}; + } + catch (...) + { + return TFListenerHandle{nullptr}; + } } -extern "C" void tf_listener_destroy(TFListenerHandle handle) { - if (handle) { - auto* tf_ptr = static_cast*>(handle); - delete tf_ptr; - } +extern "C" void tf_listener_destroy(TFListenerHandle handle) +{ + if (handle.ptr) + { + auto *tf_ptr = static_cast *>(handle.ptr); + delete tf_ptr; + } } extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle, - const char* parent_frame, - const char* child_frame, + const char *parent_frame, + const char *child_frame, double x, double y, double z, double qx, double qy, double qz, double qw) { - if (!tf_handle || !parent_frame || !child_frame) { - return false; + if (!tf_handle.ptr || !parent_frame || !child_frame) + { + return false; + } + + try + { + auto *tf_ptr = static_cast *>(tf_handle.ptr); + if (!tf_ptr || !(*tf_ptr)) + { + return false; } - try { - auto* tf_ptr = static_cast*>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) { - return false; - } + tf3::TransformStampedMsg st; + st.header.stamp = tf3::Time::now(); + st.header.frame_id = parent_frame; + st.child_frame_id = child_frame; + st.transform.translation.x = x; + st.transform.translation.y = y; + st.transform.translation.z = z; + st.transform.rotation.x = qx; + st.transform.rotation.y = qy; + st.transform.rotation.z = qz; + st.transform.rotation.w = qw; - 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; + } +} - return (*tf_ptr)->setTransform(st, "nav_c_api(static)", true /*is_static*/); - } catch (...) { - return false; - } +// ============================================================================ +// Navigation Handle Management +// ============================================================================ + +extern "C" NavigationHandle navigation_create(void) +{ + try + { + // Create a concrete MoveBase instance + // Using default constructor - initialization will be done via navigation_initialize() + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath("MoveBase"); + auto move_base_loader = boost::dll::import_alias( + path_file_so, "MoveBase", boost::dll::load_mode::append_decorations); + auto move_base_ptr = move_base_loader(); + // Store shared_ptr in handle (same pattern as TFListenerHandle) + return NavigationHandle{new std::shared_ptr(move_base_ptr)}; + } + catch (const std::exception &e) + { + // Log error if possible (in production, use proper logging) + return NavigationHandle{nullptr}; + } + catch (...) + { + return NavigationHandle{nullptr}; + } +} + +extern "C" void navigation_destroy(NavigationHandle handle) +{ + if (handle.ptr) + { + auto *nav_ptr = static_cast *>(handle.ptr); + delete nav_ptr; + } } // ============================================================================ // Navigation Interface Methods // ============================================================================ -extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) +extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { - printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); - if (!handle || !tf_handle) { - printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); - return false; - } - try { - printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); - auto* nav = static_cast(handle); - auto* tf_ptr = static_cast*>(tf_handle); - if (!tf_ptr || !(*tf_ptr)) { - printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); - return false; - } - nav->initialize(*tf_ptr); - - return true; - } catch (const std::exception &e) { - printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); - return false; - } catch (...) { - printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__); - return false; + printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); + if (!handle.ptr || !tf_handle.ptr) + { + printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); + return false; + } + try + { + printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto *tf_ptr = static_cast *>(tf_handle.ptr); + if (!tf_ptr || !(*tf_ptr)) + { + printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); + return false; } + nav->initialize(*tf_ptr); + + return true; + } + catch (const std::exception &e) + { + printf("[%s:%d]\n Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); + return false; + } + catch (...) + { + printf("[%s:%d]\n Error: Failed to initialize navigation\n", __FILE__, __LINE__); + return false; + } } -extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count) { - if (!handle || !points || point_count == 0) { - return false; - } +extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point *points, size_t point_count) +{ + if (!handle.ptr || !points || point_count == 0) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - std::vector footprint; - footprint.reserve(point_count); - for (size_t i = 0; i < point_count; ++i) { - robot_geometry_msgs::Point pt; - pt.x = points[i].x; - pt.y = points[i].y; - pt.z = points[i].z; - footprint.push_back(pt); - } - nav->setRobotFootprint(footprint); - return true; - } catch (...) { - return false; + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + std::vector footprint; + footprint.reserve(point_count); + for (size_t i = 0; i < point_count; ++i) + { + robot_geometry_msgs::Point pt; + pt.x = points[i].x; + pt.y = points[i].y; + pt.z = points[i].z; + footprint.push_back(pt); } + nav->setRobotFootprint(footprint); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) { - if (!handle || !out_points || !out_count) { - return false; +extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point *out_points, size_t &out_count) +{ + if (!handle.ptr) + { + return false; + } + + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + std::vector footprint = nav->getRobotFootprint(); + if (footprint.empty()) + { + return false; } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - std::vector footprint = nav->getRobotFootprint(); - if (footprint.empty()) { - *out_points = nullptr; - *out_count = 0; - return true; - } - - Point* points = static_cast(malloc(sizeof(Point) * footprint.size())); - if (!points) { - return false; - } - - for (size_t i = 0; i < footprint.size(); ++i) { - points[i].x = footprint[i].x; - points[i].y = footprint[i].y; - points[i].z = footprint[i].z; - } - - *out_points = points; - *out_count = footprint.size(); - return true; - } catch (...) { - return false; + out_count = footprint.size(); + for (size_t i = 0; i < out_count; ++i) + { + out_points[i].x = footprint[i].x; + out_points[i].y = footprint[i].y; + out_points[i].z = footprint[i].z; } + return true; + } + catch (...) + { + return false; + } } -extern "C" void navigation_free_points(Point* points) { - if (points) { - free(points); - } +extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal, - double xy_goal_tolerance, double yaw_goal_tolerance) { - if (!handle || !goal) { - return false; - } +// extern "C" bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, +// const PoseStamped& goal, +// double xy_goal_tolerance, double yaw_goal_tolerance) { +// if (!handle.ptr || !order.ptr || !goal) { +// return false; +// } +// try { +// auto* nav = static_cast(handle.ptr); +// auto* order_ptr = static_cast(order.ptr); +// robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); +// return nav->moveTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); +// } catch (...) { +// return false; +// } +// } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); - } catch (...) { - return false; - } +extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, + const PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + if (!handle.ptr || !marker) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker, - const PoseStamped* goal, - double xy_goal_tolerance, double yaw_goal_tolerance) { - if (!handle || !marker || !goal) { - return false; - } +// extern "C" bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, +// const PoseStamped& goal, +// double xy_goal_tolerance, double yaw_goal_tolerance) { +// if (!handle.ptr || !order.ptr || !goal) { +// return false; +// } +// try { +// auto* nav = static_cast(handle.ptr); +// auto* order_ptr = static_cast(order.ptr); +// robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); +// return nav->dockTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); +// } catch (...) { +// return false; +// } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); - } catch (...) { - return false; - } +extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped goal, + double xy_goal_tolerance) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal, - double xy_goal_tolerance) { - if (!handle || !goal) { - return false; - } +extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal, double yaw_goal_tolerance) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav->rotateTo(cpp_goal, yaw_goal_tolerance); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal, - double yaw_goal_tolerance) { - if (!handle || !goal) { - return false; +extern "C" void navigation_pause(NavigationHandle handle) +{ + if (handle.ptr) + { + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + nav->pause(); } - - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->rotateTo(cpp_goal, yaw_goal_tolerance); - } catch (...) { - return false; + catch (...) + { + // Ignore exceptions } + } } -extern "C" void navigation_pause(NavigationHandle handle) { - if (handle) { - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - nav->pause(); - } catch (...) { - // Ignore exceptions - } +extern "C" void navigation_resume(NavigationHandle handle) +{ + if (handle.ptr) + { + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + nav->resume(); } + catch (...) + { + // Ignore exceptions + } + } } -extern "C" void navigation_resume(NavigationHandle handle) { - if (handle) { - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - nav->resume(); - } catch (...) { - // Ignore exceptions - } +extern "C" void navigation_cancel(NavigationHandle handle) +{ + if (handle.ptr) + { + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + nav->cancel(); } -} - -extern "C" void navigation_cancel(NavigationHandle handle) { - if (handle) { - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - nav->cancel(); - } catch (...) { - // Ignore exceptions - } + catch (...) + { + // Ignore exceptions } + } } extern "C" bool navigation_set_twist_linear(NavigationHandle handle, - double linear_x, double linear_y, double linear_z) { - if (!handle) { - return false; - } + double linear_x, double linear_y, double linear_z) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::Vector3 linear; - linear.x = linear_x; - linear.y = linear_y; - linear.z = linear_z; - return nav->setTwistLinear(linear); - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::Vector3 linear; + linear.x = linear_x; + linear.y = linear_y; + linear.z = linear_z; + return nav->setTwistLinear(linear); + } + catch (...) + { + return false; + } } extern "C" bool navigation_set_twist_angular(NavigationHandle handle, - double angular_x, double angular_y, double angular_z) { - if (!handle) { - return false; - } + double angular_x, double angular_y, double angular_z) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::Vector3 angular; - angular.x = angular_x; - angular.y = angular_y; - angular.z = angular_z; - return nav->setTwistAngular(angular); - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::Vector3 angular; + angular.x = angular_x; + angular.y = angular_y; + angular.z = angular_z; + return nav->setTwistAngular(angular); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) { - if (!handle || !out_pose) { - return false; +extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped &out_pose) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::PoseStamped cpp_pose; + if (nav->getRobotPose(cpp_pose)) + { + out_pose = convert2CPoseStamped(cpp_pose); + return true; } + return false; + } + catch (...) + { + return false; + } +} - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::PoseStamped cpp_pose; - if (nav->getRobotPose(cpp_pose)) { - cpp_to_c_pose_stamped(cpp_pose, out_pose); - return true; - } - return false; - } catch (...) { - return false; +extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &out_pose) +{ + if (!handle.ptr) + { + return false; + } + + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_geometry_msgs::Pose2D cpp_pose; + if (nav->getRobotPose(cpp_pose)) + { + out_pose = convert2CPose2D(cpp_pose); + return true; } + return false; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) { - if (!handle || !out_pose) { - return false; - } +extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &out_twist) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_geometry_msgs::Pose2D cpp_pose; - if (nav->getRobotPose(cpp_pose)) { - cpp_to_c_pose_2d(cpp_pose, out_pose); - return true; - } - return false; - } catch (...) { - return false; - } + try + { + robot::move_base_core::BaseNavigation *nav = + static_cast(handle.ptr); + robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); + out_twist = convert2CTwist2DStamped(cpp_twist); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) { - if (!handle || !out_twist) { - return false; - } +extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback &out_feedback) +{ + if (!handle.ptr) + { + return false; + } - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); - cpp_to_c_twist2d_stamped(cpp_twist, out_twist); - return true; - } catch (...) { - return false; - } -} - -extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) { - if (!handle || !out_feedback) { - return false; - } - - try { - robot::move_base_core::BaseNavigation* nav = static_cast(handle); - if (nav->getFeedback() != nullptr) { - cpp_to_c_nav_feedback(*nav->getFeedback(), out_feedback); - return true; - } else { - return false; - } - } catch (...) { - return false; - } -} - -extern "C" void navigation_free_feedback(NavFeedback* feedback) { - if (feedback) { - nav_c_api_free_string(feedback->feed_back_str); - feedback->feed_back_str = nullptr; - } -} - -// ============================================================================ -// Complex Message Handle Management -// ============================================================================ - -extern "C" void navigation_free_occupancy_grid(OccupancyGridHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_occupancy_grid_update(OccupancyGridUpdateHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_laser_scan(LaserScanHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_point_cloud(PointCloudHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_point_cloud2(PointCloud2Handle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_odometry(OdometryHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_path2d(Path2DHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_polygon_stamped(PolygonStampedHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_order(OrderHandle handle) { - delete static_cast(handle); -} - -extern "C" void navigation_free_named_occupancy_grids(NamedOccupancyGrid* maps, size_t count) { - if (!maps) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(maps[i].name); - delete static_cast(maps[i].map); - } - free(maps); -} - -extern "C" void navigation_free_named_laser_scans(NamedLaserScan* scans, size_t count) { - if (!scans) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(scans[i].name); - delete static_cast(scans[i].scan); - } - free(scans); -} - -extern "C" void navigation_free_named_point_clouds(NamedPointCloud* clouds, size_t count) { - if (!clouds) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(clouds[i].name); - delete static_cast(clouds[i].cloud); - } - free(clouds); -} - -extern "C" void navigation_free_named_point_cloud2s(NamedPointCloud2* clouds, size_t count) { - if (!clouds) { - return; - } - for (size_t i = 0; i < count; ++i) { - free(clouds[i].name); - delete static_cast(clouds[i].cloud); - } - free(clouds); -} - -extern "C" void navigation_free_planner_data(PlannerDataOutput* data) { - clear_planner_data(data); + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot::move_base_core::NavFeedback *fb = nav->getFeedback(); + if (fb == nullptr) + return false; + out_feedback = convert2CNavFeedback(*fb); + return true; + } + catch (...) + { + return false; + } } // ============================================================================ // Navigation Data Management // ============================================================================ -extern "C" bool navigation_add_static_map(NavigationHandle handle, const char* map_name, OccupancyGridHandle map) { - if (!handle || !map_name || !map) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* map_ptr = static_cast(map); - nav->addStaticMap(std::string(map_name), *map_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid occupancy_grid) +{ + if (!handle.ptr || !map_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot::log_info("occupancy_grid.data_count: %d", occupancy_grid.data_count); + robot_nav_msgs::OccupancyGrid robot_occupancy_grid = convert2CppOccupancyGrid(occupancy_grid); + + nav->addStaticMap(std::string(map_name), robot_occupancy_grid); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char* laser_scan_name, LaserScanHandle laser_scan) { - if (!handle || !laser_scan_name || !laser_scan) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* scan_ptr = static_cast(laser_scan); - nav->addLaserScan(std::string(laser_scan_name), *scan_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan laser_scan) +{ + if (!handle.ptr || !laser_scan_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + + robot_sensor_msgs::LaserScan robot_laser_scan = convert2CppLaserScan(laser_scan); + nav->addLaserScan(std::string(laser_scan_name), robot_laser_scan); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char* point_cloud_name, PointCloudHandle point_cloud) { - if (!handle || !point_cloud_name || !point_cloud) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* cloud_ptr = static_cast(point_cloud); - nav->addPointCloud(std::string(point_cloud_name), *cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_point_cloud(NavigationHandle handle, const char *point_cloud_name, const PointCloud point_cloud) +{ + if (!handle.ptr || !point_cloud_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud cpp_cloud = convert2CppPointCloud(point_cloud); + nav->addPointCloud(std::string(point_cloud_name), cpp_cloud); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char* point_cloud2_name, PointCloud2Handle point_cloud2) { - if (!handle || !point_cloud2_name || !point_cloud2) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* cloud_ptr = static_cast(point_cloud2); - nav->addPointCloud2(std::string(point_cloud2_name), *cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_add_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, const PointCloud2 point_cloud2) +{ + if (!handle.ptr || !point_cloud2_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud2 cpp_cloud = convert2CppPointCloud2(point_cloud2); + nav->addPointCloud2(std::string(point_cloud2_name), cpp_cloud); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_add_odometry(NavigationHandle handle, const char* odometry_name, OdometryHandle odometry) { - if (!handle || !odometry_name || !odometry) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* odom_ptr = static_cast(odometry); - nav->addOdometry(std::string(odometry_name), *odom_ptr); - return true; - } catch (...) { - return false; +extern "C" bool navigation_add_odometry(NavigationHandle handle, const char *odometry_name, const Odometry odometry) +{ + if (!handle.ptr || !odometry_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + { + return false; } + auto *nav = nav_ptr->get(); + robot_nav_msgs::Odometry robot_odometry = convert2CppOdometry(odometry); + nav->addOdometry(std::string(odometry_name), robot_odometry); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_static_map(NavigationHandle handle, const char* map_name, OccupancyGridHandle* out_map) { - if (!handle || !map_name || !out_map) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_nav_msgs::OccupancyGrid map = nav->getStaticMap(std::string(map_name)); - auto* map_ptr = clone_message(map); - if (!map_ptr) { - return false; - } - *out_map = static_cast(map_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_static_map(NavigationHandle handle, const char *map_name, OccupancyGrid &out_map) +{ + if (!handle.ptr || !map_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_nav_msgs::OccupancyGrid robot_map = nav->getStaticMap(std::string(map_name)); + convert2COccupancyGrid(robot_map, out_map); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char* laser_scan_name, LaserScanHandle* out_scan) { - if (!handle || !laser_scan_name || !out_scan) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_sensor_msgs::LaserScan scan = nav->getLaserScan(std::string(laser_scan_name)); - auto* scan_ptr = clone_message(scan); - if (!scan_ptr) { - return false; - } - *out_scan = static_cast(scan_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_laser_scan(NavigationHandle handle, const char *laser_scan_name, LaserScan &out_scan) +{ + if (!handle.ptr || !laser_scan_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::LaserScan robot_scan = nav->getLaserScan(std::string(laser_scan_name)); + convert2CLaserScan(robot_scan, out_scan); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char* point_cloud_name, PointCloudHandle* out_cloud) { - if (!handle || !point_cloud_name || !out_cloud) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_sensor_msgs::PointCloud cloud = nav->getPointCloud(std::string(point_cloud_name)); - auto* cloud_ptr = clone_message(cloud); - if (!cloud_ptr) { - return false; - } - *out_cloud = static_cast(cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_point_cloud(NavigationHandle handle, const char *point_cloud_name, PointCloud &out_cloud) +{ + if (!handle.ptr || !point_cloud_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud robot_cloud = nav->getPointCloud(std::string(point_cloud_name)); + convert2CPointCloud(robot_cloud, out_cloud); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char* point_cloud2_name, PointCloud2Handle* out_cloud) { - if (!handle || !point_cloud2_name || !out_cloud) { - return false; - } - try { - auto* nav = static_cast(handle); - robot_sensor_msgs::PointCloud2 cloud = nav->getPointCloud2(std::string(point_cloud2_name)); - auto* cloud_ptr = clone_message(cloud); - if (!cloud_ptr) { - return false; - } - *out_cloud = static_cast(cloud_ptr); - return true; - } catch (...) { - return false; - } +extern "C" bool navigation_get_point_cloud2(NavigationHandle handle, const char *point_cloud2_name, PointCloud2 &out_cloud) +{ + if (!handle.ptr || !point_cloud2_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot_sensor_msgs::PointCloud2 robot_cloud = nav->getPointCloud2(std::string(point_cloud2_name)); + convert2CPointCloud2(robot_cloud, out_cloud); + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid** out_maps, size_t* out_count) { - if (!handle || !out_maps || !out_count) { - return false; - } - try { - auto* nav = static_cast(handle); - auto maps = nav->getAllStaticMaps(); - if (maps.empty()) { - *out_maps = nullptr; - *out_count = 0; - return true; - } - - NamedOccupancyGrid* result = static_cast(malloc(sizeof(NamedOccupancyGrid) * maps.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : maps) { - result[index].name = strdup(entry.first.c_str()); - result[index].map = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].map) { - navigation_free_named_occupancy_grids(result, index + 1); - return false; - } - ++index; - } - - *out_maps = result; - *out_count = maps.size(); - return true; - } catch (...) { - return false; +extern "C" bool navigation_get_all_static_maps(NavigationHandle handle, NamedOccupancyGrid *out_maps, size_t &out_count) +{ + if (!handle.ptr || !out_maps) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto maps = nav->getAllStaticMaps(); + out_count = maps.size(); + size_t i = 0; + for (const auto &p : maps) + { + out_maps[i].name = strdup(p.first.c_str()); + convert2COccupancyGrid(p.second, out_maps[i].grid); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan** out_scans, size_t* out_count) { - if (!handle || !out_scans || !out_count) { - return false; +extern "C" bool navigation_get_all_laser_scans(NavigationHandle handle, NamedLaserScan *out_scans, size_t &out_count) +{ + if (!handle.ptr || !out_scans) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto scans = nav->getAllLaserScans(); + if (scans.empty()) + { + out_count = 0; + return true; } - try { - auto* nav = static_cast(handle); - auto scans = nav->getAllLaserScans(); - if (scans.empty()) { - *out_scans = nullptr; - *out_count = 0; - return true; - } - - NamedLaserScan* result = static_cast(malloc(sizeof(NamedLaserScan) * scans.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : scans) { - result[index].name = strdup(entry.first.c_str()); - result[index].scan = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].scan) { - navigation_free_named_laser_scans(result, index + 1); - return false; - } - ++index; - } - - *out_scans = result; - *out_count = scans.size(); - return true; - } catch (...) { - return false; + out_count = scans.size(); + size_t i = 0; + for (const auto &p : scans) + { + out_scans[i].name = strdup(p.first.c_str()); + convert2CLaserScan(p.second, out_scans[i].scan); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud** out_clouds, size_t* out_count) { - if (!handle || !out_clouds || !out_count) { - return false; +extern "C" bool navigation_get_all_point_clouds(NavigationHandle handle, NamedPointCloud *out_clouds, size_t &out_count) +{ + if (!handle.ptr || !out_clouds) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto clouds = nav->getAllPointClouds(); + if (clouds.empty()) + { + out_count = 0; + return true; } - try { - auto* nav = static_cast(handle); - auto clouds = nav->getAllPointClouds(); - if (clouds.empty()) { - *out_clouds = nullptr; - *out_count = 0; - return true; - } - - NamedPointCloud* result = static_cast(malloc(sizeof(NamedPointCloud) * clouds.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : clouds) { - result[index].name = strdup(entry.first.c_str()); - result[index].cloud = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].cloud) { - navigation_free_named_point_clouds(result, index + 1); - return false; - } - ++index; - } - - *out_clouds = result; - *out_count = clouds.size(); - return true; - } catch (...) { - return false; + out_count = clouds.size(); + size_t i = 0; + for (const auto &p : clouds) + { + out_clouds[i].name = strdup(p.first.c_str()); + convert2CPointCloud(p.second, out_clouds[i].cloud); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2** out_clouds, size_t* out_count) { - if (!handle || !out_clouds || !out_count) { - return false; +extern "C" bool navigation_get_all_point_cloud2s(NavigationHandle handle, NamedPointCloud2 *out_clouds, size_t &out_count) +{ + if (!handle.ptr || !out_clouds) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + auto clouds = nav->getAllPointCloud2s(); + if (clouds.empty()) + { + out_count = 0; + return true; } - try { - auto* nav = static_cast(handle); - auto clouds = nav->getAllPointCloud2s(); - if (clouds.empty()) { - *out_clouds = nullptr; - *out_count = 0; - return true; - } - - NamedPointCloud2* result = static_cast(malloc(sizeof(NamedPointCloud2) * clouds.size())); - if (!result) { - return false; - } - - size_t index = 0; - for (const auto& entry : clouds) { - result[index].name = strdup(entry.first.c_str()); - result[index].cloud = static_cast(clone_message(entry.second)); - if (!result[index].name || !result[index].cloud) { - navigation_free_named_point_cloud2s(result, index + 1); - return false; - } - ++index; - } - - *out_clouds = result; - *out_count = clouds.size(); - return true; - } catch (...) { - return false; + out_count = clouds.size(); + size_t i = 0; + for (const auto &p : clouds) + { + out_clouds[i].name = strdup(p.first.c_str()); + convert2CPointCloud2(p.second, out_clouds[i].cloud); + ++i; } + return true; + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char* map_name) { - if (!handle || !map_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeStaticMap(std::string(map_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_static_map(NavigationHandle handle, const char *map_name) +{ + if (!handle.ptr || !map_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeStaticMap(std::string(map_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char* laser_scan_name) { - if (!handle || !laser_scan_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeLaserScan(std::string(laser_scan_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_laser_scan(NavigationHandle handle, const char *laser_scan_name) +{ + if (!handle.ptr || !laser_scan_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeLaserScan(std::string(laser_scan_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char* point_cloud_name) { - if (!handle || !point_cloud_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removePointCloud(std::string(point_cloud_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_point_cloud(NavigationHandle handle, const char *point_cloud_name) +{ + if (!handle.ptr || !point_cloud_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removePointCloud(std::string(point_cloud_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char* point_cloud2_name) { - if (!handle || !point_cloud2_name) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removePointCloud2(std::string(point_cloud2_name)); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_point_cloud2(NavigationHandle handle, const char *point_cloud2_name) +{ + if (!handle.ptr || !point_cloud2_name) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removePointCloud2(std::string(point_cloud2_name)); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllStaticMaps(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_static_maps(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllStaticMaps(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllLaserScans(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_laser_scans(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllLaserScans(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllPointClouds(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_point_clouds(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllPointClouds(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllPointCloud2s(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_point_cloud2s(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllPointCloud2s(); + } + catch (...) + { + return false; + } } -extern "C" bool navigation_remove_all_data(NavigationHandle handle) { - if (!handle) { - return false; - } - try { - auto* nav = static_cast(handle); - return nav->removeAllData(); - } catch (...) { - return false; - } +extern "C" bool navigation_remove_all_data(NavigationHandle handle) +{ + if (!handle.ptr) + { + return false; + } + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + return nav_ptr->get()->removeAllData(); + } + catch (...) + { + return false; + } } // ============================================================================ -// Planner Data +// Planner Data (disabled - PlannerDataOutput and related types not in public API) // ============================================================================ +#if 0 +extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput *out_data) +{ + if (!handle.ptr || !out_data) + { + return false; + } + clear_planner_data(out_data); + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot::move_base_core::PlannerDataOutput data = nav->getGlobalData(); + out_data->plan = Path2DHandle{clone_message(data.plan)}; + out_data->costmap = OccupancyGridHandle{clone_message(data.costmap)}; + out_data->costmap_update = OccupancyGridUpdateHandle{clone_message(data.costmap_update)}; + out_data->footprint = PolygonStampedHandle{clone_message(data.footprint)}; + out_data->is_costmap_updated = data.is_costmap_updated; -extern "C" bool navigation_get_global_data(NavigationHandle handle, PlannerDataOutput* out_data) { - if (!handle || !out_data) { - return false; + if (!out_data->plan.ptr || !out_data->costmap.ptr || !out_data->costmap_update.ptr || + !out_data->footprint.ptr) + { + clear_planner_data(out_data); + return false; } + return true; + } + catch (...) + { clear_planner_data(out_data); - try { - auto* nav = static_cast(handle); - robot::move_base_core::PlannerDataOutput data = nav->getGlobalData(); - out_data->plan = static_cast(clone_message(data.plan)); - out_data->costmap = static_cast(clone_message(data.costmap)); - out_data->costmap_update = static_cast(clone_message(data.costmap_update)); - out_data->footprint = static_cast(clone_message(data.footprint)); - out_data->is_costmap_updated = data.is_costmap_updated; - - if (!out_data->plan || !out_data->costmap || !out_data->costmap_update || !out_data->footprint) { - clear_planner_data(out_data); - return false; - } - return true; - } catch (...) { - clear_planner_data(out_data); - return false; - } + return false; + } } -extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput* out_data) { - if (!handle || !out_data) { - return false; +extern "C" bool navigation_get_local_data(NavigationHandle handle, PlannerDataOutput *out_data) +{ + if (!handle.ptr || !out_data) + { + return false; + } + clear_planner_data(out_data); + try + { + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); + robot::move_base_core::PlannerDataOutput data = nav->getLocalData(); + out_data->plan = Path2DHandle{clone_message(data.plan)}; + out_data->costmap = convert2CppOccupancyGrid(data.costmap); + out_data->costmap_update = OccupancyGridUpdateHandle{clone_message(data.costmap_update)}; + out_data->footprint = convert2CppPolygonStamped(data.footprint); + out_data->is_costmap_updated = data.is_costmap_updated; + + if (!out_data->plan.ptr || !out_data->costmap.ptr || !out_data->costmap_update.ptr || + !out_data->footprint.ptr) + { + clear_planner_data(out_data); + return false; } + return true; + } + catch (...) + { clear_planner_data(out_data); - try { - auto* nav = static_cast(handle); - robot::move_base_core::PlannerDataOutput data = nav->getLocalData(); - out_data->plan = static_cast(clone_message(data.plan)); - out_data->costmap = static_cast(clone_message(data.costmap)); - out_data->costmap_update = static_cast(clone_message(data.costmap_update)); - out_data->footprint = static_cast(clone_message(data.footprint)); - out_data->is_costmap_updated = data.is_costmap_updated; - - if (!out_data->plan || !out_data->costmap || !out_data->costmap_update || !out_data->footprint) { - clear_planner_data(out_data); - return false; - } - return true; - } catch (...) { - clear_planner_data(out_data); - return false; - } + return false; + } } - -// ============================================================================ -// Navigation Commands with Order -// ============================================================================ - -extern "C" bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order, - const PoseStamped* goal, - double xy_goal_tolerance, double yaw_goal_tolerance) { - if (!handle || !order || !goal) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* order_ptr = static_cast(order); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->moveTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); - } catch (...) { - return false; - } -} - -extern "C" bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order, - const PoseStamped* goal, - double xy_goal_tolerance, double yaw_goal_tolerance) { - if (!handle || !order || !goal) { - return false; - } - try { - auto* nav = static_cast(handle); - auto* order_ptr = static_cast(order); - robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); - return nav->dockTo(*order_ptr, cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); - } catch (...) { - return false; - } -} - +#endif diff --git a/src/APIs/c_api/src/std_msgs/Header.cpp b/src/APIs/c_api/src/std_msgs/Header.cpp new file mode 100644 index 0000000..f9f9f34 --- /dev/null +++ b/src/APIs/c_api/src/std_msgs/Header.cpp @@ -0,0 +1,21 @@ +#include "std_msgs/Header.h" +#include +#include +#include + +extern "C" Header header_create(const char *frame_id) { + Header header{}; + header.frame_id = strdup(frame_id); + header.sec = robot::Time::now().sec; + header.nsec = robot::Time::now().nsec; + return header; +} + +extern "C" Header header_set_data(uint32_t seq, uint32_t sec, uint32_t nsec, char *frame_id) { + Header header{}; + header.seq = seq; + header.sec = sec; + header.nsec = nsec; + header.frame_id = strdup(frame_id); + return header; +} diff --git a/src/APIs/c_api/src/std_msgs/Time.cpp b/src/APIs/c_api/src/std_msgs/Time.cpp new file mode 100644 index 0000000..3895350 --- /dev/null +++ b/src/APIs/c_api/src/std_msgs/Time.cpp @@ -0,0 +1,12 @@ +#include "std_msgs/Time.h" +#include +#include +#include + +extern "C" Time time_create() +{ + Time time{}; + time.sec = robot::Time::now().sec; + time.nsec = robot::Time::now().nsec; + return time; +} \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 26b188d..7e520e5 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -416,6 +416,24 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms { it->second = map; } + robot::log_info("map header: %s", map.header.frame_id.c_str()); + robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec); + robot::log_info("map header: %d", map.header.seq); + robot::log_info("map info resolution: %f", map.info.resolution); + robot::log_info("map info width: %d", map.info.width); + robot::log_info("map info height: %d", map.info.height); + robot::log_info("map info origin position x: %f", map.info.origin.position.x); + robot::log_info("map info origin position y: %f", map.info.origin.position.y); + robot::log_info("map info origin position z: %f", map.info.origin.position.z); + robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x); + robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y); + robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z); + robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w); + robot::log_info("map data size: %zu", map.data.size()); + for(size_t i = 0; i < map.data.size(); i++) { + robot::log_info("map data[%zu]: %d", i, map.data[i]); + } + robot::log_info("--------------------------------"); updateGlobalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); updateLocalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); } @@ -432,7 +450,6 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan) { - auto it = laser_scans_.find(laser_scan_name); if (it == laser_scans_.end()) { @@ -442,6 +459,29 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot { it->second = laser_scan; } + robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); + robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); + robot::log_info("angle_min: %f", laser_scan.angle_min); + robot::log_info("angle_max: %f", laser_scan.angle_max); + robot::log_info("angle_increment: %f", laser_scan.angle_increment); + robot::log_info("time_increment: %f", laser_scan.time_increment); + robot::log_info("scan_time: %f", laser_scan.scan_time); + robot::log_info("range_min: %f", laser_scan.range_min); + robot::log_info("range_max: %f", laser_scan.range_max); + robot::log_info("ranges_size: %zu", laser_scan.ranges.size()); + robot::log_info("intensities_size: %zu", laser_scan.intensities.size()); + std::stringstream ranges_str; + for (size_t i = 0; i < laser_scan.ranges.size(); i++) { + ranges_str << laser_scan.ranges[i] << " "; + } + robot::log_info("ranges: %s", ranges_str.str().c_str()); + std::stringstream intensities_str; + for (size_t i = 0; i < laser_scan.intensities.size(); i++) { + intensities_str << laser_scan.intensities[i] << " "; + } + robot::log_info("intensities: %s", intensities_str.str().c_str()); + robot::log_info("--------------------------------"); + updateLocalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); } @@ -659,6 +699,33 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) { + robot::log_info("addOdometry: %s", odometry_name.c_str()); + robot::log_info("odometry header: %s", odometry.header.frame_id.c_str()); + robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str()); + robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x); + robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y); + robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z); + robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x); + robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y); + robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z); + robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w); + std::stringstream pose_covariance_str; + for(int i = 0; i < 36; i++) { + pose_covariance_str << odometry.pose.covariance[i] << " "; + } + robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str()); + robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x); + robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y); + robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z); + robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x); + robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y); + robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z); + std::stringstream twist_covariance_str; + for(int i = 0; i < 36; i++) { + twist_covariance_str << odometry.twist.covariance[i] << " "; + } + robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str()); + odometry_ = odometry; }