HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
HiepLM
2026-03-02 07:50:30 +00:00
parent ff8a90cbaa
commit 06c2d01b4a
61 changed files with 2934 additions and 1012 deletions
Download Patch File Download Diff File Expand all files Collapse all files

292
examples/NavigationExample/NavigationAPI.cs Normal file
View File

@@ -0,0 +1,292 @@
using System;
using System.Runtime.InteropServices;
using NavigationExample;
namespace NavigationExample
{
/// <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
}
[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;
}
/// <summary>Planner data output (plan, costmap, footprint).</summary>
[StructLayout(LayoutKind.Sequential)]
public struct PlannerDataOutput
{
public Path2D plan;
public OccupancyGrid costmap;
public OccupancyGridUpdate costmap_update;
[MarshalAs(UnmanagedType.I1)]
public bool is_costmap_updated;
public PolygonStamped footprint;
}
[StructLayout(LayoutKind.Sequential)]
public struct NavigationHandle
{
public IntPtr ptr;
}
[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;
}
}
// ============================================================================
// 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);
/// <summary>Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy.</summary>
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer);
[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);
/// <summary>Send a goal for the robot to navigate to (global frame).</summary>
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal);
/// <summary>Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native.</summary>
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
[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);
/// <remarks>Goal is passed by reference to match C API: navigation_dock_to_order(..., const PoseStamped &goal).</remarks>
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, ref PoseStamped goal);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
/// <summary>Rotate in place to align with target orientation (radians).</summary>
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw);
[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);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_all_static_maps(NavigationHandle handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_remove_all_data(NavigationHandle handle);
/// <summary>Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed.</summary>
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data);
}
}

2
examples/NavigationExample/NavigationExample.csproj
View File

@@ -1,7 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net6.0</TargetFramework>
<TargetFramework>net10.0</TargetFramework>
<RuntimeIdentifier>linux-x64</RuntimeIdentifier>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>

597
examples/NavigationExample/Program.cs
View File

