375 lines
14 KiB
C#
375 lines
14 KiB
C#
using System.Runtime.InteropServices;
|
|
using RobotNet10.RobotApp.Xloc;
|
|
using RobotNet10.Shared;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Sensor;
|
|
|
|
namespace RobotNet10.RobotApp.Navigation;
|
|
|
|
/// <summary>
|
|
/// Extension methods to convert C# data structures to Navigation C API handles
|
|
/// </summary>
|
|
public static class NavigationConversionExtensions
|
|
{
|
|
/// <summary>
|
|
/// Convert xloc_occupancy_grid_t to OccupancyGrid
|
|
/// NOTE: Caller must free allocated memory using FreeNavigationOccupancyGrid
|
|
/// </summary>
|
|
public static OccupancyGrid ToNavigationOccupancyGrid(this xloc_occupancy_grid_t xlocGrid)
|
|
{
|
|
var navGrid = new OccupancyGrid
|
|
{
|
|
info = new MapMetaData
|
|
{
|
|
map_load_time = NavigationNativeInterface.time_create(),
|
|
resolution = xlocGrid.resolution,
|
|
width = xlocGrid.width,
|
|
height = xlocGrid.height,
|
|
origin = new Pose
|
|
{
|
|
position = new Point
|
|
{
|
|
x = xlocGrid.origin.position[0],
|
|
y = xlocGrid.origin.position[1],
|
|
z = xlocGrid.origin.position[2]
|
|
},
|
|
orientation = new Quaternion
|
|
{
|
|
x = xlocGrid.origin.orientation[0],
|
|
y = xlocGrid.origin.orientation[1],
|
|
z = xlocGrid.origin.orientation[2],
|
|
w = xlocGrid.origin.orientation[3]
|
|
}
|
|
}
|
|
},
|
|
data = IntPtr.Zero
|
|
};
|
|
|
|
// Marshal frame_id string
|
|
if (xlocGrid.header.frame_id != IntPtr.Zero)
|
|
{
|
|
navGrid.header.frame_id = Marshal.StringToHGlobalAnsi(
|
|
Marshal.PtrToStringAnsi(xlocGrid.header.frame_id) ?? string.Empty);
|
|
}
|
|
|
|
// Copy occupancy data
|
|
if (xlocGrid.data != IntPtr.Zero && xlocGrid.data_length > 0)
|
|
{
|
|
navGrid.data = Marshal.AllocHGlobal((int)xlocGrid.data_length);
|
|
navGrid.data_count = new UIntPtr(xlocGrid.data_length);
|
|
byte[] data = new byte[xlocGrid.data_length];
|
|
Marshal.Copy(xlocGrid.data, data, 0, (int)xlocGrid.data_length);
|
|
Marshal.Copy(data, 0, navGrid.data, (int)xlocGrid.data_length);
|
|
}
|
|
else
|
|
{
|
|
// Ensure data_count is set to 0 when no data
|
|
navGrid.data_count = UIntPtr.Zero;
|
|
}
|
|
|
|
return navGrid;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert Shared.Sensor.LaserScan to NavigationInterop.LaserScan
|
|
/// NOTE: Caller must free allocated memory using FreeNavigationLaserScan
|
|
/// </summary>
|
|
public static LaserScan ToNavigationLaserScan(this global::RobotNet10.Shared.Sensor.LaserScan scan)
|
|
{
|
|
// Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
|
|
var timeSpan = scan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
|
|
Header scanHeader = new Header
|
|
{
|
|
seq = scan.Header.Seq,
|
|
sec = (uint)timeSpan.TotalSeconds,
|
|
nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
|
|
frame_id = Marshal.StringToHGlobalAnsi(scan.Header.FrameId ?? string.Empty)
|
|
};
|
|
|
|
var navScan = new LaserScan
|
|
{
|
|
header = scanHeader,
|
|
angle_min = (float)scan.AngleMin,
|
|
angle_max = (float)scan.AngleMax,
|
|
angle_increment = (float)scan.AngleIncrement,
|
|
time_increment = (float)scan.TimeIncrement,
|
|
scan_time = (float)scan.ScanTime,
|
|
range_min = (float)scan.RangeMin,
|
|
range_max = (float)scan.RangeMax,
|
|
ranges = IntPtr.Zero,
|
|
ranges_count = 0,
|
|
intensities = IntPtr.Zero,
|
|
intensities_count = 0,
|
|
};
|
|
|
|
// Allocate and copy ranges array
|
|
if (scan.Ranges != null && scan.Ranges.Length > 0)
|
|
{
|
|
float[] rangesFloat = Array.ConvertAll(scan.Ranges, value => (float)value);
|
|
int rangesSize = rangesFloat.Length * sizeof(float);
|
|
navScan.ranges = Marshal.AllocHGlobal(rangesSize);
|
|
Marshal.Copy(rangesFloat, 0, navScan.ranges, rangesFloat.Length);
|
|
navScan.ranges_count = (nuint)rangesFloat.Length;
|
|
}
|
|
|
|
// Allocate and copy intensities array
|
|
if (scan.Intensities != null && scan.Intensities.Length > 0)
|
|
{
|
|
float[] intensitiesFloat = Array.ConvertAll(scan.Intensities, value => (float)value);
|
|
int intensitiesSize = intensitiesFloat.Length * sizeof(float);
|
|
navScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
|
|
Marshal.Copy(intensitiesFloat, 0, navScan.intensities, intensitiesFloat.Length);
|
|
navScan.intensities_count = (nuint)intensitiesFloat.Length;
|
|
}
|
|
|
|
return navScan;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert Odometry to Odometry
|
|
/// NOTE: Caller must free allocated memory using FreeNavigationOdometry
|
|
/// </summary>
|
|
public static Odometry ToNavigationOdometry(this global::RobotNet10.Shared.Sensor.Odometry odom)
|
|
{
|
|
// Create header using header_create (like in Program.cs)
|
|
Header odomHeader = NavigationNativeInterface.header_create(odom.Header.FrameId ?? string.Empty);
|
|
|
|
// Update header with sequence and timestamp
|
|
odomHeader.seq = odom.Header.Seq;
|
|
odomHeader.sec = (uint)(odom.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch).TotalSeconds;
|
|
odomHeader.nsec = (uint)(((odom.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch).Ticks % TimeSpan.TicksPerSecond) * 100);
|
|
|
|
var navOdom = new Odometry
|
|
{
|
|
header = odomHeader,
|
|
child_frame_id = IntPtr.Zero,
|
|
pose = new PoseWithCovariance
|
|
{
|
|
pose = new Pose
|
|
{
|
|
position = new Point
|
|
{
|
|
x = odom.Pose.Pose.Position.X,
|
|
y = odom.Pose.Pose.Position.Y,
|
|
z = odom.Pose.Pose.Position.Z
|
|
},
|
|
orientation = new Quaternion
|
|
{
|
|
x = odom.Pose.Pose.Orientation.X,
|
|
y = odom.Pose.Pose.Orientation.Y,
|
|
z = odom.Pose.Pose.Orientation.Z,
|
|
w = odom.Pose.Pose.Orientation.W
|
|
}
|
|
},
|
|
covariance = IntPtr.Zero,
|
|
covariance_count = UIntPtr.Zero
|
|
},
|
|
twist = new TwistWithCovariance
|
|
{
|
|
twist = new Twist
|
|
{
|
|
linear = new Vector3
|
|
{
|
|
x = odom.Twist.Twist.Linear.X,
|
|
y = odom.Twist.Twist.Linear.Y,
|
|
z = odom.Twist.Twist.Linear.Z
|
|
},
|
|
angular = new Vector3
|
|
{
|
|
x = odom.Twist.Twist.Angular.X,
|
|
y = odom.Twist.Twist.Angular.Y,
|
|
z = odom.Twist.Twist.Angular.Z
|
|
}
|
|
},
|
|
covariance = IntPtr.Zero,
|
|
covariance_count = UIntPtr.Zero
|
|
}
|
|
};
|
|
|
|
// Marshal child_frame_id string
|
|
if (!string.IsNullOrEmpty(odom.ChildFrameId))
|
|
{
|
|
navOdom.child_frame_id = Marshal.StringToHGlobalAnsi(odom.ChildFrameId);
|
|
}
|
|
|
|
// Allocate and copy pose covariance
|
|
if (odom.Pose.Covariance != null && odom.Pose.Covariance.Length == 36)
|
|
{
|
|
int covarianceSize = 36 * sizeof(double);
|
|
navOdom.pose.covariance = Marshal.AllocHGlobal(covarianceSize);
|
|
Marshal.Copy(odom.Pose.Covariance, 0, navOdom.pose.covariance, 36);
|
|
navOdom.pose.covariance_count = new UIntPtr(36);
|
|
}
|
|
|
|
// Allocate and copy twist covariance
|
|
if (odom.Twist.Covariance != null && odom.Twist.Covariance.Length == 36)
|
|
{
|
|
int covarianceSize = 36 * sizeof(double);
|
|
navOdom.twist.covariance = Marshal.AllocHGlobal(covarianceSize);
|
|
Marshal.Copy(odom.Twist.Covariance, 0, navOdom.twist.covariance, 36);
|
|
navOdom.twist.covariance_count = new UIntPtr(36);
|
|
}
|
|
|
|
return navOdom;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Allocate unmanaged memory for OccupancyGrid and return IntPtr
|
|
/// NOTE: Caller must free using FreeNavigationOccupancyGridPtr
|
|
/// </summary>
|
|
public static IntPtr AllocateNavigationOccupancyGrid(OccupancyGrid grid)
|
|
{
|
|
int size = Marshal.SizeOf<OccupancyGrid>();
|
|
IntPtr ptr = Marshal.AllocHGlobal(size);
|
|
Marshal.StructureToPtr(grid, ptr, false);
|
|
return ptr;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Allocate unmanaged memory for LaserScanHandle and return IntPtr
|
|
/// NOTE: Caller must free using FreeNavigationLaserScanPtr
|
|
/// </summary>
|
|
public static IntPtr AllocateNavigationLaserScan(LaserScan scan)
|
|
{
|
|
int size = Marshal.SizeOf<LaserScan>();
|
|
IntPtr ptr = Marshal.AllocHGlobal(size);
|
|
Marshal.StructureToPtr(scan, ptr, false);
|
|
return ptr;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Allocate unmanaged memory for Odometry and return IntPtr
|
|
/// NOTE: Caller must free using FreeNavigationOdometryPtr
|
|
/// </summary>
|
|
public static IntPtr AllocateNavigationOdometry(Odometry odom)
|
|
{
|
|
int size = Marshal.SizeOf<Odometry>();
|
|
IntPtr ptr = Marshal.AllocHGlobal(size);
|
|
Marshal.StructureToPtr(odom, ptr, false);
|
|
return ptr;
|
|
}
|
|
|
|
#region Memory Management
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for OccupancyGrid (both struct and IntPtr)
|
|
/// </summary>
|
|
public static void FreeNavigationOccupancyGridPtr(IntPtr ptr)
|
|
{
|
|
if (ptr == IntPtr.Zero)
|
|
return;
|
|
|
|
var grid = Marshal.PtrToStructure<OccupancyGrid>(ptr);
|
|
FreeNavigationOccupancyGrid(ref grid);
|
|
Marshal.FreeHGlobal(ptr);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for OccupancyGrid
|
|
/// </summary>
|
|
public static void FreeNavigationOccupancyGrid(ref OccupancyGrid grid)
|
|
{
|
|
if (grid.header.frame_id != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(grid.header.frame_id);
|
|
grid.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
if (grid.data != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(grid.data);
|
|
grid.data = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for LaserScanHandle (both struct and IntPtr)
|
|
/// </summary>
|
|
public static void FreeNavigationLaserScanPtr(IntPtr ptr)
|
|
{
|
|
if (ptr == IntPtr.Zero)
|
|
return;
|
|
|
|
var scan = Marshal.PtrToStructure<LaserScan>(ptr);
|
|
FreeNavigationLaserScan(ref scan);
|
|
Marshal.FreeHGlobal(ptr);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for LaserScanHandle
|
|
/// </summary>
|
|
public static void FreeNavigationLaserScan(ref LaserScan scan)
|
|
{
|
|
// frame_id is allocated by Marshal.StringToHGlobalAnsi(), so use Marshal.FreeHGlobal()
|
|
if (scan.header.frame_id != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(scan.header.frame_id);
|
|
scan.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
// ranges and intensities are allocated by Marshal.AllocHGlobal(), so use Marshal.FreeHGlobal()
|
|
if (scan.ranges != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(scan.ranges);
|
|
scan.ranges = IntPtr.Zero;
|
|
}
|
|
|
|
if (scan.intensities != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(scan.intensities);
|
|
scan.intensities = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for Odometry (both struct and IntPtr)
|
|
/// </summary>
|
|
public static void FreeNavigationOdometryPtr(IntPtr ptr)
|
|
{
|
|
if (ptr == IntPtr.Zero)
|
|
return;
|
|
|
|
var odom = Marshal.PtrToStructure<Odometry>(ptr);
|
|
FreeNavigationOdometry(ref odom);
|
|
Marshal.FreeHGlobal(ptr);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for Odometry
|
|
/// </summary>
|
|
public static void FreeNavigationOdometry(ref Odometry odom)
|
|
{
|
|
// header.frame_id is allocated by header_create() using strdup() (malloc), so use nav_c_api_free_string()
|
|
if (odom.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(odom.header.frame_id);
|
|
odom.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
// child_frame_id is allocated by Marshal.StringToHGlobalAnsi(), so use Marshal.FreeHGlobal()
|
|
if (odom.child_frame_id != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(odom.child_frame_id);
|
|
odom.child_frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
// covariance arrays are allocated by Marshal.AllocHGlobal(), so use Marshal.FreeHGlobal()
|
|
if (odom.pose.covariance != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(odom.pose.covariance);
|
|
odom.pose.covariance = IntPtr.Zero;
|
|
odom.pose.covariance_count = UIntPtr.Zero;
|
|
}
|
|
|
|
if (odom.twist.covariance != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(odom.twist.covariance);
|
|
odom.twist.covariance = IntPtr.Zero;
|
|
odom.twist.covariance_count = UIntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
|