951 lines
30 KiB
C#
951 lines
30 KiB
C#
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);
|
|
}
|
|
}
|
|
} |