update c_api
This commit is contained in:
@@ -1,620 +0,0 @@
|
||||
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 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 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 Twist2DStamped
|
||||
{
|
||||
public Header header;
|
||||
public Twist2D velocity;
|
||||
}
|
||||
|
||||
[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;
|
||||
}
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedOccupancyGrid
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr map; // OccupancyGridHandle
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedLaserScan
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr scan; // LaserScanHandle
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedPointCloud
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr cloud; // PointCloudHandle
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedPointCloud2
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr cloud; // PointCloud2Handle
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PlannerDataOutput
|
||||
{
|
||||
public IntPtr plan; // Path2DHandle
|
||||
public IntPtr costmap; // OccupancyGridHandle
|
||||
public IntPtr costmap_update; // OccupancyGridUpdateHandle
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool is_costmap_updated;
|
||||
public IntPtr footprint; // PolygonStampedHandle
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// String Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void nav_c_api_free_string(IntPtr str);
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Complex Message Handle Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_occupancy_grid(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_occupancy_grid_update(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_laser_scan(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_point_cloud(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_point_cloud2(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_odometry(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_path2d(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_polygon_stamped(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_order(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_planner_data(ref PlannerDataOutput data);
|
||||
|
||||
// ============================================================================
|
||||
// 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);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool tf_listener_set_static_transform(
|
||||
IntPtr tf_handle,
|
||||
string parent_frame,
|
||||
string child_frame,
|
||||
double x, double y, double z,
|
||||
double qx, double qy, double qz, double qw);
|
||||
|
||||
// ============================================================================
|
||||
// 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_get_robot_footprint(
|
||||
IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_points(IntPtr points);
|
||||
|
||||
[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)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_move_to_order(
|
||||
IntPtr handle, IntPtr order, 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_dock_to_order(
|
||||
IntPtr handle, IntPtr order, 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_twist(
|
||||
IntPtr handle, ref Twist2DStamped out_twist);
|
||||
|
||||
[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);
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Navigation Data Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_static_map(IntPtr handle, string map_name, IntPtr map);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_static_map(IntPtr handle, string map_name);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_laser_scan(IntPtr 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(IntPtr 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(IntPtr handle, string point_cloud2_name);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_static_maps(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_laser_scans(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_point_clouds(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_data(IntPtr handle);
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Planner Data
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data);
|
||||
|
||||
// ============================================================================
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
// Get global planner data (opaque handles)
|
||||
NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
|
||||
if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
|
||||
{
|
||||
Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})");
|
||||
NavigationAPI.navigation_free_planner_data(ref globalData);
|
||||
}
|
||||
|
||||
// Get all static maps (names + opaque handles)
|
||||
IntPtr mapsPtr;
|
||||
UIntPtr mapsCount;
|
||||
if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount))
|
||||
{
|
||||
ulong count = mapsCount.ToUInt64();
|
||||
Console.WriteLine($"Static maps: {count}");
|
||||
if (mapsPtr != IntPtr.Zero && count > 0)
|
||||
{
|
||||
int itemSize = Marshal.SizeOf<NavigationAPI.NamedOccupancyGrid>();
|
||||
for (ulong i = 0; i < count; i++)
|
||||
{
|
||||
IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize)));
|
||||
var item = Marshal.PtrToStructure<NavigationAPI.NamedOccupancyGrid>(itemPtr);
|
||||
string name = NavigationAPI.MarshalString(item.name);
|
||||
Console.WriteLine($"- {name}");
|
||||
}
|
||||
NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count);
|
||||
}
|
||||
}
|
||||
|
||||
// Cleanup
|
||||
NavigationAPI.navigation_destroy(navHandle);
|
||||
NavigationAPI.tf_listener_destroy(tfHandle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -53,6 +53,18 @@ namespace NavigationExample
|
||||
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
|
||||
{
|
||||
@@ -85,11 +97,27 @@ namespace NavigationExample
|
||||
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 long sec;
|
||||
public uint sec;
|
||||
public uint nsec;
|
||||
public IntPtr frame_id; // char*
|
||||
}
|
||||
@@ -108,384 +136,232 @@ namespace NavigationExample
|
||||
public Twist2D velocity;
|
||||
}
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Vector3
|
||||
public struct NavigationHandle
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
public IntPtr ptr;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NavFeedback
|
||||
public struct TFListenerHandle
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedOccupancyGrid
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr map; // OccupancyGridHandle
|
||||
public IntPtr ptr;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedLaserScan
|
||||
public struct LaserScan
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr scan; // LaserScanHandle
|
||||
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 NamedPointCloud
|
||||
public struct PoseWithCovariance
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr cloud; // PointCloudHandle
|
||||
public Pose pose;
|
||||
public IntPtr covariance;
|
||||
public UIntPtr covariance_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NamedPointCloud2
|
||||
{
|
||||
public IntPtr name; // char*
|
||||
public IntPtr cloud; // PointCloud2Handle
|
||||
public struct TwistWithCovariance {
|
||||
public Twist twist;
|
||||
public IntPtr covariance;
|
||||
public UIntPtr covariance_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PlannerDataOutput
|
||||
public struct Odometry
|
||||
{
|
||||
public IntPtr plan; // Path2DHandle
|
||||
public IntPtr costmap; // OccupancyGridHandle
|
||||
public IntPtr costmap_update; // OccupancyGridUpdateHandle
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool is_costmap_updated;
|
||||
public IntPtr footprint; // PolygonStampedHandle
|
||||
public Header header;
|
||||
public IntPtr child_frame_id;
|
||||
public PoseWithCovariance pose;
|
||||
public TwistWithCovariance twist;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// String Management
|
||||
// ============================================================================
|
||||
[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);
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Complex Message Handle Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_occupancy_grid(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_occupancy_grid_update(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_laser_scan(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_point_cloud(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_point_cloud2(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_odometry(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_path2d(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_polygon_stamped(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_order(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_occupancy_grids(IntPtr maps, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_laser_scans(IntPtr scans, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_point_clouds(IntPtr clouds, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_point_cloud2s(IntPtr clouds, UIntPtr count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_planner_data(ref PlannerDataOutput data);
|
||||
|
||||
// ============================================================================
|
||||
// State Conversion
|
||||
// ============================================================================
|
||||
/// <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);
|
||||
|
||||
// ============================================================================
|
||||
// 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);
|
||||
public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
|
||||
|
||||
// ============================================================================
|
||||
// Navigation Handle Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr navigation_create();
|
||||
/// <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;
|
||||
}
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_destroy(IntPtr handle);
|
||||
/// <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 IntPtr tf_listener_create();
|
||||
public static extern TFListenerHandle tf_listener_create();
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void tf_listener_destroy(IntPtr handle);
|
||||
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(
|
||||
IntPtr tf_handle,
|
||||
TFListenerHandle tf_handle,
|
||||
string parent_frame,
|
||||
string child_frame,
|
||||
double x, double y, double z,
|
||||
double qx, double qy, double qz, double qw);
|
||||
|
||||
// ============================================================================
|
||||
// Navigation Interface Methods
|
||||
// Navigation Handle Management
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
|
||||
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_set_robot_footprint(
|
||||
IntPtr handle, Point[] points, UIntPtr point_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_robot_footprint(
|
||||
IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_points(IntPtr points);
|
||||
|
||||
[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)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_move_to_order(
|
||||
IntPtr handle, IntPtr order, ref PoseStamped goal,
|
||||
double xy_goal_tolerance, double yaw_goal_tolerance);
|
||||
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_dock_to(
|
||||
IntPtr handle, string marker, ref PoseStamped goal,
|
||||
double xy_goal_tolerance, double yaw_goal_tolerance);
|
||||
public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_dock_to_order(
|
||||
IntPtr handle, IntPtr order, ref PoseStamped goal,
|
||||
double xy_goal_tolerance, double yaw_goal_tolerance);
|
||||
public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_move_straight_to(
|
||||
IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
|
||||
public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance, double yaw_goal_tolerance);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_rotate_to(
|
||||
IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
|
||||
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)]
|
||||
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)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_set_twist_linear(
|
||||
IntPtr handle, double linear_x, double linear_y, double linear_z);
|
||||
public static extern bool navigation_move_straight_to(NavigationHandle handle, PoseStamped goal, double xy_goal_tolerance);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_set_twist_angular(
|
||||
IntPtr handle, double angular_x, double angular_y, double angular_z);
|
||||
public static extern bool navigation_rotate_to(NavigationHandle handle, PoseStamped goal, double yaw_goal_tolerance);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_robot_pose_stamped(
|
||||
IntPtr handle, ref PoseStamped out_pose);
|
||||
public static extern bool navigation_pause(NavigationHandle handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_robot_pose_2d(
|
||||
IntPtr handle, ref Pose2D out_pose);
|
||||
public static extern bool navigation_resume(NavigationHandle handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_twist(
|
||||
IntPtr handle, ref Twist2DStamped out_twist);
|
||||
public static extern bool navigation_cancel(NavigationHandle handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_feedback(
|
||||
IntPtr handle, ref NavFeedback out_feedback);
|
||||
public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_feedback(ref NavFeedback feedback);
|
||||
[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_static_map(IntPtr handle, string map_name, IntPtr map);
|
||||
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_laser_scan(IntPtr handle, string laser_scan_name, IntPtr laser_scan);
|
||||
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_add_point_cloud(IntPtr handle, string point_cloud_name, IntPtr point_cloud);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_point_cloud2(IntPtr handle, string point_cloud2_name, IntPtr point_cloud2);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_add_odometry(IntPtr handle, string odometry_name, IntPtr odometry);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_static_map(IntPtr handle, string map_name, out IntPtr out_map);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_laser_scan(IntPtr handle, string laser_scan_name, out IntPtr out_scan);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_point_cloud(IntPtr handle, string point_cloud_name, out IntPtr out_cloud);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_point_cloud2(IntPtr handle, string point_cloud2_name, out IntPtr out_cloud);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_static_maps(IntPtr handle, out IntPtr out_maps, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_laser_scans(IntPtr handle, out IntPtr out_scans, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_point_clouds(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_all_point_cloud2s(IntPtr handle, out IntPtr out_clouds, out UIntPtr out_count);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_static_map(IntPtr handle, string map_name);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_laser_scan(IntPtr 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(IntPtr 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(IntPtr handle, string point_cloud2_name);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_static_maps(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_laser_scans(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_point_clouds(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_point_cloud2s(IntPtr handle);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_remove_all_data(IntPtr handle);
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Planner Data
|
||||
// ============================================================================
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_global_data(IntPtr handle, ref PlannerDataOutput out_data);
|
||||
|
||||
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data);
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Navigation Commands with Order
|
||||
// ============================================================================
|
||||
|
||||
|
||||
|
||||
|
||||
// ============================================================================
|
||||
// Helper Methods for String Conversion
|
||||
// ============================================================================
|
||||
public static string MarshalString(IntPtr ptr)
|
||||
{
|
||||
if (ptr == IntPtr.Zero)
|
||||
return string.Empty;
|
||||
return Marshal.PtrToStringAnsi(ptr);
|
||||
}
|
||||
public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
|
||||
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
@@ -507,8 +383,8 @@ namespace NavigationExample
|
||||
static void Main(string[] args)
|
||||
{
|
||||
// Create TF listener
|
||||
IntPtr tfHandle = NavigationAPI.tf_listener_create();
|
||||
if (tfHandle == IntPtr.Zero)
|
||||
NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
|
||||
if (tfHandle.ptr == IntPtr.Zero)
|
||||
{
|
||||
LogError("Failed to create TF listener");
|
||||
return;
|
||||
@@ -544,8 +420,8 @@ namespace NavigationExample
|
||||
}
|
||||
|
||||
// Create navigation instance
|
||||
IntPtr navHandle = NavigationAPI.navigation_create();
|
||||
if (navHandle == IntPtr.Zero)
|
||||
NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
|
||||
if (navHandle.ptr == IntPtr.Zero)
|
||||
{
|
||||
LogError("Failed to create navigation instance");
|
||||
NavigationAPI.tf_listener_destroy(tfHandle);
|
||||
@@ -571,54 +447,132 @@ namespace NavigationExample
|
||||
};
|
||||
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));
|
||||
IntPtr stateStrPtr = NavigationAPI.navigation_state_to_string(feedback.navigation_state);
|
||||
string stateStr = NavigationAPI.MarshalString(stateStrPtr);
|
||||
NavigationAPI.nav_c_api_free_string(stateStrPtr);
|
||||
string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
|
||||
Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
|
||||
NavigationAPI.navigation_free_feedback(ref feedback);
|
||||
}
|
||||
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
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);
|
||||
|
||||
// Get global planner data (opaque handles)
|
||||
NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
|
||||
if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
|
||||
{
|
||||
Console.WriteLine($"Global data received (costmap_updated={globalData.is_costmap_updated})");
|
||||
NavigationAPI.navigation_free_planner_data(ref globalData);
|
||||
|
||||
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);
|
||||
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
|
||||
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);
|
||||
|
||||
// Get all static maps (names + opaque handles)
|
||||
IntPtr mapsPtr;
|
||||
UIntPtr mapsCount;
|
||||
if (NavigationAPI.navigation_get_all_static_maps(navHandle, out mapsPtr, out mapsCount))
|
||||
{
|
||||
ulong count = mapsCount.ToUInt64();
|
||||
Console.WriteLine($"Static maps: {count}");
|
||||
if (mapsPtr != IntPtr.Zero && count > 0)
|
||||
{
|
||||
int itemSize = Marshal.SizeOf<NavigationAPI.NamedOccupancyGrid>();
|
||||
for (ulong i = 0; i < count; i++)
|
||||
{
|
||||
IntPtr itemPtr = IntPtr.Add(mapsPtr, checked((int)(i * (ulong)itemSize)));
|
||||
var item = Marshal.PtrToStructure<NavigationAPI.NamedOccupancyGrid>(itemPtr);
|
||||
string name = NavigationAPI.MarshalString(item.name);
|
||||
Console.WriteLine($"- {name}");
|
||||
}
|
||||
NavigationAPI.navigation_free_named_occupancy_grids(mapsPtr, (UIntPtr)count);
|
||||
}
|
||||
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);
|
||||
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
// Add static map
|
||||
NavigationAPI.Time mapLoadTime = NavigationAPI.time_create();
|
||||
NavigationAPI.MapMetaData mapMetaData = new NavigationAPI.MapMetaData();
|
||||
mapMetaData.map_load_time = mapLoadTime;
|
||||
mapMetaData.resolution = 0.05f;
|
||||
mapMetaData.width = 3;
|
||||
mapMetaData.height = 10;
|
||||
mapMetaData.origin = new NavigationAPI.Pose();
|
||||
mapMetaData.origin.position.x = 0.0;
|
||||
mapMetaData.origin.position.y = 0.0;
|
||||
mapMetaData.origin.position.z = 0.0;
|
||||
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;
|
||||
byte[] data = new byte[30];
|
||||
for (int i = 0; i < data.Length; i++) {
|
||||
data[i] = 100;
|
||||
}
|
||||
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);
|
||||
|
||||
// Cleanup
|
||||
NavigationAPI.navigation_destroy(navHandle);
|
||||
NavigationAPI.tf_listener_destroy(tfHandle);
|
||||
|
||||
Binary file not shown.
@@ -82,8 +82,6 @@ fi
|
||||
|
||||
# Luôn copy source code mới nhất (cập nhật file nếu đã có)
|
||||
cd "$EXAMPLE_DIR/NavigationExample"
|
||||
echo "Updating Program.cs from CSharpExample.cs..."
|
||||
cp ../CSharpExample.cs Program.cs
|
||||
|
||||
# Bước 3: Copy library
|
||||
echo "Copying library..."
|
||||
|
||||
Reference in New Issue
Block a user