Initial commit
This commit is contained in:
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user