Initial commit
This commit is contained in:
@@ -0,0 +1,951 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace RobotNet10.RobotApp.Navigation;
|
||||
|
||||
/// <summary>
|
||||
/// C-compatible structs for Navigation C API interop
|
||||
/// Based on /home/robotics/sonvh/pnkx_nav_core/src/APIs/c_api/include/nav_c_api.h
|
||||
/// </summary>
|
||||
|
||||
/// <summary>
|
||||
/// Navigation states, including planning and controller status
|
||||
/// </summary>
|
||||
public enum NavigationState : int
|
||||
{
|
||||
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
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Point structure (x, y, z)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Point
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Pose2D structure (x, y, theta)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Pose2D
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double theta;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Pose2DStamped
|
||||
{
|
||||
public Header header;
|
||||
public Pose2D pose;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Path2D
|
||||
{
|
||||
public Header header;
|
||||
public IntPtr poses;
|
||||
public nuint poses_count;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Twist2D structure (x, y, theta velocities)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Twist2D
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double theta;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Quaternion structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Quaternion
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
public double w;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Position structure (alias for Point to match C API)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Position
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
// Point is the same as Position in C API
|
||||
// public using Point = Position;
|
||||
|
||||
/// <summary>
|
||||
/// Pose structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Pose
|
||||
{
|
||||
public Point position; // Use Point to match C API exactly
|
||||
public Quaternion orientation;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Header structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Header
|
||||
{
|
||||
public uint seq;
|
||||
public uint sec; // uint32_t
|
||||
public uint nsec;
|
||||
public IntPtr frame_id; // char* - must be allocated and freed
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PoseStamped structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PoseStamped
|
||||
{
|
||||
public Header header;
|
||||
public Pose pose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Twist2DStamped structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Twist2DStamped
|
||||
{
|
||||
public Header header;
|
||||
public Twist2D velocity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Vector3 structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Vector3
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// OccupancyGrid structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct OccupancyGrid
|
||||
{
|
||||
public Header header;
|
||||
public MapMetaData info;
|
||||
public IntPtr data;
|
||||
public nuint data_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct OccupancyGridUpdate
|
||||
{
|
||||
public Header header;
|
||||
public int x;
|
||||
public int y;
|
||||
public uint width;
|
||||
public uint height;
|
||||
public IntPtr data;
|
||||
public nuint data_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Point32
|
||||
{
|
||||
public float x;
|
||||
public float y;
|
||||
public float z;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Polygon
|
||||
{
|
||||
public IntPtr points;
|
||||
public nuint points_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PolygonStamped
|
||||
{
|
||||
public Header header;
|
||||
public Polygon polygon;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PlannerDataOutput
|
||||
{
|
||||
public Path2D plan;
|
||||
public OccupancyGrid costmap;
|
||||
public OccupancyGridUpdate costmap_update;
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool is_costmap_updated;
|
||||
public PolygonStamped footprint;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Time structure (sec, nsec)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Time
|
||||
{
|
||||
public uint sec;
|
||||
public uint nsec;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct MapMetaData
|
||||
{
|
||||
public Time map_load_time; // Time when map was loaded
|
||||
public float resolution; // meters per cell
|
||||
public uint width; // cells
|
||||
public uint height; // cells
|
||||
public Pose origin; // pose of cell (0,0) in map frame
|
||||
}
|
||||
/// <summary>
|
||||
/// Navigation feedback structure
|
||||
/// </summary>
|
||||
|
||||
[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 Odometry
|
||||
{
|
||||
public Header header;
|
||||
public IntPtr child_frame_id; // char* - must be allocated and freed
|
||||
public PoseWithCovariance pose;
|
||||
public TwistWithCovariance twist;
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PoseWithCovariance{
|
||||
public Pose pose;
|
||||
public IntPtr covariance; // double* - array of 36 doubles (6x6 matrix)
|
||||
public nuint covariance_count; // size_t - should be 36
|
||||
}
|
||||
// [StructLayout(LayoutKind.Sequential)]
|
||||
// public struct Position{
|
||||
// public double x;
|
||||
// public double y;
|
||||
// public double z;
|
||||
// }
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct TwistWithCovariance{
|
||||
public Twist twist;
|
||||
public IntPtr covariance; // double* - array of 36 doubles (6x6 matrix)
|
||||
public nuint covariance_count; // size_t - should be 36
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Twist{
|
||||
public Vector3 linear;
|
||||
public Vector3 angular;
|
||||
}
|
||||
// [StructLayout(LayoutKind.Sequential)]
|
||||
// public struct Vector3{
|
||||
// public double x;
|
||||
// public double y;
|
||||
// public double z;
|
||||
// }
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct LaserScan
|
||||
{
|
||||
public Header header;
|
||||
public float angle_min;
|
||||
public float angle_max;
|
||||
public float angle_increment;
|
||||
public float time_increment;
|
||||
public float scan_time;
|
||||
public float range_min;
|
||||
public float range_max;
|
||||
public IntPtr ranges;
|
||||
public nuint ranges_count;
|
||||
public IntPtr intensities;
|
||||
public nuint intensities_count;
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct ControlPoint
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double weight;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct ActionParameter
|
||||
{
|
||||
public IntPtr key; // char*
|
||||
public IntPtr value; // char*
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NodePosition
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double theta;
|
||||
public float allowedDeviationXY;
|
||||
public float allowedDeviationTheta;
|
||||
public IntPtr mapId; // char*
|
||||
public IntPtr mapDescription; // char*
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Trajectory
|
||||
{
|
||||
public uint degree;
|
||||
public IntPtr knotVector; // double*
|
||||
public nuint knotVector_count;
|
||||
public IntPtr controlPoints; // ControlPoint*
|
||||
public nuint controlPoints_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Action
|
||||
{
|
||||
public IntPtr actionType; // char*
|
||||
public IntPtr actionId; // char*
|
||||
public IntPtr actionDescription;// char*
|
||||
public IntPtr blockingType; // char*
|
||||
public IntPtr actionParameters; // ActionParameter*
|
||||
public nuint actionParameters_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Node
|
||||
{
|
||||
public IntPtr nodeId; // char*
|
||||
public int sequenceId;
|
||||
public IntPtr nodeDescription; // char*
|
||||
public byte released;
|
||||
public NodePosition nodePosition;
|
||||
public IntPtr actions; // Action*
|
||||
public nuint actions_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Edge
|
||||
{
|
||||
public IntPtr edgeId; // char*
|
||||
public int sequenceId;
|
||||
public IntPtr edgeDescription; // char*
|
||||
public byte released;
|
||||
public IntPtr startNodeId; // char*
|
||||
public IntPtr endNodeId; // char*
|
||||
public double maxSpeed;
|
||||
public double maxHeight;
|
||||
public double minHeight;
|
||||
public double orientation;
|
||||
public IntPtr orientationType; // char*
|
||||
public IntPtr direction; // char*
|
||||
public byte rotationAllowed;
|
||||
public double maxRotationSpeed;
|
||||
public Trajectory trajectory;
|
||||
public double length;
|
||||
public IntPtr actions; // Action*
|
||||
public nuint actions_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Order
|
||||
{
|
||||
public int headerId;
|
||||
public IntPtr timestamp; // char*
|
||||
public IntPtr version; // char*
|
||||
public IntPtr manufacturer; // char*
|
||||
public IntPtr serialNumber; // char*
|
||||
public IntPtr orderId; // char*
|
||||
public uint orderUpdateId;
|
||||
public IntPtr nodes; // Node*
|
||||
public nuint nodes_count;
|
||||
public IntPtr edges; // Edge*
|
||||
public nuint edges_count;
|
||||
public IntPtr zoneSetId; // char*
|
||||
}
|
||||
/// <summary>
|
||||
/// OrderHandle - wrapper for OrderHandle (void* in C)
|
||||
/// Represents a pointer to a robot_protocol_msgs::Order object
|
||||
/// </summary>
|
||||
public class OrderHandle : IDisposable
|
||||
{
|
||||
private IntPtr _handle;
|
||||
private bool _disposed = false;
|
||||
private readonly bool _ownsHandle;
|
||||
|
||||
/// <summary>
|
||||
/// Create OrderHandle from IntPtr
|
||||
/// </summary>
|
||||
/// <param name="handle">Pointer to Order object</param>
|
||||
/// <param name="ownsHandle">Whether this handle owns the memory (will free on dispose)</param>
|
||||
public OrderHandle(IntPtr handle, bool ownsHandle = false)
|
||||
{
|
||||
_handle = handle;
|
||||
_ownsHandle = ownsHandle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the underlying IntPtr
|
||||
/// </summary>
|
||||
public IntPtr Handle => _handle;
|
||||
|
||||
/// <summary>
|
||||
/// Check if handle is valid (not zero)
|
||||
/// </summary>
|
||||
public bool IsValid => _handle != IntPtr.Zero;
|
||||
|
||||
/// <summary>
|
||||
/// Implicit conversion to IntPtr
|
||||
/// </summary>
|
||||
public static implicit operator IntPtr(OrderHandle orderHandle) => orderHandle?._handle ?? IntPtr.Zero;
|
||||
|
||||
/// <summary>
|
||||
/// Explicit conversion from IntPtr
|
||||
/// </summary>
|
||||
public static explicit operator OrderHandle(IntPtr handle) => new OrderHandle(handle);
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
if (disposing && _ownsHandle && _handle != IntPtr.Zero)
|
||||
{
|
||||
// Free the Order object if we own it
|
||||
// Note: This requires navigation_free_order function
|
||||
// NavigationNativeInterface.navigation_free_order(_handle);
|
||||
_handle = IntPtr.Zero;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
}
|
||||
|
||||
~OrderHandle()
|
||||
{
|
||||
Dispose(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order data structure (C# representation of robot_protocol_msgs::Order)
|
||||
/// This is a simplified version for API usage
|
||||
/// </summary>
|
||||
public class OrderData
|
||||
{
|
||||
public int HeaderId { get; set; }
|
||||
public string Timestamp { get; set; } = string.Empty;
|
||||
public string Version { get; set; } = string.Empty;
|
||||
public string Manufacturer { get; set; } = string.Empty;
|
||||
public string SerialNumber { get; set; } = string.Empty;
|
||||
public string OrderId { get; set; } = string.Empty;
|
||||
public uint OrderUpdateId { get; set; }
|
||||
public string? ZoneSetId { get; set; }
|
||||
public List<OrderNodeData> Nodes { get; set; } = new();
|
||||
public List<OrderEdgeData> Edges { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Node data
|
||||
/// </summary>
|
||||
public class OrderNodeData
|
||||
{
|
||||
public string NodeId { get; set; } = string.Empty;
|
||||
public int SequenceId { get; set; }
|
||||
public string NodeDescription { get; set; } = string.Empty;
|
||||
public bool Released { get; set; }
|
||||
public OrderNodePositionData? NodePosition { get; set; }
|
||||
public List<OrderActionData> Actions { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Node Position data
|
||||
/// </summary>
|
||||
public class OrderNodePositionData
|
||||
{
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double? Theta { get; set; }
|
||||
public double? AllowedDeviationXY { get; set; }
|
||||
public double? AllowedDeviationTheta { get; set; }
|
||||
public string? MapId { get; set; }
|
||||
public string? MapDescription { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Edge data
|
||||
/// </summary>
|
||||
public class OrderEdgeData
|
||||
{
|
||||
public string EdgeId { get; set; } = string.Empty;
|
||||
public int SequenceId { get; set; }
|
||||
public string? EdgeDescription { get; set; }
|
||||
public bool Released { get; set; }
|
||||
public string StartNodeId { get; set; } = string.Empty;
|
||||
public string EndNodeId { get; set; } = string.Empty;
|
||||
public double? MaxSpeed { get; set; }
|
||||
public double? MaxHeight { get; set; }
|
||||
public double? MinHeight { get; set; }
|
||||
public double Orientation { get; set; }
|
||||
/// <summary>VDA5050 can send number (e.g. 1) or string; we accept both via JsonElement.</summary>
|
||||
public JsonElement? OrientationType { get; set; }
|
||||
public string Direction { get; set; } = "forward";
|
||||
public bool? RotationAllowed { get; set; }
|
||||
public double? MaxRotationSpeed { get; set; }
|
||||
public double Length { get; set; }
|
||||
public OrderTrajectoryData? Trajectory { get; set; }
|
||||
public List<OrderActionData> Actions { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Trajectory data
|
||||
/// </summary>
|
||||
public class OrderTrajectoryData
|
||||
{
|
||||
public uint Degree { get; set; }
|
||||
public List<double> KnotVector { get; set; } = new();
|
||||
public List<OrderControlPointData> ControlPoints { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Control Point data
|
||||
/// </summary>
|
||||
public class OrderControlPointData
|
||||
{
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double Weight { get; set; } = 1.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Action data
|
||||
/// </summary>
|
||||
public class OrderActionData
|
||||
{
|
||||
public string ActionType { get; set; } = string.Empty;
|
||||
public string ActionId { get; set; } = string.Empty;
|
||||
public string ActionDescription { get; set; } = string.Empty;
|
||||
public string BlockingType { get; set; } = "NONE";
|
||||
public List<OrderActionParameterData> ActionParameters { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Action Parameter data
|
||||
/// </summary>
|
||||
public class OrderActionParameterData
|
||||
{
|
||||
public string Key { get; set; } = string.Empty;
|
||||
public string Value { get; set; } = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Helper class to convert OrderData to native Order struct
|
||||
/// </summary>
|
||||
public static class OrderConverter
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert OrderData (managed) to Order (native struct)
|
||||
/// </summary>
|
||||
/// <param name="orderData">Managed order data</param>
|
||||
/// <returns>Native Order struct with allocated memory</returns>
|
||||
public static Order ConvertToNativeOrder(OrderData orderData)
|
||||
{
|
||||
var order = new Order
|
||||
{
|
||||
headerId = orderData.HeaderId,
|
||||
timestamp = Marshal.StringToHGlobalAnsi(orderData.Timestamp),
|
||||
version = Marshal.StringToHGlobalAnsi(orderData.Version),
|
||||
manufacturer = Marshal.StringToHGlobalAnsi(orderData.Manufacturer),
|
||||
serialNumber = Marshal.StringToHGlobalAnsi(orderData.SerialNumber),
|
||||
orderId = Marshal.StringToHGlobalAnsi(orderData.OrderId),
|
||||
orderUpdateId = orderData.OrderUpdateId,
|
||||
zoneSetId = Marshal.StringToHGlobalAnsi(orderData.ZoneSetId ?? string.Empty),
|
||||
nodes_count = (nuint)orderData.Nodes.Count,
|
||||
edges_count = (nuint)orderData.Edges.Count
|
||||
};
|
||||
|
||||
// Allocate and convert nodes
|
||||
if (orderData.Nodes.Count > 0)
|
||||
{
|
||||
int nodeSize = Marshal.SizeOf<Node>();
|
||||
order.nodes = Marshal.AllocHGlobal(nodeSize * orderData.Nodes.Count);
|
||||
|
||||
for (int i = 0; i < orderData.Nodes.Count; i++)
|
||||
{
|
||||
var nodeData = orderData.Nodes[i];
|
||||
var node = ConvertToNativeNode(nodeData);
|
||||
IntPtr nodePtr = IntPtr.Add(order.nodes, i * nodeSize);
|
||||
Marshal.StructureToPtr(node, nodePtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
order.nodes = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// Allocate and convert edges
|
||||
if (orderData.Edges.Count > 0)
|
||||
{
|
||||
int edgeSize = Marshal.SizeOf<Edge>();
|
||||
order.edges = Marshal.AllocHGlobal(edgeSize * orderData.Edges.Count);
|
||||
|
||||
for (int i = 0; i < orderData.Edges.Count; i++)
|
||||
{
|
||||
var edgeData = orderData.Edges[i];
|
||||
var edge = ConvertToNativeEdge(edgeData);
|
||||
IntPtr edgePtr = IntPtr.Add(order.edges, i * edgeSize);
|
||||
Marshal.StructureToPtr(edge, edgePtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
order.edges = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return order;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for native Order struct
|
||||
/// </summary>
|
||||
public static void FreeNativeOrder(ref Order order)
|
||||
{
|
||||
// Free string fields
|
||||
if (order.timestamp != IntPtr.Zero) Marshal.FreeHGlobal(order.timestamp);
|
||||
if (order.version != IntPtr.Zero) Marshal.FreeHGlobal(order.version);
|
||||
if (order.manufacturer != IntPtr.Zero) Marshal.FreeHGlobal(order.manufacturer);
|
||||
if (order.serialNumber != IntPtr.Zero) Marshal.FreeHGlobal(order.serialNumber);
|
||||
if (order.orderId != IntPtr.Zero) Marshal.FreeHGlobal(order.orderId);
|
||||
if (order.zoneSetId != IntPtr.Zero) Marshal.FreeHGlobal(order.zoneSetId);
|
||||
|
||||
// Free nodes
|
||||
if (order.nodes != IntPtr.Zero)
|
||||
{
|
||||
int nodeSize = Marshal.SizeOf<Node>();
|
||||
for (int i = 0; i < (int)order.nodes_count; i++)
|
||||
{
|
||||
IntPtr nodePtr = IntPtr.Add(order.nodes, i * nodeSize);
|
||||
Node node = Marshal.PtrToStructure<Node>(nodePtr);
|
||||
FreeNativeNode(ref node);
|
||||
}
|
||||
Marshal.FreeHGlobal(order.nodes);
|
||||
}
|
||||
|
||||
// Free edges
|
||||
if (order.edges != IntPtr.Zero)
|
||||
{
|
||||
int edgeSize = Marshal.SizeOf<Edge>();
|
||||
for (int i = 0; i < (int)order.edges_count; i++)
|
||||
{
|
||||
IntPtr edgePtr = IntPtr.Add(order.edges, i * edgeSize);
|
||||
Edge edge = Marshal.PtrToStructure<Edge>(edgePtr);
|
||||
FreeNativeEdge(ref edge);
|
||||
}
|
||||
Marshal.FreeHGlobal(order.edges);
|
||||
}
|
||||
}
|
||||
|
||||
private static Node ConvertToNativeNode(OrderNodeData nodeData)
|
||||
{
|
||||
var node = new Node
|
||||
{
|
||||
nodeId = Marshal.StringToHGlobalAnsi(nodeData.NodeId),
|
||||
sequenceId = nodeData.SequenceId,
|
||||
nodeDescription = Marshal.StringToHGlobalAnsi(nodeData.NodeDescription),
|
||||
released = (byte)(nodeData.Released ? 1 : 0),
|
||||
actions_count = (nuint)nodeData.Actions.Count
|
||||
};
|
||||
|
||||
// Convert node position
|
||||
if (nodeData.NodePosition != null)
|
||||
{
|
||||
node.nodePosition = new NodePosition
|
||||
{
|
||||
x = nodeData.NodePosition.X,
|
||||
y = nodeData.NodePosition.Y,
|
||||
theta = nodeData.NodePosition.Theta ?? 0.0,
|
||||
allowedDeviationXY = (float)(nodeData.NodePosition.AllowedDeviationXY ?? 0.0),
|
||||
allowedDeviationTheta = (float)(nodeData.NodePosition.AllowedDeviationTheta ?? 0.0),
|
||||
mapId = Marshal.StringToHGlobalAnsi(nodeData.NodePosition.MapId ?? string.Empty),
|
||||
mapDescription = Marshal.StringToHGlobalAnsi(nodeData.NodePosition.MapDescription ?? string.Empty)
|
||||
};
|
||||
}
|
||||
|
||||
// Convert actions
|
||||
if (nodeData.Actions.Count > 0)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
node.actions = Marshal.AllocHGlobal(actionSize * nodeData.Actions.Count);
|
||||
|
||||
for (int i = 0; i < nodeData.Actions.Count; i++)
|
||||
{
|
||||
var actionData = nodeData.Actions[i];
|
||||
var action = ConvertToNativeAction(actionData);
|
||||
IntPtr actionPtr = IntPtr.Add(node.actions, i * actionSize);
|
||||
Marshal.StructureToPtr(action, actionPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
node.actions = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
private static void FreeNativeNode(ref Node node)
|
||||
{
|
||||
if (node.nodeId != IntPtr.Zero) Marshal.FreeHGlobal(node.nodeId);
|
||||
if (node.nodeDescription != IntPtr.Zero) Marshal.FreeHGlobal(node.nodeDescription);
|
||||
if (node.nodePosition.mapId != IntPtr.Zero) Marshal.FreeHGlobal(node.nodePosition.mapId);
|
||||
if (node.nodePosition.mapDescription != IntPtr.Zero) Marshal.FreeHGlobal(node.nodePosition.mapDescription);
|
||||
|
||||
if (node.actions != IntPtr.Zero)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
for (int i = 0; i < (int)node.actions_count; i++)
|
||||
{
|
||||
IntPtr actionPtr = IntPtr.Add(node.actions, i * actionSize);
|
||||
Action action = Marshal.PtrToStructure<Action>(actionPtr);
|
||||
FreeNativeAction(ref action);
|
||||
}
|
||||
Marshal.FreeHGlobal(node.actions);
|
||||
}
|
||||
}
|
||||
|
||||
private static string OrientationTypeToString(JsonElement? je)
|
||||
{
|
||||
if (!je.HasValue) return string.Empty;
|
||||
var e = je.Value;
|
||||
if (e.ValueKind == JsonValueKind.Number) return e.GetDouble().ToString(System.Globalization.CultureInfo.InvariantCulture);
|
||||
if (e.ValueKind == JsonValueKind.String) return e.GetString() ?? string.Empty;
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
private static Edge ConvertToNativeEdge(OrderEdgeData edgeData)
|
||||
{
|
||||
var edge = new Edge
|
||||
{
|
||||
edgeId = Marshal.StringToHGlobalAnsi(edgeData.EdgeId),
|
||||
sequenceId = edgeData.SequenceId,
|
||||
edgeDescription = Marshal.StringToHGlobalAnsi(edgeData.EdgeDescription ?? string.Empty),
|
||||
released = (byte)(edgeData.Released ? 1 : 0),
|
||||
startNodeId = Marshal.StringToHGlobalAnsi(edgeData.StartNodeId),
|
||||
endNodeId = Marshal.StringToHGlobalAnsi(edgeData.EndNodeId),
|
||||
maxSpeed = edgeData.MaxSpeed ?? 0.0,
|
||||
maxHeight = edgeData.MaxHeight ?? 0.0,
|
||||
minHeight = edgeData.MinHeight ?? 0.0,
|
||||
orientation = edgeData.Orientation,
|
||||
orientationType = Marshal.StringToHGlobalAnsi(OrientationTypeToString(edgeData.OrientationType)),
|
||||
direction = Marshal.StringToHGlobalAnsi(edgeData.Direction ?? string.Empty),
|
||||
rotationAllowed = (byte)((edgeData.RotationAllowed ?? false) ? 1 : 0),
|
||||
maxRotationSpeed = edgeData.MaxRotationSpeed ?? 0.0,
|
||||
length = edgeData.Length,
|
||||
actions_count = (nuint)edgeData.Actions.Count
|
||||
};
|
||||
|
||||
// Convert trajectory
|
||||
if (edgeData.Trajectory != null)
|
||||
{
|
||||
edge.trajectory = ConvertToNativeTrajectory(edgeData.Trajectory);
|
||||
}
|
||||
|
||||
// Convert actions
|
||||
if (edgeData.Actions.Count > 0)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
edge.actions = Marshal.AllocHGlobal(actionSize * edgeData.Actions.Count);
|
||||
|
||||
for (int i = 0; i < edgeData.Actions.Count; i++)
|
||||
{
|
||||
var actionData = edgeData.Actions[i];
|
||||
var action = ConvertToNativeAction(actionData);
|
||||
IntPtr actionPtr = IntPtr.Add(edge.actions, i * actionSize);
|
||||
Marshal.StructureToPtr(action, actionPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
edge.actions = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return edge;
|
||||
}
|
||||
|
||||
private static void FreeNativeEdge(ref Edge edge)
|
||||
{
|
||||
if (edge.edgeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.edgeId);
|
||||
if (edge.edgeDescription != IntPtr.Zero) Marshal.FreeHGlobal(edge.edgeDescription);
|
||||
if (edge.startNodeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.startNodeId);
|
||||
if (edge.endNodeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.endNodeId);
|
||||
if (edge.orientationType != IntPtr.Zero) Marshal.FreeHGlobal(edge.orientationType);
|
||||
if (edge.direction != IntPtr.Zero) Marshal.FreeHGlobal(edge.direction);
|
||||
|
||||
FreeNativeTrajectory(ref edge.trajectory);
|
||||
|
||||
if (edge.actions != IntPtr.Zero)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
for (int i = 0; i < (int)edge.actions_count; i++)
|
||||
{
|
||||
IntPtr actionPtr = IntPtr.Add(edge.actions, i * actionSize);
|
||||
Action action = Marshal.PtrToStructure<Action>(actionPtr);
|
||||
FreeNativeAction(ref action);
|
||||
}
|
||||
Marshal.FreeHGlobal(edge.actions);
|
||||
}
|
||||
}
|
||||
|
||||
private static Trajectory ConvertToNativeTrajectory(OrderTrajectoryData trajectoryData)
|
||||
{
|
||||
var trajectory = new Trajectory
|
||||
{
|
||||
degree = trajectoryData.Degree,
|
||||
knotVector_count = (nuint)trajectoryData.KnotVector.Count,
|
||||
controlPoints_count = (nuint)trajectoryData.ControlPoints.Count
|
||||
};
|
||||
|
||||
// Allocate knot vector
|
||||
if (trajectoryData.KnotVector.Count > 0)
|
||||
{
|
||||
trajectory.knotVector = Marshal.AllocHGlobal(sizeof(double) * trajectoryData.KnotVector.Count);
|
||||
Marshal.Copy(trajectoryData.KnotVector.ToArray(), 0, trajectory.knotVector, trajectoryData.KnotVector.Count);
|
||||
}
|
||||
else
|
||||
{
|
||||
trajectory.knotVector = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// Allocate control points
|
||||
if (trajectoryData.ControlPoints.Count > 0)
|
||||
{
|
||||
int cpSize = Marshal.SizeOf<ControlPoint>();
|
||||
trajectory.controlPoints = Marshal.AllocHGlobal(cpSize * trajectoryData.ControlPoints.Count);
|
||||
|
||||
for (int i = 0; i < trajectoryData.ControlPoints.Count; i++)
|
||||
{
|
||||
var cpData = trajectoryData.ControlPoints[i];
|
||||
var cp = new ControlPoint
|
||||
{
|
||||
x = cpData.X,
|
||||
y = cpData.Y,
|
||||
weight = cpData.Weight
|
||||
};
|
||||
IntPtr cpPtr = IntPtr.Add(trajectory.controlPoints, i * cpSize);
|
||||
Marshal.StructureToPtr(cp, cpPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
trajectory.controlPoints = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return trajectory;
|
||||
}
|
||||
|
||||
private static void FreeNativeTrajectory(ref Trajectory trajectory)
|
||||
{
|
||||
if (trajectory.knotVector != IntPtr.Zero) Marshal.FreeHGlobal(trajectory.knotVector);
|
||||
if (trajectory.controlPoints != IntPtr.Zero) Marshal.FreeHGlobal(trajectory.controlPoints);
|
||||
}
|
||||
|
||||
private static Action ConvertToNativeAction(OrderActionData actionData)
|
||||
{
|
||||
var action = new Action
|
||||
{
|
||||
actionType = Marshal.StringToHGlobalAnsi(actionData.ActionType),
|
||||
actionId = Marshal.StringToHGlobalAnsi(actionData.ActionId),
|
||||
actionDescription = Marshal.StringToHGlobalAnsi(actionData.ActionDescription),
|
||||
blockingType = Marshal.StringToHGlobalAnsi(actionData.BlockingType),
|
||||
actionParameters_count = (nuint)actionData.ActionParameters.Count
|
||||
};
|
||||
|
||||
// Convert parameters
|
||||
if (actionData.ActionParameters.Count > 0)
|
||||
{
|
||||
int paramSize = Marshal.SizeOf<ActionParameter>();
|
||||
action.actionParameters = Marshal.AllocHGlobal(paramSize * actionData.ActionParameters.Count);
|
||||
|
||||
for (int i = 0; i < actionData.ActionParameters.Count; i++)
|
||||
{
|
||||
var paramData = actionData.ActionParameters[i];
|
||||
var param = new ActionParameter
|
||||
{
|
||||
key = Marshal.StringToHGlobalAnsi(paramData.Key),
|
||||
value = Marshal.StringToHGlobalAnsi(paramData.Value)
|
||||
};
|
||||
IntPtr paramPtr = IntPtr.Add(action.actionParameters, i * paramSize);
|
||||
Marshal.StructureToPtr(param, paramPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
action.actionParameters = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return action;
|
||||
}
|
||||
|
||||
private static void FreeNativeAction(ref Action action)
|
||||
{
|
||||
if (action.actionType != IntPtr.Zero) Marshal.FreeHGlobal(action.actionType);
|
||||
if (action.actionId != IntPtr.Zero) Marshal.FreeHGlobal(action.actionId);
|
||||
if (action.actionDescription != IntPtr.Zero) Marshal.FreeHGlobal(action.actionDescription);
|
||||
if (action.blockingType != IntPtr.Zero) Marshal.FreeHGlobal(action.blockingType);
|
||||
|
||||
if (action.actionParameters != IntPtr.Zero)
|
||||
{
|
||||
int paramSize = Marshal.SizeOf<ActionParameter>();
|
||||
for (int i = 0; i < (int)action.actionParameters_count; i++)
|
||||
{
|
||||
IntPtr paramPtr = IntPtr.Add(action.actionParameters, i * paramSize);
|
||||
ActionParameter param = Marshal.PtrToStructure<ActionParameter>(paramPtr);
|
||||
if (param.key != IntPtr.Zero) Marshal.FreeHGlobal(param.key);
|
||||
if (param.value != IntPtr.Zero) Marshal.FreeHGlobal(param.value);
|
||||
}
|
||||
Marshal.FreeHGlobal(action.actionParameters);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user