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

2
config/costmap_common_params.yaml
View File

@@ -5,7 +5,7 @@ rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner
robot_base_frame: base_footprint
robot_base_frame: base_link
transform_tolerance: 1.0
obstacle_range: 3.0
#mark_threshold: 1

2
config/costmap_global_params.yaml
View File

@@ -1,6 +1,6 @@
global_costmap:
library_path: libplugins
robot_base_frame: base_footprint
robot_base_frame: base_link
global_frame: map
update_frequency: 1.0
publish_frequency: 1.0

2
config/costmap_local_params.yaml
View File

@@ -1,7 +1,7 @@
local_costmap:
library_path: libplugins
global_frame: odom
robot_base_frame: base_footprint
robot_base_frame: base_link
update_frequency: 6.0
publish_frequency: 6.0
rolling_window: true

2
config/mprim/ekf.yaml
View File

@@ -62,7 +62,7 @@ publish_acceleration: false
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified
base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified
base_link_frame: $(arg tf_prefix)base_link # Defaults to "base_link" if unspecified
world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry,

2
config/mprim/localization.yaml
View File

@@ -25,7 +25,7 @@ Amcl:
update_min_d: 0.05
update_min_a: 0.05
odom_frame_id: odom
base_frame_id: base_footprint
base_frame_id: base_link
global_frame_id: map
resample_interval: 1
transform_tolerance: 0.2

2
config/mprim/ros_diff_drive_controller.yaml
View File

@@ -25,7 +25,7 @@ wheel_radius_multiplier : 1.0 # default: 1.0
cmd_vel_timeout: 1.0
# frame_ids (same as real MiR platform)
base_frame_id: base_footprint # default: base_link base_footprint
base_frame_id: base_link # default: base_link base_link
odom_frame_id: odom # default: odom
# Velocity and acceleration limits

6
config/pnkx_local_planner_params.yaml
View File

@@ -1,4 +1,4 @@
yaw_goal_tolerance: 0.017
yaw_goal_tolerance: 0.03
xy_goal_tolerance: 0.02
min_approach_linear_velocity: 0.05
@@ -41,8 +41,8 @@ PNKXRotateLocalPlanner:
LimitedAccelGenerator:
library_path: libmkt_plugins_standard_traj_generator
max_vel_x: 1.2
min_vel_x: -1.2
max_vel_x: 0.2
min_vel_x: -0.2
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot

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

29
pnkx_nav_core.sln Normal file
View File

@@ -0,0 +1,29 @@
Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17
VisualStudioVersion = 17.5.2.0
MinimumVisualStudioVersion = 10.0.40219.1
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "examples", "examples", "{B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "NavigationExample", "examples\NavigationExample\NavigationExample.csproj", "{995839D6-1E72-F444-6587-97EF24F93814}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
Release|Any CPU = Release|Any CPU
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.Build.0 = Debug|Any CPU
{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.ActiveCfg = Release|Any CPU
{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.Build.0 = Release|Any CPU
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
GlobalSection(NestedProjects) = preSolution
{995839D6-1E72-F444-6587-97EF24F93814} = {B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}
EndGlobalSection
GlobalSection(ExtensibilityGlobals) = postSolution
SolutionGuid = {1A56EED1-5E6A-48A6-9AEE-F39361B34305}
EndGlobalSection
EndGlobal

0
robotapp.db Normal file
View File

3
src/APIs/c_api/CMakeLists.txt
View File

@@ -21,6 +21,9 @@ set(PACKAGES_DIR
robot_time
robot_cpp
geometry_msgs
angles
data_convert
robot_protocol_msgs
)
# Thư mục include

46
src/APIs/c_api/include/convertor.h
View File

@@ -13,6 +13,10 @@
#include "geometry_msgs/Pose2D.h"
#include "nav_2d_msgs/Twist2D.h"
#include "nav_2d_msgs/Twist2DStamped.h"
#include "nav_2d_msgs/Path2D.h"
#include "map_msgs/OccupancyGridUpdate.h"
#include "geometry_msgs/PolygonStamped.h"
#include "protocol_msgs/Order.h"
// C++
#include <robot_sensor_msgs/LaserScan.h>
@@ -24,6 +28,10 @@
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_protocol_msgs/Order.h>
#include <move_base_core/navigation.h>
#include <move_base_core/common.h>
@@ -49,6 +57,21 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu
*/
void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out);
/**
* @brief Convert C++ Path2D to C Path2D
*/
void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out);
/**
* @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate
*/
void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out);
/**
* @brief Convert C++ PolygonStamped to C PolygonStamped
*/
void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out);
/**
* @brief Convert C++ LaserScan to C LaserScan
* @param cpp C++ LaserScan
@@ -150,4 +173,27 @@ robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped
*/
Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped);
/**
* @brief Convert C Order to C++ Order
* @param order C Order
* @return C++ Order
*/
robot_protocol_msgs::Order convert2CppOrder(const Order& order);
/**
* @brief Convert C++ Order to C Order
* @param cpp_order C++ Order
* @return C Order (caller must call order_free when done to release memory)
*/
Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
/**
* @brief Free dynamic memory held by an Order returned from convert2COrder
* @param order Order to free (pointers and counts are zeroed)
*/
#ifdef __cplusplus
extern "C"
#endif
void order_free(Order* order);
#endif // C_API_CONVERTOR_H

114
src/APIs/c_api/include/nav_c_api.h
View File

