1869 lines
63 KiB
C#
1869 lines
63 KiB
C#
using System.Runtime.InteropServices;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using Microsoft.Extensions.Logging;
|
|
using RobotNet10.RobotApp.Xloc;
|
|
using RobotNet10.Shared.Geometry;
|
|
using System.Security.AccessControl;
|
|
using System.Formats.Tar;
|
|
|
|
namespace RobotNet10.RobotApp.Navigation;
|
|
|
|
/// <summary>
|
|
/// High-level wrapper for Navigation library
|
|
/// Provides thread-safe access to Navigation functionality with automatic resource management
|
|
/// </summary>
|
|
public class NavigationClient : IDisposable
|
|
{
|
|
[DllImport("libc", EntryPoint = "free", CallingConvention = CallingConvention.Cdecl)]
|
|
private static extern void NativeFree(IntPtr ptr);
|
|
|
|
private IntPtr _handle = IntPtr.Zero;
|
|
private IntPtr _tfBuffer = IntPtr.Zero;
|
|
private readonly object _lock = new object();
|
|
private bool _disposed = false;
|
|
private readonly ILogger? _logger;
|
|
private uint _sequenceNumber = 0;
|
|
|
|
public NavigationClient(IntPtr tfBuffer, ILogger? logger = null)
|
|
{
|
|
_logger = logger;
|
|
_tfBuffer = tfBuffer;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initialize Navigation instance
|
|
/// Note: TF3BufferManager must be initialized before calling this method
|
|
/// </summary>
|
|
public void Initialize()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_handle != IntPtr.Zero)
|
|
{
|
|
_logger?.LogWarning("NavigationClient already initialized");
|
|
return;
|
|
}
|
|
|
|
string navCorePath = "/usr/local/pnkx_nav_core";
|
|
string originalDir = Directory.GetCurrentDirectory();
|
|
bool directoryChanged = false;
|
|
|
|
try
|
|
{
|
|
if (Directory.Exists(navCorePath))
|
|
{
|
|
try
|
|
{
|
|
Directory.SetCurrentDirectory(navCorePath);
|
|
directoryChanged = true;
|
|
_logger?.LogInformation("Changed working directory to: {Path} (original: {Original})", navCorePath, originalDir);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogWarning("Failed to change working directory: {Message}. Continuing with current directory.", ex.Message);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("Navigation core path not found: {Path}. Config files may not be found.", navCorePath);
|
|
}
|
|
|
|
if (_tfBuffer == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("TF buffer not initialized. Ensure TF3BufferManager is initialized first.");
|
|
}
|
|
|
|
_logger?.LogInformation("[NAV] ═══════════════════════════════════════════");
|
|
_logger?.LogInformation("[NAV] Initializing NavigationClient...");
|
|
_logger?.LogInformation("[NAV] TF buffer available: {TfBuffer}", _tfBuffer);
|
|
|
|
_logger?.LogInformation("[NAV] Waiting for TF3 transforms to be indexed (500ms)...");
|
|
System.Threading.Thread.Sleep(500);
|
|
|
|
_handle = NavigationNativeInterface.navigation_create();
|
|
|
|
if (_handle == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("Failed to create Navigation instance. navigation_create returned null.");
|
|
}
|
|
|
|
bool initResult = NavigationNativeInterface.navigation_initialize(_handle, _tfBuffer);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[NAV] ❌ Exception in Initialize: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
if (directoryChanged)
|
|
{
|
|
try
|
|
{
|
|
Directory.SetCurrentDirectory(originalDir);
|
|
_logger?.LogInformation("Restored working directory to: {Path}", originalDir);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogWarning("Failed to restore working directory: {Message}", ex.Message);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if Navigation is initialized
|
|
/// </summary>
|
|
public bool IsInitialized
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _handle != IntPtr.Zero;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get navigation handle (for direct API calls)
|
|
/// </summary>
|
|
internal IntPtr Handle
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _handle;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update map -> odom transform from Xloc pose
|
|
/// </summary>
|
|
public bool UpdateMapToOdomTransform(double x, double y, double z, double qx, double qy, double qz, double qw)
|
|
{
|
|
if (_tfBuffer == IntPtr.Zero)
|
|
return false;
|
|
|
|
try
|
|
{
|
|
double quatNorm = Math.Sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
|
|
double finalQx = qx;
|
|
double finalQy = qy;
|
|
double finalQz = qz;
|
|
double finalQw = qw;
|
|
|
|
if (quatNorm < 1e-6)
|
|
{
|
|
finalQx = 0.0;
|
|
finalQy = 0.0;
|
|
finalQz = 0.0;
|
|
finalQw = 1.0;
|
|
_logger?.LogWarning("Invalid quaternion (norm={Norm:F6}), using identity quaternion for map->odom transform", quatNorm);
|
|
}
|
|
else if (Math.Abs(quatNorm - 1.0) > 1e-3)
|
|
{
|
|
finalQx = qx / quatNorm;
|
|
finalQy = qy / quatNorm;
|
|
finalQz = qz / quatNorm;
|
|
finalQw = qw / quatNorm;
|
|
_logger?.LogTrace("Quaternion normalized from norm {Norm:F6} to 1.0", quatNorm);
|
|
}
|
|
|
|
var now = DateTime.UtcNow;
|
|
long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
|
|
long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;
|
|
|
|
var mapToOdom = new TF3.TF3_Transform
|
|
{
|
|
timestamp_sec = sec,
|
|
timestamp_nsec = nsec,
|
|
frame_id = "map",
|
|
child_frame_id = "odom",
|
|
translation_x = x,
|
|
translation_y = y,
|
|
translation_z = z,
|
|
rotation_x = finalQx,
|
|
rotation_y = finalQy,
|
|
rotation_z = finalQz,
|
|
rotation_w = finalQw
|
|
};
|
|
|
|
lock (_lock)
|
|
{
|
|
// Dynamic transform: costmap checks stamp freshness (transform_tolerance ~1s).
|
|
bool success = TF3.TF3NativeInterface.tf3_set_transform(
|
|
_tfBuffer, ref mapToOdom, "navigation_client", false);
|
|
|
|
if (success)
|
|
{
|
|
_logger?.LogTrace("Updated transform: map → odom ({X:F2}, {Y:F2}, {Z:F2})", x, y, z);
|
|
}
|
|
return success;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error updating map->odom transform");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch static map to navigation system
|
|
/// </summary>
|
|
public bool DispatchStaticMap(OccupancyGridData mapData, string mapName)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
if (mapData == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(mapData));
|
|
}
|
|
|
|
if (!IsNavigationReady())
|
|
{
|
|
_logger?.LogWarning("Navigation is not ready (is_ready = false). Cannot add static map {MapName}", mapName);
|
|
return false;
|
|
}
|
|
|
|
OccupancyGrid navGrid = default;
|
|
IntPtr navGridPtr = IntPtr.Zero;
|
|
try
|
|
{
|
|
var timestamp = DateTime.Now.ToString("HH:mm:ss.fff");
|
|
|
|
var xlocGrid = new xloc_occupancy_grid_t
|
|
{
|
|
header = new xloc_header_t
|
|
{
|
|
seq = 0,
|
|
stamp = new xloc_unix_time_t { sec = 0, nsec = 0 },
|
|
frame_id = Marshal.StringToHGlobalAnsi(mapData.FrameId ?? "map")
|
|
},
|
|
resolution = mapData.Resolution,
|
|
width = mapData.Width,
|
|
height = mapData.Height,
|
|
origin = new xloc_pose_t
|
|
{
|
|
position = new double[] { mapData.Origin.X, mapData.Origin.Y, mapData.Origin.Z },
|
|
orientation = new double[] { mapData.Origin.Qx, mapData.Origin.Qy, mapData.Origin.Qz, mapData.Origin.Qw }
|
|
},
|
|
data = IntPtr.Zero,
|
|
data_length = (nuint)(mapData.Data?.Length ?? 0)
|
|
};
|
|
|
|
if (mapData.Data != null && mapData.Data.Length > 0)
|
|
{
|
|
xlocGrid.data = Marshal.AllocHGlobal(mapData.Data.Length);
|
|
Marshal.Copy(mapData.Data, 0, xlocGrid.data, mapData.Data.Length);
|
|
}
|
|
|
|
try
|
|
{
|
|
navGrid = xlocGrid.ToNavigationOccupancyGrid();
|
|
navGridPtr = NavigationConversionExtensions.AllocateNavigationOccupancyGrid(navGrid);
|
|
|
|
bool result;
|
|
lock (_lock)
|
|
{
|
|
result = NavigationNativeInterface.navigation_add_static_map(_handle, mapName, navGrid);
|
|
}
|
|
|
|
if (result)
|
|
{
|
|
_logger?.LogInformation("[{Time}] [DISPATCH-DONE] Static map {MapName} added successfully", timestamp, mapName);
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogError("[{Time}] [DISPATCH-ERROR] Failed to add static map {MapName}", timestamp, mapName);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
finally
|
|
{
|
|
if (xlocGrid.data != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(xlocGrid.data);
|
|
}
|
|
if (xlocGrid.header.frame_id != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(xlocGrid.header.frame_id);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] Static map dispatch failed: {Message}", ex.Message);
|
|
return false;
|
|
}
|
|
finally
|
|
{
|
|
if (navGridPtr != IntPtr.Zero)
|
|
{
|
|
NavigationConversionExtensions.FreeNavigationOccupancyGridPtr(navGridPtr);
|
|
}
|
|
else
|
|
{
|
|
NavigationConversionExtensions.FreeNavigationOccupancyGrid(ref navGrid);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch laser scan to navigation system
|
|
/// NOTE: This method does NOT free the memory allocated in the LaserScan struct.
|
|
/// Caller is responsible for freeing memory using FreeNavigationLaserScan after dispatch.
|
|
/// </summary>
|
|
public void DispatchLaserScan(LaserScan scan, string laserScanName)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
NavigationNativeInterface.navigation_add_laser_scan(_handle, laserScanName, scan);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] LaserScan dispatch failed: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch odometry to navigation system
|
|
/// NOTE: This method does NOT free the memory allocated in the Odometry struct.
|
|
/// Caller is responsible for freeing memory using FreeNavigationOdometry after dispatch.
|
|
/// </summary>
|
|
public void DispatchOdometry(Odometry odom, string odometryName)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
var now = DateTime.UtcNow;
|
|
long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
|
|
long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;
|
|
var odomTobaselink = new TF3.TF3_Transform
|
|
{
|
|
timestamp_sec = sec,
|
|
timestamp_nsec = nsec,
|
|
frame_id = "odom",
|
|
child_frame_id = "base_footprint",
|
|
translation_x = odom.pose.pose.position.x,
|
|
translation_y = odom.pose.pose.position.y,
|
|
translation_z = odom.pose.pose.position.z,
|
|
rotation_x = odom.pose.pose.orientation.x,
|
|
rotation_y = odom.pose.pose.orientation.y,
|
|
rotation_z = odom.pose.pose.orientation.z,
|
|
rotation_w = odom.pose.pose.orientation.w
|
|
};
|
|
TF3.TF3NativeInterface.tf3_set_transform(_tfBuffer, ref odomTobaselink, "navigation_client", false);
|
|
NavigationNativeInterface.navigation_add_odometry(_handle, odometryName, odom);
|
|
var errorCode = new TF3.TF3_ErrorCode();
|
|
TF3.TF3_Transform result = new TF3.TF3_Transform();
|
|
bool lookup = TF3.TF3NativeInterface.tf3_lookup_transform(
|
|
_tfBuffer,
|
|
"map",
|
|
"base_footprint",
|
|
sec,
|
|
nsec,
|
|
ref result,
|
|
ref errorCode);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] Odometry dispatch failed: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch gridmap to navigation system
|
|
/// NOTE: This method does NOT free the memory allocated in the OccupancyGrid struct.
|
|
/// Caller is responsible for freeing memory using FreeNavigationOccupancyGrid after dispatch.
|
|
/// </summary>
|
|
public void DispatchGridMap(OccupancyGrid grid, string gridMapName)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
NavigationNativeInterface.navigation_add_static_map(_handle, gridMapName, grid);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DISPATCH-ERROR] GridMap dispatch failed: {Message}", ex.Message);
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if navigation is ready to receive data
|
|
/// </summary>
|
|
private bool IsNavigationReady()
|
|
{
|
|
NavFeedback feedback = default;
|
|
bool feedbackRetrieved = false;
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_get_feedback(_handle, ref feedback);
|
|
if (!success)
|
|
{
|
|
return false;
|
|
}
|
|
feedbackRetrieved = true;
|
|
return feedback.is_ready;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogTrace(ex, "Failed to check navigation ready status");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set the robot's footprint (outline shape)
|
|
/// </summary>
|
|
public bool SetRobotFootprint(Point[] points)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
if (points == null || points.Length == 0)
|
|
{
|
|
throw new ArgumentException("Points array cannot be null or empty", nameof(points));
|
|
}
|
|
|
|
try
|
|
{
|
|
GCHandle pointsHandle = GCHandle.Alloc(points, GCHandleType.Pinned);
|
|
try
|
|
{
|
|
IntPtr pointsPtr = pointsHandle.AddrOfPinnedObject();
|
|
|
|
lock (_lock)
|
|
{
|
|
return NavigationNativeInterface.navigation_set_robot_footprint(
|
|
_handle,
|
|
pointsPtr,
|
|
(nuint)points.Length);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
if (pointsHandle.IsAllocated)
|
|
{
|
|
pointsHandle.Free();
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error setting robot footprint");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's footprint (outline shape)
|
|
/// </summary>
|
|
public Point[]? GetRobotFootprint()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
IntPtr pointsPtr = IntPtr.Zero;
|
|
nuint count = 0;
|
|
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_get_robot_footprint(
|
|
_handle,
|
|
out pointsPtr,
|
|
out count);
|
|
|
|
if (!success || pointsPtr == IntPtr.Zero)
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
int pointCount = (int)count.ToUInt32();
|
|
Point[] points = new Point[pointCount];
|
|
|
|
for (int i = 0; i < pointCount; i++)
|
|
{
|
|
IntPtr pointPtr = IntPtr.Add(pointsPtr, i * Marshal.SizeOf<Point>());
|
|
points[i] = Marshal.PtrToStructure<Point>(pointPtr);
|
|
}
|
|
|
|
return points;
|
|
}
|
|
finally
|
|
{
|
|
if (pointsPtr != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.navigation_free_points(pointsPtr);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting robot footprint");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to
|
|
/// </summary>
|
|
public bool MoveTo(double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);
|
|
|
|
lock (_lock)
|
|
{
|
|
bool result = NavigationNativeInterface.navigation_move_to(_handle, goal);
|
|
FreePoseStamped(ref goal);
|
|
return result;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending move_to command");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to with order
|
|
/// </summary>
|
|
public bool MoveToOrder(IntPtr orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
if (orderHandle == IntPtr.Zero)
|
|
{
|
|
throw new ArgumentException("Order handle cannot be zero", nameof(orderHandle));
|
|
}
|
|
|
|
long handleValue = orderHandle.ToInt64();
|
|
if (handleValue < 0x1000 || handleValue > 0x7FFFFFFFFFFF)
|
|
{
|
|
string errorMsg = $"Invalid order handle value: {handleValue} (0x{handleValue:X}). Order handle must be a valid pointer to an Order object created by the navigation system. Value must be between 0x1000 and 0x7FFFFFFFFFFF.";
|
|
_logger?.LogDebug(errorMsg);
|
|
throw new ArgumentException(errorMsg, nameof(orderHandle));
|
|
}
|
|
|
|
if (IsLikelyTestValue(handleValue))
|
|
{
|
|
string errorMsg = $"Order handle {handleValue} (0x{handleValue:X}) appears to be a test value. Order handle must be a valid pointer to an Order object created by the navigation system, not a test value.";
|
|
_logger?.LogDebug(errorMsg);
|
|
throw new ArgumentException(errorMsg, nameof(orderHandle));
|
|
}
|
|
|
|
try
|
|
{
|
|
var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);
|
|
|
|
lock (_lock)
|
|
{
|
|
try
|
|
{
|
|
Order order = Marshal.PtrToStructure<Order>(orderHandle);
|
|
|
|
bool result = NavigationNativeInterface.navigation_move_to_order(_handle, order, goal);
|
|
|
|
FreePoseStamped(ref goal);
|
|
|
|
if (result)
|
|
{
|
|
_logger?.LogInformation("navigation_move_to_order returned true. Command sent successfully with order_handle: {OrderHandle}", orderHandle.ToInt64());
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("navigation_move_to_order returned false. Order handle may be invalid: {OrderHandle}", orderHandle.ToInt64());
|
|
}
|
|
|
|
return result;
|
|
}
|
|
catch (AccessViolationException avex)
|
|
{
|
|
FreePoseStamped(ref goal);
|
|
_logger?.LogError(avex, "Access violation when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
|
|
throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), avex);
|
|
}
|
|
catch (SEHException sehex)
|
|
{
|
|
FreePoseStamped(ref goal);
|
|
_logger?.LogError(sehex, "Structured exception (SEH) when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
|
|
throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), sehex);
|
|
}
|
|
}
|
|
}
|
|
catch (ArgumentException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (AccessViolationException ex)
|
|
{
|
|
_logger?.LogError(ex, "Access violation when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
|
|
throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), ex);
|
|
}
|
|
catch (SEHException ex)
|
|
{
|
|
_logger?.LogError(ex, "Structured exception (SEH) when calling navigation_move_to_order. Order handle may be invalid pointer: {OrderHandle}", orderHandle.ToInt64());
|
|
throw new ArgumentException($"Invalid order handle: {orderHandle.ToInt64()}. Order handle must be a valid pointer to Order object.", nameof(orderHandle), ex);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending move_to_order command with order handle: {OrderHandle}", orderHandle.ToInt64());
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if a handle value looks like a test value (common patterns like 0x12345678, 0xDEADBEEF, etc.)
|
|
/// </summary>
|
|
private static bool IsLikelyTestValue(long handleValue)
|
|
{
|
|
long[] testValues = {
|
|
0x12345678, 0x123456789, 0x1234567890, 0x12345678901, 0x123456789012,
|
|
0xDEADBEEF, unchecked((long)0xDEADBEEFDEADBEEFUL),
|
|
0xCAFEBABE, unchecked((long)0xCAFEBABECAFEBABEUL),
|
|
0x00000000, unchecked((long)0xFFFFFFFFUL), unchecked((long)0xFFFFFFFFFFFFFFFFUL),
|
|
0x11111111, 0x22222222, 0x33333333, 0x44444444,
|
|
0x55555555, 0x66666666, 0x77777777, 0x88888888,
|
|
0x99999999, unchecked((long)0xAAAAAAAAUL), unchecked((long)0xBBBBBBBBUL), unchecked((long)0xCCCCCCCCUL),
|
|
unchecked((long)0xDDDDDDDDUL), unchecked((long)0xEEEEEEEEUL), unchecked((long)0xFFFFFFFFUL)
|
|
};
|
|
|
|
foreach (var testVal in testValues)
|
|
{
|
|
if (handleValue == testVal)
|
|
return true;
|
|
}
|
|
|
|
string hexStr = handleValue.ToString("X");
|
|
if (hexStr.Length >= 4)
|
|
{
|
|
if (hexStr.All(c => c == hexStr[0]) ||
|
|
(hexStr.Length >= 8 && hexStr.Substring(0, 4) == hexStr.Substring(4, 4)))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to with order (using OrderHandle wrapper)
|
|
/// </summary>
|
|
public bool MoveToOrder(OrderHandle orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
if (orderHandle == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(orderHandle));
|
|
}
|
|
|
|
if (!orderHandle.IsValid)
|
|
{
|
|
throw new ArgumentException("Order handle is not valid (zero pointer)", nameof(orderHandle));
|
|
}
|
|
|
|
return MoveToOrder(orderHandle.Handle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a goal for the robot to navigate to with order (using OrderData)
|
|
/// </summary>
|
|
public bool MoveToOrder(OrderData orderData, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.1,
|
|
double yawTolerance = 0.1)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
if (orderData == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(orderData));
|
|
}
|
|
|
|
Order nativeOrder = default;
|
|
try
|
|
{
|
|
nativeOrder = OrderConverter.ConvertToNativeOrder(orderData);
|
|
|
|
var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);
|
|
|
|
lock (_lock)
|
|
{
|
|
bool result = NavigationNativeInterface.navigation_move_to_order(_handle, nativeOrder, goal);
|
|
|
|
FreePoseStamped(ref goal);
|
|
|
|
if (result)
|
|
{
|
|
_logger?.LogInformation("navigation_move_to_order returned true. Command sent successfully with order: {OrderId}", orderData.OrderId);
|
|
}
|
|
else
|
|
{
|
|
_logger?.LogWarning("navigation_move_to_order returned false for order: {OrderId}", orderData.OrderId);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending move_to_order command with order: {OrderId}", orderData.OrderId);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
OrderConverter.FreeNativeOrder(ref nativeOrder);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a docking goal to a predefined marker
|
|
/// </summary>
|
|
public bool DockTo(string marker, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double xyTolerance = 0.05,
|
|
double yawTolerance = 0.05)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);
|
|
|
|
lock (_lock)
|
|
{
|
|
bool result = NavigationNativeInterface.navigation_dock_to(_handle, marker, goal);
|
|
FreePoseStamped(ref goal);
|
|
return result;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending dock_to command");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send a docking goal with VDA5050 order payload.
|
|
/// </summary>
|
|
public bool DockToOrder(OrderData orderData, string marker, double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map")
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
if (orderData == null)
|
|
throw new ArgumentNullException(nameof(orderData));
|
|
|
|
Order nativeOrder = default;
|
|
try
|
|
{
|
|
nativeOrder = OrderConverter.ConvertToNativeOrder(orderData);
|
|
var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);
|
|
|
|
lock (_lock)
|
|
{
|
|
bool result = NavigationNativeInterface.navigation_dock_to_order(_handle, nativeOrder, marker, goal);
|
|
FreePoseStamped(ref goal);
|
|
return result;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending dock_to_order command with order: {OrderId}", orderData.OrderId);
|
|
throw;
|
|
}
|
|
finally
|
|
{
|
|
OrderConverter.FreeNativeOrder(ref nativeOrder);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Move straight toward the target position
|
|
/// </summary>
|
|
public bool MoveStraightTo(double distance)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
bool result = NavigationNativeInterface.navigation_move_straight_to(_handle, distance);
|
|
Console.WriteLine($"navigation_move_straight_to returned: success");
|
|
return result;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending move_straight_to command");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rotate in place to align with target orientation
|
|
/// </summary>
|
|
public bool RotateTo(double x, double y, double z, double qx, double qy, double qz, double qw,
|
|
string frameId = "map",
|
|
double yawTolerance = 0.1)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
var goal = CreatePoseStamped(x, y, z, qx, qy, qz, qw, frameId);
|
|
|
|
lock (_lock)
|
|
{
|
|
bool result = NavigationNativeInterface.navigation_rotate_to(_handle, goal);
|
|
FreePoseStamped(ref goal);
|
|
return result;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error sending rotate_to command");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pause the robot's movement
|
|
/// </summary>
|
|
public void Pause()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
NavigationNativeInterface.navigation_pause(_handle);
|
|
}
|
|
_logger?.LogInformation("Navigation paused");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error pausing navigation");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Resume motion after a pause
|
|
/// </summary>
|
|
public void Resume()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
NavigationNativeInterface.navigation_resume(_handle);
|
|
}
|
|
_logger?.LogInformation("Navigation resumed");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error resuming navigation");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cancel the current goal and stop the robot
|
|
/// </summary>
|
|
public void Cancel()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
NavigationNativeInterface.navigation_cancel(_handle);
|
|
}
|
|
_logger?.LogInformation("Navigation cancelled");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error cancelling navigation");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send limited linear velocity command
|
|
/// </summary>
|
|
public bool SetTwistLinear(double linearX, double linearY, double linearZ)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_set_twist_linear(
|
|
_handle,
|
|
linearX, linearY, linearZ);
|
|
_logger?.LogInformation("Set linear twist: ({LinearX:F2}, {LinearY:F2}, {LinearZ:F2}), success: {Success}", linearX, linearY, linearZ, success);
|
|
return success;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error setting linear twist");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Send limited angular velocity command
|
|
/// </summary>
|
|
public bool SetTwistAngular(double angularX, double angularY, double angularZ)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
NavFeedback feedback = default;
|
|
bool feedbackRetrieved = false;
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_get_feedback(_handle, ref feedback);
|
|
if (!success)
|
|
{
|
|
return false;
|
|
}
|
|
feedbackRetrieved = true;
|
|
if (!feedback.is_ready)
|
|
{
|
|
_logger?.LogTrace("Navigation is not ready (is_ready = false). Cannot set twist angular");
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogTrace(ex, "Failed to check navigation ready status. Proceeding with caution.");
|
|
}
|
|
finally
|
|
{
|
|
if (feedbackRetrieved)
|
|
{
|
|
if (feedback.feed_back_str != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(feedback.feed_back_str);
|
|
}
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return NavigationNativeInterface.navigation_set_twist_angular(
|
|
_handle,
|
|
angularX, angularY, angularZ);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error setting angular twist");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's pose as a PoseStamped
|
|
/// </summary>
|
|
public (double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)? GetRobotPoseStamped()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
PoseStamped pose = default;
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_get_robot_pose_stamped(_handle, ref pose);
|
|
if (!success)
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
string frameId = string.Empty;
|
|
if (pose.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(pose.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
return (
|
|
pose.pose.position.x,
|
|
pose.pose.position.y,
|
|
pose.pose.position.z,
|
|
pose.pose.orientation.x,
|
|
pose.pose.orientation.y,
|
|
pose.pose.orientation.z,
|
|
pose.pose.orientation.w,
|
|
frameId
|
|
);
|
|
}
|
|
finally
|
|
{
|
|
if (pose.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(pose.header.frame_id);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting robot pose");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's pose as a 2D pose
|
|
/// </summary>
|
|
public (double x, double y, double theta)? GetRobotPose2D()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
Pose2D pose = default;
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_get_robot_pose_2d(_handle, ref pose);
|
|
if (!success)
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
return (pose.x, pose.y, pose.theta);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting robot pose 2D");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's current twist
|
|
/// </summary>
|
|
public (double x, double y, double theta, string frameId)? GetTwist()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
var twist = new Twist2DStamped
|
|
{
|
|
header = new Header
|
|
{
|
|
seq = _sequenceNumber++,
|
|
sec = (uint)DateTime.Now.Second,
|
|
nsec = (uint)(DateTime.Now.Millisecond * 1_000_000),
|
|
frame_id = Marshal.StringToHGlobalAnsi("base_footprint")
|
|
},
|
|
velocity = new Twist2D
|
|
{
|
|
x = default,
|
|
y = default,
|
|
theta = default
|
|
}
|
|
};
|
|
bool success = false;
|
|
|
|
lock (_lock)
|
|
{
|
|
success = NavigationNativeInterface.navigation_get_twist(_handle, ref twist);
|
|
|
|
if (!success)
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
try
|
|
{
|
|
string frameId = string.Empty;
|
|
if (twist.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(twist.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
return (twist.velocity.x, twist.velocity.y, twist.velocity.theta, frameId);
|
|
}
|
|
finally
|
|
{
|
|
if (success && twist.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(twist.header.frame_id);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting twist");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the robot's current twist using struct reference (thread-safe)
|
|
/// Caller is responsible for freeing frame_id string using nav_c_api_free_string
|
|
/// </summary>
|
|
internal bool GetTwistStruct(ref Twist2DStamped twist)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
lock (_lock)
|
|
{
|
|
return NavigationNativeInterface.navigation_get_twist(_handle, ref twist);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get navigation feedback
|
|
/// </summary>
|
|
public NavigationFeedbackData? GetFeedback()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
NavFeedback feedback = default;
|
|
bool feedbackRetrieved = false;
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_get_feedback(_handle, ref feedback);
|
|
if (!success)
|
|
{
|
|
return null;
|
|
}
|
|
feedbackRetrieved = true;
|
|
}
|
|
|
|
try
|
|
{
|
|
string feedbackStr = string.Empty;
|
|
if (feedback.feed_back_str != IntPtr.Zero)
|
|
{
|
|
feedbackStr = Marshal.PtrToStringAnsi(feedback.feed_back_str) ?? string.Empty;
|
|
}
|
|
|
|
return new NavigationFeedbackData
|
|
{
|
|
NavigationState = feedback.navigation_state,
|
|
FeedbackString = feedbackStr,
|
|
CurrentPose = new Pose2DData
|
|
{
|
|
X = feedback.current_pose.x,
|
|
Y = feedback.current_pose.y,
|
|
Theta = feedback.current_pose.theta
|
|
},
|
|
GoalChecked = feedback.goal_checked,
|
|
IsReady = feedback.is_ready
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting feedback");
|
|
return null;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting feedback");
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (feedbackRetrieved)
|
|
{
|
|
if (feedback.feed_back_str != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(feedback.feed_back_str);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get global planner path data from navigation_get_global_data.
|
|
/// </summary>
|
|
public GlobalPathData? GetGlobalPathData()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
PlannerDataOutput plannerData = default;
|
|
bool fetched = false;
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
fetched = NavigationNativeInterface.navigation_get_global_data(_handle, ref plannerData);
|
|
}
|
|
|
|
if (!fetched)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
string frameId = string.Empty;
|
|
if (plannerData.plan.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(plannerData.plan.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
var points = new List<GlobalPathPointData>();
|
|
int pointCount = (int)plannerData.plan.poses_count;
|
|
if (plannerData.plan.poses != IntPtr.Zero && pointCount > 0)
|
|
{
|
|
int poseSize = Marshal.SizeOf<Pose2DStamped>();
|
|
for (int index = 0; index < pointCount; index++)
|
|
{
|
|
IntPtr posePtr = IntPtr.Add(plannerData.plan.poses, index * poseSize);
|
|
Pose2DStamped pose = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
|
|
points.Add(new GlobalPathPointData
|
|
{
|
|
X = pose.pose.x,
|
|
Y = pose.pose.y,
|
|
Theta = pose.pose.theta
|
|
});
|
|
}
|
|
}
|
|
|
|
return new GlobalPathData
|
|
{
|
|
FrameId = frameId,
|
|
Points = points
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting global planner data");
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (fetched)
|
|
{
|
|
FreePlannerDataOutput(ref plannerData);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get local planner path data from navigation_get_local_data.
|
|
/// </summary>
|
|
public GlobalPathData? GetLocalPathData()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
PlannerDataOutput plannerData = default;
|
|
bool fetched = false;
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
fetched = NavigationNativeInterface.navigation_get_local_data(_handle, ref plannerData);
|
|
}
|
|
|
|
if (!fetched)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
string frameId = string.Empty;
|
|
if (plannerData.plan.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(plannerData.plan.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
var points = new List<GlobalPathPointData>();
|
|
int pointCount = (int)plannerData.plan.poses_count;
|
|
if (plannerData.plan.poses != IntPtr.Zero && pointCount > 0)
|
|
{
|
|
int poseSize = Marshal.SizeOf<Pose2DStamped>();
|
|
for (int index = 0; index < pointCount; index++)
|
|
{
|
|
IntPtr posePtr = IntPtr.Add(plannerData.plan.poses, index * poseSize);
|
|
Pose2DStamped pose = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
|
|
points.Add(new GlobalPathPointData
|
|
{
|
|
X = pose.pose.x,
|
|
Y = pose.pose.y,
|
|
Theta = pose.pose.theta
|
|
});
|
|
}
|
|
}
|
|
|
|
return new GlobalPathData
|
|
{
|
|
FrameId = frameId,
|
|
Points = points
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting local planner path data");
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (fetched)
|
|
{
|
|
FreePlannerDataOutput(ref plannerData);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get cost map data from local planner (navigation_get_local_data)
|
|
/// Returns occupancy grid with cost values (0-254 where 254=obstacle, 0=free)
|
|
/// </summary>
|
|
public CostMapData? GetCostMapData()
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
PlannerDataOutput plannerData = default;
|
|
bool fetched = false;
|
|
|
|
try
|
|
{
|
|
lock (_lock)
|
|
{
|
|
fetched = NavigationNativeInterface.navigation_get_local_data(_handle, ref plannerData);
|
|
}
|
|
|
|
if (!fetched)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
string frameId = string.Empty;
|
|
if (plannerData.costmap.header.frame_id != IntPtr.Zero)
|
|
{
|
|
frameId = Marshal.PtrToStringAnsi(plannerData.costmap.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
string updateFrameId = string.Empty;
|
|
if (plannerData.costmap_update.header.frame_id != IntPtr.Zero)
|
|
{
|
|
updateFrameId = Marshal.PtrToStringAnsi(plannerData.costmap_update.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
bool hasFullMap = plannerData.costmap.data != IntPtr.Zero
|
|
&& plannerData.costmap.data_count != 0
|
|
&& plannerData.costmap.info.width > 0
|
|
&& plannerData.costmap.info.height > 0;
|
|
|
|
bool isCostmapUpdated = plannerData.is_costmap_updated
|
|
&& plannerData.costmap_update.data != IntPtr.Zero
|
|
&& plannerData.costmap_update.data_count != 0
|
|
&& plannerData.costmap_update.width > 0
|
|
&& plannerData.costmap_update.height > 0;
|
|
|
|
if (!hasFullMap && !isCostmapUpdated)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
byte[] costData = Array.Empty<byte>();
|
|
if (hasFullMap)
|
|
{
|
|
int dataCount = (int)plannerData.costmap.data_count;
|
|
costData = new byte[dataCount];
|
|
if (dataCount > 0)
|
|
{
|
|
Marshal.Copy(plannerData.costmap.data, costData, 0, dataCount);
|
|
}
|
|
}
|
|
|
|
byte[] updateData = Array.Empty<byte>();
|
|
if (isCostmapUpdated)
|
|
{
|
|
int updateCount = (int)plannerData.costmap_update.data_count;
|
|
updateData = new byte[updateCount];
|
|
if (updateCount > 0)
|
|
{
|
|
Marshal.Copy(plannerData.costmap_update.data, updateData, 0, updateCount);
|
|
}
|
|
}
|
|
|
|
double originTheta = hasFullMap
|
|
? QuaternionToYaw(plannerData.costmap.info.origin.orientation)
|
|
: 0.0;
|
|
|
|
return new CostMapData
|
|
{
|
|
FrameId = frameId,
|
|
Resolution = hasFullMap ? plannerData.costmap.info.resolution : 0.0,
|
|
Width = hasFullMap ? (int)plannerData.costmap.info.width : 0,
|
|
Height = hasFullMap ? (int)plannerData.costmap.info.height : 0,
|
|
OriginX = hasFullMap ? plannerData.costmap.info.origin.position.x : 0.0,
|
|
OriginY = hasFullMap ? plannerData.costmap.info.origin.position.y : 0.0,
|
|
OriginTheta = originTheta,
|
|
Data = costData,
|
|
HasFullMap = hasFullMap,
|
|
UpdateFrameId = updateFrameId,
|
|
IsCostmapUpdated = isCostmapUpdated,
|
|
UpdateX = plannerData.costmap_update.x,
|
|
UpdateY = plannerData.costmap_update.y,
|
|
UpdateWidth = (int)plannerData.costmap_update.width,
|
|
UpdateHeight = (int)plannerData.costmap_update.height,
|
|
UpdateData = updateData
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error getting cost map data");
|
|
return null;
|
|
}
|
|
finally
|
|
{
|
|
if (fetched)
|
|
{
|
|
FreePlannerDataOutput(ref plannerData);
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void FreePlannerDataOutput(ref PlannerDataOutput plannerData)
|
|
{
|
|
if (plannerData.plan.poses != IntPtr.Zero)
|
|
{
|
|
int pointCount = (int)plannerData.plan.poses_count;
|
|
int poseSize = Marshal.SizeOf<Pose2DStamped>();
|
|
for (int index = 0; index < pointCount; index++)
|
|
{
|
|
IntPtr posePtr = IntPtr.Add(plannerData.plan.poses, index * poseSize);
|
|
Pose2DStamped pose = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
|
|
if (pose.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(pose.header.frame_id);
|
|
}
|
|
}
|
|
|
|
NativeFree(plannerData.plan.poses);
|
|
plannerData.plan.poses = IntPtr.Zero;
|
|
plannerData.plan.poses_count = 0;
|
|
}
|
|
|
|
if (plannerData.plan.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(plannerData.plan.header.frame_id);
|
|
plannerData.plan.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
if (plannerData.costmap.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(plannerData.costmap.header.frame_id);
|
|
plannerData.costmap.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
if (plannerData.costmap.data != IntPtr.Zero)
|
|
{
|
|
NativeFree(plannerData.costmap.data);
|
|
plannerData.costmap.data = IntPtr.Zero;
|
|
plannerData.costmap.data_count = 0;
|
|
}
|
|
|
|
if (plannerData.costmap_update.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(plannerData.costmap_update.header.frame_id);
|
|
plannerData.costmap_update.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
if (plannerData.costmap_update.data != IntPtr.Zero)
|
|
{
|
|
NativeFree(plannerData.costmap_update.data);
|
|
plannerData.costmap_update.data = IntPtr.Zero;
|
|
plannerData.costmap_update.data_count = 0;
|
|
}
|
|
|
|
if (plannerData.footprint.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(plannerData.footprint.header.frame_id);
|
|
plannerData.footprint.header.frame_id = IntPtr.Zero;
|
|
}
|
|
|
|
if (plannerData.footprint.polygon.points != IntPtr.Zero)
|
|
{
|
|
NativeFree(plannerData.footprint.polygon.points);
|
|
plannerData.footprint.polygon.points = IntPtr.Zero;
|
|
plannerData.footprint.polygon.points_count = 0;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert quaternion (x, y, z, w) to yaw angle (theta) in radians
|
|
/// </summary>
|
|
private static double QuaternionToYaw(Quaternion q)
|
|
{
|
|
double yaw = Math.Atan2(
|
|
2.0 * (q.w * q.z + q.x * q.y),
|
|
1.0 - 2.0 * (q.y * q.y + q.z * q.z)
|
|
);
|
|
return yaw;
|
|
}
|
|
|
|
public (double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)? OffsetGoal2D(
|
|
double poseX, double poseY, double poseTheta,
|
|
string frameId,
|
|
double offsetDistance)
|
|
{
|
|
ThrowIfNotInitialized();
|
|
|
|
try
|
|
{
|
|
PoseStamped outGoal;
|
|
lock (_lock)
|
|
{
|
|
bool success = NavigationNativeInterface.navigation_offset_goal_2d(
|
|
poseX, poseY, poseTheta,
|
|
frameId,
|
|
offsetDistance,
|
|
out outGoal);
|
|
|
|
if (!success)
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
string resultFrameId = string.Empty;
|
|
if (outGoal.header.frame_id != IntPtr.Zero)
|
|
{
|
|
resultFrameId = Marshal.PtrToStringAnsi(outGoal.header.frame_id) ?? string.Empty;
|
|
}
|
|
|
|
return (
|
|
outGoal.pose.position.x,
|
|
outGoal.pose.position.y,
|
|
outGoal.pose.position.z,
|
|
outGoal.pose.orientation.x,
|
|
outGoal.pose.orientation.y,
|
|
outGoal.pose.orientation.z,
|
|
outGoal.pose.orientation.w,
|
|
resultFrameId
|
|
);
|
|
}
|
|
finally
|
|
{
|
|
if (outGoal.header.frame_id != IntPtr.Zero)
|
|
{
|
|
NavigationNativeInterface.nav_c_api_free_string(outGoal.header.frame_id);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error offsetting goal 2D");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Publish static transforms to TF buffer
|
|
/// Sets up the robot's coordinate frame tree
|
|
/// </summary>
|
|
private void PublishStaticTransforms()
|
|
{
|
|
try
|
|
{
|
|
bool success1 = NavigationNativeInterface.tf_listener_set_static_transform(
|
|
_tfBuffer, "map", "odom",
|
|
0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 1.0);
|
|
|
|
if (success1)
|
|
_logger?.LogInformation("[DEBUG] Published static transform: map → odom");
|
|
else
|
|
_logger?.LogWarning("[DEBUG] Failed to publish static transform: map → odom");
|
|
|
|
bool success2 = NavigationNativeInterface.tf_listener_set_static_transform(
|
|
_tfBuffer, "odom", "base_footprint",
|
|
0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 1.0);
|
|
|
|
if (success2)
|
|
_logger?.LogInformation("[DEBUG] Published static transform: odom → base_footprint");
|
|
else
|
|
_logger?.LogWarning("[DEBUG] Failed to publish static transform: odom → base_footprint");
|
|
|
|
bool success3 = NavigationNativeInterface.tf_listener_set_static_transform(
|
|
_tfBuffer, "base_footprint", "base_link",
|
|
0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 1.0);
|
|
|
|
if (success3)
|
|
_logger?.LogInformation("[DEBUG] Published static transform: base_footprint → base_link");
|
|
else
|
|
_logger?.LogWarning("[DEBUG] Failed to publish static transform: base_footprint → base_link");
|
|
|
|
var now = DateTime.UtcNow;
|
|
long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
|
|
long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;
|
|
|
|
var baseToLidar1 = new TF3.TF3_Transform
|
|
{
|
|
timestamp_sec = sec,
|
|
timestamp_nsec = nsec,
|
|
frame_id = "base_link",
|
|
child_frame_id = "scan_1",
|
|
translation_x = -0.15,
|
|
translation_y = 0.0,
|
|
translation_z = 0.0,
|
|
rotation_x = 0.0,
|
|
rotation_y = 0.0,
|
|
rotation_z = 1.0,
|
|
rotation_w = 0.0
|
|
};
|
|
|
|
TF3.TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar1, "navigation_client", true);
|
|
_logger?.LogInformation("[DEBUG] Published static transform: base_link → scan_1");
|
|
|
|
bool success6 = NavigationNativeInterface.tf_listener_set_static_transform(
|
|
_tfBuffer, "base_link", "scan_2",
|
|
0.707, -0.2825, 0.0,
|
|
0.0, 0.0, -0.3826834, 0.9238795);
|
|
|
|
if (success6)
|
|
_logger?.LogInformation("[DEBUG] Published static transform: base_link → scan_2 (right lidar, -45°)");
|
|
else
|
|
_logger?.LogWarning("[DEBUG] Failed to publish static transform: base_link → scan_2");
|
|
|
|
bool success7 = NavigationNativeInterface.tf_listener_set_static_transform(
|
|
_tfBuffer, "base_link", "scan_3",
|
|
0.707, 0.2825, 0.0,
|
|
0.0, 0.0, 0.3802634, 0.9248782);
|
|
|
|
if (success7)
|
|
_logger?.LogInformation("[DEBUG] Published static transform: base_link → scan_3 (left lidar, +45°)");
|
|
else
|
|
_logger?.LogWarning("[DEBUG] Failed to publish static transform: base_link → scan_3");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "[DEBUG] Error publishing static transforms: {Message}", ex.Message);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create a PoseStamped structure from parameters
|
|
/// </summary>
|
|
private PoseStamped CreatePoseStamped(double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)
|
|
{
|
|
var now = DateTime.UtcNow;
|
|
long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
|
|
long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;
|
|
|
|
return new PoseStamped
|
|
{
|
|
header = new Header
|
|
{
|
|
seq = 0,
|
|
sec = (uint)sec,
|
|
nsec = (uint)nsec,
|
|
frame_id = Marshal.StringToHGlobalAnsi(frameId)
|
|
},
|
|
pose = new Pose
|
|
{
|
|
position = new Point { x = x, y = y, z = z },
|
|
orientation = new Quaternion { x = qx, y = qy, z = qz, w = qw }
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Free memory allocated for PoseStamped
|
|
/// </summary>
|
|
private void FreePoseStamped(ref PoseStamped poseStamped)
|
|
{
|
|
if (poseStamped.header.frame_id != IntPtr.Zero)
|
|
{
|
|
Marshal.FreeHGlobal(poseStamped.header.frame_id);
|
|
poseStamped.header.frame_id = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
private void ThrowIfNotInitialized()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_handle == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("NavigationClient is not initialized. Call Initialize() first.");
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
Dispose(true);
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
|
|
protected virtual void Dispose(bool disposing)
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
|
|
lock (_lock)
|
|
{
|
|
_disposed = true;
|
|
if (_handle != IntPtr.Zero)
|
|
{
|
|
try
|
|
{
|
|
NavigationNativeInterface.navigation_destroy(_handle);
|
|
_logger?.LogInformation("NavigationClient disposed successfully");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger?.LogError(ex, "Error disposing NavigationClient");
|
|
}
|
|
finally
|
|
{
|
|
_handle = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
_tfBuffer = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
~NavigationClient()
|
|
{
|
|
Dispose(false);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Navigation feedback data
|
|
/// </summary>
|
|
public class NavigationFeedbackData
|
|
{
|
|
public NavigationState NavigationState { get; set; }
|
|
public string FeedbackString { get; set; } = string.Empty;
|
|
public Pose2DData CurrentPose { get; set; } = new();
|
|
public bool GoalChecked { get; set; }
|
|
public bool IsReady { get; set; }
|
|
|
|
public string StateString => NavigationState switch
|
|
{
|
|
NavigationState.Pending => "Pending",
|
|
NavigationState.Active => "Active",
|
|
NavigationState.Preempted => "Preempted",
|
|
NavigationState.Succeeded => "Succeeded",
|
|
NavigationState.Aborted => "Aborted",
|
|
NavigationState.Rejected => "Rejected",
|
|
NavigationState.Preempting => "Preempting",
|
|
NavigationState.Recalling => "Recalling",
|
|
NavigationState.Recalled => "Recalled",
|
|
NavigationState.Lost => "Lost",
|
|
NavigationState.Planning => "Planning",
|
|
NavigationState.Controlling => "Controlling",
|
|
NavigationState.Clearing => "Clearing",
|
|
NavigationState.Paused => "Paused",
|
|
_ => $"Unknown ({(int)NavigationState})"
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pose2D data
|
|
/// </summary>
|
|
public class Pose2DData
|
|
{
|
|
public double X { get; set; }
|
|
public double Y { get; set; }
|
|
public double Theta { get; set; }
|
|
}
|
|
|
|
public class GlobalPathData
|
|
{
|
|
public string FrameId { get; set; } = string.Empty;
|
|
public List<GlobalPathPointData> Points { get; set; } = new();
|
|
}
|
|
|
|
public class GlobalPathPointData
|
|
{
|
|
public double X { get; set; }
|
|
public double Y { get; set; }
|
|
public double Theta { get; set; }
|
|
}
|
|
|
|
public class CostMapData
|
|
{
|
|
public string FrameId { get; set; } = string.Empty;
|
|
public double Resolution { get; set; }
|
|
public int Width { get; set; }
|
|
public int Height { get; set; }
|
|
public double OriginX { get; set; }
|
|
public double OriginY { get; set; }
|
|
public double OriginTheta { get; set; }
|
|
public byte[] Data { get; set; } = Array.Empty<byte>();
|
|
public bool HasFullMap { get; set; }
|
|
public string UpdateFrameId { get; set; } = string.Empty;
|
|
public bool IsCostmapUpdated { get; set; }
|
|
public int UpdateX { get; set; }
|
|
public int UpdateY { get; set; }
|
|
public int UpdateWidth { get; set; }
|
|
public int UpdateHeight { get; set; }
|
|
public byte[] UpdateData { get; set; } = Array.Empty<byte>();
|
|
} |