created
This commit is contained in:
13
examples/NavigationExample/NavigationExample.csproj
Normal file
13
examples/NavigationExample/NavigationExample.csproj
Normal file
@@ -0,0 +1,13 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
<PropertyGroup>
|
||||
<OutputType>Exe</OutputType>
|
||||
<TargetFramework>net6.0</TargetFramework>
|
||||
<RuntimeIdentifier>linux-x64</RuntimeIdentifier>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<None Include="libnav_c_api.so">
|
||||
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
|
||||
</None>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
322
examples/NavigationExample/Program.cs
Normal file
322
examples/NavigationExample/Program.cs
Normal file
@@ -0,0 +1,322 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Runtime.CompilerServices;
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 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 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 Header
|
||||
{
|
||||
public uint seq;
|
||||
public long 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 Vector3
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NavFeedback
|
||||
{
|
||||
public NavigationState navigation_state;
|
||||
public IntPtr feed_back_str; // char*
|
||||
public Pose2D current_pose;
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool goal_checked;
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool is_ready;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// String Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void nav_c_api_free_string(IntPtr str);
|
||||
|
||||
// ============================================================================
|
||||
// State Conversion
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
public static extern IntPtr navigation_state_to_string(NavigationState state);
|
||||
|
||||
// ============================================================================
|
||||
// Helper Functions
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_offset_goal_2d(
|
||||
double pose_x, double pose_y, double pose_theta,
|
||||
string frame_id, double offset_distance,
|
||||
ref PoseStamped out_goal);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_offset_goal_stamped(
|
||||
ref PoseStamped in_pose, double offset_distance,
|
||||
ref PoseStamped out_goal);
|
||||
|
||||
// ============================================================================
|
||||
// Navigation Handle Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr navigation_create();
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_destroy(IntPtr handle);
|
||||
|
||||
// ============================================================================
|
||||
// TF Listener Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr tf_listener_create();
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void tf_listener_destroy(IntPtr handle);
|
||||
|
||||
// ============================================================================
|
||||
// Navigation Interface Methods
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_set_robot_footprint(
|
||||
IntPtr handle, Point[] points, UIntPtr point_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_move_to(
|
||||
IntPtr handle, ref 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(
|
||||
IntPtr handle, string marker, ref PoseStamped goal,
|
||||
double xy_goal_tolerance, double yaw_goal_tolerance);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_move_straight_to(
|
||||
IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_rotate_to(
|
||||
IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_pause(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_resume(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_cancel(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_set_twist_linear(
|
||||
IntPtr handle, double linear_x, double linear_y, double linear_z);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_set_twist_angular(
|
||||
IntPtr handle, double angular_x, double angular_y, double angular_z);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_robot_pose_stamped(
|
||||
IntPtr handle, ref PoseStamped out_pose);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_robot_pose_2d(
|
||||
IntPtr handle, ref Pose2D out_pose);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_feedback(
|
||||
IntPtr handle, ref NavFeedback out_feedback);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_feedback(ref NavFeedback feedback);
|
||||
|
||||
// ============================================================================
|
||||
// Helper Methods for String Conversion
|
||||
// ============================================================================
|
||||
public static string MarshalString(IntPtr ptr)
|
||||
{
|
||||
if (ptr == IntPtr.Zero)
|
||||
return string.Empty;
|
||||
return Marshal.PtrToStringAnsi(ptr);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Example Usage
|
||||
// ============================================================================
|
||||
class Program
|
||||
{
|
||||
// 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
|
||||
IntPtr tfHandle = NavigationAPI.tf_listener_create();
|
||||
if (tfHandle == IntPtr.Zero)
|
||||
{
|
||||
LogError("Failed to create TF listener");
|
||||
return;
|
||||
}
|
||||
|
||||
// Create navigation instance
|
||||
IntPtr navHandle = NavigationAPI.navigation_create();
|
||||
if (navHandle == IntPtr.Zero)
|
||||
{
|
||||
LogError("Failed to create navigation instance");
|
||||
NavigationAPI.tf_listener_destroy(tfHandle);
|
||||
return;
|
||||
}
|
||||
|
||||
// Initialize navigation
|
||||
if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
|
||||
{
|
||||
LogError("Failed to initialize navigation");
|
||||
NavigationAPI.navigation_destroy(navHandle);
|
||||
NavigationAPI.tf_listener_destroy(tfHandle);
|
||||
return;
|
||||
}
|
||||
|
||||
// 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));
|
||||
|
||||
// Get robot pose
|
||||
NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
|
||||
if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
|
||||
{
|
||||
Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
|
||||
}
|
||||
|
||||
// Get navigation feedback
|
||||
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
|
||||
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
|
||||
{
|
||||
string stateStr = NavigationAPI.MarshalString(
|
||||
NavigationAPI.navigation_state_to_string(feedback.navigation_state));
|
||||
string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
|
||||
Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
|
||||
NavigationAPI.navigation_free_feedback(ref feedback);
|
||||
}
|
||||
|
||||
// Cleanup
|
||||
NavigationAPI.navigation_destroy(navHandle);
|
||||
NavigationAPI.tf_listener_destroy(tfHandle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BIN
examples/NavigationExample/libnav_c_api.so
Executable file
BIN
examples/NavigationExample/libnav_c_api.so
Executable file
Binary file not shown.
Reference in New Issue
Block a user