@@ -24,22 +24,13 @@ extern "C"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "geometry_msgs/PolygonStamped.h"
#include "protocol_msgs/Order.h"
typedef struct { char *name; OccupancyGrid grid; } NamedOccupancyGrid;
typedef struct { char *name; LaserScan scan; } NamedLaserScan;
typedef struct { char *name; PointCloud cloud; } NamedPointCloud;
typedef struct { char *name; PointCloud2 cloud; } NamedPointCloud2;
typedef struct
{
void *ptr;
} NavigationHandle;
// typedef struct
// {
// void *ptr;
// } TFListenerHandle;
typedef void* NavigationHandle;
typedef void* TFListenerHandle;
typedef enum
@@ -100,53 +91,18 @@ extern "C"
*/
void navigation_destroy(NavigationHandle handle);
/**
* @brief Create a TF listener instance
* @return TF listener handle, or NULL on failure
*/
TFListenerHandle tf_listener_create(void);
/**
* @brief Destroy a TF listener instance
* @param handle TF listener handle to destroy
*/
void tf_listener_destroy(TFListenerHandle handle);
/**
* @brief Inject a static transform into the TF buffer.
*
* This is a convenience for standalone usage where no external TF publisher exists yet.
* It will create/ensure the frames exist and become transformable.
*
* @param tf_handle TF listener handle
* @param parent_frame Parent frame id (e.g. "map")
* @param child_frame Child frame id (e.g. "base_link")
* @param x Translation x (meters)
* @param y Translation y (meters)
* @param z Translation z (meters)
* @param qx Rotation quaternion x
* @param qy Rotation quaternion y
* @param qz Rotation quaternion z
* @param qw Rotation quaternion w
* @return true on success, false on failure
*/
bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
const char *parent_frame,
const char *child_frame,
double x, double y, double z,
double qx, double qy, double qz, double qw);
// ============================================================================
// Navigation Interface Methods
// ============================================================================
/**
* @brief Initialize the navigation system
* @brief Initialize the navigation system using an existing tf3 buffer (from libtf3 tf3_buffer_create).
* Caller retains ownership of the buffer and must call tf3_buffer_destroy when done (after navigation_destroy).
* @param handle Navigation handle
* @param tf_handle TF listener handle
* @param tf3_buffer Pointer to tf3 BufferCore (TF3_BufferCore from libtf3)
* @return true on success, false on failure
*/
bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf3_buffer);
/**
* @brief Set the robot's footprint (outline shape)
@@ -171,51 +127,37 @@ extern "C"
* @brief Send a goal for the robot to navigate to
* @param handle Navigation handle
* @param goal Target pose in the global frame
* @param xy_goal_tolerance Acceptable error in X/Y (meters)
* @param yaw_goal_tolerance Acceptable angular error (radians)
* @return true if goal was accepted and sent successfully
*/
bool navigation_move_to(NavigationHandle handle, const PoseStamped goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
bool navigation_move_to(NavigationHandle handle, const PoseStamped goal);
// /**
// * @brief Send a goal for the robot to navigate to
// * @param handle Navigation handle
// * @param order Order message
// * @param goal Target pose in the global frame
// * @param xy_goal_tolerance Acceptable error in X/Y (meters)
// * @param yaw_goal_tolerance Acceptable angular error (radians)
// * @return true if goal was accepted and sent successfully
// */
// bool navigation_move_to_order(NavigationHandle handle, const OrderHandle order,
// const PoseStamped &goal,
// double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Send a goal for the robot to navigate to
* @param handle Navigation handle
* @param order Order message
* @param goal Target pose in the global frame
* @return true if goal was accepted and sent successfully
* @note If order was obtained from convert2COrder(), call order_free(&order) when done
*/
bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
/**
* @brief Send a docking goal to a predefined marker
* @param handle Navigation handle
* @param marker Marker name or ID (null-terminated string)
* @param goal Target pose for docking
* @param xy_goal_tolerance Acceptable XY error (meters)
* @param yaw_goal_tolerance Acceptable heading error (radians)
* @return true if docking command succeeded
*/
bool navigation_dock_to(NavigationHandle handle, const char *marker,
const PoseStamped goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal);
// /**
// * @brief Send a docking goal to a predefined marker
// * @param handle Navigation handle
// * @param order Order message
// * @param goal Target pose for docking
// * @param xy_goal_tolerance Acceptable XY error (meters)
// * @param yaw_goal_tolerance Acceptable heading error (radians)
// * @return true if docking command succeeded
// */
// bool navigation_dock_to_order(NavigationHandle handle, const OrderHandle order,
// const PoseStamped &goal,
// double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Send a docking goal to a predefined marker
* @param handle Navigation handle
* @param order Order message
* @param goal Target pose for docking
* @return true if docking command succeeded
*/
bool navigation_dock_to_order(NavigationHandle handle, const Order order, const char *marker, const PoseStamped goal);
/**
* @brief Move straight toward the target position
@@ -228,12 +170,10 @@ extern "C"
/**
* @brief Rotate in place to align with target orientation
* @param handle Navigation handle
* @param goal Pose containing desired heading (only Z-axis used)
* @param yaw_goal_tolerance Acceptable angular error (radians)
* @param goal_yaw Desired heading (radians)
* @return true if rotation command was sent successfully
*/
bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal,
double yaw_goal_tolerance);
bool navigation_rotate_to(NavigationHandle handle, const PoseStamped goal);
/**
* @brief Pause the robot's movement

30
src/APIs/c_api/include/protocol_msgs/Action.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef C_API_PROTOCOL_MSGS_ACTION_H
#define C_API_PROTOCOL_MSGS_ACTION_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/ActionParameter.h"
#define PROTOCOL_MSGS_ACTION_BLOCKING_NONE "NONE"
#define PROTOCOL_MSGS_ACTION_BLOCKING_SOFT "SOFT"
#define PROTOCOL_MSGS_ACTION_BLOCKING_HARD "HARD"
typedef struct
{
char *actionType;
char *actionId;
char *actionDescription;
char *blockingType;
ActionParameter *actionParameters;
size_t actionParameters_count;
} Action;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_ACTION_H

21
src/APIs/c_api/include/protocol_msgs/ActionParameter.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H
#define C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
typedef struct
{
char *key;
char *value;
} ActionParameter;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_ACTIONPARAMETER_H

22
src/APIs/c_api/include/protocol_msgs/ControlPoint.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef C_API_PROTOCOL_MSGS_CONTROLPOINT_H
#define C_API_PROTOCOL_MSGS_CONTROLPOINT_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
typedef struct
{
double x;
double y;
double weight;
} ControlPoint;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_CONTROLPOINT_H

42
src/APIs/c_api/include/protocol_msgs/Edge.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef C_API_PROTOCOL_MSGS_EDGE_H
#define C_API_PROTOCOL_MSGS_EDGE_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/Trajectory.h"
#include "protocol_msgs/Action.h"
#define PROTOCOL_MSGS_EDGE_ORIENTATION_GLOBAL "GLOBAL"
#define PROTOCOL_MSGS_EDGE_ORIENTATION_TANGENTIAL "TANGENTIAL"
typedef struct
{
char *edgeId;
int32_t sequenceId;
char *edgeDescription;
uint8_t released;
char *startNodeId;
char *endNodeId;
double maxSpeed;
double maxHeight;
double minHeight;
double orientation;
char *orientationType;
char *direction;
uint8_t rotationAllowed;
double maxRotationSpeed;
Trajectory trajectory;
double length;
Action *actions;
size_t actions_count;
} Edge;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_EDGE_H

28
src/APIs/c_api/include/protocol_msgs/Error.h Normal file
View File

@@ -0,0 +1,28 @@
#ifndef C_API_PROTOCOL_MSGS_ERROR_H
#define C_API_PROTOCOL_MSGS_ERROR_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/ErrorReference.h"
#define PROTOCOL_MSGS_ERROR_LEVEL_WARNING "WARNING"
#define PROTOCOL_MSGS_ERROR_LEVEL_FATAL "FATAL"
typedef struct
{
char *errorType;
ErrorReference *errorReferences;
size_t errorReferences_count;
char *errorDescription;
char *errorLevel;
} Error;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_ERROR_H

21
src/APIs/c_api/include/protocol_msgs/ErrorReference.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef C_API_PROTOCOL_MSGS_ERRORREFERENCE_H
#define C_API_PROTOCOL_MSGS_ERRORREFERENCE_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
typedef struct
{
char *referenceKey;
char *referenceValue;
} ErrorReference;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_ERRORREFERENCE_H

28
src/APIs/c_api/include/protocol_msgs/Info.h Normal file
View File

@@ -0,0 +1,28 @@
#ifndef C_API_PROTOCOL_MSGS_INFO_H
#define C_API_PROTOCOL_MSGS_INFO_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/InfoReference.h"
#define PROTOCOL_MSGS_INFO_LEVEL_DEBUG "DEBUG"
#define PROTOCOL_MSGS_INFO_LEVEL_INFO "INFO"
typedef struct
{
char *infoType;
InfoReference *infoReferences;
size_t infoReferences_count;
char *infoDescription;
char *infoLevel;
} Info;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_INFO_H

21
src/APIs/c_api/include/protocol_msgs/InfoReference.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef C_API_PROTOCOL_MSGS_INFOREFERENCE_H
#define C_API_PROTOCOL_MSGS_INFOREFERENCE_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
typedef struct
{
char *referenceKey;
char *referenceValue;
} InfoReference;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_INFOREFERENCE_H

22
src/APIs/c_api/include/protocol_msgs/Information.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef C_API_PROTOCOL_MSGS_INFORMATION_H
#define C_API_PROTOCOL_MSGS_INFORMATION_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/Info.h"
typedef struct
{
Info *information;
size_t information_count;
} Information;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_INFORMATION_H

28
src/APIs/c_api/include/protocol_msgs/Node.h Normal file
View File

@@ -0,0 +1,28 @@
#ifndef C_API_PROTOCOL_MSGS_NODE_H
#define C_API_PROTOCOL_MSGS_NODE_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/NodePosition.h"
#include "protocol_msgs/Action.h"
typedef struct
{
char *nodeId;
int32_t sequenceId;
char *nodeDescription;
uint8_t released;
NodePosition nodePosition;
Action *actions;
size_t actions_count;
} Node;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_NODE_H

26
src/APIs/c_api/include/protocol_msgs/NodePosition.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef C_API_PROTOCOL_MSGS_NODEPOSITION_H
#define C_API_PROTOCOL_MSGS_NODEPOSITION_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
typedef struct
{
double x;
double y;
double theta;
float allowedDeviationXY;
float allowedDeviationTheta;
char *mapId;
char *mapDescription;
} NodePosition;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_NODEPOSITION_H

33
src/APIs/c_api/include/protocol_msgs/Order.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef C_API_PROTOCOL_MSGS_ORDER_H
#define C_API_PROTOCOL_MSGS_ORDER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/Node.h"
#include "protocol_msgs/Edge.h"
typedef struct
{
int32_t headerId;
char *timestamp;
char *version;
char *manufacturer;
char *serialNumber;
char *orderId;
uint32_t orderUpdateId;
Node *nodes;
size_t nodes_count;
Edge *edges;
size_t edges_count;
char *zoneSetId;
} Order;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_ORDER_H

25
src/APIs/c_api/include/protocol_msgs/Trajectory.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef C_API_PROTOCOL_MSGS_TRAJECTORY_H
#define C_API_PROTOCOL_MSGS_TRAJECTORY_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stddef.h>
#include "protocol_msgs/ControlPoint.h"
typedef struct
{
uint32_t degree;
double *knotVector;
size_t knotVector_count;
ControlPoint *controlPoints;
size_t controlPoints_count;
} Trajectory;
#ifdef __cplusplus
}
#endif
#endif // C_API_PROTOCOL_MSGS_TRAJECTORY_H

435
src/APIs/c_api/src/convertor.cpp
View File

@@ -201,6 +201,70 @@ void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyG
}
}
static void convert2CPose2DStamped(const robot_nav_2d_msgs::Pose2DStamped& cpp, Pose2DStamped& out)
{
out.header = convert2CHeader(cpp.header);
out.pose = convert2CPose2D(cpp.pose);
}
void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out)
{
out.header = convert2CHeader(cpp.header);
out.poses_count = cpp.poses.size();
out.poses = nullptr;
if (out.poses_count > 0)
{
out.poses = static_cast<Pose2DStamped*>(malloc(out.poses_count * sizeof(Pose2DStamped)));
if (out.poses)
{
for (size_t i = 0; i < out.poses_count; i++)
convert2CPose2DStamped(cpp.poses[i], out.poses[i]);
}
}
}
void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out)
{
out.header = convert2CHeader(cpp.header);
out.x = cpp.x;
out.y = cpp.y;
out.width = cpp.width;
out.height = cpp.height;
out.data_count = cpp.data.size();
out.data = nullptr;
if (out.data_count > 0)
{
out.data = static_cast<int8_t*>(malloc(out.data_count * sizeof(int8_t)));
if (out.data)
memcpy(out.data, cpp.data.data(), out.data_count * sizeof(int8_t));
}
}
static void convert2CPolygon(const robot_geometry_msgs::Polygon& cpp, Polygon& out)
{
out.points_count = cpp.points.size();
out.points = nullptr;
if (out.points_count > 0)
{
out.points = static_cast<Point32*>(malloc(out.points_count * sizeof(Point32)));
if (out.points)
{
for (size_t i = 0; i < out.points_count; i++)
{
out.points[i].x = cpp.points[i].x;
out.points[i].y = cpp.points[i].y;
out.points[i].z = cpp.points[i].z;
}
}
}
}
void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out)
{
out.header = convert2CHeader(cpp.header);
convert2CPolygon(cpp.polygon, out.polygon);
}
void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out)
{
out.header.seq = cpp.header.seq;
@@ -361,3 +425,374 @@ robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c)
memcpy(cpp.data.data(), c.data, c.data_count);
return cpp;
}
// --- protocol_msgs helpers (C <-> C++ robot_protocol_msgs) ---
static inline char* dup_cstr(const char* s)
{
if (!s) return nullptr;
const size_t n = std::strlen(s) + 1;
char* p = static_cast<char*>(std::malloc(n));
if (p) std::memcpy(p, s, n);
return p;
}
static robot_protocol_msgs::ActionParameter convert2CppActionParameter(const ActionParameter& c)
{
robot_protocol_msgs::ActionParameter cpp;
cpp.key = c.key ? c.key : "";
cpp.value = c.value ? c.value : "";
return cpp;
}
static ActionParameter convert2CActionParameter(const robot_protocol_msgs::ActionParameter& cpp)
{
ActionParameter c;
c.key = dup_cstr(cpp.key.c_str());
c.value = dup_cstr(cpp.value.c_str());
return c;
}
static robot_protocol_msgs::ControlPoint convert2CppControlPoint(const ControlPoint& c)
{
robot_protocol_msgs::ControlPoint cpp;
cpp.x = c.x;
cpp.y = c.y;
cpp.weight = c.weight;
return cpp;
}
static ControlPoint convert2CControlPoint(const robot_protocol_msgs::ControlPoint& cpp)
{
ControlPoint c;
c.x = cpp.x;
c.y = cpp.y;
c.weight = cpp.weight;
return c;
}
static robot_protocol_msgs::NodePosition convert2CppNodePosition(const NodePosition& c)
{
robot_protocol_msgs::NodePosition cpp;
cpp.x = c.x;
cpp.y = c.y;
cpp.theta = c.theta;
cpp.allowedDeviationXY = c.allowedDeviationXY;
cpp.allowedDeviationTheta = c.allowedDeviationTheta;
cpp.mapId = c.mapId ? c.mapId : "";
cpp.mapDescription = c.mapDescription ? c.mapDescription : "";
return cpp;
}
static NodePosition convert2CNodePosition(const robot_protocol_msgs::NodePosition& cpp)
{
NodePosition c;
c.x = cpp.x;
c.y = cpp.y;
c.theta = cpp.theta;
c.allowedDeviationXY = cpp.allowedDeviationXY;
c.allowedDeviationTheta = cpp.allowedDeviationTheta;
c.mapId = dup_cstr(cpp.mapId.c_str());
c.mapDescription = dup_cstr(cpp.mapDescription.c_str());
return c;
}
static robot_protocol_msgs::Trajectory convert2CppTrajectory(const Trajectory& c)
{
robot_protocol_msgs::Trajectory cpp;
cpp.degree = c.degree;
const size_t kv = c.knotVector ? c.knotVector_count : 0;
cpp.knotVector.resize(kv);
for (size_t i = 0; i < kv; i++)
cpp.knotVector[i] = c.knotVector[i];
const size_t ncp = c.controlPoints ? c.controlPoints_count : 0;
cpp.controlPoints.resize(ncp);
for (size_t i = 0; i < ncp; i++)
cpp.controlPoints[i] = convert2CppControlPoint(c.controlPoints[i]);
return cpp;
}
static Trajectory convert2CTrajectory(const robot_protocol_msgs::Trajectory& cpp)
{
Trajectory c;
c.degree = cpp.degree;
c.knotVector_count = cpp.knotVector.size();
c.knotVector = c.knotVector_count ? static_cast<double*>(std::malloc(c.knotVector_count * sizeof(double))) : nullptr;
if (c.knotVector)
for (size_t i = 0; i < c.knotVector_count; i++)
c.knotVector[i] = cpp.knotVector[i];
c.controlPoints_count = cpp.controlPoints.size();
c.controlPoints = c.controlPoints_count ? static_cast<ControlPoint*>(std::malloc(c.controlPoints_count * sizeof(ControlPoint))) : nullptr;
if (c.controlPoints)
for (size_t i = 0; i < c.controlPoints_count; i++)
c.controlPoints[i] = convert2CControlPoint(cpp.controlPoints[i]);
return c;
}
static robot_protocol_msgs::Action convert2CppAction(const Action& c)
{
robot_protocol_msgs::Action cpp;
cpp.actionType = c.actionType ? c.actionType : "";
cpp.actionId = c.actionId ? c.actionId : "";
cpp.actionDescription = c.actionDescription ? c.actionDescription : "";
cpp.blockingType = c.blockingType ? c.blockingType : "";
const size_t n = c.actionParameters ? c.actionParameters_count : 0;
cpp.actionParameters.resize(n);
for (size_t i = 0; i < n; i++)
cpp.actionParameters[i] = convert2CppActionParameter(c.actionParameters[i]);
return cpp;
}
static Action convert2CAction(const robot_protocol_msgs::Action& cpp)
{
Action c;
c.actionType = dup_cstr(cpp.actionType.c_str());
c.actionId = dup_cstr(cpp.actionId.c_str());
c.actionDescription = dup_cstr(cpp.actionDescription.c_str());
c.blockingType = dup_cstr(cpp.blockingType.c_str());
c.actionParameters_count = cpp.actionParameters.size();
c.actionParameters = c.actionParameters_count ? static_cast<ActionParameter*>(std::malloc(c.actionParameters_count * sizeof(ActionParameter))) : nullptr;
if (c.actionParameters)
for (size_t i = 0; i < c.actionParameters_count; i++)
c.actionParameters[i] = convert2CActionParameter(cpp.actionParameters[i]);
return c;
}
static robot_protocol_msgs::Node convert2CppNode(const Node& c)
{
robot_protocol_msgs::Node cpp;
cpp.nodeId = c.nodeId ? c.nodeId : "";
cpp.sequenceId = c.sequenceId;
cpp.nodeDescription = c.nodeDescription ? c.nodeDescription : "";
cpp.released = c.released;
cpp.nodePosition = convert2CppNodePosition(c.nodePosition);
const size_t n = c.actions ? c.actions_count : 0;
cpp.actions.resize(n);
for (size_t i = 0; i < n; i++)
cpp.actions[i] = convert2CppAction(c.actions[i]);
return cpp;
}
static Node convert2CNode(const robot_protocol_msgs::Node& cpp)
{
Node c;
c.nodeId = dup_cstr(cpp.nodeId.c_str());
c.sequenceId = cpp.sequenceId;
c.nodeDescription = dup_cstr(cpp.nodeDescription.c_str());
c.released = cpp.released;
c.nodePosition = convert2CNodePosition(cpp.nodePosition);
c.actions_count = cpp.actions.size();
c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
if (c.actions)
for (size_t i = 0; i < c.actions_count; i++)
c.actions[i] = convert2CAction(cpp.actions[i]);
return c;
}
static robot_protocol_msgs::Edge convert2CppEdge(const Edge& c)
{
robot_protocol_msgs::Edge cpp;
cpp.edgeId = c.edgeId ? c.edgeId : "";
cpp.sequenceId = c.sequenceId;
cpp.edgeDescription = c.edgeDescription ? c.edgeDescription : "";
cpp.released = c.released;
cpp.startNodeId = c.startNodeId ? c.startNodeId : "";
cpp.endNodeId = c.endNodeId ? c.endNodeId : "";
cpp.maxSpeed = c.maxSpeed;
cpp.maxHeight = c.maxHeight;
cpp.minHeight = c.minHeight;
cpp.orientation = c.orientation;
cpp.orientationType = c.orientationType ? c.orientationType : "";
cpp.direction = c.direction ? c.direction : "";
cpp.rotationAllowed = c.rotationAllowed;
cpp.maxRotationSpeed = c.maxRotationSpeed;
cpp.trajectory = convert2CppTrajectory(c.trajectory);
cpp.length = c.length;
const size_t n = c.actions ? c.actions_count : 0;
cpp.actions.resize(n);
for (size_t i = 0; i < n; i++)
cpp.actions[i] = convert2CppAction(c.actions[i]);
return cpp;
}
static Edge convert2CEdge(const robot_protocol_msgs::Edge& cpp)
{
Edge c;
c.edgeId = dup_cstr(cpp.edgeId.c_str());
c.sequenceId = cpp.sequenceId;
c.edgeDescription = dup_cstr(cpp.edgeDescription.c_str());
c.released = cpp.released;
c.startNodeId = dup_cstr(cpp.startNodeId.c_str());
c.endNodeId = dup_cstr(cpp.endNodeId.c_str());
c.maxSpeed = cpp.maxSpeed;
c.maxHeight = cpp.maxHeight;
c.minHeight = cpp.minHeight;
c.orientation = cpp.orientation;
c.orientationType = dup_cstr(cpp.orientationType.c_str());
c.direction = dup_cstr(cpp.direction.c_str());
c.rotationAllowed = cpp.rotationAllowed;
c.maxRotationSpeed = cpp.maxRotationSpeed;
c.trajectory = convert2CTrajectory(cpp.trajectory);
c.length = cpp.length;
c.actions_count = cpp.actions.size();
c.actions = c.actions_count ? static_cast<Action*>(std::malloc(c.actions_count * sizeof(Action))) : nullptr;
if (c.actions)
for (size_t i = 0; i < c.actions_count; i++)
c.actions[i] = convert2CAction(cpp.actions[i]);
return c;
}
static void free_action_parameter(ActionParameter* p)
{
if (!p) return;
std::free(p->key);
std::free(p->value);
p->key = nullptr;
p->value = nullptr;
}
static void free_action(Action* p)
{
if (!p) return;
std::free(p->actionType);
std::free(p->actionId);
std::free(p->actionDescription);
std::free(p->blockingType);
p->actionType = p->actionId = p->actionDescription = p->blockingType = nullptr;
if (p->actionParameters) {
for (size_t i = 0; i < p->actionParameters_count; i++)
free_action_parameter(&p->actionParameters[i]);
std::free(p->actionParameters);
p->actionParameters = nullptr;
}
p->actionParameters_count = 0;
}
static void free_node_position(NodePosition* p)
{
if (!p) return;
std::free(p->mapId);
std::free(p->mapDescription);
p->mapId = p->mapDescription = nullptr;
}
static void free_trajectory(Trajectory* p)
{
if (!p) return;
std::free(p->knotVector);
p->knotVector = nullptr;
p->knotVector_count = 0;
std::free(p->controlPoints);
p->controlPoints = nullptr;
p->controlPoints_count = 0;
}
static void free_node(Node* p)
{
if (!p) return;
std::free(p->nodeId);
std::free(p->nodeDescription);
p->nodeId = p->nodeDescription = nullptr;
free_node_position(&p->nodePosition);
if (p->actions) {
for (size_t i = 0; i < p->actions_count; i++)
free_action(&p->actions[i]);
std::free(p->actions);
p->actions = nullptr;
}
p->actions_count = 0;
}
static void free_edge(Edge* p)
{
if (!p) return;
std::free(p->edgeId);
std::free(p->edgeDescription);
std::free(p->startNodeId);
std::free(p->endNodeId);
std::free(p->orientationType);
std::free(p->direction);
p->edgeId = p->edgeDescription = p->startNodeId = p->endNodeId = nullptr;
p->orientationType = p->direction = nullptr;
free_trajectory(&p->trajectory);
if (p->actions) {
for (size_t i = 0; i < p->actions_count; i++)
free_action(&p->actions[i]);
std::free(p->actions);
p->actions = nullptr;
}
p->actions_count = 0;
}
robot_protocol_msgs::Order convert2CppOrder(const Order& order)
{
robot_protocol_msgs::Order cpp_order;
cpp_order.headerId = order.headerId;
cpp_order.timestamp = order.timestamp ? order.timestamp : "";
cpp_order.version = order.version ? order.version : "";
cpp_order.manufacturer = order.manufacturer ? order.manufacturer : "";
cpp_order.serialNumber = order.serialNumber ? order.serialNumber : "";
cpp_order.orderId = order.orderId ? order.orderId : "";
cpp_order.orderUpdateId = order.orderUpdateId;
const size_t nn = order.nodes ? order.nodes_count : 0;
cpp_order.nodes.resize(nn);
for (size_t i = 0; i < nn; i++)
cpp_order.nodes[i] = convert2CppNode(order.nodes[i]);
const size_t ne = order.edges ? order.edges_count : 0;
cpp_order.edges.resize(ne);
for (size_t i = 0; i < ne; i++)
cpp_order.edges[i] = convert2CppEdge(order.edges[i]);
cpp_order.zoneSetId = order.zoneSetId ? order.zoneSetId : "";
return cpp_order;
}
Order convert2COrder(const robot_protocol_msgs::Order& cpp_order)
{
Order order;
order.headerId = cpp_order.headerId;
order.timestamp = dup_cstr(cpp_order.timestamp.c_str());
order.version = dup_cstr(cpp_order.version.c_str());
order.manufacturer = dup_cstr(cpp_order.manufacturer.c_str());
order.serialNumber = dup_cstr(cpp_order.serialNumber.c_str());
order.orderId = dup_cstr(cpp_order.orderId.c_str());
order.orderUpdateId = cpp_order.orderUpdateId;
order.nodes_count = cpp_order.nodes.size();
order.nodes = order.nodes_count ? static_cast<Node*>(std::malloc(order.nodes_count * sizeof(Node))) : nullptr;
if (order.nodes)
for (size_t i = 0; i < order.nodes_count; i++)
order.nodes[i] = convert2CNode(cpp_order.nodes[i]);
order.edges_count = cpp_order.edges.size();
order.edges = order.edges_count ? static_cast<Edge*>(std::malloc(order.edges_count * sizeof(Edge))) : nullptr;
if (order.edges)
for (size_t i = 0; i < order.edges_count; i++)
order.edges[i] = convert2CEdge(cpp_order.edges[i]);
order.zoneSetId = dup_cstr(cpp_order.zoneSetId.c_str());
return order;
}
extern "C" void order_free(Order* order)
{
if (!order) return;
std::free(order->timestamp);
std::free(order->version);
std::free(order->manufacturer);
std::free(order->serialNumber);
std::free(order->orderId);
std::free(order->zoneSetId);
order->timestamp = order->version = order->manufacturer = nullptr;
order->serialNumber = order->orderId = order->zoneSetId = nullptr;
if (order->nodes) {
for (size_t i = 0; i < order->nodes_count; i++)
free_node(&order->nodes[i]);
std::free(order->nodes);
order->nodes = nullptr;
}
order->nodes_count = 0;
if (order->edges) {
for (size_t i = 0; i < order->edges_count; i++)
free_edge(&order->edges[i]);
std::free(order->edges);
order->edges = nullptr;
}
order->edges_count = 0;
}

774
src/APIs/c_api/src/nav_c_api.cpp
View File

File diff suppressed because it is too large Load Diff

10
src/Algorithms/Libraries/kalman/src/kalman.cpp
View File

@@ -24,7 +24,15 @@ KalmanFilter::KalmanFilter(
I.setIdentity();
}
KalmanFilter::KalmanFilter() {}
KalmanFilter::KalmanFilter()
: m(0),
n(0),
t0(0.0),
t(0.0),
dt(0.0),
initialized(false)
{
}
void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
x_hat = x0;

4
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h
View File

@@ -15,9 +15,9 @@ namespace mkt_algorithm
class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory
{
public:
GoStraight() {};
GoStraight();
virtual ~GoStraight() {};
virtual ~GoStraight();
/**
* @brief Initialize parameters as needed

9
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@@ -30,9 +30,14 @@ namespace mkt_algorithm
class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
{
public:
PredictiveTrajectory() : initialized_(false), nav_stop_(false),
near_goal_heading_integral_(0.0), near_goal_heading_last_error_(0.0), near_goal_heading_was_active_(false) {};
/**
* @brief Constructor
*/
PredictiveTrajectory();
/**
* @brief Destructor
*/
virtual ~PredictiveTrajectory();
// Standard ScoreAlgorithm Interface

