created

src/APIs/c_api/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.10)
+project(nav_c_api VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# Dependencies
+set(PACKAGES_DIR
+    move_base_core
+    move_base
+    tf3
+    robot_time
+    geometry_msgs
+)
+
+# Thư mục include
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+)
+
+# Tìm tất cả file source
+file(GLOB SOURCES "src/*.cpp")
+file(GLOB HEADERS "include/*.h")
+
+# Tạo thư viện shared (.so)
+add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
+
+# Link libraries
+target_link_libraries(nav_c_api
+    PUBLIC
+        ${PACKAGES_DIR}
+)
+
+# Set include directories
+target_include_directories(nav_c_api
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Cài đặt header files
+install(DIRECTORY include/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+)
+
+# Cài đặt library
+install(TARGETS nav_c_api
+    EXPORT nav_c_api-targets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+# Export targets
+install(EXPORT nav_c_api-targets
+    FILE nav_c_api-targets.cmake
+    DESTINATION lib/cmake/nav_c_api
+)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# Flags biên dịch
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "=================================")
+

src/APIs/c_api/include/nav_c_api.h

@@ -0,0 +1,349 @@
+#ifndef NAVIGATION_C_API_H
+#define NAVIGATION_C_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+// Forward declarations
+typedef void* NavigationHandle;
+typedef void* TFListenerHandle;
+
+// ============================================================================
+// Enums
+// ============================================================================
+
+/**
+ * @brief Navigation states, including planning and controller status
+ */
+typedef enum {
+    NAV_STATE_PENDING = 0,
+    NAV_STATE_ACTIVE = 1,
+    NAV_STATE_PREEMPTED = 2,
+    NAV_STATE_SUCCEEDED = 3,
+    NAV_STATE_ABORTED = 4,
+    NAV_STATE_REJECTED = 5,
+    NAV_STATE_PREEMPTING = 6,
+    NAV_STATE_RECALLING = 7,
+    NAV_STATE_RECALLED = 8,
+    NAV_STATE_LOST = 9,
+    NAV_STATE_PLANNING = 10,
+    NAV_STATE_CONTROLLING = 11,
+    NAV_STATE_CLEARING = 12,
+    NAV_STATE_PAUSED = 13
+} NavigationState;
+
+// ============================================================================
+// Structures
+// ============================================================================
+
+/**
+ * @brief Point structure (x, y, z)
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+} Point;
+
+/**
+ * @brief Pose2D structure (x, y, theta)
+ */
+typedef struct {
+    double x;
+    double y;
+    double theta;
+} Pose2D;
+
+/**
+ * @brief Quaternion structure
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+    double w;
+} Quaternion;
+
+/**
+ * @brief Position structure
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+} Position;
+
+/**
+ * @brief Pose structure
+ */
+typedef struct {
+    Position position;
+    Quaternion orientation;
+} Pose;
+
+/**
+ * @brief Header structure
+ */
+typedef struct {
+    uint32_t seq;
+    int64_t sec;
+    uint32_t nsec;
+    char* frame_id;
+} Header;
+
+/**
+ * @brief PoseStamped structure
+ */
+typedef struct {
+    Header header;
+    Pose pose;
+} PoseStamped;
+
+/**
+ * @brief Vector3 structure
+ */
+typedef struct {
+    double x;
+    double y;
+    double z;
+} Vector3;
+
+/**
+ * @brief Navigation feedback structure
+ */
+typedef struct {
+    NavigationState navigation_state;
+    char* feed_back_str;
+    Pose2D current_pose;
+    bool goal_checked;
+    bool is_ready;
+} NavFeedback;
+
+// ============================================================================
+// String Management
+// ============================================================================
+
+/**
+ * @brief Free a string allocated by the library
+ * @param str String to free
+ */
+void nav_c_api_free_string(char* str);
+
+// ============================================================================
+// State Conversion
+// ============================================================================
+
+/**
+ * @brief Convert a State enum to its string representation
+ * @param state Enum value of NavigationState
+ * @return String representation (caller must free with nav_c_api_free_string)
+ */
+char* navigation_state_to_string(NavigationState state);
+
+// ============================================================================
+// Helper Functions
+// ============================================================================
+
+/**
+ * @brief Creates a target pose by offsetting a given 2D pose along its heading direction
+ * @param pose_x X coordinate of the original pose
+ * @param pose_y Y coordinate of the original pose
+ * @param pose_theta Heading angle in radians
+ * @param frame_id The coordinate frame ID (null-terminated string)
+ * @param offset_distance Distance to offset along heading (positive = forward, negative = backward)
+ * @param out_goal Output parameter for the offset pose
+ * @return true on success, false on failure
+ */
+bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
+                               const char* frame_id, double offset_distance,
+                               PoseStamped* out_goal);
+
+/**
+ * @brief Creates an offset target pose from a given PoseStamped
+ * @param in_pose Input pose
+ * @param offset_distance Distance to offset along heading direction
+ * @param out_goal Output parameter for the offset pose
+ * @return true on success, false on failure
+ */
+bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
+                                     PoseStamped* out_goal);
+
+// ============================================================================
+// Navigation Handle Management
+// ============================================================================
+
+/**
+ * @brief Create a new navigation instance
+ * @return Navigation handle, or NULL on failure
+ */
+NavigationHandle navigation_create(void);
+
+/**
+ * @brief Destroy a navigation instance
+ * @param handle Navigation handle to destroy
+ */
+void navigation_destroy(NavigationHandle handle);
+
+// ============================================================================
+// TF Listener Management
+// ============================================================================
+
+/**
+ * @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);
+
+// ============================================================================
+// Navigation Interface Methods
+// ============================================================================
+
+/**
+ * @brief Initialize the navigation system
+ * @param handle Navigation handle
+ * @param tf_handle TF listener handle
+ * @return true on success, false on failure
+ */
+bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
+
+/**
+ * @brief Set the robot's footprint (outline shape)
+ * @param handle Navigation handle
+ * @param points Array of points representing the footprint polygon
+ * @param point_count Number of points in the array
+ * @return true on success, false on failure
+ */
+bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
+
+/**
+ * @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);
+
+/**
+ * @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);
+
+/**
+ * @brief Move straight toward the target position
+ * @param handle Navigation handle
+ * @param goal Target pose
+ * @param xy_goal_tolerance Acceptable positional error (meters)
+ * @return true if command issued successfully
+ */
+bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
+                                  double xy_goal_tolerance);
+
+/**
+ * @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)
+ * @return true if rotation command was sent successfully
+ */
+bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
+                         double yaw_goal_tolerance);
+
+/**
+ * @brief Pause the robot's movement
+ * @param handle Navigation handle
+ */
+void navigation_pause(NavigationHandle handle);
+
+/**
+ * @brief Resume motion after a pause
+ * @param handle Navigation handle
+ */
+void navigation_resume(NavigationHandle handle);
+
+/**
+ * @brief Cancel the current goal and stop the robot
+ * @param handle Navigation handle
+ */
+void navigation_cancel(NavigationHandle handle);
+
+/**
+ * @brief Send limited linear velocity command
+ * @param handle Navigation handle
+ * @param linear_x Linear velocity in X direction
+ * @param linear_y Linear velocity in Y direction
+ * @param linear_z Linear velocity in Z direction
+ * @return true if the command was accepted
+ */
+bool navigation_set_twist_linear(NavigationHandle handle,
+                                  double linear_x, double linear_y, double linear_z);
+
+/**
+ * @brief Send limited angular velocity command
+ * @param handle Navigation handle
+ * @param angular_x Angular velocity around X axis
+ * @param angular_y Angular velocity around Y axis
+ * @param angular_z Angular velocity around Z axis
+ * @return true if the command was accepted
+ */
+bool navigation_set_twist_angular(NavigationHandle handle,
+                                   double angular_x, double angular_y, double angular_z);
+
+/**
+ * @brief Get the robot's pose as a PoseStamped
+ * @param handle Navigation handle
+ * @param out_pose Output parameter with the robot's current pose
+ * @return true if pose was successfully retrieved
+ */
+bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose);
+
+/**
+ * @brief Get the robot's pose as a 2D pose
+ * @param handle Navigation handle
+ * @param out_pose Output parameter with the robot's current 2D pose
+ * @return true if pose was successfully retrieved
+ */
+bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
+
+/**
+ * @brief Get navigation feedback
+ * @param handle Navigation handle
+ * @param out_feedback Output parameter with navigation feedback
+ * @return true if feedback was successfully retrieved
+ * @note The feed_back_str field must be freed using nav_c_api_free_string
+ */
+bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback);
+
+/**
+ * @brief Free navigation feedback structure
+ * @param feedback Feedback structure to free
+ */
+void navigation_free_feedback(NavFeedback* feedback);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // NAVIGATION_C_API_H
+

