QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

374
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/NavigationConversionExtensions.cs Normal file
View File

@@ -0,0 +1,374 @@
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
}