@@ -6,6 +6,8 @@ using System.Runtime.CompilerServices;
using System.Text.RegularExpressions;
using SixLabors.ImageSharp;
using SixLabors.ImageSharp.PixelFormats;
using NavigationExample;
namespace NavigationExample
{
@@ -19,366 +21,6 @@ namespace NavigationExample
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
@@ -497,69 +139,53 @@ namespace NavigationExample
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)
// Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init)
IntPtr tf3Buffer = TF3API.tf3_buffer_create(10);
if (tf3Buffer == IntPtr.Zero)
{
LogError("Failed to create TF listener");
LogError("Failed to create tf3 buffer (libtf3.so may be missing)");
return;
}
Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}");
Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}");
string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?";
Console.WriteLine($"[TF3] {version}");
// 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))
// Inject static transforms: map -> odom -> base_footprint -> base_link
var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1);
var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1);
var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1);
if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) ||
!TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) ||
!TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true))
{
LogError("Failed to inject static TF map -> odom");
NavigationAPI.tf_listener_destroy(tfHandle);
LogError("Failed to set static TF");
TF3API.tf3_buffer_destroy(tf3Buffer);
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
// Create navigation instance and initialize with tf3 buffer
NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
if (navHandle.ptr == IntPtr.Zero)
{
LogError("Failed to create navigation instance");
NavigationAPI.tf_listener_destroy(tfHandle);
TF3API.tf3_buffer_destroy(tf3Buffer);
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))
if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer))
{
LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')");
LogError("Failed to initialize navigation with tf3 buffer");
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
TF3API.tf3_buffer_destroy(tf3Buffer);
return;
}
while(true)
while (true)
{
// Get navigation feedback
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
{
if(feedback.is_ready)
if (feedback.is_ready)
{
Console.WriteLine("Navigation is ready");
break;
@@ -571,22 +197,23 @@ namespace NavigationExample
}
System.Threading.Thread.Sleep(100);
}
Console.WriteLine("[NavigationExample] Navigation initialized with TF successfully");
Console.WriteLine("[NavigationExample] Navigation initialized successfully");
// Set robot footprint
NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
Point[] footprint = new 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 }
new Point { x = 0.3, y = -0.2, z = 0.0 },
new Point { x = 0.3, y = 0.2, z = 0.0 },
new Point { x = -0.3, y = 0.2, z = 0.0 },
new 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;
Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
LaserScan fscanHandle;
fscanHandle.header = fscanHeader;
fscanHandle.angle_min = -1.57f;
fscanHandle.angle_max = 1.57f;
@@ -604,8 +231,8 @@ namespace NavigationExample
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;
Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
LaserScan bscanHandle;
bscanHandle.header = bscanHeader;
bscanHandle.angle_min = 1.57f;
bscanHandle.angle_max = -1.57f;
@@ -623,8 +250,8 @@ namespace NavigationExample
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();
Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
Odometry odometryHandle = new Odometry();
odometryHandle.header = odometryHeader;
IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint");
odometryHandle.child_frame_id = childFrameId;
@@ -695,13 +322,13 @@ namespace NavigationExample
Console.WriteLine("maze.yaml not found, using default map 3x10");
}
NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
Time mapLoadTime = NavigationAPI.time_create();
MapMetaData mapMetaData = new 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 = new Pose();
mapMetaData.origin.position.x = mapConfig.OriginX;
mapMetaData.origin.position.y = mapConfig.OriginY;
mapMetaData.origin.position.z = mapConfig.OriginZ;
@@ -709,7 +336,7 @@ namespace NavigationExample
mapMetaData.origin.orientation.y = 0.0;
mapMetaData.origin.orientation.z = 0.0;
mapMetaData.origin.orientation.w = 1.0;
NavigationAPI.OccupancyGrid occupancyGrid = new NavigationAPI.OccupancyGrid();
OccupancyGrid occupancyGrid = new OccupancyGrid();
IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
occupancyGrid.info = mapMetaData;
@@ -718,26 +345,148 @@ namespace NavigationExample
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"));
Marshal.SizeOf<OccupancyGrid>(),
Marshal.OffsetOf<OccupancyGrid>("data"),
Marshal.OffsetOf<OccupancyGrid>("data_count"));
NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid);
NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
Twist2DStamped twist = new 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);
}
// // Build order (thao cách bom order): header + nodes + edges giống C++
// Order order = new Order();
// order.headerId = 1;
// order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00");
// order.version = Marshal.StringToHGlobalAnsi("1.0.0");
// order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer");
// order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number");
// order.orderId = Marshal.StringToHGlobalAnsi("Order ID");
// order.orderUpdateId = 1;
// // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); }
// int nodeCount = 1;
// order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf<Node>() * nodeCount);
// order.nodes_count = new UIntPtr((uint)nodeCount);
// Node node1 = new Node();
// node1.nodeId = Marshal.StringToHGlobalAnsi("node-1");
// node1.sequenceId = 0;
// node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node");
// node1.released = 0;
// node1.nodePosition.x = 1.0;
// node1.nodePosition.y = 1.0;
// node1.nodePosition.theta = 0.0;
// node1.nodePosition.allowedDeviationXY = 0.1f;
// node1.nodePosition.allowedDeviationTheta = 0.05f;
// node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map");
// node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi("");
// node1.actions = IntPtr.Zero;
// node1.actions_count = UIntPtr.Zero;
// Marshal.StructureToPtr(node1, order.nodes, false);
// // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...)
// order.edges = IntPtr.Zero;
// order.edges_count = UIntPtr.Zero;
// order.zoneSetId = Marshal.StringToHGlobalAnsi("");
// PoseStamped goal = new PoseStamped();
// goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
// goal.pose.position.x = 1.0;
// goal.pose.position.y = 1.0;
// goal.pose.position.z = 0.0;
// goal.pose.orientation.x = 0.0;
// goal.pose.orientation.y = 0.0;
// goal.pose.orientation.z = 0.0;
// goal.pose.orientation.w = 1.0;
// NavigationAPI.navigation_move_to_order(navHandle, order, goal);
NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
// NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
// while (true)
// {
// System.Threading.Thread.Sleep(100);
// NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
// if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
// {
// if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded)
// {
// Console.WriteLine("Navigation is Succeeded");
// break;
// }
// }
// NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
// if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
// {
// int n = (int)(uint)globalData.plan.poses_count;
// int poseSize = Marshal.SizeOf<Pose2DStamped>();
// for (int i = 0; i < n; i++)
// {
// IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize);
// Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
// double p_x = p.pose.x;
// double p_y = p.pose.y;
// double p_theta = p.pose.theta;
// Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
// }
// if(globalData.is_costmap_updated) {
// for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) {
// byte cellValue = Marshal.ReadByte(globalData.costmap.data, i);
// Console.WriteLine("Costmap: {0} {1}", i, cellValue);
// }
// }
// else {
// Console.WriteLine("Global Costmap is not updated");
// }
// }
// NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput();
// if(NavigationAPI.navigation_get_local_data(navHandle, ref localData))
// {
// int n = (int)(uint)localData.plan.poses_count;
// int poseSize = Marshal.SizeOf<Pose2DStamped>();
// for (int i = 0; i < n; i++)
// {
// IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize);
// Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
// double p_x = p.pose.x;
// double p_y = p.pose.y;
// double p_theta = p.pose.theta;
// Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
// }
// if(localData.is_costmap_updated) {
// for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) {
// byte cellValue = Marshal.ReadByte(localData.costmap.data, i);
// Console.WriteLine("Costmap: {0} {1}", i, cellValue);
// }
// }
// else {
// Console.WriteLine("Local Costmap is not updated");
// }
// }
// }
// Cleanup (destroy nav first, then tf3 buffer)
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
TF3API.tf3_buffer_destroy(tf3Buffer);
Console.WriteLine("Press any key to exit...");
Console.ReadKey();
try
{
Console.ReadKey(intercept: true);
}
catch (InvalidOperationException)
{
// Running without a real console (e.g. redirected/automated run).
}
}
}
}

