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:
hoangson02
2026-02-26 10:12:04 +00:00
parent 34cabd2083
commit ab3e65de1b
6 changed files with 40 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
examples/NavigationExample/Program.cs
View File

@@ -497,13 +497,14 @@ namespace NavigationExample
static void Main(string[] args)
{
// Create TF listener
// Create TF listener (required for costmap and local planner; must be valid when calling navigation_initialize)
NavigationAPI.TFListenerHandle tfHandle = NavigationAPI.tf_listener_create();
if (tfHandle.ptr == IntPtr.Zero)
{
LogError("Failed to create TF listener");
return;
}
Console.WriteLine($"[NavigationExample] TF listener created, handle = 0x{tfHandle.ptr.ToInt64():X16}");
// Inject a static TF so costmap can immediately canTransform(map <-> base_link).
// If you already publish TF from localization/odometry, you can remove this call.
@@ -543,15 +544,35 @@ namespace NavigationExample
return;
}
// Initialize navigation
// Initialize navigation (passes TF to move_base; navigation keeps its own copy, so tfHandle can be destroyed later)
if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
{
LogError("Failed to initialize navigation");
LogError("Failed to initialize navigation (check native log for 'Invalid TF listener' or 'tf is nullptr')");
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
return;
}
while(true)
{
// Get navigation feedback
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
{
if(feedback.is_ready)
{
Console.WriteLine("Navigation is ready");
break;
}
else
{
Console.WriteLine("Navigation is not ready");
}
}
System.Threading.Thread.Sleep(100);
}
Console.WriteLine("[NavigationExample] Navigation initialized with TF successfully");
// Set robot footprint
NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
{
@@ -562,17 +583,6 @@ namespace NavigationExample
};
NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
// Get navigation feedback
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
{
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);
}
IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
NavigationAPI.Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
@@ -650,8 +660,7 @@ namespace NavigationExample
NavigationAPI.navigation_add_odometry(navHandle, "odometry", odometryHandle);
// Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
string mapYamlPath = args.Length > 0 ? args[0] : Path.Combine(
"/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws/src/Managerments/maps/maze", "maze.yaml");
string mapYamlPath = "maze.yaml";
int mapWidth, mapHeight;
byte[] data;
MazeMapConfig mapConfig;
@@ -715,8 +724,6 @@ namespace NavigationExample
NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid);
System.Threading.Thread.Sleep(500);
NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
{