745 lines
33 KiB
C#
745 lines
33 KiB
C#
using System;
|
|
using System.Globalization;
|
|
using System.IO;
|
|
using System.Runtime.InteropServices;
|
|
using System.Runtime.CompilerServices;
|
|
using System.Text.RegularExpressions;
|
|
using SixLabors.ImageSharp;
|
|
using SixLabors.ImageSharp.PixelFormats;
|
|
|
|
namespace NavigationExample
|
|
{
|
|
/// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
|
|
public struct MazeMapConfig
|
|
{
|
|
public string ImagePath; // đường dẫn đầy đủ tới file ảnh
|
|
public float Resolution;
|
|
public double OriginX, OriginY, OriginZ;
|
|
public int Negate; // 0 hoặc 1
|
|
public double OccupiedThresh;
|
|
public double FreeThresh;
|
|
}
|
|
/// <summary>
|
|
/// C# P/Invoke wrapper for Navigation C API
|
|
/// </summary>
|
|
public class NavigationAPI
|
|
{
|
|
private const string DllName = "libnav_c_api.so"; // Linux
|
|
// For Windows: "nav_c_api.dll"
|
|
// For macOS: "libnav_c_api.dylib"
|
|
|
|
// ============================================================================
|
|
// Enums
|
|
// ============================================================================
|
|
public enum NavigationState
|
|
{
|
|
Pending = 0,
|
|
Active = 1,
|
|
Preempted = 2,
|
|
Succeeded = 3,
|
|
Aborted = 4,
|
|
Rejected = 5,
|
|
Preempting = 6,
|
|
Recalling = 7,
|
|
Recalled = 8,
|
|
Lost = 9,
|
|
Planning = 10,
|
|
Controlling = 11,
|
|
Clearing = 12,
|
|
Paused = 13
|
|
}
|
|
|
|
// ============================================================================
|
|
// Structures
|
|
// ============================================================================
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Point
|
|
{
|
|
public double x;
|
|
public double y;
|
|
public double z;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Pose2D
|
|
{
|
|
public double x;
|
|
public double y;
|
|
public double theta;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct NavFeedback
|
|
{
|
|
public NavigationState navigation_state;
|
|
public IntPtr feed_back_str; // char*; free with nav_c_api_free_string
|
|
public Pose2D current_pose;
|
|
[MarshalAs(UnmanagedType.I1)]
|
|
public bool goal_checked;
|
|
[MarshalAs(UnmanagedType.I1)]
|
|
public bool is_ready;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Twist2D
|
|
{
|
|
public double x;
|
|
public double y;
|
|
public double theta;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Quaternion
|
|
{
|
|
public double x;
|
|
public double y;
|
|
public double z;
|
|
public double w;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Position
|
|
{
|
|
public double x;
|
|
public double y;
|
|
public double z;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Pose
|
|
{
|
|
public Position position;
|
|
public Quaternion orientation;
|
|
}
|
|
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Vector3
|
|
{
|
|
public double x;
|
|
public double y;
|
|
public double z;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Twist
|
|
{
|
|
public Vector3 linear;
|
|
public Vector3 angular;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Header
|
|
{
|
|
public uint seq;
|
|
public uint sec;
|
|
public uint nsec;
|
|
public IntPtr frame_id; // char*
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct PoseStamped
|
|
{
|
|
public Header header;
|
|
public Pose pose;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Twist2DStamped
|
|
{
|
|
public Header header;
|
|
public Twist2D velocity;
|
|
}
|
|
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct NavigationHandle
|
|
{
|
|
public IntPtr ptr;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct TFListenerHandle
|
|
{
|
|
public IntPtr ptr;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct LaserScan
|
|
{
|
|
public Header 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;
|
|
public UIntPtr ranges_count;
|
|
public IntPtr intensities;
|
|
public UIntPtr intensities_count;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct PoseWithCovariance
|
|
{
|
|
public Pose pose;
|
|
public IntPtr covariance;
|
|
public UIntPtr covariance_count;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct TwistWithCovariance {
|
|
public Twist twist;
|
|
public IntPtr covariance;
|
|
public UIntPtr covariance_count;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Odometry
|
|
{
|
|
public Header header;
|
|
public IntPtr child_frame_id;
|
|
public PoseWithCovariance pose;
|
|
public TwistWithCovariance twist;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct OccupancyGrid
|
|
{
|
|
public Header header;
|
|
public MapMetaData info;
|
|
public IntPtr data;
|
|
public UIntPtr data_count;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct MapMetaData
|
|
{
|
|
public Time map_load_time;
|
|
public float resolution;
|
|
public uint width;
|
|
public uint height;
|
|
public Pose origin;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Time
|
|
{
|
|
public uint sec;
|
|
public uint nsec;
|
|
}
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
public static extern Header header_create(string frame_id);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
public static extern Header header_set_data(
|
|
uint seq,
|
|
uint sec,
|
|
uint nsec,
|
|
string frame_id);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern Time time_create();
|
|
|
|
/// <summary>Free a string allocated by the API (strdup).</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void nav_c_api_free_string(IntPtr str);
|
|
|
|
/// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
public static extern IntPtr navigation_state_to_string(NavigationState state);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
|
|
|
|
|
|
/// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
|
|
public static string MarshalString(IntPtr p)
|
|
{
|
|
if (p == IntPtr.Zero) return string.Empty;
|
|
return Marshal.PtrToStringAnsi(p) ?? string.Empty;
|
|
}
|
|
|
|
/// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
|
|
public static void navigation_free_feedback(ref NavFeedback feedback)
|
|
{
|
|
if (feedback.feed_back_str != IntPtr.Zero)
|
|
{
|
|
nav_c_api_free_string(feedback.feed_back_str);
|
|
feedback.feed_back_str = IntPtr.Zero;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// TF Listener Management
|
|
// ============================================================================
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern TFListenerHandle tf_listener_create();
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void tf_listener_destroy(TFListenerHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool tf_listener_set_static_transform(
|
|
TFListenerHandle tf_handle,
|
|
string parent_frame,
|
|
string child_frame,
|
|
double x, double y, double z,
|
|
double qx, double qy, double qz, double qw);
|
|
|
|
// ============================================================================
|
|
// Navigation Handle Management
|
|
// ============================================================================
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern NavigationHandle navigation_create();
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
public static extern void navigation_destroy(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_pause(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_resume(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_cancel(NavigationHandle handle);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
|
|
// ============================================================================
|
|
// Navigation Data Management
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
|
|
|
|
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
|
[return: MarshalAs(UnmanagedType.I1)]
|
|
public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
|
|
|
|
}
|
|
|
|
// ============================================================================
|
|
// Example Usage
|
|
// ============================================================================
|
|
class Program
|
|
{
|
|
/// <summary>
|
|
/// Đọc file maze.yaml (image, resolution, origin, negate, occupied_thresh, free_thresh).
|
|
/// ImagePath được resolve tuyệt đối theo thư mục chứa file yaml.
|
|
/// </summary>
|
|
static bool TryLoadMazeYaml(string yamlPath, out MazeMapConfig config)
|
|
{
|
|
config = default;
|
|
if (string.IsNullOrEmpty(yamlPath) || !File.Exists(yamlPath))
|
|
return false;
|
|
string dir = Path.GetDirectoryName(Path.GetFullPath(yamlPath)) ?? "";
|
|
try
|
|
{
|
|
string[] lines = File.ReadAllLines(yamlPath);
|
|
foreach (string line in lines)
|
|
{
|
|
string trimmed = line.Trim();
|
|
if (trimmed.Length == 0 || trimmed.StartsWith("#")) continue;
|
|
int colon = trimmed.IndexOf(':');
|
|
if (colon <= 0) continue;
|
|
string key = trimmed.Substring(0, colon).Trim();
|
|
string value = trimmed.Substring(colon + 1).Trim();
|
|
if (key == "image")
|
|
config.ImagePath = Path.Combine(dir, value);
|
|
else if (key == "resolution" && float.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out float res))
|
|
config.Resolution = res;
|
|
else if (key == "origin")
|
|
{
|
|
var m = Regex.Match(value, @"\[\s*([-\d.]+\s*),\s*([-\d.]+\s*),\s*([-\d.]+)\s*\]");
|
|
if (m.Success)
|
|
{
|
|
config.OriginX = double.Parse(m.Groups[1].Value, CultureInfo.InvariantCulture);
|
|
config.OriginY = double.Parse(m.Groups[2].Value, CultureInfo.InvariantCulture);
|
|
config.OriginZ = double.Parse(m.Groups[3].Value, CultureInfo.InvariantCulture);
|
|
}
|
|
}
|
|
else if (key == "negate" && int.TryParse(value, out int neg))
|
|
config.Negate = neg;
|
|
else if (key == "occupied_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ot))
|
|
config.OccupiedThresh = ot;
|
|
else if (key == "free_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ft))
|
|
config.FreeThresh = ft;
|
|
}
|
|
return !string.IsNullOrEmpty(config.ImagePath);
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Đọc file ảnh PNG và chuyển pixel sang byte[] occupancy (0=free, 100=occupied, 255=unknown).
|
|
/// Dùng negate, occupied_thresh, free_thresh từ maze.yaml nếu cung cấp.
|
|
/// </summary>
|
|
static byte[] LoadMazePixelsAsOccupancy(string imagePath, out int width, out int height,
|
|
int negate = 0, double occupiedThresh = 0.65, double freeThresh = 0.196)
|
|
{
|
|
width = 0;
|
|
height = 0;
|
|
if (string.IsNullOrEmpty(imagePath) || !File.Exists(imagePath))
|
|
{
|
|
LogError($"File not found: {imagePath}");
|
|
return null;
|
|
}
|
|
try
|
|
{
|
|
using var image = Image.Load<Rgba32>(imagePath);
|
|
int w = image.Width;
|
|
int h = image.Height;
|
|
width = w;
|
|
height = h;
|
|
byte[] data = new byte[w * h];
|
|
image.ProcessPixelRows(accessor =>
|
|
{
|
|
for (int y = 0; y < accessor.Height; y++)
|
|
{
|
|
Span<Rgba32> row = accessor.GetRowSpan(y);
|
|
for (int x = 0; x < row.Length; x++)
|
|
{
|
|
ref readonly Rgba32 p = ref row[x];
|
|
double gray = (p.R + p.G + p.B) / 255.0 / 3.0;
|
|
if (negate != 0) gray = 1.0 - gray;
|
|
byte occ;
|
|
if (gray <= freeThresh) occ = 0;
|
|
else if (gray >= occupiedThresh) occ = 100;
|
|
else occ = 255; // unknown
|
|
data[y * w + x] = occ;
|
|
}
|
|
}
|
|
});
|
|
return data;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
LogError($"Load image failed: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
// Helper method để hiển thị file và line number tự động
|
|
static void LogError(string message,
|
|
[CallerFilePath] string filePath = "",
|
|
[CallerLineNumber] int lineNumber = 0,
|
|
[CallerMemberName] string memberName = "")
|
|
{
|
|
// Lấy tên file từ đường dẫn đầy đủ
|
|
string fileName = System.IO.Path.GetFileName(filePath);
|
|
Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}");
|
|
}
|
|
|
|
static void Main(string[] args)
|
|
{
|
|
// Create TF listener (required for costmap and local planner; must be valid when calling navigation_initialize)
|
|
NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
|
|
if (tfHandle.ptr == IntPtr.Zero)
|
|
{
|
|
LogError("Failed to create TF listener");
|
|
return;
|
|
}
|
|
Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}");
|
|
|
|
// Inject a static TF so costmap can immediately canTransform(map <-> base_link).
|
|
// If you already publish TF from localization/odometry, you can remove this call.
|
|
if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "map", "odom",
|
|
0, 0, 0,
|
|
0, 0, 0, 1))
|
|
{
|
|
LogError("Failed to inject static TF map -> odom");
|
|
NavigationAPI.tf_listener_destroy(tfHandle);
|
|
return;
|
|
}
|
|
|
|
if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint",
|
|
0, 0, 0,
|
|
0, 0, 0, 1))
|
|
{
|
|
LogError("Failed to inject static TF map -> base_link");
|
|
NavigationAPI.tf_listener_destroy(tfHandle);
|
|
return;
|
|
}
|
|
|
|
if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link",
|
|
0, 0, 0,
|
|
0, 0, 0, 1))
|
|
{
|
|
LogError("Failed to inject static TF map -> base_link");
|
|
NavigationAPI.tf_listener_destroy(tfHandle);
|
|
return;
|
|
}
|
|
|
|
// Create navigation instance
|
|
NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
|
|
if (navHandle.ptr == IntPtr.Zero)
|
|
{
|
|
LogError("Failed to create navigation instance");
|
|
NavigationAPI.tf_listener_destroy(tfHandle);
|
|
return;
|
|
}
|
|
|
|
// Initialize navigation (passes TF to move_base; navigation keeps its own copy, so tfHandle can be destroyed later)
|
|
if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
|
|
{
|
|
LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')");
|
|
NavigationAPI.navigation_destroy(navHandle);
|
|
NavigationAPI.tf_listener_destroy(tfHandle);
|
|
return;
|
|
}
|
|
|
|
while(true)
|
|
{
|
|
// Get navigation feedback
|
|
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
|
|
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
|
|
{
|
|
if(feedback.is_ready)
|
|
{
|
|
Console.WriteLine("Navigation is ready");
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("Navigation is not ready");
|
|
}
|
|
}
|
|
System.Threading.Thread.Sleep(100);
|
|
}
|
|
Console.WriteLine("[NavigationExample] Navigation initialized with TF successfully");
|
|
|
|
// Set robot footprint
|
|
NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
|
|
{
|
|
new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
|
|
new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
|
|
new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
|
|
new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
|
|
};
|
|
NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
|
|
|
|
|
|
IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
|
|
NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
|
|
NavigationAPI.LaserScan fscanHandle;
|
|
fscanHandle.header = fscanHeader;
|
|
fscanHandle.angle_min = -1.57f;
|
|
fscanHandle.angle_max = 1.57f;
|
|
fscanHandle.angle_increment = 0.785f;
|
|
fscanHandle.time_increment = 0.0f;
|
|
fscanHandle.scan_time = 0.1f;
|
|
fscanHandle.range_min = 0.05f;
|
|
fscanHandle.range_max = 10.0f;
|
|
fscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
|
|
Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, fscanHandle.ranges, 5);
|
|
fscanHandle.ranges_count = new UIntPtr(5);
|
|
fscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
|
|
Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, fscanHandle.intensities, 5);
|
|
fscanHandle.intensities_count = new UIntPtr(5);
|
|
NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan", fscanHandle);
|
|
|
|
IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
|
|
NavigationAPI.Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
|
|
NavigationAPI.LaserScan bscanHandle;
|
|
bscanHandle.header = bscanHeader;
|
|
bscanHandle.angle_min = 1.57f;
|
|
bscanHandle.angle_max = -1.57f;
|
|
bscanHandle.angle_increment = -0.785f;
|
|
bscanHandle.time_increment = 0.0f;
|
|
bscanHandle.scan_time = 0.1f;
|
|
bscanHandle.range_min = 0.05f;
|
|
bscanHandle.range_max = 10.0f;
|
|
bscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
|
|
Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, bscanHandle.ranges, 5);
|
|
bscanHandle.ranges_count = new UIntPtr(5);
|
|
bscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
|
|
Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, bscanHandle.intensities, 5);
|
|
bscanHandle.intensities_count = new UIntPtr(5);
|
|
NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan", bscanHandle);
|
|
|
|
IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
|
|
NavigationAPI.Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
|
|
NavigationAPI.Odometry odometryHandle = new NavigationAPI.Odometry();
|
|
odometryHandle.header = odometryHeader;
|
|
IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint");
|
|
odometryHandle.child_frame_id = childFrameId;
|
|
odometryHandle.pose.pose.position.x = 0.0;
|
|
odometryHandle.pose.pose.position.y = 0.0;
|
|
odometryHandle.pose.pose.position.z = 0.0;
|
|
odometryHandle.pose.pose.orientation.x = 0.0;
|
|
odometryHandle.pose.pose.orientation.y = 0.0;
|
|
odometryHandle.pose.pose.orientation.z = 0.0;
|
|
odometryHandle.pose.pose.orientation.w = 1.0;
|
|
|
|
double[] pose_covariance = new double[36];
|
|
for(int i = 0; i < pose_covariance.Length; i++) {
|
|
pose_covariance[i] = 0.0;
|
|
}
|
|
odometryHandle.pose.covariance = Marshal.AllocHGlobal(sizeof(double) * pose_covariance.Length);
|
|
Marshal.Copy(pose_covariance, 0, odometryHandle.pose.covariance, pose_covariance.Length);
|
|
odometryHandle.pose.covariance_count = new UIntPtr((uint)pose_covariance.Length);
|
|
|
|
odometryHandle.twist.twist.linear.x = 0.0;
|
|
odometryHandle.twist.twist.linear.y = 0.0;
|
|
odometryHandle.twist.twist.linear.z = 0.0;
|
|
odometryHandle.twist.twist.angular.x = 0.0;
|
|
odometryHandle.twist.twist.angular.y = 0.0;
|
|
odometryHandle.twist.twist.angular.z = 0.0;
|
|
double[] twist_covariance = new double[36];
|
|
for(int i = 0; i < twist_covariance.Length; i++) {
|
|
twist_covariance[i] = 0.0;
|
|
}
|
|
odometryHandle.twist.covariance = Marshal.AllocHGlobal(sizeof(double) * twist_covariance.Length);
|
|
Marshal.Copy(twist_covariance, 0, odometryHandle.twist.covariance, twist_covariance.Length);
|
|
odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length);
|
|
NavigationAPI.navigation_add_odometry(navHandle, "odometry", odometryHandle);
|
|
|
|
// Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
|
|
string mapYamlPath = "maze.yaml";
|
|
int mapWidth, mapHeight;
|
|
byte[] data;
|
|
MazeMapConfig mapConfig;
|
|
if (TryLoadMazeYaml(mapYamlPath, out mapConfig))
|
|
{
|
|
data = LoadMazePixelsAsOccupancy(mapConfig.ImagePath, out mapWidth, out mapHeight,
|
|
mapConfig.Negate, mapConfig.OccupiedThresh, mapConfig.FreeThresh);
|
|
if (data == null)
|
|
{
|
|
mapWidth = 3;
|
|
mapHeight = 10;
|
|
data = new byte[30];
|
|
for (int i = 0; i < data.Length; i++) data[i] = 100;
|
|
Console.WriteLine("YAML loaded but image failed; using default map 3x10");
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("Loaded map from {0}: {1}x{2}, resolution={3}, origin=({4},{5},{6})",
|
|
mapConfig.ImagePath, mapWidth, mapHeight, mapConfig.Resolution,
|
|
mapConfig.OriginX, mapConfig.OriginY, mapConfig.OriginZ);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
mapWidth = 3;
|
|
mapHeight = 10;
|
|
data = new byte[30];
|
|
for (int i = 0; i < data.Length; i++) data[i] = 100;
|
|
mapConfig = default;
|
|
mapConfig.Resolution = 0.05f;
|
|
mapConfig.OriginX = mapConfig.OriginY = mapConfig.OriginZ = 0.0;
|
|
Console.WriteLine("maze.yaml not found, using default map 3x10");
|
|
}
|
|
|
|
NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
|
|
NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
|
|
mapMetaData.map_load_time = mapLoadTime;
|
|
mapMetaData.resolution = mapConfig.Resolution;
|
|
mapMetaData.width = (uint)mapWidth;
|
|
mapMetaData.height = (uint)mapHeight;
|
|
mapMetaData.origin = new NavigationAPI.Pose();
|
|
mapMetaData.origin.position.x = mapConfig.OriginX;
|
|
mapMetaData.origin.position.y = mapConfig.OriginY;
|
|
mapMetaData.origin.position.z = mapConfig.OriginZ;
|
|
mapMetaData.origin.orientation.x = 0.0;
|
|
mapMetaData.origin.orientation.y = 0.0;
|
|
mapMetaData.origin.orientation.z = 0.0;
|
|
mapMetaData.origin.orientation.w = 1.0;
|
|
NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid();
|
|
IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
|
|
occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
|
|
occupancyGrid.info = mapMetaData;
|
|
occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length);
|
|
Marshal.Copy(data, 0, occupancyGrid.data, data.Length);
|
|
occupancyGrid.data_count = new UIntPtr((uint)data.Length);
|
|
Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count);
|
|
Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}",
|
|
Marshal.SizeOf<NavigationAPI.OccupancyGrid>(),
|
|
Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data"),
|
|
Marshal.OffsetOf<NavigationAPI.OccupancyGrid>("data_count"));
|
|
|
|
NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid);
|
|
|
|
NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
|
|
if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
|
|
{
|
|
Console.WriteLine(
|
|
"Twist: {0}, {1}, {2}, {3}",
|
|
NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
|
|
}
|
|
// Cleanup
|
|
NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
|
|
|
|
NavigationAPI.navigation_destroy(navHandle);
|
|
NavigationAPI.tf_listener_destroy(tfHandle);
|
|
Console.WriteLine("Press any key to exit...");
|
|
Console.ReadKey();
|
|
}
|
|
}
|
|
}
|
|
|