133
examples/NavigationExample/TF3API.cs Normal file
View File

@@ -0,0 +1,133 @@
using System;
using System.Runtime.InteropServices;
namespace NavigationExample
{
// ============================================================================
// TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore)
// ============================================================================
public static class TF3API
{
private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib
public enum TF3_ErrorCode
{
TF3_OK = 0,
TF3_ERROR_LOOKUP = 1,
TF3_ERROR_CONNECTIVITY = 2,
TF3_ERROR_EXTRAPOLATION = 3,
TF3_ERROR_INVALID_ARGUMENT = 4,
TF3_ERROR_TIMEOUT = 5,
TF3_ERROR_UNKNOWN = 99
}
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
public struct TF3_Transform
{
public long timestamp_sec;
public long timestamp_nsec;
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
public string frame_id;
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
public string child_frame_id;
public double translation_x;
public double translation_y;
public double translation_z;
public double rotation_x;
public double rotation_y;
public double rotation_z;
public double rotation_w;
}
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr tf3_buffer_create(int cache_time_sec);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void tf3_buffer_destroy(IntPtr buffer);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool tf3_set_transform(
IntPtr buffer,
ref TF3_Transform transform,
string authority,
[MarshalAs(UnmanagedType.I1)] bool is_static);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool tf3_lookup_transform(
IntPtr buffer,
string target_frame,
string source_frame,
long time_sec,
long time_nsec,
out TF3_Transform transform,
out TF3_ErrorCode error_code);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool tf3_lookup_transform_full(
IntPtr buffer,
string target_frame,
long target_time_sec,
long target_time_nsec,
string source_frame,
long source_time_sec,
long source_time_nsec,
string fixed_frame,
out TF3_Transform transform,
out TF3_ErrorCode error_code);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool tf3_can_transform(
IntPtr buffer,
string target_frame,
string source_frame,
long time_sec,
long time_nsec,
System.Text.StringBuilder error_msg,
int error_msg_len);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void tf3_clear(IntPtr buffer);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void tf3_get_current_time(out long sec, out long nsec);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len);
[DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr tf3_get_version();
/// <summary>Helper: create TF3_Transform for static transform (identity or given pose).</summary>
public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame,
double tx = 0, double ty = 0, double tz = 0,
double qx = 0, double qy = 0, double qz = 0, double qw = 1)
{
var t = new TF3_Transform();
t.timestamp_sec = 0;
t.timestamp_nsec = 0;
t.frame_id = parentFrame ?? "";
t.child_frame_id = childFrame ?? "";
t.translation_x = tx;
t.translation_y = ty;
t.translation_z = tz;
t.rotation_x = qx;
t.rotation_y = qy;
t.rotation_z = qz;
t.rotation_w = qw;
return t;
}
}
}

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

274
examples/NavigationExample/msgs/CommonMsgs.cs Normal file
View File