4
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h
View File

@@ -14,9 +14,9 @@ namespace mkt_algorithm
class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory
{
public:
RotateToGoal() {};
RotateToGoal();
virtual ~RotateToGoal() {};
virtual ~RotateToGoal();
/**
* @brief Initialize parameters as needed

7
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@@ -2,6 +2,13 @@
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
mkt_algorithm::diff::GoStraight::GoStraight()
: PredictiveTrajectory()
{
}
mkt_algorithm::diff::GoStraight::~GoStraight() {}
void mkt_algorithm::diff::GoStraight::initialize(
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{

42
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -1,6 +1,48 @@
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
#include <boost/dll/alias.hpp>
mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
: initialized_(false),
nav_stop_(false),
x_direction_(0.0),
y_direction_(0.0),
theta_direction_(0.0),
use_velocity_scaled_lookahead_dist_(false),
lookahead_time_(0.0),
lookahead_dist_(0.0),
min_lookahead_dist_(0.0),
max_lookahead_dist_(0.0),
max_lateral_accel_(0.0),
use_rotate_to_heading_(false),
rotate_to_heading_min_angle_(0.0),
min_path_distance_(0.0),
max_path_distance_(0.0),
use_final_heading_alignment_(false),
final_heading_xy_tolerance_(0.0),
final_heading_angle_tolerance_(0.0),
final_heading_min_velocity_(0.0),
final_heading_kp_angular_(0.0),
final_heading_ki_angular_(0.0),
final_heading_kd_angular_(0.0),
near_goal_heading_integral_(0.0),
near_goal_heading_last_error_(0.0),
near_goal_heading_was_active_(false),
use_regulated_linear_velocity_scaling_(false),
min_approach_linear_velocity_(0.0),
regulated_linear_scaling_min_radius_(0.0),
regulated_linear_scaling_min_speed_(0.0),
use_cost_regulated_linear_velocity_scaling_(false),
inflation_cost_scaling_factor_(0.0),
cost_scaling_dist_(0.0),
cost_scaling_gain_(0.0),
control_duration_(0.0),
kf_(nullptr),
m_(0),
n_(0)
{
}
mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
void mkt_algorithm::diff::PredictiveTrajectory::initialize(

8
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp
View File

@@ -4,6 +4,14 @@
#include <boost/dll/alias.hpp>
#include <angles/angles.h>
mkt_algorithm::diff::RotateToGoal::RotateToGoal()
: PredictiveTrajectory()
{
}
mkt_algorithm::diff::RotateToGoal::~RotateToGoal() {}
void mkt_algorithm::diff::RotateToGoal::initialize(
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{

1
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h
View File

@@ -15,6 +15,7 @@ namespace mkt_plugins
{
public:
KinematicParameters();
virtual ~KinematicParameters();
void initialize(const robot::NodeHandle &nh);
/**

9
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h
View File

@@ -12,6 +12,15 @@ namespace mkt_plugins
class LimitedAccelGenerator : public StandardTrajectoryGenerator
{
public:
/**
* @brief Constructor
*/
LimitedAccelGenerator();
/**
* @brief Destructor
*/
virtual ~LimitedAccelGenerator();
/**
* @brief Initialize the generator with parameters from NodeHandle
* @param nh NodeHandle for loading acceleration_time parameter

10
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h
View File

@@ -17,6 +17,16 @@ namespace mkt_plugins
class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator
{
public:
/**
* @brief Constructor
*/
StandardTrajectoryGenerator();
/**
* @brief Destructor
*/
virtual ~StandardTrajectoryGenerator();
/**
* @brief Initialize the trajectory generator with parameters from NodeHandle
* @param nh NodeHandle for loading configuration parameters

7
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h
View File

@@ -21,7 +21,12 @@ namespace mkt_plugins
* @brief Default constructor
* Initializes all iterators to nullptr
*/
XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
XYThetaIterator();
/**
* @brief Destructor
*/
virtual ~XYThetaIterator();
/**
* @brief Initialize the iterator with parameters and kinematics

7
src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
View File

@@ -7,7 +7,12 @@
#include <angles/angles.h>
#include <cmath>
mkt_plugins::GoalChecker::GoalChecker() : xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25)
mkt_plugins::GoalChecker::GoalChecker()
: nh_(),
line_generator_(nullptr),
yaw_goal_tolerance_(0.25),
xy_goal_tolerance_(0.25),
old_xy_goal_tolerance_(0.0)
{
}

42
src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp
View File

@@ -32,12 +32,42 @@ namespace mkt_plugins
nh.setParam(decel_param, -accel);
}
KinematicParameters::KinematicParameters() : xytheta_direct_(),
min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0),
min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0),
acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0),
decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0),
min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0)
KinematicParameters::KinematicParameters()
: xytheta_direct_(),
min_vel_x_(0.0),
min_vel_y_(0.0),
max_vel_x_(0.0),
max_vel_y_(0.0),
max_vel_theta_(0.0),
min_speed_xy_(0.0),
max_speed_xy_(0.0),
min_speed_theta_(0.0),
acc_lim_x_(0.0),
acc_lim_y_(0.0),
acc_lim_theta_(0.0),
decel_lim_x_(0.0),
decel_lim_y_(0.0),
decel_lim_theta_(0.0),
min_speed_xy_sq_(0.0),
max_speed_xy_sq_(0.0),
original_min_vel_x_(0.0),
original_min_vel_y_(0.0),
original_max_vel_x_(0.0),
original_max_vel_y_(0.0),
original_max_vel_theta_(0.0),
original_min_speed_xy_(0.0),
original_max_speed_xy_(0.0),
original_min_speed_theta_(0.0),
original_acc_lim_x_(0.0),
original_acc_lim_y_(0.0),
original_acc_lim_theta_(0.0),
original_decel_lim_x_(0.0),
original_decel_lim_y_(0.0),
original_decel_lim_theta_(0.0)
{
}
KinematicParameters::~KinematicParameters()
{
}

10
src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp
View File

@@ -8,6 +8,16 @@
namespace mkt_plugins
{
LimitedAccelGenerator::LimitedAccelGenerator()
: StandardTrajectoryGenerator(),
acceleration_time_(0.0)
{
}
LimitedAccelGenerator::~LimitedAccelGenerator()
{
}
void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
{
StandardTrajectoryGenerator::initialize(nh);

9
src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp
View File

@@ -7,8 +7,13 @@
namespace mkt_plugins
{
SimpleGoalChecker::SimpleGoalChecker() :
xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625)
SimpleGoalChecker::SimpleGoalChecker()
: nh_(),
xy_goal_tolerance_(0.25),
yaw_goal_tolerance_(0.25),
stateful_(true),
check_xy_(true),
xy_goal_tolerance_sq_(0.0625)
{
}

18
src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
View File

@@ -13,6 +13,24 @@ using robot_nav_2d_utils::loadParameterWithDeprecation;
namespace mkt_plugins
{
StandardTrajectoryGenerator::StandardTrajectoryGenerator()
: nh_kinematics_(),
kinematics_(nullptr),
velocity_iterator_(nullptr),
sim_time_(0.0),
discretize_by_time_(false),
time_granularity_(0.0),
linear_granularity_(0.0),
angular_granularity_(0.0),
include_last_point_(false)
{
}
StandardTrajectoryGenerator::~StandardTrajectoryGenerator()
{
}
void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh)
{
nh_kinematics_ = nh;

15
src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp
View File

@@ -4,6 +4,21 @@
namespace mkt_plugins
{
XYThetaIterator::XYThetaIterator()
: vx_samples_(0),
vy_samples_(0),
vtheta_samples_(0),
kinematics_(nullptr),
x_it_(nullptr),
y_it_(nullptr),
th_it_(nullptr)
{
}
XYThetaIterator::~XYThetaIterator()
{
}
void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics)
{
kinematics_ = kinematics;

12
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@@ -13,7 +13,17 @@
namespace two_points_planner
{
TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}
TwoPointsPlanner::TwoPointsPlanner()
: initialized_(false),
lethal_obstacle_(0),
inscribed_inflated_obstacle_(0),
circumscribed_cost_(0),
allow_unknown_(false),
costmap_robot_(nullptr),
current_env_width_(0),
current_env_height_(0)
{
}
TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
: initialized_(false), costmap_robot_(NULL)

1
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h
View File

@@ -79,7 +79,6 @@ namespace pnkx_local_planner
*/
robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;
bool is_ready_;
};
} // namespace pnkx_local_planner

