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-03 07:24:12 +00:00
parent 06c2d01b4a
commit b690e93650
2 changed files with 44 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
examples/NavigationExample/Program.cs
View File

@@ -359,53 +359,54 @@ namespace NavigationExample
NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
}
// // 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;
// 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);
// 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("");
// 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;
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_dock_to_order(navHandle, order, "docking_point", goal);
// 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);