@@ -0,0 +1,274 @@
using System;
using System.Runtime.InteropServices;
namespace NavigationExample
{
// ============================================================================
// 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 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 Point 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 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 PointCloud
{
public Header header;
public IntPtr points;
public UIntPtr points_count;
public IntPtr channels;
public UIntPtr channels_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct PointCloud2
{
public Header header;
public uint height;
public uint width;
public IntPtr fields;
public UIntPtr fields_count;
[MarshalAs(UnmanagedType.I1)]
public bool is_bigendian;
public uint point_step;
public uint row_step;
public IntPtr data;
public UIntPtr data_count;
[MarshalAs(UnmanagedType.I1)]
public bool is_dense;
}
[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;
}
[StructLayout(LayoutKind.Sequential)]
public struct Point32
{
public float x;
public float y;
public float z;
}
[StructLayout(LayoutKind.Sequential)]
public struct Pose2DStamped
{
public Header header;
public Pose2D pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Polygon
{
public IntPtr points;
public UIntPtr points_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct PolygonStamped
{
public Header header;
public Polygon polygon;
}
[StructLayout(LayoutKind.Sequential)]
public struct OccupancyGridUpdate
{
public Header header;
public int x;
public int y;
public uint width;
public uint height;
public IntPtr data;
public UIntPtr data_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct Path2D
{
public Header header;
public IntPtr poses;
public UIntPtr poses_count;
}
/// <summary>Name (char*) + OccupancyGrid for static map list API.</summary>
[StructLayout(LayoutKind.Sequential)]
public struct NamedOccupancyGrid
{
public IntPtr name;
public OccupancyGrid grid;
}
/// <summary>Name (char*) + LaserScan for laser scan list API.</summary>
[StructLayout(LayoutKind.Sequential)]
public struct NamedLaserScan
{
public IntPtr name;
public LaserScan scan;
}
/// <summary>Name (char*) + PointCloud for point cloud list API.</summary>
[StructLayout(LayoutKind.Sequential)]
public struct NamedPointCloud
{
public IntPtr name;
public PointCloud cloud;
}
/// <summary>Name (char*) + PointCloud2 for point cloud2 list API.</summary>
[StructLayout(LayoutKind.Sequential)]
public struct NamedPointCloud2
{
public IntPtr name;
public PointCloud2 cloud;
}
}

109
examples/NavigationExample/msgs/ProtocolMsgsTypes.cs Normal file
View File

@@ -0,0 +1,109 @@
using System;
using System.Runtime.InteropServices;
namespace NavigationExample
{
/// <summary>
/// C# struct layout cho protocol_msgs (robot_protocol_msgs C API).
/// Khớp với pnkx_nav_core/src/APIs/c_api/include/protocol_msgs/*.h
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct ControlPoint
{
public double x;
public double y;
public double weight;
}
[StructLayout(LayoutKind.Sequential)]
public struct ActionParameter
{
public IntPtr key; // char*
public IntPtr value; // char*
}
[StructLayout(LayoutKind.Sequential)]
public struct NodePosition
{
public double x;
public double y;
public double theta;
public float allowedDeviationXY;
public float allowedDeviationTheta;
public IntPtr mapId; // char*
public IntPtr mapDescription; // char*
}
[StructLayout(LayoutKind.Sequential)]
public struct Trajectory
{
public uint degree;
public IntPtr knotVector; // double*
public UIntPtr knotVector_count;
public IntPtr controlPoints; // ControlPoint*
public UIntPtr controlPoints_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct Action
{
public IntPtr actionType; // char*
public IntPtr actionId; // char*
public IntPtr actionDescription;// char*
public IntPtr blockingType; // char*
public IntPtr actionParameters; // ActionParameter*
public UIntPtr actionParameters_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct Node
{
public IntPtr nodeId; // char*
public int sequenceId;
public IntPtr nodeDescription; // char*
public byte released;
public NodePosition nodePosition;
public IntPtr actions; // Action*
public UIntPtr actions_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct Edge
{
public IntPtr edgeId; // char*
public int sequenceId;
public IntPtr edgeDescription; // char*
public byte released;
public IntPtr startNodeId; // char*
public IntPtr endNodeId; // char*
public double maxSpeed;
public double maxHeight;
public double minHeight;
public double orientation;
public IntPtr orientationType; // char*
public IntPtr direction; // char*
public byte rotationAllowed;
public double maxRotationSpeed;
public Trajectory trajectory;
public double length;
public IntPtr actions; // Action*
public UIntPtr actions_count;
}
[StructLayout(LayoutKind.Sequential)]
public struct Order
{
public int headerId;
public IntPtr timestamp; // char*
public IntPtr version; // char*
public IntPtr manufacturer; // char*
public IntPtr serialNumber; // char*
public IntPtr orderId; // char*
public uint orderUpdateId;
public IntPtr nodes; // Node*
public UIntPtr nodes_count;
public IntPtr edges; // Edge*
public UIntPtr edges_count;
public IntPtr zoneSetId; // char*
}
}

0
examples/NavigationExample/msgs/SensorMsgs.cs Normal file
View File