5
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@@ -14,7 +14,10 @@
#include <boost/dll/alias.hpp>
pnkx_local_planner::PNKXDockingLocalPlanner::PNKXDockingLocalPlanner()
: start_docking_(false)
: PNKXLocalPlanner(),
start_docking_(false),
original_xy_goal_tolerance_(0.0),
original_yaw_goal_tolerance_(0.0)
{
}

1
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
View File

@@ -15,6 +15,7 @@
#include <boost/dll/alias.hpp>
pnkx_local_planner::PNKXGoStraightLocalPlanner::PNKXGoStraightLocalPlanner()
: PNKXLocalPlanner()
{
}

16
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@@ -17,7 +17,21 @@
#include <robot/robot.h>
pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
: initialized_(false)
: initialized_(false),
traj_generator_(nullptr),
nav_algorithm_(nullptr),
rotate_algorithm_(nullptr),
goal_checker_(nullptr),
tf_(nullptr),
costmap_(nullptr),
costmap_robot_(nullptr),
info_(),
update_costmap_before_planning_(false),
ret_angle_(false),
ret_nav_(false),
yaw_goal_tolerance_(0.0),
xy_goal_tolerance_(0.0),
lock_(false)
{
}

1
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
View File

@@ -14,6 +14,7 @@
#include <boost/dll/alias.hpp>
pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner()
: PNKXLocalPlanner()
{
}

