Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/MarkerDetection/MarkerDetectionClient.cs

@@ -0,0 +1,440 @@
+using Microsoft.Extensions.Logging;
+using RobotNet10.Shared.Sensor;
+using System.Runtime.InteropServices;
+
+namespace RobotNet10.RobotApp.MarkerDetection;
+
+/// <summary>
+/// High-level wrapper for marker_detection library
+/// Provides thread-safe access to marker detection functionality with automatic resource management
+/// </summary>
+public class MarkerDetectionClient : IDisposable
+{
+    private IntPtr _handle = IntPtr.Zero;
+    private IntPtr _tfBuffer = IntPtr.Zero;  // TF3 buffer (optional)
+    private readonly object _lock = new object();
+    private bool _disposed = false;
+    private readonly ILogger? _logger;
+    public MarkerDetectionClient(ILogger? logger = null)
+    {
+        _logger = logger;
+    }
+
+    /// <summary>
+    /// Initialize marker detection instance with rectangle marker options
+    /// </summary>
+    /// <param name="options">Rectangle marker options</param>
+    /// <param name="tfBuffer">Optional TF3 buffer (pass IntPtr.Zero if not using TF)</param>
+    public void InitializeRectangle(MarkerDetectionRectangleOptions options, IntPtr tfBuffer = default)
+    {
+        lock (_lock)
+        {
+            if (_handle != IntPtr.Zero)
+            {
+                _logger?.LogWarning("MarkerDetectionClient already initialized");
+                return;
+            }
+
+            try
+            {
+                _tfBuffer = tfBuffer;
+
+                // Convert options to C struct
+                var mdOptions = new md_rectangle_marker_options_t
+                {
+                    marker_frame_id = string.IsNullOrEmpty(options.MarkerFrameId)
+                        ? IntPtr.Zero
+                        : Marshal.StringToHGlobalAnsi(options.MarkerFrameId),
+                    marker_length = options.MarkerLength,
+                    marker_width = options.MarkerWidth,
+                    marker_geometric_error = options.MarkerGeometricError,
+                    marker_intensity_threshold_ = options.MarkerIntensityThreshold,
+                    angle_min_of_scan = options.AngleMinOfScan,
+                    angle_max_of_scan = options.AngleMaxOfScan,
+                    min_range_of_scan = options.MinRangeOfScan,
+                    max_range_of_scan = options.MaxRangeOfScan,
+                    use_estimate_marker_pose = options.UseEstimateMarkerPose ? 1 : 0,
+                    max_position_error = options.MaxPositionError,
+                    max_yaw_angle_error = options.MaxYawAngleError,
+                    num_filter_samples = options.NumFilterSamples,
+                    leg_thickness_offset = options.LegThicknessOffset,
+                    run_debug = options.RunDebug ? 1 : 0
+                };
+
+                _handle = MarkerDetectionNativeInterface.marker_detection_create_rectangle_md(ref mdOptions, _tfBuffer);
+                // Free allocated string
+                if (mdOptions.marker_frame_id != IntPtr.Zero)
+                {
+                    Marshal.FreeHGlobal(mdOptions.marker_frame_id);
+                }
+
+                if (_handle == IntPtr.Zero)
+                {
+                    throw new InvalidOperationException("Failed to create marker detection instance. marker_detection_create_rectangle_md returned null.");
+                }
+
+                _logger?.LogInformation("MarkerDetectionClient initialized successfully with rectangle marker (handle: {Handle})", _handle);
+            }
+            catch (Exception ex)
+            {
+                _logger?.LogError(ex, "Exception in InitializeRectangle: {Message}", ex.Message);
+                throw;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Initialize marker detection instance with segment marker options
+    /// </summary>
+    /// <param name="options">Segment marker options</param>
+    /// <param name="tfBuffer">Optional TF3 buffer (pass IntPtr.Zero if not using TF)</param>
+    public void InitializeSegment(MarkerDetectionSegmentOptions options, IntPtr tfBuffer = default)
+    {
+        lock (_lock)
+        {
+            if (_handle != IntPtr.Zero)
+            {
+                _logger?.LogWarning("MarkerDetectionClient already initialized");
+                return;
+            }
+
+            try
+            {
+                _tfBuffer = tfBuffer;
+
+                // Convert options to C struct
+                var mdOptions = new md_segment_marker_options_t
+                {
+                    marker_frame_id = string.IsNullOrEmpty(options.MarkerFrameId)
+                        ? IntPtr.Zero
+                        : Marshal.StringToHGlobalAnsi(options.MarkerFrameId),
+                    marker_length = options.MarkerLength,
+                    marker_geometric_error = options.MarkerGeometricError,
+                    marker_intensity_threshold_ = options.MarkerIntensityThreshold,
+                    angle_min_of_scan = options.AngleMinOfScan,
+                    angle_max_of_scan = options.AngleMaxOfScan,
+                    min_range_of_scan = options.MinRangeOfScan,
+                    max_range_of_scan = options.MaxRangeOfScan,
+                    use_estimate_marker_pose = options.UseEstimateMarkerPose ? 1 : 0,
+                    max_position_error = options.MaxPositionError,
+                    max_yaw_angle_error = options.MaxYawAngleError,
+                    num_filter_samples = options.NumFilterSamples,
+                    leg_thickness_offset = options.LegThicknessOffset,
+                    run_debug = options.RunDebug ? 1 : 0
+                };
+
+                _handle = MarkerDetectionNativeInterface.marker_detection_create_segment_md(ref mdOptions, _tfBuffer);
+
+                // Free allocated string
+                if (mdOptions.marker_frame_id != IntPtr.Zero)
+                {
+                    Marshal.FreeHGlobal(mdOptions.marker_frame_id);
+                }
+
+                if (_handle == IntPtr.Zero)
+                {
+                    throw new InvalidOperationException("Failed to create marker detection instance. marker_detection_create_segment_md returned null.");
+                }
+
+                _logger?.LogInformation("MarkerDetectionClient initialized successfully with segment marker (handle: {Handle})", _handle);
+            }
+            catch (Exception ex)
+            {
+                _logger?.LogError(ex, "Exception in InitializeSegment: {Message}", ex.Message);
+                throw;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Check if marker detection is initialized
+    /// </summary>
+    public bool IsInitialized
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _handle != IntPtr.Zero;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Enable or disable marker detection
+    /// </summary>
+    /// <param name="enable">True to enable detection, false to disable</param>
+    public void SetEnableDetection(bool enable)
+    {
+        ThrowIfNotInitialized();
+        try{
+            lock (_lock)
+            {
+                // Console.WriteLine($"[MarkerDetectionClient] Setting enable detection to {enable}");
+                MarkerDetectionNativeInterface.marker_detection_set_enable_detection(_handle, enable ? 1 : 0);
+                // Console.WriteLine($"[MarkerDetectionClient] Finish !!!");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex, "Exception in SetEnableDetection: {Message}", ex.Message);
+            throw;
+        }
+        _logger?.LogDebug("Marker detection {Status}", enable ? "enabled" : "disabled");
+    }
+
+    /// <summary>
+    /// Set estimated marker pose to improve stability
+    /// The pose is the position and orientation of the marker (cart or station) 
+    /// relative to the laser scan sensor coordinate system
+    /// </summary>
+    /// <param name="x">X position in meters</param>
+    /// <param name="y">Y position in meters</param>
+    /// <param name="theta">Orientation angle in radians</param>
+    public void SetEstimateMarkerPose(double x, double y, double theta)
+    {
+        ThrowIfNotInitialized();
+
+        var pose = new md_pose2d_t
+        {
+            x = x,
+            y = y,
+            theta = theta
+        };
+
+        lock (_lock)
+        {
+            MarkerDetectionNativeInterface.marker_detection_set_estimate_marker_pose(_handle, ref pose);
+        }
+
+        _logger?.LogDebug("Set estimated marker pose: x={X:F3}, y={Y:F3}, theta={Theta:F3}", x, y, theta);
+    }
+
+    /// <summary>
+    /// Dispatch laser scan data to marker detection
+    /// </summary>
+    /// <param name="scan">Laser scan data</param>
+    /// <param name="sensorId">Sensor ID</param>
+    public void DispatchLaserScan(LaserScan scan, string sensorId)
+    {
+        ThrowIfNotInitialized();
+
+        md_laserscan_t mdScan = default;
+        try
+        {
+            _logger?.LogTrace("[DISPATCH-START] LaserScan from {SensorId}, ranges={RangeCount}",
+                sensorId, scan.Ranges?.Length ?? 0);
+
+            mdScan = scan.ToMarkerDetectionLaserScan();
+
+            lock (_lock)
+            {
+                // _logger?.LogTrace("[NATIVE-CALL] marker_detection_dispatch_laserscan...");
+                // in ra dữ liệu trong mảng intensities và ranges trên cùng một dòng
+                // Console.WriteLine($"[MarkerDetectionClient] Dispatching LaserScan - Ranges: [{string.Join(", ", scan.Ranges ?? new float[0])}], Intensities: [{string.Join(", ", scan.Intensities ?? new float[0])}]");
+                MarkerDetectionNativeInterface.marker_detection_dispatch_laserscan(_handle, sensorId, ref mdScan);
+            //     _logger?.LogInformation("[NATIVE-DONE] marker_detection_dispatch_laserscan OK");
+            }
+
+            _logger?.LogTrace("[DISPATCH-DONE] LaserScan from {SensorId}", sensorId);
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex, "[DISPATCH-ERROR] LaserScan dispatch failed: {Message}", ex.Message);
+            throw;
+        }
+        finally
+        {
+            // Free allocated memory
+            MarkerDetectionConversionExtensions.FreeMarkerDetectionLaserScan(ref mdScan);
+        }
+    }
+
+    /// <summary>
+    /// Get current marker pose estimate
+    /// Returns the marker coordinates in the laser scan sensor coordinate system
+    /// </summary>
+    /// <returns>Pose stamped with header and pose, or null if no pose available</returns>
+    public MarkerDetectionPoseStamped? GetMarkerPose()
+    {
+        ThrowIfNotInitialized();
+
+        IntPtr posePtr = IntPtr.Zero;
+        try
+        {
+            _logger?.LogTrace("[DEBUG] About to call marker_detection_get_marker_pose()...");
+            lock (_lock)
+            {
+                posePtr = MarkerDetectionNativeInterface.marker_detection_get_marker_pose(_handle);
+            }
+            _logger?.LogTrace("[DEBUG] marker_detection_get_marker_pose() returned: {PosePtr}", posePtr);
+
+            if (posePtr == IntPtr.Zero)
+            {
+                _logger?.LogTrace("marker_detection_get_marker_pose returned null (no pose available)");
+                return null;
+            }
+
+            // _logger?.LogTrace("[DEBUG] Marshaling pose_stamped structure...");
+            // Marshal the pose_stamped from unmanaged memory
+            var poseStamped = Marshal.PtrToStructure<md_pose_stamped_t>(posePtr);
+            // _logger?.LogTrace("[DEBUG] Pose_stamped marshaled successfully");
+
+            // Convert to managed type
+            var result = new MarkerDetectionPoseStamped
+            {
+                Header = new RobotNet10.Shared.Header
+                {
+                    Seq = poseStamped.header.seq,
+                    Stamp = poseStamped.header.stamp.ToDateTime(),
+                    FrameId = poseStamped.header.frame_id != IntPtr.Zero
+                        ? Marshal.PtrToStringAnsi(poseStamped.header.frame_id) ?? string.Empty
+                        : string.Empty
+                },
+                Pose = new MarkerDetectionPose
+                {
+                    Position = new double[]
+                    {
+                        poseStamped.pose.position[0],
+                        poseStamped.pose.position[1],
+                        poseStamped.pose.position[2]
+                    },
+                    Orientation = new double[]
+                    {
+                        poseStamped.pose.orientation[0],
+                        poseStamped.pose.orientation[1],
+                        poseStamped.pose.orientation[2],
+                        poseStamped.pose.orientation[3]
+                    }
+                }
+            };
+            //In ra dữ liệu của marker pose 
+            // Console.WriteLine($"[DEBUG] Marker Pose - Position: x={result.Pose.Position[0]:F3}, y={result.Pose.Position[1]:F3}, z={result.Pose.Position[2]:F3}");
+            // Console.WriteLine($"[DEBUG] Marker Pose - Orientation: x={result.Pose.Orientation[0]:F3}, y={result.Pose.Orientation[1]:F3}, z={result.Pose.Orientation[2]:F3}, w={result.Pose.Orientation[3]:F3}");
+            return result;
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex, "[DEBUG] Exception in GetMarkerPose: {Message}", ex.Message);
+            return null;
+        }
+        finally
+        {
+            if (posePtr != IntPtr.Zero)
+            {
+                MarkerDetectionNativeInterface.marker_detection_free_pose_stamped(posePtr);
+            }
+        }
+    }
+
+    private void ThrowIfNotInitialized()
+    {
+        if (!IsInitialized)
+        {
+            throw new InvalidOperationException("MarkerDetectionClient is not initialized. Call InitializeRectangle or InitializeSegment first.");
+        }
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        _disposed = true;
+
+        lock (_lock)
+        {
+            if (_handle != IntPtr.Zero)
+            {
+                try
+                {
+                    MarkerDetectionNativeInterface.marker_detection_destroy(_handle);
+                    _logger?.LogInformation("MarkerDetectionClient disposed successfully");
+                }
+                catch (Exception ex)
+                {
+                    _logger?.LogError(ex, "Error disposing MarkerDetectionClient");
+                }
+                finally
+                {
+                    _handle = IntPtr.Zero;
+                }
+            }
+        }
+    }
+}
+
+/// <summary>
+/// Helper extension to convert md_unix_time_t to DateTime
+/// </summary>
+internal static class MarkerDetectionTimeExtensions
+{
+    public static DateTime ToDateTime(this md_unix_time_t unixTime)
+    {
+        long totalNanoseconds = (long)unixTime.sec * 1_000_000_000 + unixTime.nsec;
+        long ticks = totalNanoseconds / 100 + DateTime.UnixEpoch.Ticks;
+        return new DateTime(ticks, DateTimeKind.Utc);
+    }
+}
+
+/// <summary>
+/// Marker detection pose stamped (managed wrapper)
+/// </summary>
+public class MarkerDetectionPoseStamped
+{
+    public RobotNet10.Shared.Header Header { get; set; } = new();
+    public MarkerDetectionPose Pose { get; set; } = new();
+}
+
+/// <summary>
+/// Marker detection pose (managed wrapper)
+/// </summary>
+public class MarkerDetectionPose
+{
+    public double[] Position { get; set; } = new double[3]; // x, y, z
+    public double[] Orientation { get; set; } = new double[4]; // x, y, z, w (quaternion)
+}
+
+/// <summary>
+/// Rectangle marker detection options
+/// </summary>
+public class MarkerDetectionRectangleOptions
+{
+    public string MarkerFrameId { get; set; } = string.Empty;
+    public double MarkerLength { get; set; } = 0.0;
+    public double MarkerWidth { get; set; } = 0.0;
+    public double MarkerGeometricError { get; set; } = 0.0;
+    public double MarkerIntensityThreshold { get; set; } = 0.0;
+    public double AngleMinOfScan { get; set; } = 0.0;
+    public double AngleMaxOfScan { get; set; } = 0.0;
+    public double MinRangeOfScan { get; set; } = 0.0;
+    public double MaxRangeOfScan { get; set; } = 100.0;
+    public bool UseEstimateMarkerPose { get; set; } = false;
+    public double MaxPositionError { get; set; } = 0.0;
+    public double MaxYawAngleError { get; set; } = 0.0;
+    public ushort NumFilterSamples { get; set; } = 0;
+    public double LegThicknessOffset { get; set; } = 0.0;
+    public bool RunDebug { get; set; } = false;
+}
+
+/// <summary>
+/// Segment marker detection options
+/// </summary>
+public class MarkerDetectionSegmentOptions
+{
+    public string MarkerFrameId { get; set; } = string.Empty;
+    public double MarkerLength { get; set; } = 0.0;
+    public double MarkerGeometricError { get; set; } = 0.0;
+    public double MarkerIntensityThreshold { get; set; } = 0.0;
+    public double AngleMinOfScan { get; set; } = 0.0;
+    public double AngleMaxOfScan { get; set; } = 0.0;
+    public double MinRangeOfScan { get; set; } = 0.0;
+    public double MaxRangeOfScan { get; set; } = 100.0;
+    public bool UseEstimateMarkerPose { get; set; } = false;
+    public double MaxPositionError { get; set; } = 0.0;
+    public double MaxYawAngleError { get; set; } = 0.0;
+    public ushort NumFilterSamples { get; set; } = 0;
+    public double LegThicknessOffset { get; set; } = 0.0;
+    public bool RunDebug { get; set; } = false;
+}
+

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
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/MarkerDetection/MarkerDetectionIntegrationService.cs

@@ -0,0 +1,699 @@
+using Microsoft.Extensions.Configuration;
+using Microsoft.Extensions.Hosting;
+using Microsoft.Extensions.Logging;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Sensor;
+using System;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace RobotNet10.RobotApp.MarkerDetection;
+
+/// <summary>
+/// Integration service to dispatch laser scan data to marker detection engine
+/// Provides rectangle or segment marker detection with pose estimation
+/// </summary>
+public class MarkerDetectionIntegrationService : IHostedService, IDisposable
+{
+    private readonly MarkerDetectionIntegrationConfiguration _config;
+    private readonly IDeviceProvider _deviceProvider;
+    private readonly ILogger<MarkerDetectionIntegrationService> _logger;
+    private readonly object _lock = new();
+    private readonly SemaphoreSlim _dispatchSemaphore = new SemaphoreSlim(1, 1);  // Only 1 dispatch at a time
+
+    private MarkerDetectionClient? _markerDetectionClient;
+    
+    // Timer for periodic marker pose updates and logging
+    private Timer? _updateTimer;
+    
+    // Cached device references
+    private ILidar? _lidarDevice;
+    
+    // Statistics (accessed from multiple threads)
+    private long _laserScanCount = 0;
+    private long _markerDetectionCount = 0;
+    private DateTime _lastStatsLog = DateTime.UtcNow;
+
+    private bool _disposed = false;
+    private bool _isInitialized = false;
+
+    public MarkerDetectionIntegrationService(
+        IConfiguration configuration,
+        IDeviceProvider deviceProvider,
+        ILogger<MarkerDetectionIntegrationService> logger)
+    {
+        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
+        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
+
+        // Load configuration
+        var configSection = configuration.GetSection("MarkerDetection:Integration");
+        if (!configSection.Exists())
+        {
+            _logger.LogWarning("Configuration section 'MarkerDetection:Integration' not found. Using defaults.");
+            _config = new MarkerDetectionIntegrationConfiguration();
+        }
+        else
+        {
+            _config = new MarkerDetectionIntegrationConfiguration();
+            configSection.Bind(_config);
+        }
+
+        ValidateConfiguration();
+    }
+
+    private void ValidateConfiguration()
+    {
+        if (_config.Enabled)
+        {
+            if (_config.LaserScanDispatchIntervalMs <= 0 || _config.LaserScanDispatchIntervalMs > 1000)
+            {
+                _logger.LogWarning("LaserScanDispatchIntervalMs {Interval} is out of range [1-1000]. Using default 50 ms.", _config.LaserScanDispatchIntervalMs);
+                _config.LaserScanDispatchIntervalMs = 50;
+            }
+
+            if (_config.MarkerType == MarkerDetectionType.Rectangle && _config.RectangleMarkerOptions == null)
+            {
+                _logger.LogWarning("Rectangle marker type selected but options not provided. Using defaults.");
+                _config.RectangleMarkerOptions = new MarkerDetectionRectangleOptions();
+            }
+
+            if (_config.MarkerType == MarkerDetectionType.Segment && _config.SegmentMarkerOptions == null)
+            {
+                _logger.LogWarning("Segment marker type selected but options not provided. Using defaults.");
+                _config.SegmentMarkerOptions = new MarkerDetectionSegmentOptions();
+            }
+        }
+    }
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        if (!_config.Enabled)
+        {
+            _logger.LogInformation("MarkerDetectionIntegrationService is disabled in configuration");
+            return;
+        }
+
+        _logger.LogInformation("Starting MarkerDetectionIntegrationService...");
+
+        try
+        {
+            // Wait for devices to be connected
+            _logger.LogInformation("Waiting for devices to be connected...");
+            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
+
+            if (!connected)
+            {
+                _logger.LogWarning("Timeout waiting for devices. MarkerDetectionIntegrationService will retry initialization.");
+                _ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
+                return;
+            }
+
+            await InitializeAsync(cancellationToken);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Error starting MarkerDetectionIntegrationService: {Message}", ex.Message);
+        }
+    }
+
+    private async Task RetryInitializationAsync(CancellationToken cancellationToken)
+    {
+        const int maxRetries = 60; // 5 minutes with 5 second intervals
+        int retryCount = 0;
+
+        while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested && !_isInitialized)
+        {
+            await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);
+
+            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(1), cancellationToken);
+            if (connected)
+            {
+                _logger.LogInformation("Devices are now connected. Initializing MarkerDetectionIntegrationService...");
+                await InitializeAsync(cancellationToken);
+                return;
+            }
+
+            retryCount++;
+            if (retryCount % 12 == 0) // Log every minute
+            {
+                _logger.LogInformation("Still waiting for devices... (attempt {Attempt}/{MaxAttempts})",
+                    retryCount, maxRetries);
+            }
+        }
+
+        if (retryCount >= maxRetries)
+        {
+            _logger.LogWarning("Timeout waiting for devices. MarkerDetectionIntegrationService will not be initialized.");
+        }
+    }
+
+    private async Task InitializeAsync(CancellationToken cancellationToken)
+    {
+        lock (_lock)
+        {
+            if (_isInitialized)
+                return;
+
+            try
+            {
+                // Get LIDAR device reference
+                if (!string.IsNullOrEmpty(_config.LidarDeviceId))
+                {
+                    var device = _deviceProvider.GetDevice(_config.LidarDeviceId);
+                    if (device != null)
+                    {
+                        _logger.LogInformation(
+                            "[LIDAR-INIT] Device '{DeviceId}' found. Type: {DeviceType}, IsConnected: {IsConnected}",
+                            _config.LidarDeviceId, device.GetType().Name, device.IsConnected);
+                    }
+                    
+                    if (device is ILidar lidar && device.IsConnected)
+                    {
+                        _lidarDevice = lidar;
+                        _logger.LogInformation(
+                            "[LIDAR-INIT] SUCCESS: LIDAR device '{DeviceId}' is initialized and connected. Ready to receive laser scan data.",
+                            _config.LidarDeviceId);
+                    }
+                    else
+                    {
+                        _logger.LogWarning("LIDAR device '{DeviceId}' not found or not connected. Marker detection will not work.",
+                            _config.LidarDeviceId);
+                        return;
+                    }
+                }
+                else
+                {
+                    _logger.LogWarning("No LIDAR device ID configured. Marker detection will not work.");
+                    return;
+                }
+
+                // Create and initialize marker detection client
+                _markerDetectionClient = new MarkerDetectionClient(_logger);
+
+                // Initialize with appropriate marker type
+                if (_config.MarkerType == MarkerDetectionType.Rectangle && _config.RectangleMarkerOptions != null)
+                {
+                    _markerDetectionClient.InitializeRectangle(_config.RectangleMarkerOptions);
+                }
+                else if (_config.MarkerType == MarkerDetectionType.Segment && _config.SegmentMarkerOptions != null)
+                {
+                    _markerDetectionClient.InitializeSegment(_config.SegmentMarkerOptions);
+                }
+                else
+                {
+                    throw new InvalidOperationException("Invalid marker type configuration or missing marker options.");
+                }
+
+                _logger.LogInformation("MarkerDetectionClient initialized successfully");
+
+                // Start laser scan dispatch loop as background task (non-blocking)
+                if (_lidarDevice != null)
+                {
+                    _ = Task.Run(async () => await LaserScanDispatchLoopAsync(cancellationToken), cancellationToken);
+                    _logger.LogInformation("Laser scan dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
+                }
+
+                // Start periodic update timer for pose logging and status updates
+                _updateTimer = new Timer(UpdatePoseAndDiagnostics, null,
+                    TimeSpan.FromSeconds(2), TimeSpan.FromSeconds(2));
+                _logger.LogInformation("MarkerDetection update timer started (every 2 seconds)");
+
+                _isInitialized = true;
+                _logger.LogInformation("MarkerDetectionIntegrationService initialized successfully");
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "Failed to initialize MarkerDetectionIntegrationService: {Message}", ex.Message);
+                throw;
+            }
+        }
+    }
+
+    public async Task StopAsync(CancellationToken cancellationToken)
+    {
+        _logger.LogInformation("Stopping MarkerDetectionIntegrationService...");
+
+        try
+        {
+            lock (_lock)
+            {
+                _isInitialized = false;
+                
+                // Dispose timer
+                _updateTimer?.Dispose();
+                _updateTimer = null;
+            }
+
+            // Wait a bit for dispatch loops to notice _isInitialized = false and stop
+            _logger.LogInformation("Waiting for dispatch loops to stop...");
+            await Task.Delay(500, cancellationToken);
+
+            // Clean up marker detection client
+            if (_markerDetectionClient != null)
+            {
+                _markerDetectionClient.Dispose();
+                _markerDetectionClient = null;
+                _logger.LogInformation("MarkerDetectionClient disposed successfully");
+            }
+
+            LogFinalStatistics();
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Error stopping MarkerDetectionIntegrationService: {Message}", ex.Message);
+        }
+
+        await Task.CompletedTask;
+    }
+
+    /// <summary>
+    /// Laser scan dispatch loop - runs in background task
+    /// </summary>
+    private async Task LaserScanDispatchLoopAsync(CancellationToken cancellationToken)
+    {
+        int noDataCounter = 0;
+        
+        while (_config.Enabled && !cancellationToken.IsCancellationRequested)
+        {
+            if (!_isInitialized || _markerDetectionClient == null || _lidarDevice == null || !_markerDetectionClient.IsInitialized)
+            {
+                await Task.Delay(100, cancellationToken);
+                continue;
+            }
+
+            try
+            {
+                var scan = _lidarDevice.CurrentMeasurementData;
+                
+                // Log periodically to monitor intensities status (every ~5 seconds at 20Hz)
+                if (scan.HasValue && _laserScanCount % 100 == 0)
+                {
+                    var rawScan = scan.Value;
+                    if (rawScan.Intensities == null || rawScan.Intensities.Length == 0)
+                    {
+                        _logger.LogWarning(
+                            "[MARKER-DETECTION] LIDAR NOT providing intensities (Ranges: {RangeCount}). " +
+                            "Using default values. Check LIDAR RSSI configuration.",
+                            rawScan.Ranges?.Length ?? 0);
+                    }
+                    // else
+                    // {
+                    //     _logger.LogInformation(
+                    //         "[MARKER-DETECTION] ✓ LIDAR providing REAL intensities: {Count} values, " +
+                    //         "Sample: [{I0:F3}, {I1:F3}, {I2:F3}]",
+                    //         rawScan.Intensities.Length,
+                    //         rawScan.Intensities[0],
+                    //         rawScan.Intensities.Length > 1 ? rawScan.Intensities[1] : 0,
+                    //         rawScan.Intensities.Length > 2 ? rawScan.Intensities[2] : 0);
+                    // }
+                }
+                if (scan == null)
+                {
+                    noDataCounter++;
+                    if (noDataCounter % 100 == 0)  // Log every 100 iterations (5 seconds at 20Hz)
+                    {
+                        // _logger.LogWarning(
+                        //     "[LIDAR-DATA-DEBUG] CurrentMeasurementData is NULL. " +
+                        //     "LIDAR device may not be connected or initialized. " +
+                        //     "Configured device: '{LidarDeviceId}'. Missing data for {MissingIterations} iterations.",
+                        //     _config.LidarDeviceId, noDataCounter);
+                    }
+                    continue;
+                }
+                
+                var originalScan = scan.Value; // Assuming CurrentMeasurementData is nullable
+                //in ra dữ liệu intensities và ranges để debug
+                // _logger.LogInformation("LIDAR Data Debug - Ranges Count: {RangesCount}, Intensities Count: {IntensitiesCount}", 
+                //     originalScan.Ranges?.Length ?? 0, originalScan.Intensities?.Length ?? 0);
+
+                // Validate scan data before processing
+                // LIDAR must provide at least range data (Intensities may be empty)
+                if (originalScan.Ranges == null || originalScan.Ranges.Length == 0)
+                {
+                    noDataCounter++;
+                    // if (noDataCounter % 100 == 0)
+                    // {
+                    //     _logger.LogWarning(
+                    //         "[LIDAR-DATA-DEBUG] No RANGE data in laser scan. " +
+                    //         "Ranges: {RangeLength}, Intensities: {IntensityLength}. " +
+                    //         "LIDAR device '{LidarDeviceId}' is not providing valid range measurements. " +
+                    //         "Missing data for {MissingIterations} iterations.",
+                    //         originalScan.Ranges?.Length ?? 0,
+                    //         originalScan.Intensities?.Length ?? 0,
+                    //         _config.LidarDeviceId,
+                    //         noDataCounter);
+                    // }
+                    continue;
+                }
+                
+                // Ensure Intensities array is not null
+                // If LIDAR doesn't provide intensities, populate with default values
+                // if (originalScan.Intensities == null || originalScan.Intensities.Length == 0)
+                // {
+                //     originalScan.Intensities = new float[originalScan.Ranges.Length];
+                //     for (int i = 0; i < originalScan.Intensities.Length; i++)
+                //     {
+                //         originalScan.Intensities[i] = 1.0f;  // Default fallback value
+                //     }
+                // }
+                
+                // Data is valid - reset counter
+                noDataCounter = 0;
+                
+                // Log once per second when valid data is being received
+                // if (noDataCounter == 0 && _laserScanCount % 20 == 0)  // ~1 second at 20Hz
+                // {
+                //     _logger.LogDebug(
+                //         "[LIDAR-DATA-OK] Valid laser scan received: Ranges: {RangeLength}, Intensities: {IntensityLength}",
+                //         originalScan.Ranges.Length,
+                //         originalScan.Intensities.Length);
+                // }
+
+                // Create updated scan with current timestamp
+                var updatedScan = new LaserScan
+                {
+                    Header = new Header
+                    {
+                        Seq = originalScan.Header.Seq,
+                        Stamp = DateTime.UtcNow,
+                        FrameId = "scan" 
+                    },
+                    AngleMin = (float)-2.356194496154785,
+                    AngleMax = (float)2.356194496154785,
+                    AngleIncrement =(float) 0.00581718236207962,
+                    TimeIncrement = (float) 6.172839493956417e-05,
+                    ScanTime = (float) 0.06666667014360428,
+                    RangeMin = (float) 0.0,
+                    RangeMax = (float) 100.0,
+                    Ranges = originalScan.Ranges,
+                    Intensities = originalScan.Intensities
+                };
+                // In ra tất cả giũ liệu trong mảng Intensities 
+                // if (updatedScan.Intensities != null)
+                // {
+                //     _logger.LogDebug("Intensities array length: {Length}", updatedScan.Intensities.Length);
+                //     for (int i = 0; i < Math.Min(811, updatedScan.Intensities.Length); i++)
+                //     {
+                //         _logger.LogInformation("Intensity[{Index}]: {Value}", i, originalScan.Intensities[i]);
+                //         _logger.LogInformation("Range[{Index}]: {Value}", i, updatedScan.Ranges[i]);
+                //     }
+                // }
+                // In ra tất cả giũ liệu trong mảng Ranges và Intensities trong cùng 1 dòng và Seq, Stamp, FrameId để debug
+                // if (updatedScan.Intensities != null)
+                // {
+                //     for (int i = 0; i < Math.Min(811, updatedScan.Intensities.Length); i++)
+                //     {
+                //         _logger.LogInformation("Detect marker Seq: {Seq}, Stamp: {Stamp}, FrameId: {FrameId}, Range[{Index}]: {Range}, Intensity[{Index}]: {Intensity}",
+                //             updatedScan.Header.Seq,
+                //             updatedScan.Header.Stamp.ToString("HH:mm:ss.ffff"),
+                //             updatedScan.Header.FrameId,
+                //             i,
+                //             updatedScan.Ranges[i],
+                //             i,
+                //             updatedScan.Intensities[i]);
+                //     }
+                // }
+                // Dispatch laser scan to marker detection
+                // Re-check that client is still valid (could have been disposed by another thread)
+                if (_markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
+                {
+                    continue;
+                }
+
+                await _dispatchSemaphore.WaitAsync(cancellationToken);
+                try
+                {
+                    // Double-check again after acquiring semaphore
+                    if (_markerDetectionClient != null && _markerDetectionClient.IsInitialized)
+                    {
+                        int rangeCount = updatedScan.Ranges?.Length ?? 0;
+                        int intensityCount = updatedScan.Intensities?.Length ?? 0;
+                        
+                        // _logger.LogInformation(
+                        //     "[MARKER-DETECTION-DISPATCH] Sending to native code - " +
+                        //     "Ranges: {RangeCount}, Intensities: {IntensityStatus} (Count: {IntensityCount})",
+                        //     rangeCount,
+                        //     updatedScan.Intensities == null ? "NULL" : "OK",
+                        //     intensityCount);
+                        
+                        // if (updatedScan.Intensities != null && intensityCount > 2)
+                        // {
+                        //     _logger.LogInformation(
+                        //         "[MARKER-DETECTION-DISPATCH] First 3 intensities being sent: [{I0:F2}, {I1:F2}, {I2:F2}]",
+                        //         updatedScan.Intensities[0],
+                        //         updatedScan.Intensities[1],
+                        //         updatedScan.Intensities[2]);
+                        // }
+                        
+                        _markerDetectionClient.DispatchLaserScan(updatedScan, _config.LaserScanSensorId ?? "scan");
+                        Interlocked.Increment(ref _laserScanCount);
+                    }
+                }
+                finally
+                {
+                    _dispatchSemaphore.Release();
+                }
+            }
+            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
+            {
+                // MarkerDetectionClient was disposed during shutdown - this is normal, just exit gracefully
+                _logger.LogDebug("Laser scan dispatch loop stopped: {Message}", ex.Message);
+                break;
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "Error dispatching laser scan data: {Message}", ex.Message);
+            }
+
+            // Wait for next cycle
+            await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
+        }
+    }
+
+    /// <summary>
+    /// Periodic timer callback to update marker pose and log diagnostics
+    /// </summary>
+    private void UpdatePoseAndDiagnostics(object? state)
+    {
+        if (!_isInitialized || _markerDetectionClient == null)
+            return;
+
+        try
+        {
+            // Get current marker pose if available
+            if (_markerDetectionClient.IsInitialized)
+            {
+                var pose = _markerDetectionClient.GetMarkerPose();
+                if (pose != null)
+                {
+                    Interlocked.Increment(ref _markerDetectionCount);
+                    // _logger.LogDebug("Marker pose: x={X}, y={Y}, z={Z}, qx={Qx}, qy={Qy}, qz={Qz}, qw={Qw}, frame={Frame}",
+                    //     pose.Pose.Position[0], pose.Pose.Position[1], pose.Pose.Position[2],
+                    //     pose.Pose.Orientation[0], pose.Pose.Orientation[1], pose.Pose.Orientation[2], pose.Pose.Orientation[3],
+                    //     pose.Header.FrameId);
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Error updating marker pose and diagnostics: {Message}", ex.Message);
+        }
+
+        // Log statistics periodically
+        var now = DateTime.UtcNow;
+        if ((now - _lastStatsLog).TotalSeconds >= 10)
+        {
+            _lastStatsLog = now;
+            LogStatistics();
+        }
+    }
+
+    private void LogStatistics()
+    {
+        var laserCount = Interlocked.Read(ref _laserScanCount);
+        var markerCount = Interlocked.Read(ref _markerDetectionCount);
+
+        if (laserCount > 0)
+        {
+            var laserRate = laserCount / ((DateTime.UtcNow - _lastStatsLog).TotalSeconds + 0.001);
+            // _logger.LogInformation(
+            //     "[MarkerDetection Stats] LaserScans: {LaserCount} (avg {LaserRate:F2} Hz), Marker Poses: {MarkerCount}",
+            //     laserCount, laserRate, markerCount);
+        }
+        else
+        {
+            // LIDAR data still empty - provide diagnostic info
+            _logger.LogWarning(
+                "[MarkerDetection Stats] No laser scan data received. " +
+                "LIDAR Device ID configured: '{ConfigDeviceId}'. " +
+                "Please verify LIDAR device is connected and providing data.",
+                _config.LidarDeviceId);
+        }
+    }
+
+    private void LogFinalStatistics()
+    {
+        var laserCount = Interlocked.Read(ref _laserScanCount);
+        var markerCount = Interlocked.Read(ref _markerDetectionCount);
+        
+        _logger.LogInformation(
+            "[MarkerDetection Final Stats] Total LaserScans: {LaserCount}, Total Marker Poses: {MarkerCount}",
+            laserCount, markerCount);
+    }
+
+    /// <summary>
+    /// Enable or disable marker detection
+    /// </summary>
+    public void SetEnableDetection(bool enable)
+    {
+        lock (_lock)
+        {
+            if (!_isInitialized || _markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
+            {
+                _logger.LogWarning("Cannot set detection enable/disable: MarkerDetectionIntegrationService not initialized");
+                return;
+            }
+
+            try
+            {
+                _markerDetectionClient.SetEnableDetection(enable);
+                _logger.LogInformation("Marker detection {Status}", enable ? "enabled" : "disabled");
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "Error setting detection enable/disable: {Message}", ex.Message);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Get current marker pose
+    /// </summary>
+    public MarkerDetectionPoseStamped? GetMarkerPose()
+    {
+        lock (_lock)
+        {
+            if (!_isInitialized || _markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
+            {
+                _logger.LogWarning("Cannot get marker pose: MarkerDetectionIntegrationService not initialized");
+                return null;
+            }
+
+            try
+            {
+                return _markerDetectionClient.GetMarkerPose();
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "Error getting marker pose: {Message}", ex.Message);
+                return null;
+            }
+        }
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        _disposed = true;
+
+        try
+        {
+            lock (_lock)
+            {
+                _isInitialized = false;
+            }
+            
+            // Give loop time to stop
+            Thread.Sleep(500);
+            
+            _updateTimer?.Dispose();
+            _markerDetectionClient?.Dispose();
+            _dispatchSemaphore?.Dispose();
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Error disposing MarkerDetectionIntegrationService: {Message}", ex.Message);
+        }
+    }
+}
+
+/// <summary>
+/// Marker detection type
+/// </summary>
+public enum MarkerDetectionType
+{
+    Rectangle,
+    Segment
+}
+
+/// <summary>
+/// Configuration for MarkerDetectionIntegrationService
+/// </summary>
+public class MarkerDetectionIntegrationConfiguration
+{
+    /// <summary>
+    /// Enable/disable the integration service
+    /// </summary>
+    public bool Enabled { get; set; } = false;
+
+    /// <summary>
+    /// Type of marker to detect (Rectangle or Segment)
+    /// </summary>
+    public MarkerDetectionType MarkerType { get; set; } = MarkerDetectionType.Rectangle;
+
+    /// <summary>
+    /// Rectangle marker detection options
+    /// Required if MarkerType is Rectangle
+    /// </summary>
+    public MarkerDetectionRectangleOptions? RectangleMarkerOptions { get; set; } = null;
+
+    /// <summary>
+    /// Segment marker detection options
+    /// Required if MarkerType is Segment
+    /// </summary>
+    public MarkerDetectionSegmentOptions? SegmentMarkerOptions { get; set; } = null;
+
+    /// <summary>
+    /// LIDAR device ID for laser scan dispatch
+    /// </summary>
+    public string LidarDeviceId { get; set; } = "scan_1";
+
+    /// <summary>
+    /// Sensor ID for laser scan
+    /// </summary>
+    public string? LaserScanSensorId { get; set; } = "scan_1";
+
+    /// <summary>
+    /// Frame ID for laser scan
+    /// </summary>
+    public string? LaserScanFrameId { get; set; } = "scan_1";
+
+    /// <summary>
+    /// Laser scan data dispatch interval in milliseconds (default: 50ms = 20Hz)
+    /// </summary>
+    public int LaserScanDispatchIntervalMs { get; set; } = 50;
+
+    /// <summary>
+    /// Enable estimated marker pose for improved stability
+    /// </summary>
+    public bool EnableEstimateMarkerPose { get; set; } = false;
+
+    /// <summary>
+    /// Initial estimated marker pose X (meters)
+    /// </summary>
+    public double EstimateMarkerPoseX { get; set; } = 0.0;
+
+    /// <summary>
+    /// Initial estimated marker pose Y (meters)
+    /// </summary>
+    public double EstimateMarkerPoseY { get; set; } = 0.0;
+
+    /// <summary>
+    /// Initial estimated marker pose theta (radians)
+    /// </summary>
+    public double EstimateMarkerPoseTheta { get; set; } = 0.0;
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/MarkerDetection/MarkerDetectionInterop.cs

@@ -0,0 +1,131 @@
+using System.Runtime.InteropServices;
+
+namespace RobotNet10.RobotApp.MarkerDetection;
+
+/// <summary>
+/// C-compatible structs for marker_detection C API interop
+/// Based on marker_detection_cpp-main/include/marker_detection_c_api.h
+/// </summary>
+
+/// <summary>
+/// Unix timestamp with seconds and nanoseconds (md_unix_time_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_unix_time_t
+{
+    public uint sec;
+    public uint nsec;
+}
+
+/// <summary>
+/// Message header (md_header_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_header_t
+{
+    public uint seq;
+    public md_unix_time_t stamp;
+    public IntPtr frame_id; // char* - must be allocated and freed
+}
+
+/// <summary>
+/// 2D Pose (md_pose2d_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_pose2d_t
+{
+    public double x;
+    public double y;
+    public double theta;
+}
+
+/// <summary>
+/// 3D Pose (md_pose_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_pose_t
+{
+    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
+    public double[] position;    // [3] - x, y, z
+    
+    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
+    public double[] orientation; // [4] - x, y, z, w (quaternion)
+}
+
+/// <summary>
+/// Pose Stamped (md_pose_stamped_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_pose_stamped_t
+{
+    public md_header_t header;
+    public md_pose_t pose;
+}
+
+/// <summary>
+/// LaserScan message (md_laserscan_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_laserscan_t
+{
+    public md_header_t 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;               // float* - pointer to array
+    public nuint ranges_length;          // size_t - number of elements
+    
+    public IntPtr intensities;          // float* - pointer to array
+    public nuint intensities_length;     // size_t - number of elements
+}
+
+/// <summary>
+/// Rectangle marker options (md_rectangle_marker_options_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_rectangle_marker_options_t
+{
+    public IntPtr marker_frame_id;       // char* - must be allocated and freed
+    public double marker_length;
+    public double marker_width;
+    public double marker_geometric_error;
+    public double marker_intensity_threshold_;
+    public double angle_min_of_scan;
+    public double angle_max_of_scan;
+    public double min_range_of_scan;
+    public double max_range_of_scan;
+    public int use_estimate_marker_pose; // bool
+    public double max_position_error;
+    public double max_yaw_angle_error;
+    public ushort num_filter_samples;
+    public double leg_thickness_offset;
+    public int run_debug; // bool
+}
+
+/// <summary>
+/// Segment marker options (md_segment_marker_options_t)
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+public struct md_segment_marker_options_t
+{
+    public IntPtr marker_frame_id;       // char* - must be allocated and freed
+    public double marker_length;
+    public double marker_geometric_error;
+    public double marker_intensity_threshold_;
+    public double angle_min_of_scan;
+    public double angle_max_of_scan;
+    public double min_range_of_scan;
+    public double max_range_of_scan;
+    public int use_estimate_marker_pose; // bool
+    public double max_position_error;
+    public double max_yaw_angle_error;
+    public ushort num_filter_samples;
+    public double leg_thickness_offset;
+    public int run_debug; // bool
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/MarkerDetection/MarkerDetectionNativeInterface.cs

@@ -0,0 +1,112 @@
+using System.Runtime.InteropServices;
+
+namespace RobotNet10.RobotApp.MarkerDetection;
+
+/// <summary>
+/// P/Invoke declarations for marker_detection C API
+/// </summary>
+public static class MarkerDetectionNativeInterface
+{
+    // Library path - adjust if needed
+    private const string LibraryPath = "/usr/local/lib/libmarker_detection.so";
+
+    #region Creation / Destruction
+
+    /// <summary>
+    /// Create a rectangle marker detection instance
+    /// </summary>
+    /// <param name="options">Marker options</param>
+    /// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
+    /// <returns>Handle to marker detection instance</returns>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern IntPtr marker_detection_create_rectangle_md(
+        ref md_rectangle_marker_options_t options,
+        IntPtr tfBuffer);
+
+    /// <summary>
+    /// Create a segment marker detection instance
+    /// </summary>
+    /// <param name="options">Marker options</param>
+    /// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
+    /// <returns>Handle to marker detection instance</returns>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern IntPtr marker_detection_create_segment_md(
+        ref md_segment_marker_options_t options,
+        IntPtr tfBuffer);
+
+    /// <summary>
+    /// Destroy a marker detection instance
+    /// </summary>
+    /// <param name="handle">Handle to marker detection instance</param>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern void marker_detection_destroy(IntPtr handle);
+
+    #endregion
+
+    #region Control
+
+    /// <summary>
+    /// Enable or disable marker detection
+    /// </summary>
+    /// <param name="handle">Handle to marker detection instance</param>
+    /// <param name="enable">1 to enable, 0 to disable</param>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern void marker_detection_set_enable_detection(IntPtr handle, int enable);
+
+    /// <summary>
+    /// Set estimated marker pose to improve stability
+    /// </summary>
+    /// <param name="handle">Handle to marker detection instance</param>
+    /// <param name="pose">Estimated 2D pose (x, y, theta) relative to laser scan sensor frame</param>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern void marker_detection_set_estimate_marker_pose(IntPtr handle, ref md_pose2d_t pose);
+
+    #endregion
+
+    #region Sensor Data Dispatch
+
+    /// <summary>
+    /// Dispatch laser scan data to marker detection
+    /// </summary>
+    /// <param name="handle">Handle to marker detection instance</param>
+    /// <param name="sensorId">Sensor ID (null-terminated string)</param>
+    /// <param name="scan">Pointer to laser scan struct</param>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern void marker_detection_dispatch_laserscan(
+        IntPtr handle,
+        [MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
+        ref md_laserscan_t scan);
+
+    #endregion
+
+    #region Getters
+
+    /// <summary>
+    /// Get current marker pose estimate
+    /// </summary>
+    /// <param name="handle">Handle to marker detection instance</param>
+    /// <returns>Pointer to pose_stamped (must be freed with marker_detection_free_pose_stamped) or IntPtr.Zero if no pose available</returns>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern IntPtr marker_detection_get_marker_pose(IntPtr handle);
+
+    #endregion
+
+    #region Memory Management
+
+    /// <summary>
+    /// Free pose_stamped allocated by marker_detection_get_marker_pose
+    /// </summary>
+    /// <param name="p">Pointer to pose_stamped to free</param>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern void marker_detection_free_pose_stamped(IntPtr p);
+
+    /// <summary>
+    /// Free C string allocated by the API
+    /// </summary>
+    /// <param name="s">Pointer to C string to free</param>
+    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
+    public static extern void marker_detection_free_cstring(IntPtr s);
+
+    #endregion
+}
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/MarkerDetection/TEST_MARKER_DETECTION_API.md

@@ -0,0 +1,109 @@
+# Marker Detection API - Curl Test Commands
+
+## Enable Marker Detection
+```bash
+curl -X POST "https://localhost:7002/api/marker-detection/detection/enable" \
+  -H "Content-Type: application/json" \
+  -d '{"enable": true}' \
+  --insecure
+```
+
+**Expected Response:**
+```json
+{
+  "status": "success",
+  "message": "Marker detection enabled",
+  "enabled": true
+}
+```
+
+---
+
+## Disable Marker Detection
+```bash
+curl -X POST "https://localhost:7002/api/marker-detection/detection/enable" \
+  -H "Content-Type: application/json" \
+  -d '{"enable": false}' \
+  --insecure
+```
+
+**Expected Response:**
+```json
+{
+  "status": "success",
+  "message": "Marker detection disabled",
+  "enabled": false
+}
+```
+
+---
+
+## Get Current Marker Pose
+```bash
+curl -X GET "https://localhost:7002/api/marker-detection/pose/current" \
+  --insecure
+```
+
+**Expected Response (when marker is detected):**
+```json
+{
+  "status": "success",
+  "pose": {
+    "header": {
+      "seq": 123,
+      "stamp": "2026-02-25T10:30:45.123456Z",
+      "frameId": "scan"
+    },
+    "position": {
+      "x": 1.234,
+      "y": 5.678,
+      "z": 0.0
+    },
+    "orientation": {
+      "x": 0.0,
+      "y": 0.0,
+      "z": 0.707,
+      "w": 0.707
+    }
+  }
+}
+```
+
+**Expected Response (when marker not detected):**
+```json
+{
+  "status": "error",
+  "message": "No marker pose available"
+}
+```
+
+---
+
+## Test with Pretty Print (requires jq)
+```bash
+curl -X GET "https://localhost:7002/api/marker-detection/pose/current" \
+  --insecure | jq .
+```
+
+---
+
+## Test from Command Line (One-liner for Enable)
+```bash
+curl -X POST "https://localhost:7002/api/marker-detection/detection/enable" -H "Content-Type: application/json" -d '{"enable":true}' --insecure
+```
+
+---
+
+## Test from Command Line (One-liner for Get Pose)
+```bash
+curl -X GET "https://localhost:7002/api/marker-detection/pose/current" --insecure
+```
+
+---
+
+## Notes
+- Replace `https://localhost:7002` with your actual app URL
+- Use `--insecure` flag only for self-signed certificates (development environment)
+- For production, use proper HTTPS certificates
+- The `jq` tool helps format JSON output nicely (install with: `sudo apt install jq`)
+- HTTP Status Code 200 = Success, 404 = Not Found, 400 = Bad Request, 500 = Server Error