src/APIs/c_api/src/nav_c_api.cpp

@@ -0,0 +1,432 @@
+#include "nav_c_api.h"
+#include <move_base_core/navigation.h>
+#include <move_base_core/common.h>
+#include <move_base/move_base.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Vector3.h>
+#include <tf3/buffer_core.h>
+#include <string>
+#include <vector>
+#include <cstring>
+#include <cstdlib>
+#include <boost/dll/import.hpp>
+
+
+// ============================================================================
+// Internal Helper Functions
+// ============================================================================
+
+namespace {
+    // Convert C PoseStamped to C++ PoseStamped
+    geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
+        geometry_msgs::PoseStamped cpp_pose;
+        cpp_pose.header.seq = c_pose->header.seq;
+        cpp_pose.header.stamp.sec = c_pose->header.sec;
+        cpp_pose.header.stamp.nsec = c_pose->header.nsec;
+        if (c_pose->header.frame_id) {
+            cpp_pose.header.frame_id = c_pose->header.frame_id;
+        }
+        cpp_pose.pose.position.x = c_pose->pose.position.x;
+        cpp_pose.pose.position.y = c_pose->pose.position.y;
+        cpp_pose.pose.position.z = c_pose->pose.position.z;
+        cpp_pose.pose.orientation.x = c_pose->pose.orientation.x;
+        cpp_pose.pose.orientation.y = c_pose->pose.orientation.y;
+        cpp_pose.pose.orientation.z = c_pose->pose.orientation.z;
+        cpp_pose.pose.orientation.w = c_pose->pose.orientation.w;
+        return cpp_pose;
+    }
+
+    // Convert C++ PoseStamped to C PoseStamped
+    void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
+        c_pose->header.seq = cpp_pose.header.seq;
+        c_pose->header.sec = cpp_pose.header.stamp.sec;
+        c_pose->header.nsec = cpp_pose.header.stamp.nsec;
+        if (!cpp_pose.header.frame_id.empty()) {
+            c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str());
+        } else {
+            c_pose->header.frame_id = nullptr;
+        }
+        c_pose->pose.position.x = cpp_pose.pose.position.x;
+        c_pose->pose.position.y = cpp_pose.pose.position.y;
+        c_pose->pose.position.z = cpp_pose.pose.position.z;
+        c_pose->pose.orientation.x = cpp_pose.pose.orientation.x;
+        c_pose->pose.orientation.y = cpp_pose.pose.orientation.y;
+        c_pose->pose.orientation.z = cpp_pose.pose.orientation.z;
+        c_pose->pose.orientation.w = cpp_pose.pose.orientation.w;
+    }
+
+    // Convert C++ Pose2D to C Pose2D
+    void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
+        c_pose->x = cpp_pose.x;
+        c_pose->y = cpp_pose.y;
+        c_pose->theta = cpp_pose.theta;
+    }
+
+    // Convert C++ NavFeedback to C NavFeedback
+    void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
+        c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
+        if (!cpp_feedback.feed_back_str.empty()) {
+            c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str());
+        } else {
+            c_feedback->feed_back_str = nullptr;
+        }
+        c_feedback->current_pose.x = cpp_feedback.current_pose.x;
+        c_feedback->current_pose.y = cpp_feedback.current_pose.y;
+        c_feedback->current_pose.theta = cpp_feedback.current_pose.theta;
+        c_feedback->goal_checked = cpp_feedback.goal_checked;
+        c_feedback->is_ready = cpp_feedback.is_ready;
+    }
+}
+
+// ============================================================================
+// String Management
+// ============================================================================
+
+extern "C" void nav_c_api_free_string(char* str) {
+    if (str) {
+        free(str);
+    }
+}
+
+// ============================================================================
+// State Conversion
+// ============================================================================
+
+extern "C" char* navigation_state_to_string(NavigationState state) {
+    move_base_core::State cpp_state = static_cast<move_base_core::State>(static_cast<int>(state));
+    std::string str = move_base_core::toString(cpp_state);
+    return strdup(str.c_str());
+}
+
+// ============================================================================
+// Helper Functions
+// ============================================================================
+
+extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
+                                          const char* frame_id, double offset_distance,
+                                          PoseStamped* out_goal) {
+    if (!out_goal || !frame_id) {
+        return false;
+    }
+
+    geometry_msgs::Pose2D pose;
+    pose.x = pose_x;
+    pose.y = pose_y;
+    pose.theta = pose_theta;
+
+    geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
+    cpp_to_c_pose_stamped(result, out_goal);
+    return true;
+}
+
+extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
+                                                PoseStamped* out_goal) {
+    if (!in_pose || !out_goal) {
+        return false;
+    }
+
+    geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
+    geometry_msgs::PoseStamped result = move_base_core::offset_goal(cpp_pose, offset_distance);
+    cpp_to_c_pose_stamped(result, out_goal);
+    return true;
+}
+
+// ============================================================================
+// Navigation Handle Management
+// ============================================================================
+
+extern "C" NavigationHandle navigation_create(void) {
+    try {
+        // Create a concrete MoveBase instance
+        // Using default constructor - initialization will be done via navigation_initialize()
+        move_base::MoveBase* move_base = new move_base::MoveBase();
+        return static_cast<NavigationHandle>(move_base);
+    } catch (const std::exception& e) {
+        // Log error if possible (in production, use proper logging)
+        return nullptr;
+    } catch (...) {
+        return nullptr;
+    }
+}
+
+extern "C" void navigation_destroy(NavigationHandle handle) {
+    if (handle) {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        delete nav;
+    }
+}
+
+// ============================================================================
+// TF Listener Management
+// ============================================================================
+
+extern "C" TFListenerHandle tf_listener_create(void) {
+    try {
+        auto tf = std::make_shared<tf3::BufferCore>();
+        return new std::shared_ptr<tf3::BufferCore>(tf);
+    } catch (...) {
+        return nullptr;
+    }
+}
+
+extern "C" void tf_listener_destroy(TFListenerHandle handle) {
+    if (handle) {
+        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(handle);
+        delete tf_ptr;
+    }
+}
+
+// ============================================================================
+// Navigation Interface Methods
+// ============================================================================
+
+extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) 
+{
+    printf("[%s:%d] Begin: navigation_initialize\n", __FILE__, __LINE__);
+    if (!handle || !tf_handle) {
+        printf("[%s:%d] Error: Invalid parameters\n", __FILE__, __LINE__);
+        return false;
+    }
+    try {
+        printf("[%s:%d] Initialize navigation\n", __FILE__, __LINE__);
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
+        
+        auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
+            "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so",
+            "MoveBase", 
+            boost::dll::load_mode::append_decorations);
+            
+        move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
+        printf("[%s:%d] Navigation created\n", __FILE__, __LINE__);
+        if (nav_ptr == nullptr) {
+            printf("[%s:%d] Error: Failed to create navigation\n", __FILE__, __LINE__);
+            return false;
+        }
+        nav_ptr->initialize(*tf_ptr);
+    
+        return true;    
+    } catch (const std::exception &e) {
+        printf("[%s:%d] Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
+        return false;
+    } catch (...) {
+        printf("[%s:%d] Error: Failed to initialize navigation\n", __FILE__, __LINE__);
+        return false;
+    }
+}
+
+extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count) {
+    if (!handle || !points || point_count == 0) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        std::vector<geometry_msgs::Point> footprint;
+        footprint.reserve(point_count);
+        for (size_t i = 0; i < point_count; ++i) {
+            geometry_msgs::Point pt;
+            pt.x = points[i].x;
+            pt.y = points[i].y;
+            pt.z = points[i].z;
+            footprint.push_back(pt);
+        }
+        nav->setRobotFootprint(footprint);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
+                                    double xy_goal_tolerance, double yaw_goal_tolerance) {
+    if (!handle || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
+                                     const PoseStamped* goal,
+                                     double xy_goal_tolerance, double yaw_goal_tolerance) {
+    if (!handle || !marker || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
+                                             double xy_goal_tolerance) {
+    if (!handle || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
+                                      double yaw_goal_tolerance) {
+    if (!handle || !goal) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" void navigation_pause(NavigationHandle handle) {
+    if (handle) {
+        try {
+            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            nav->pause();
+        } catch (...) {
+            // Ignore exceptions
+        }
+    }
+}
+
+extern "C" void navigation_resume(NavigationHandle handle) {
+    if (handle) {
+        try {
+            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            nav->resume();
+        } catch (...) {
+            // Ignore exceptions
+        }
+    }
+}
+
+extern "C" void navigation_cancel(NavigationHandle handle) {
+    if (handle) {
+        try {
+            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            nav->cancel();
+        } catch (...) {
+            // Ignore exceptions
+        }
+    }
+}
+
+extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
+                                             double linear_x, double linear_y, double linear_z) {
+    if (!handle) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::Vector3 linear;
+        linear.x = linear_x;
+        linear.y = linear_y;
+        linear.z = linear_z;
+        return nav->setTwistLinear(linear);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
+                                              double angular_x, double angular_y, double angular_z) {
+    if (!handle) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::Vector3 angular;
+        angular.x = angular_x;
+        angular.y = angular_y;
+        angular.z = angular_z;
+        return nav->setTwistAngular(angular);
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) {
+    if (!handle || !out_pose) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::PoseStamped cpp_pose;
+        if (nav->getRobotPose(cpp_pose)) {
+            cpp_to_c_pose_stamped(cpp_pose, out_pose);
+            return true;
+        }
+        return false;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) {
+    if (!handle || !out_pose) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        geometry_msgs::Pose2D cpp_pose;
+        if (nav->getRobotPose(cpp_pose)) {
+            cpp_to_c_pose_2d(cpp_pose, out_pose);
+            return true;
+        }
+        return false;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
+    if (!handle || !out_feedback) {
+        return false;
+    }
+
+    try {
+        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        if (nav->nav_feedback_) {
+            cpp_to_c_nav_feedback(*nav->nav_feedback_, out_feedback);
+            return true;
+        }
+        return false;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" void navigation_free_feedback(NavFeedback* feedback) {
+    if (feedback) {
+        nav_c_api_free_string(feedback->feed_back_str);
+        feedback->feed_back_str = nullptr;
+    }
+}
+