Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/MarkerDetection/MarkerDetectionConversionExtensions.cs

@@ -0,0 +1,178 @@
+using System.Runtime.InteropServices;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Sensor;
+
+namespace RobotNet10.RobotApp.MarkerDetection;
+
+/// <summary>
+/// Extension methods to convert C# sensor structs to marker_detection C-compatible structs
+/// </summary>
+public static class MarkerDetectionConversionExtensions
+{
+    /// <summary>
+    /// Convert C# Header to md_header_t
+    /// NOTE: Caller must free FrameId using Marshal.FreeHGlobal
+    /// </summary>
+    public static md_header_t ToMarkerDetectionHeader(this Header header)
+    {
+        var mdHeader = new md_header_t
+        {
+            seq = header.Seq,
+            stamp = header.Stamp.ToMarkerDetectionUnixTime(),
+            frame_id = IntPtr.Zero
+        };
+
+        // Marshal frame_id string to unmanaged memory
+        if (!string.IsNullOrEmpty(header.FrameId))
+        {
+            mdHeader.frame_id = Marshal.StringToHGlobalAnsi(header.FrameId);
+        }
+
+        return mdHeader;
+    }
+
+    /// <summary>
+    /// Convert DateTime to md_unix_time_t (Unix epoch time)
+    /// </summary>
+    public static md_unix_time_t ToMarkerDetectionUnixTime(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 md_unix_time_t
+        {
+            sec = sec,
+            nsec = nsec
+        };
+    }
+
+    /// <summary>
+    /// Convert C# LaserScan to md_laserscan_t
+    /// NOTE: Caller must free all allocated memory using FreeMarkerDetectionLaserScan
+    /// </summary>
+    public static md_laserscan_t ToMarkerDetectionLaserScan(this LaserScan scan)
+    {
+        var mdScan = new md_laserscan_t
+        {
+            header = scan.Header.ToMarkerDetectionHeader(),
+            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 = (nuint)(scan.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($"[MarkerDetection] Sanitized {invalidCount}/{scan.Ranges.Length} invalid laser scan ranges");
+            }
+            
+            int rangesSize = sanitizedRanges.Length * sizeof(float);
+            mdScan.ranges = Marshal.AllocHGlobal(rangesSize);
+            Marshal.Copy(sanitizedRanges, 0, mdScan.ranges, sanitizedRanges.Length);
+        }
+        else
+        {
+            mdScan.ranges = IntPtr.Zero;
+        }
+
+        // Allocate and copy intensities array
+        if (scan.Intensities != null && scan.Intensities.Length > 0)
+        {
+            int intensitiesSize = scan.Intensities.Length * sizeof(float);
+            mdScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
+            Marshal.Copy(scan.Intensities, 0, mdScan.intensities, scan.Intensities.Length);
+        }
+        else
+        {
+            mdScan.intensities = IntPtr.Zero;
+        }
+
+        return mdScan;
+    }
+
+    #region Memory Management
+
+    /// <summary>
+    /// Free memory allocated for md_laserscan_t
+    /// </summary>
+    public static void FreeMarkerDetectionLaserScan(ref md_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>
+    /// Free memory allocated for md_rectangle_marker_options_t
+    /// </summary>
+    public static void FreeMarkerDetectionRectangleOptions(ref md_rectangle_marker_options_t options)
+    {
+        if (options.marker_frame_id != IntPtr.Zero)
+        {
+            Marshal.FreeHGlobal(options.marker_frame_id);
+            options.marker_frame_id = IntPtr.Zero;
+        }
+    }
+
+    /// <summary>
+    /// Free memory allocated for md_segment_marker_options_t
+    /// </summary>
+    public static void FreeMarkerDetectionSegmentOptions(ref md_segment_marker_options_t options)
+    {
+        if (options.marker_frame_id != IntPtr.Zero)
+        {
+            Marshal.FreeHGlobal(options.marker_frame_id);
+            options.marker_frame_id = IntPtr.Zero;
+        }
+    }
+
+    #endregion
+}
+