3
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@@ -406,7 +406,8 @@ namespace robot
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const std::string &marker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;

1
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -303,6 +303,7 @@ namespace move_base
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const std::string &marker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;

521
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -33,20 +33,50 @@
#include <robot_nav_2d_utils/conversions.h>
move_base::MoveBase::MoveBase()
: initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false)
: initialized_(false),
tf_(),
as_(NULL),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
recovery_index_(0),
recovery_behavior_enabled_(false),
planner_frequency_(0.0), controller_frequency_(0.0),
inscribed_radius_(0.0), circumscribed_radius_(0.0),
planner_patience_(0.0), controller_patience_(0.0),
max_planning_retries_(0), planning_retries_(0),
conservative_reset_dist_(0.0), clearing_radius_(0.0),
shutdown_costmaps_(false), clearing_rotation_allowed_(false),
make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false),
oscillation_timeout_(0.0), oscillation_distance_(0.0),
state_(PLANNING), recovery_trigger_(PLANNING_R),
runPlanner_(false), planner_thread_(NULL),
setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false), cancel_ctr_(false),
original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0)
{
}
move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
: initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false)
: initialized_(false),
tf_(),
as_(NULL),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
recovery_index_(0),
recovery_behavior_enabled_(false),
planner_frequency_(0.0), controller_frequency_(0.0),
inscribed_radius_(0.0), circumscribed_radius_(0.0),
planner_patience_(0.0), controller_patience_(0.0),
max_planning_retries_(0), planning_retries_(0),
conservative_reset_dist_(0.0), clearing_radius_(0.0),
shutdown_costmaps_(false), clearing_rotation_allowed_(false),
make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false),
oscillation_timeout_(0.0), oscillation_distance_(0.0),
state_(PLANNING), recovery_trigger_(PLANNING_R),
runPlanner_(false), planner_thread_(NULL),
setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false), cancel_ctr_(false),
original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0)
{
initialize(tf);
}
@@ -134,6 +164,11 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
}
tf_ = tf;
if(tf_)
{
std::string all_frames_string = tf_->allFramesAsString();
robot::log_info("[%s:%d]\n INFO: tf_ allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
}
as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true);
setupActionServerCallbacks();
@@ -341,8 +376,6 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
}
initialized_ = true;
setup_ = true;
if(tf_)
robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
robot::log_info("========== End: initialize() - SUCCESS ==========");
}
else
@@ -717,37 +750,37 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
// robot::log_info("addOdometry: %s", odometry_name.c_str());
// robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
// robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
if(tf_)
{
// try
// {
// robot_geometry_msgs::PoseStamped global_pose_stamped;
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
// robot_geometry_msgs::PoseStamped robot_pose;
// tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
// robot_pose.header.frame_id = robot_base_frame_;
// robot_pose.header.stamp = robot::Time();
// robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
// if(tf_)
// {
// try
// {
// robot_geometry_msgs::PoseStamped global_pose_stamped;
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
// robot_geometry_msgs::PoseStamped robot_pose;
// tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
// robot_pose.header.frame_id = robot_base_frame_;
// robot_pose.header.stamp = robot::Time();
// robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
// // Convert robot::Time to tf3::Time
// tf3::Time tf3_current_time = data_convert::convertTime(current_time);
// tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
// // Convert robot::Time to tf3::Time
// tf3::Time tf3_current_time = data_convert::convertTime(current_time);
// tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
// std::string error_msg;
// if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg))
// {
// // Transform is available at current time
// tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time);
// tf3::doTransform(robot_pose, global_pose_stamped, transform);
// robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation));
// }
// }
// catch (const tf3::TransformException &ex)
// {
// robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what());
// }
robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
}
// std::string error_msg;
// if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg))
// {
// // Transform is available at current time
// tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time);
// tf3::doTransform(robot_pose, global_pose_stamped, transform);
// robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation));
// }
// }
// catch (const tf3::TransformException &ex)
// {
// robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what());
// }
// // robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
// }
// robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation));
// std::stringstream pose_covariance_str;
// for(int i = 0; i < 36; i++) {
@@ -770,7 +803,8 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
odometry_ = odometry;
}
bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
if (setup_)
{
@@ -816,12 +850,25 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
// Check pointers
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::moveTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
@@ -829,7 +876,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
// Swap planner
try
{
if(!tc_->swapPlanner(position_planner_name_))
if (!tc_->swapPlanner(position_planner_name_))
{
robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
return false;
@@ -837,8 +884,9 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
}
catch (const std::exception &e)
{
std::cerr << e.what() << "\n";
throw std::exception(e);
robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
// Update navigation feedback
@@ -935,20 +983,33 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));
robot::Duration(0.01).sleep();
robot::log_info("[MoveBase] In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::moveTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
if(!tc_->swapPlanner(position_planner_name_))
if (!tc_->swapPlanner(position_planner_name_))
{
robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
return false;
@@ -967,49 +1028,76 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::moveTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread
if (!as_)
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
}
if (!planner_order_)
{
robot::log_error("[MoveBase::moveTo] Failed to store Order message for planning");
lock.unlock();
return false;
}
as_->processGoal(action_goal);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what());
lock.unlock();
return false;
}
as_->processGoal(action_goal);
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
std::string maker_sources;
// private_nh_.param("maker_sources", maker_sources, std::string(""));
private_nh_.param("maker_sources", maker_sources, std::string(""));
std::stringstream ss(maker_sources);
std::string source;
bool has_maker = false;
while (ss >> source)
{
if (maker == source)
if (marker == source)
{
private_nh_.setParam("maker_name", maker);
private_nh_.setParam("maker_name", marker);
has_maker = true;
}
}
@@ -1019,7 +1107,120 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
std::stringstream fb_ss;
fb_ss << "The system has not been '" << maker << "'";
fb_ss << "The system has not been '" << marker << "'";
nav_feedback_->feed_back_str = fb_ss.str();
nav_feedback_->goal_checked = false;
}
return false;
}
if (setup_)
{
swapPlanner(default_config_.base_global_planner);
}
else
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
nav_feedback_->goal_checked = false;
}
return false;
}
if (fabs(xy_goal_tolerance) > 0.001)
this->setXyGoalTolerance(fabs(xy_goal_tolerance));
else
this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
if (fabs(yaw_goal_tolerance) > 0.001)
this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
else
this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));
if (!tc_)
{
robot::log_error("[MoveBase::dockTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
robot::log_error("[MoveBase::dockTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
if (!tc_->swapPlanner(docking_planner_name_))
{
robot::log_error("[MoveBase::dockTo] Failed to swapPlanner");
return false;
}
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::dockTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
const std::string &marker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
std::string maker_sources;
private_nh_.param("maker_sources", maker_sources, std::string(""));
std::stringstream ss(maker_sources);
std::string source;
bool has_maker = false;
while (ss >> source)
{
if (marker == source)
{
private_nh_.setParam("maker_name", marker);
has_maker = true;
}
}
if (!has_maker)
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
std::stringstream fb_ss;
fb_ss << "The system has not been '" << marker << "'";
nav_feedback_->feed_back_str = fb_ss.str();
nav_feedback_->goal_checked = false;
}
@@ -1084,17 +1285,53 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
if (cancel_ctr_)
cancel_ctr_ = false;
if (!as_)
{
robot::log_error("[MoveBase::dockTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_dock_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::dockTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread (same as moveTo with Order)
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::dockTo] Stored Order message for planning");
}
if (!planner_order_)
{
robot::log_error("[MoveBase::dockTo] Failed to store Order message for planning");
lock.unlock();
return false;
}
as_->processGoal(action_goal);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::dockTo] Exception during processGoal: %s", e.what());
lock.unlock();
return false;
}
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
return false;
}
bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
robot::log_info("[MoveBase::moveStraightTo] Entry");
@@ -1117,13 +1354,28 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
this->setXyGoalTolerance(fabs(xy_goal_tolerance));
else
this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::moveStraightTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
@@ -1131,17 +1383,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
try
{
robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner");
if(!tc_->swapPlanner(go_straight_planner_name_))
if (!tc_->swapPlanner(go_straight_planner_name_))
{
if(tf_ == nullptr)
{
robot::log_error("[MoveBase::moveTo] tf_ pointer is null!");
throw std::runtime_error("Null 'tf_' pointer encountered");
}
else
{
robot::log_info("[MoveBase::moveTo] tf_ pointer is not null!");
}
robot::log_error("[MoveBase::moveStraightTo] Failed to swapPlanner");
lock.unlock();
return false;
}
}
@@ -1158,13 +1403,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::moveStraightTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
// Check if action server exists
if (!as_)
{
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
robot::log_error("[MoveBase::moveStraightTo] as_ pointer is null!");
lock.unlock();
return false;
}
@@ -1174,31 +1424,24 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
// Clear Order message since this is a non-Order moveStraightTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
if(controller_costmap_robot_ == nullptr)
{
robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ is null!");
return false;
}
robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
}
@@ -1240,19 +1483,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::rotateTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::rotateTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
if(!tc_->swapPlanner(rotate_planner_name_))
if (!tc_->swapPlanner(rotate_planner_name_))
{
robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner");
lock.unlock();
return false;
}
}
@@ -1269,29 +1527,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::rotateTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
// Check if action server exists
if (!as_)
{
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
robot::log_error("[MoveBase::rotateTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_geometry_msgs::Pose2D pose;
if (!this->getRobotPose(pose))
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
nav_feedback_->feed_back_str = std::string("Couldn't get robot pose");
nav_feedback_->goal_checked = false;
}
lock.unlock();
return false;
}
@@ -1302,23 +1565,22 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta);
action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta);
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
// Clear Order message since this is a non-Order rotateTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::rotateTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server");
@@ -1326,7 +1588,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
catch (const std::exception &e)
{
lock.unlock();
robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
robot::log_error("[MoveBase::rotateTo] Exception during processGoal: %s", e.what());
return false;
}
@@ -1459,7 +1721,6 @@ bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &lin
{
if (tc_->islock())
return true;
return tc_->setTwistLinear(linear);
}
else if (tc_ && cancel_ctr_)
@@ -1878,7 +2139,7 @@ void move_base::MoveBase::planThread()
while (wait_for_wake || !runPlanner_)
{
// if we should not be running the planner then suspend this thread
std::cout << "Planner thread is suspending" << std::endl;
robot::log_info("Planner thread is suspending");
planner_cond_.wait(lock);
wait_for_wake = false;
}
@@ -1888,7 +2149,7 @@ void move_base::MoveBase::planThread()
robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
boost::shared_ptr<robot_protocol_msgs::Order> temp_order = planner_order_;
lock.unlock();
std::cout << "Planning..." << std::endl;
robot::log_info("Planning...");
// run planner
planner_plan_->clear();
// ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
@@ -1910,7 +2171,7 @@ void move_base::MoveBase::planThread()
if (gotPlan)
{
std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl;
robot::log_info("Got Plan with %d points!", planner_plan_->size());
// pointer swap the plans under mutex (the controller will pull from latest_plan_)
std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
@@ -1921,7 +2182,7 @@ void move_base::MoveBase::planThread()
planning_retries_ = 0;
new_global_plan_ = true;
std::cout << "Generated a plan from the base_global_planner" << std::endl;
robot::log_info("Generated a plan from the base_global_planner");
// make sure we only start the controller if we still haven't reached the goal
if (runPlanner_)
@@ -1937,7 +2198,7 @@ void move_base::MoveBase::planThread()
// if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if (state_ == move_base::PLANNING)
{
std::cout << "No Plan..." << std::endl;
robot::log_info("No Plan...");
robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);
// check if we've tried to make a plan for over our time limit or our maximum number of retries