Initial commit
This commit is contained in:
@@ -0,0 +1,357 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
// using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods to convert C# sensor structs to xloc C-compatible structs
|
||||
/// </summary>
|
||||
public static class XlocConversionExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert C# Header to xloc_header_t
|
||||
/// NOTE: Caller must free FrameId using Marshal.FreeHGlobal
|
||||
/// </summary>
|
||||
public static xloc_header_t ToXlocHeader(this Header header)
|
||||
{
|
||||
var xlocHeader = new xloc_header_t
|
||||
{
|
||||
seq = header.Seq,
|
||||
stamp = header.Stamp.ToXlocUnixTime(),
|
||||
frame_id = IntPtr.Zero
|
||||
};
|
||||
|
||||
// Marshal frame_id string to unmanaged memory
|
||||
if (!string.IsNullOrEmpty(header.FrameId))
|
||||
{
|
||||
xlocHeader.frame_id = Marshal.StringToHGlobalAnsi(header.FrameId);
|
||||
}
|
||||
|
||||
return xlocHeader;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert DateTime to xloc_unix_time_t (Unix epoch time)
|
||||
/// </summary>
|
||||
// public static xloc_unix_time_t ToXlocUnixTime(this DateTime timestamp)
|
||||
// {
|
||||
// // Convert to Unix time (seconds and nanoseconds since 1970-01-01)
|
||||
// var epoch = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
|
||||
// var duration = timestamp.ToUniversalTime() - epoch;
|
||||
|
||||
// var totalSeconds = (long)duration.TotalSeconds;
|
||||
// var nanoseconds = (long)((duration.TotalSeconds - totalSeconds) * 1_000_000_000);
|
||||
|
||||
// var xlocTime = new xloc_unix_time_t
|
||||
// {
|
||||
// sec = (uint)totalSeconds,
|
||||
// nsec = (uint)nanoseconds
|
||||
// };
|
||||
// return xlocTime;
|
||||
// }
|
||||
public static xloc_unix_time_t ToXlocUnixTime(this DateTime timestamp)
|
||||
{
|
||||
var utc = timestamp.Kind == DateTimeKind.Utc
|
||||
? timestamp
|
||||
: timestamp.ToUniversalTime();
|
||||
|
||||
long ticksSinceEpoch = utc.Ticks - DateTime.UnixEpoch.Ticks;
|
||||
long totalNanoseconds = ticksSinceEpoch * 100; // 1 tick = 100 ns
|
||||
|
||||
uint sec = (uint)(totalNanoseconds / 1_000_000_000);
|
||||
uint nsec = (uint)(totalNanoseconds % 1_000_000_000);
|
||||
|
||||
return new xloc_unix_time_t
|
||||
{
|
||||
sec = sec,
|
||||
nsec = nsec
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Convert C# Odometry to xloc_odometry_t
|
||||
/// NOTE: Caller must free frame_id and child_frame_id using FreeXlocOdometry
|
||||
/// </summary>
|
||||
public static xloc_odometry_t ToXlocOdometry(this Odometry odom)
|
||||
{
|
||||
var xlocOdom = new xloc_odometry_t
|
||||
{
|
||||
header = odom.Header.ToXlocHeader(),
|
||||
child_frame_id = Marshal.StringToHGlobalAnsi(odom.ChildFrameId ?? string.Empty),
|
||||
|
||||
// Pose - flattened arrays (matching C API)
|
||||
pose_position = new double[]
|
||||
{
|
||||
odom.Pose.Pose.Position.X,
|
||||
odom.Pose.Pose.Position.Y,
|
||||
odom.Pose.Pose.Position.Z
|
||||
},
|
||||
pose_orientation = new double[]
|
||||
{
|
||||
odom.Pose.Pose.Orientation.X,
|
||||
odom.Pose.Pose.Orientation.Y,
|
||||
odom.Pose.Pose.Orientation.Z,
|
||||
odom.Pose.Pose.Orientation.W
|
||||
},
|
||||
pose_covariance = odom.Pose.Covariance ?? new double[36],
|
||||
|
||||
// Twist - flattened arrays (matching C API)
|
||||
twist_linear = new double[]
|
||||
{
|
||||
odom.Twist.Twist.Linear.X,
|
||||
odom.Twist.Twist.Linear.Y,
|
||||
odom.Twist.Twist.Linear.Z
|
||||
},
|
||||
twist_angular = new double[]
|
||||
{
|
||||
odom.Twist.Twist.Angular.X,
|
||||
odom.Twist.Twist.Angular.Y,
|
||||
odom.Twist.Twist.Angular.Z
|
||||
},
|
||||
twist_covariance = odom.Twist.Covariance ?? new double[36]
|
||||
};
|
||||
|
||||
return xlocOdom;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert C# Imu to xloc_imu_t
|
||||
/// NOTE: Caller must free FrameId using FreeXlocImu
|
||||
/// </summary>
|
||||
public static xloc_imu_t ToXlocImu(this Imu imu)
|
||||
{
|
||||
// Validate and normalize quaternion
|
||||
var quat = imu.Orientation;
|
||||
double quatLength = Math.Sqrt(quat.X * quat.X + quat.Y * quat.Y + quat.Z * quat.Z + quat.W * quat.W);
|
||||
|
||||
if (quatLength < 0.001 || double.IsNaN(quatLength) || double.IsInfinity(quatLength))
|
||||
{
|
||||
// Invalid quaternion, use identity
|
||||
Console.WriteLine("[XLOC] Invalid IMU quaternion detected, using identity");
|
||||
quat = new RobotNet10.Shared.Geometry.Quaternion { X = 0, Y = 0, Z = 0, W = 1 };
|
||||
quatLength = 1.0;
|
||||
}
|
||||
|
||||
// Normalize quaternion
|
||||
quat = new RobotNet10.Shared.Geometry.Quaternion
|
||||
{
|
||||
X = quat.X / quatLength,
|
||||
Y = quat.Y / quatLength,
|
||||
Z = quat.Z / quatLength,
|
||||
W = quat.W / quatLength
|
||||
};
|
||||
|
||||
// Validate angular velocity (clamp extremely large values)
|
||||
const double MAX_ANGULAR_VEL = 10.0; // rad/s
|
||||
var angVel = imu.AngularVelocity;
|
||||
if (Math.Abs(angVel.X) > MAX_ANGULAR_VEL || Math.Abs(angVel.Y) > MAX_ANGULAR_VEL ||
|
||||
Math.Abs(angVel.Z) > MAX_ANGULAR_VEL ||
|
||||
double.IsNaN(angVel.X) || double.IsNaN(angVel.Y) || double.IsNaN(angVel.Z))
|
||||
{
|
||||
Console.WriteLine($"[XLOC] Invalid angular velocity detected: ({angVel.X}, {angVel.Y}, {angVel.Z}), clamping");
|
||||
angVel = new Vector3
|
||||
{
|
||||
X = Math.Clamp(double.IsNaN(angVel.X) ? 0 : angVel.X, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL),
|
||||
Y = Math.Clamp(double.IsNaN(angVel.Y) ? 0 : angVel.Y, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL),
|
||||
Z = Math.Clamp(double.IsNaN(angVel.Z) ? 0 : angVel.Z, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL)
|
||||
};
|
||||
}
|
||||
|
||||
var xlocImu = new xloc_imu_t
|
||||
{
|
||||
header = imu.Header.ToXlocHeader(),
|
||||
|
||||
orientation = new double[4]
|
||||
{
|
||||
quat.X,
|
||||
quat.Y,
|
||||
quat.Z,
|
||||
quat.W
|
||||
},
|
||||
orientation_covariance = imu.OrientationCovariance ?? new double[9],
|
||||
|
||||
angular_velocity = new double[3]
|
||||
{
|
||||
angVel.X,
|
||||
angVel.Y,
|
||||
angVel.Z
|
||||
},
|
||||
angular_velocity_covariance = imu.AngularVelocityCovariance ?? new double[9],
|
||||
|
||||
linear_acceleration = new double[3]
|
||||
{
|
||||
imu.LinearAcceleration.X,
|
||||
imu.LinearAcceleration.Y,
|
||||
imu.LinearAcceleration.Z
|
||||
},
|
||||
linear_acceleration_covariance = imu.LinearAccelerationCovariance ?? new double[9]
|
||||
};
|
||||
|
||||
return xlocImu;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert C# LaserScan to xloc_laserscan_t
|
||||
/// NOTE: Caller must free all allocated memory using FreeXlocLaserScan
|
||||
/// </summary>
|
||||
public static xloc_laserscan_t ToXlocLaserScan(this LaserScan scan)
|
||||
{
|
||||
var xlocScan = new xloc_laserscan_t
|
||||
{
|
||||
header = scan.Header.ToXlocHeader(),
|
||||
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_length = (nuint)(scan.Ranges?.Length ?? 0),
|
||||
intensities_length = 0
|
||||
};
|
||||
|
||||
// Allocate and copy ranges array with validation
|
||||
if (scan.Ranges != null && scan.Ranges.Length > 0)
|
||||
{
|
||||
// Sanitize ranges: replace NaN/Infinity/negative with max range
|
||||
var sanitizedRanges = new float[scan.Ranges.Length];
|
||||
int invalidCount = 0;
|
||||
|
||||
for (int i = 0; i < scan.Ranges.Length; i++)
|
||||
{
|
||||
float range = (float)scan.Ranges[i];
|
||||
if (float.IsNaN(range) || float.IsInfinity(range) || range < 0)
|
||||
{
|
||||
sanitizedRanges[i] = (float)scan.RangeMax;
|
||||
invalidCount++;
|
||||
}
|
||||
else
|
||||
{
|
||||
sanitizedRanges[i] = range;
|
||||
}
|
||||
}
|
||||
|
||||
if (invalidCount > 0)
|
||||
{
|
||||
// Console.WriteLine($"[XLOC] Sanitized {invalidCount}/{scan.Ranges.Length} invalid laser scan ranges");
|
||||
}
|
||||
|
||||
int rangesSize = sanitizedRanges.Length * sizeof(float);
|
||||
xlocScan.ranges = Marshal.AllocHGlobal(rangesSize);
|
||||
Marshal.Copy(sanitizedRanges, 0, xlocScan.ranges, sanitizedRanges.Length);
|
||||
}
|
||||
else
|
||||
{
|
||||
xlocScan.ranges = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// Intensities are optional; when provided they should match ranges count.
|
||||
if (scan.Intensities != null && scan.Intensities.Length > 0)
|
||||
{
|
||||
int expectedLength = scan.Ranges?.Length ?? 0;
|
||||
if (scan.Intensities.Length == expectedLength)
|
||||
{
|
||||
var sanitizedIntensities = new float[scan.Intensities.Length];
|
||||
for (int i = 0; i < scan.Intensities.Length; i++)
|
||||
{
|
||||
float intensity = (float)scan.Intensities[i];
|
||||
sanitizedIntensities[i] = (float.IsNaN(intensity) || float.IsInfinity(intensity)) ? 0f : intensity;
|
||||
}
|
||||
|
||||
int intensitiesSize = sanitizedIntensities.Length * sizeof(float);
|
||||
xlocScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
|
||||
Marshal.Copy(sanitizedIntensities, 0, xlocScan.intensities, sanitizedIntensities.Length);
|
||||
xlocScan.intensities_length = (nuint)sanitizedIntensities.Length;
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine($"[XLOC] Ignoring intensities due to length mismatch: ranges={expectedLength}, intensities={scan.Intensities.Length}");
|
||||
xlocScan.intensities = IntPtr.Zero;
|
||||
xlocScan.intensities_length = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
xlocScan.intensities = IntPtr.Zero;
|
||||
xlocScan.intensities_length = 0;
|
||||
}
|
||||
|
||||
return xlocScan;
|
||||
}
|
||||
|
||||
#region Memory Management
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for xloc_odometry_t
|
||||
/// </summary>
|
||||
public static void FreeXlocOdometry(ref xloc_odometry_t odom)
|
||||
{
|
||||
if (odom.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.header.frame_id);
|
||||
odom.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (odom.child_frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.child_frame_id);
|
||||
odom.child_frame_id = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for xloc_imu_t
|
||||
/// </summary>
|
||||
public static void FreeXlocImu(ref xloc_imu_t imu)
|
||||
{
|
||||
if (imu.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(imu.header.frame_id);
|
||||
imu.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for xloc_laserscan_t
|
||||
/// </summary>
|
||||
public static void FreeXlocLaserScan(ref xloc_laserscan_t scan)
|
||||
{
|
||||
if (scan.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.header.frame_id);
|
||||
scan.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
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>
|
||||
/// Get managed string from xloc status response and free the C string
|
||||
/// </summary>
|
||||
public static string GetMessageAndFree(ref xloc_status_response_t response)
|
||||
{
|
||||
if (response.message == IntPtr.Zero)
|
||||
return string.Empty;
|
||||
|
||||
string message = Marshal.PtrToStringUTF8(response.message) ?? string.Empty;
|
||||
XlocNativeInterface.xloc_free_cstring(response.message);
|
||||
response.message = IntPtr.Zero;
|
||||
return message;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user