update

CMakeLists.txt

@@ -141,14 +141,14 @@ endif()
 
 # 2. Main packages (phụ thuộc vào cores)
 message(STATUS "[move_base] Shared library configured")
-# if (NOT TARGET move_base)
+if (NOT TARGET move_base)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
-# endif()
+endif()
 
 # C API for .NET/C# integration
-# if (NOT TARGET navigation_c_api)
+if (NOT TARGET navigation_c_api)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
-# endif()
+endif()
 
 message(STATUS "========================================")
 message(STATUS "All packages configured successfully")

src/APIs/c_api/CMakeLists.txt

@@ -10,12 +10,16 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# Find Boost
+find_package(Boost REQUIRED)
+
 # Dependencies
 set(PACKAGES_DIR
     move_base_core
     move_base
     tf3
     robot_time
+    robot_cpp
     geometry_msgs
 )
 
@@ -35,6 +39,9 @@ add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
 target_link_libraries(nav_c_api
     PUBLIC
         ${PACKAGES_DIR}
+    PRIVATE
+        Boost::boost
+        dl
 )
 
 set_target_properties(nav_c_api PROPERTIES

src/APIs/c_api/src/nav_c_api.cpp

@@ -66,7 +66,7 @@ namespace {
     }
 
     // Convert C++ NavFeedback to C NavFeedback
-    void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
+    void cpp_to_c_nav_feedback(const robot::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());
@@ -96,8 +96,8 @@ extern "C" void nav_c_api_free_string(char* str) {
 // ============================================================================
 
 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));
+    robot::move_base_core::State cpp_state = static_cast<robot::move_base_core::State>(static_cast<int>(state));
-    std::string str = move_base_core::toString(cpp_state);
+    std::string str = robot::move_base_core::toString(cpp_state);
     return strdup(str.c_str());
 }
 
@@ -117,7 +117,7 @@ extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double p
     pose.y = pose_y;
     pose.theta = pose_theta;
 
-    geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
+    geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
     cpp_to_c_pose_stamped(result, out_goal);
     return true;
 }
@@ -129,7 +129,7 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, doubl
     }
 
     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);
+    geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
     cpp_to_c_pose_stamped(result, out_goal);
     return true;
 }
@@ -154,7 +154,7 @@ extern "C" NavigationHandle navigation_create(void) {
 
 extern "C" void navigation_destroy(NavigationHandle handle) {
     if (handle) {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         delete nav;
     }
 }
@@ -226,16 +226,16 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
     }
     try {
         printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
         robot::PluginLoaderHelper loader;
         std::string lib_path = loader.findLibraryPath("MoveBase");
-        auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
+        auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
             lib_path,
             "MoveBase", 
             boost::dll::load_mode::append_decorations);
             
-        move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
+        robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
         printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
         if (nav_ptr == nullptr) {
             printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
@@ -259,7 +259,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         std::vector<geometry_msgs::Point> footprint;
         footprint.reserve(point_count);
         for (size_t i = 0; i < point_count; ++i) {
@@ -283,7 +283,7 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* g
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 (...) {
@@ -299,7 +299,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 (...) {
@@ -314,7 +314,7 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 (...) {
@@ -329,7 +329,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 (...) {
@@ -340,7 +340,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
 extern "C" void navigation_pause(NavigationHandle handle) {
     if (handle) {
         try {
-            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
             nav->pause();
         } catch (...) {
             // Ignore exceptions
@@ -351,7 +351,7 @@ extern "C" void navigation_pause(NavigationHandle handle) {
 extern "C" void navigation_resume(NavigationHandle handle) {
     if (handle) {
         try {
-            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
             nav->resume();
         } catch (...) {
             // Ignore exceptions
@@ -362,7 +362,7 @@ extern "C" void navigation_resume(NavigationHandle handle) {
 extern "C" void navigation_cancel(NavigationHandle handle) {
     if (handle) {
         try {
-            move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+            robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
             nav->cancel();
         } catch (...) {
             // Ignore exceptions
@@ -377,7 +377,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         geometry_msgs::Vector3 linear;
         linear.x = linear_x;
         linear.y = linear_y;
@@ -395,7 +395,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         geometry_msgs::Vector3 angular;
         angular.x = angular_x;
         angular.y = angular_y;
@@ -412,7 +412,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         geometry_msgs::PoseStamped cpp_pose;
         if (nav->getRobotPose(cpp_pose)) {
             cpp_to_c_pose_stamped(cpp_pose, out_pose);
@@ -430,7 +430,7 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         geometry_msgs::Pose2D cpp_pose;
         if (nav->getRobotPose(cpp_pose)) {
             cpp_to_c_pose_2d(cpp_pose, out_pose);
@@ -448,8 +448,8 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou
     }
 
     try {
-        move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        move_base_core::NavFeedback cpp_feedback;
+        robot::move_base_core::NavFeedback cpp_feedback;
         if (nav->getFeedback(cpp_feedback)) {
             cpp_to_c_nav_feedback(cpp_feedback, out_feedback);
             return true;

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd
+Subproject commit a28f05c6d5cb6cf2d8c4ffb52a637ecfa2878db2

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h

@@ -39,7 +39,6 @@
 #include <nav_core2/bounds.h>
 #include <vector>
 
-using namespace robot;
 /**
  * @brief A set of utility functions for Bounds objects interacting with other messages/types
  */

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h

@@ -98,8 +98,8 @@ namespace robot_nav_2d_utils
   nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
 
   // Bounds Transformations
-  ::robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds);
+  robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds);
-  robot::nav_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg);
+  nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
 
 } // namespace robot_nav_2d_utils
 

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h

@@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
    * @param frame_id Frame to transform the pose into
    * @return The resulting transformed pose
    */
-  ::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
+  geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
                                              const ::std::string &frame_id);
 
 } // namespace robot_nav_2d_utils

src/Libraries/robot_nav_2d_utils/src/bounds.cpp

@@ -38,7 +38,6 @@
 #include <stdexcept>
 #include <vector>
 
-using namespace robot;
 namespace robot_nav_2d_utils
 {
   nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)

src/Libraries/robot_nav_2d_utils/src/conversions.cpp

@@ -318,7 +318,7 @@ namespace robot_nav_2d_utils
     return metadata;
   }
 
-  robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds)
+  robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds)
   {
     robot_nav_2d_msgs::UIntBounds msg;
     msg.min_x = bounds.getMinX();
@@ -328,9 +328,9 @@ namespace robot_nav_2d_utils
     return msg;
   }
 
-  robot::nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
+  nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
   {
-    return robot::nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
+    return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
   }
 
 } // namespace robot_nav_2d_utils

src/Navigations/Cores/move_base_core/include/move_base_core/common.h

@@ -11,6 +11,8 @@
 #include <tf3/buffer_core.h>
 #include <memory>
 
-using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
+namespace robot
-
+{
-#endif  // NAV_CORE2_COMMON_H
+  using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
+} // namespace robot
+#endif // NAV_CORE2_COMMON_H

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -8,6 +8,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Pose2D.h>
 #include <geometry_msgs/Vector3.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
 
 // robot protocol msgs
 #include <robot_protocol_msgs/Order.h>
@@ -19,6 +20,9 @@
 #include <robot/time.h>
 #include <move_base_core/common.h>
 
+
+namespace robot
+{
 namespace move_base_core
 {
 	// Navigation states, including planning and controller status
@@ -299,13 +303,28 @@ namespace move_base_core
 		 */
 		virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
 
-		// Shared feedback data for navigation status tracking
+		/**
-		std::shared_ptr<NavFeedback> nav_feedback_;
+		 * @brief Get the robot’s twist.
+		 * @param twist Output parameter with the robot’s current twist.
+		 * @return True if twist was successfully retrieved.
+		 */
+		virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) = 0;
+
+		/**
+		 * @brief Get the navigation feedback.
+		 * @param feedback Output parameter with the navigation feedback.
+		 * @return True if feedback was successfully retrieved.
+		 */
+		virtual bool getFeedback(NavFeedback &feedback) = 0;
 
 	protected:
+		// Shared feedback data for navigation status tracking
+		std::shared_ptr<NavFeedback> nav_feedback_;
+		std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
 		BaseNavigation() = default;
 	};
 
 } // namespace move_base_core
+};
 
 #endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

src/Navigations/Cores/nav_core2/CMakeLists.txt

@@ -27,7 +27,7 @@ target_link_libraries(nav_core2 INTERFACE
         costmap_2d 
         tf3 
         nav_grid 
-        nav_2d_msgs
+        robot_nav_2d_msgs
         robot_cpp
 )
 

src/Navigations/Cores/nav_core2/README.md

@@ -2,7 +2,7 @@
 A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces.
 
 There were a few key reasons for creating new interfaces rather than extending the old ones.
- * Use `nav_2d_msgs` to eliminate unused data fields
+ * Use `robot_nav_2d_msgs` to eliminate unused data fields
  * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
  * Provide more data in the interfaces for easier testing.
  * Use Exceptions rather than booleans to provide more information about the types of errors encountered.
@@ -30,7 +30,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
 |`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
-|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
+|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
 
 ## Local Planner
 Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
@@ -38,10 +38,10 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
 |`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
-|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
+|(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
-|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
+|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
-|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
+|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
-|`bool isGoalReached()` | `bool isGoalReached(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
+|`bool isGoalReached()` | `bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
 
 ## Exceptions
 A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.

src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h

@@ -34,7 +34,7 @@
 #ifndef NAV_CORE2_EXCEPTIONS_H
 #define NAV_CORE2_EXCEPTIONS_H
 
-#include <nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <stdexcept>
 #include <exception>
 #include <string>
@@ -69,7 +69,7 @@
 namespace nav_core2
 {
 
-inline std::string poseToString(const nav_2d_msgs::Pose2DStamped& pose)
+inline std::string poseToString(const robot_nav_2d_msgs::Pose2DStamped& pose)
 {
   return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta)
          + " : " + pose.header.frame_id + ")";
@@ -199,7 +199,7 @@ class InvalidStartPoseException: public GlobalPlannerException
 public:
   explicit InvalidStartPoseException(const std::string& description, int result_code = 5)
     : GlobalPlannerException(description, result_code) {}
-  InvalidStartPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
+  InvalidStartPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
     InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {}
 };
 
@@ -212,7 +212,7 @@ class StartBoundsException: public InvalidStartPoseException
 public:
   explicit StartBoundsException(const std::string& description, int result_code = 6)
     : InvalidStartPoseException(description, result_code) {}
-  explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit StartBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidStartPoseException(pose, "out of bounds", 6) {}
 };
 
@@ -225,7 +225,7 @@ class OccupiedStartException: public InvalidStartPoseException
 public:
   explicit OccupiedStartException(const std::string& description, int result_code = 7)
     : InvalidStartPoseException(description, result_code) {}
-  explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit OccupiedStartException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidStartPoseException(pose, "occupied", 7) {}
 };
 
@@ -238,7 +238,7 @@ class InvalidGoalPoseException: public GlobalPlannerException
 public:
   explicit InvalidGoalPoseException(const std::string& description, int result_code = 8)
     : GlobalPlannerException(description, result_code) {}
-  InvalidGoalPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
+  InvalidGoalPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
     GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {}
 };
 
@@ -251,7 +251,7 @@ class GoalBoundsException: public InvalidGoalPoseException
 public:
   explicit GoalBoundsException(const std::string& description, int result_code = 9)
     : InvalidGoalPoseException(description, result_code) {}
-  explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit GoalBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidGoalPoseException(pose, "out of bounds", 9) {}
 };
 
@@ -264,7 +264,7 @@ class OccupiedGoalException: public InvalidGoalPoseException
 public:
   explicit OccupiedGoalException(const std::string& description, int result_code = 10)
     : InvalidGoalPoseException(description, result_code) {}
-  explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) :
+  explicit OccupiedGoalException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
     InvalidGoalPoseException(pose, "occupied", 10) {}
 };
 

src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h

@@ -36,8 +36,8 @@
 
 #include <nav_core2/common.h>
 #include <nav_core2/costmap.h>
-#include <nav_2d_msgs/Path2D.h>
+#include <robot_nav_2d_msgs/Path2D.h>
-#include <nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <string>
 #include <memory>
 
@@ -77,8 +77,8 @@ public:
    * @param goal  The goal pose of the robot
    * @return      The sequence of poses to get from start to goal, if any
    */
-  virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
+  virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
-                                       const nav_2d_msgs::Pose2DStamped& goal) = 0;
+                                       const robot_nav_2d_msgs::Pose2DStamped& goal) = 0;
 };
 }  // namespace nav_core2
 

src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h

@@ -36,10 +36,10 @@
 
 #include <nav_core2/common.h>
 #include <nav_core2/costmap.h>
-#include <nav_2d_msgs/Path2D.h>
+#include <robot_nav_2d_msgs/Path2D.h>
-#include <nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
-#include <nav_2d_msgs/Twist2D.h>
+#include <robot_nav_2d_msgs/Twist2D.h>
-#include <nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <string>
 #include <costmap_2d/costmap_2d_robot.h>
@@ -83,14 +83,14 @@ namespace nav_core2
      * @brief Sets the global goal for this local planner.
      * @param goal_pose The Goal Pose
      */
-    virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
+    virtual void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
 
     /**
      * @brief Sets the global plan for this local planner.
      *
      * @param path The global plan
      */
-    virtual void setPlan(const nav_2d_msgs::Path2D &path) = 0;
+    virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
 
     /**
      * @brief Compute the best command given the current pose, velocity and goal
@@ -102,8 +102,8 @@ namespace nav_core2
      * @param velocity  Current robot velocity
      * @return          The best computed velocity
      */
-    virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
+    virtual robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
-                                                                const nav_2d_msgs::Twist2D &velocity) = 0;
+                                                                const robot_nav_2d_msgs::Twist2D &velocity) = 0;
 
     /**
      * @brief set velocity for x, y asix of the robot
@@ -166,7 +166,7 @@ namespace nav_core2
      * @param velocity   Velocity to check
      * @return           True if the goal conditions have been met
      */
-    virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
+    virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0;
   };
 } // namespace nav_core2
 

src/Navigations/Cores/nav_core_adapter/CMakeLists.txt

@@ -11,7 +11,7 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 nav_2d_utils pthread)
+set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 robot_nav_2d_utils pthread)
 
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h

@@ -57,8 +57,8 @@ public:
   // Nav Core 2 Global Planner Interface
   void initialize(const robot::NodeHandle& parent, const std::string& name,
                   TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
-  nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
+  robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
-                               const nav_2d_msgs::Pose2DStamped& goal) override;
+                               const robot_nav_2d_msgs::Pose2DStamped& goal) override;
 
   static nav_core2::GlobalPlanner::Ptr create();
 

src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h

@@ -39,7 +39,7 @@
 #include <nav_core/base_local_planner.h>
 #include <nav_core2/local_planner.h>
 #include <nav_core_adapter/costmap_adapter.h>
-#include <nav_2d_utils/odom_subscriber.h>
+#include <robot_nav_2d_utils/odom_subscriber.h>
 #include <boost/dll/import.hpp>
 #include <memory>
 #include <string>
@@ -168,22 +168,22 @@ namespace nav_core_adapter
     /**
      * @brief Get the robot pose from the costmap and store as Pose2DStamped
      */
-    bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d);
+    bool getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d);
 
     /**
      * @brief See if the back of the global plan matches the most recent goal pose
      * @return True if the plan has changed
      */
-    bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal);
+    bool hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal);
 
     // The most recent goal pose
-    nav_2d_msgs::Pose2DStamped last_goal_;
+    robot_nav_2d_msgs::Pose2DStamped last_goal_;
     bool has_active_goal_;
 
     /**
      * @brief helper class for subscribing to odometry
      */
-    std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
+    std::shared_ptr<robot_nav_2d_utils::OdomSubscriber> odom_sub_;
 
     // Plugin handling
     boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_;

src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp

@@ -34,8 +34,8 @@
 
 #include <nav_core_adapter/global_planner_adapter.h>
 #include <nav_core_adapter/costmap_adapter.h>
-#include <nav_2d_utils/conversions.h>
+#include <robot_nav_2d_utils/conversions.h>
-#include <nav_2d_utils/tf_help.h>
+#include <robot_nav_2d_utils/tf_help.h>
 #include <nav_core2/exceptions.h>
 #include <boost/dll/alias.hpp>
 #include <memory>
@@ -78,18 +78,18 @@ namespace nav_core_adapter
     planner_->initialize(planner_name, costmap_robot_);
   }
 
-  nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const nav_2d_msgs::Pose2DStamped& start,
+  robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
-                                                      const nav_2d_msgs::Pose2DStamped& goal)
+                                                      const robot_nav_2d_msgs::Pose2DStamped& goal)
   {
-    geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
+    geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
-                              goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
+                              goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal);
     std::vector<geometry_msgs::PoseStamped> plan;
     bool ret = planner_->makePlan(start3d, goal3d, plan);
     if (!ret)
     {
       throw nav_core2::PlannerException("Generic Global Planner Error");
     }
-    return nav_2d_utils::posesToPath2D(plan);
+    return robot_nav_2d_utils::posesToPath2D(plan);
   }
 
   nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() 

src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp

@@ -36,9 +36,9 @@
 #include <nav_core_adapter/local_planner_adapter.h>
 #include <nav_core_adapter/costmap_adapter.h>
 #include <nav_core_adapter/shared_pointers.h>
-#include <nav_2d_utils/conversions.h>
+#include <robot_nav_2d_utils/conversions.h>
-#include <nav_2d_utils/tf_help.h>
+#include <robot_nav_2d_utils/tf_help.h>
-#include <nav_2d_utils/parameters.h>
+#include <robot_nav_2d_utils/parameters.h>
 #include <nav_core2/exceptions.h>
 #include <boost/dll/alias.hpp>
 #include <boost/dll/import.hpp>
@@ -90,7 +90,7 @@ namespace nav_core_adapter
     }
     else
     {
-      planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
+      planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
       adapter_nh_.setParam("planner_name", planner_name);
     }
     try
@@ -174,15 +174,15 @@ namespace nav_core_adapter
     }
 
     // Get the Pose
-    nav_2d_msgs::Pose2DStamped pose2d;
+    robot_nav_2d_msgs::Pose2DStamped pose2d;
     if (!getRobotPose(pose2d))
     {
       return false;
     }
 
     // Get the Velocity
-    nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+    robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
-    nav_2d_msgs::Twist2DStamped cmd_vel_2d;
+    robot_nav_2d_msgs::Twist2DStamped cmd_vel_2d;
     try
     {
       cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
@@ -192,7 +192,7 @@ namespace nav_core_adapter
       std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
       return false;
     }
-    cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
+    cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
     return true;
   }
 
@@ -263,7 +263,7 @@ namespace nav_core_adapter
   geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
     if (odom_sub_)
-      return nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
+      return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
     else
       throw std::runtime_error("Failed to get twist");
   }
@@ -326,11 +326,11 @@ namespace nav_core_adapter
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     // Get the Pose
-    nav_2d_msgs::Pose2DStamped pose2d;
+    robot_nav_2d_msgs::Pose2DStamped pose2d;
     if (!getRobotPose(pose2d))
       return false;
 
-    nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
+    robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
     bool ret = planner_->isGoalReached(pose2d, velocity);
     if (ret)
     {
@@ -345,12 +345,12 @@ namespace nav_core_adapter
   bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
-    nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
+    robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan);
     try
     {
       if (path.poses.size() > 0)
       {
-        nav_2d_msgs::Pose2DStamped goal_pose;
+        robot_nav_2d_msgs::Pose2DStamped goal_pose;
         goal_pose = path.poses.back();
 
         if (!has_active_goal_ || hasGoalChanged(goal_pose))
@@ -370,7 +370,7 @@ namespace nav_core_adapter
     }
   }
 
-  bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal)
+  bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
   {
     if (last_goal_.header.frame_id != new_goal.header.frame_id ||
         last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec())
@@ -382,7 +382,7 @@ namespace nav_core_adapter
            last_goal_.pose.theta != new_goal.pose.theta;
   }
 
-  bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d)
+  bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d)
   {
     geometry_msgs::PoseStamped current_pose;
     if (!costmap_robot_->getRobotPose(current_pose))
@@ -390,7 +390,7 @@ namespace nav_core_adapter
       std::cout << "Could not get robot pose" << std::endl;
       return false;
     }
-    pose2d = nav_2d_utils::poseStampedToPose2D(current_pose);
+    pose2d = robot_nav_2d_utils::poseStampedToPose2D(current_pose);
     return true;
   }
 

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -148,7 +148,7 @@ else()
     data_convert
     dl
     pthread
-    nav_2d_utils
+    robot_nav_2d_utils
   )
   
   target_link_libraries(move_base 
@@ -196,8 +196,8 @@ endif()
 # Set RPATH for executable to find libraries in build directory first
 # Use RPATH instead of RUNPATH for higher priority
 set_target_properties(move_base_main PROPERTIES
-  BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils"
+  BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/robot_nav_2d_utils"
-  INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils"
+  INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp:${CMAKE_BINARY_DIR}/src/Libraries/robot_nav_2d_utils"
   BUILD_WITH_INSTALL_RPATH FALSE
   INSTALL_RPATH_USE_LINK_PATH TRUE
   SKIP_BUILD_RPATH FALSE

src/Navigations/Packages/move_base/include/move_base/move_base.h

@@ -58,7 +58,7 @@ namespace move_base
    * @class MoveBase
    * @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
    */
-  class MoveBase : public move_base_core::BaseNavigation
+  class MoveBase : public robot::move_base_core::BaseNavigation
   {
   public:
     /**
@@ -66,14 +66,14 @@ namespace move_base
      * @param name The name of the action
      * @param tf A reference to a TransformListener
      */
-    MoveBase(TFListenerPtr tf);
+    MoveBase(robot::TFListenerPtr tf);
     MoveBase();
 
     /**
      * @brief Initialize the navigation system.
      * @param tf Shared pointer to the TF listener or buffer.
      */
-    virtual void initialize(TFListenerPtr tf) override;
+    virtual void initialize(robot::TFListenerPtr tf) override;
 
     /**
      * @brief Set the robot's footprint (outline shape) in the global frame.
@@ -196,25 +196,40 @@ namespace move_base
      */
     virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override;
 
+
+    /**
+     * @brief Get the robot’s twist.
+     * @param twist Output parameter with the robot’s current twist.
+     * @return True if twist was successfully retrieved.
+     */
+    virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) override;
+
+    /**
+     * @brief Get the navigation feedback.
+     * @param feedback Output parameter with the navigation feedback.
+     * @return True if feedback was successfully retrieved.
+     */
+    virtual bool getFeedback(robot::move_base_core::NavFeedback &feedback) override;
+
     /**
      * @brief  Destructor - Cleans up
      */
     virtual ~MoveBase();
 
+    /**
+     * @brief Create a new MoveBase
+     * @return A shared pointer to the new MoveBase
+     */
+    static robot::move_base_core::BaseNavigation::Ptr create();
+
+  private:
+
     /**
      * @brief  Performs a control cycle
      * @param goal A reference to the goal to pursue
      * @return True if processing of the goal is done, false otherwise
      */
-    bool executeCycle(geometry_msgs::PoseStamped &goal);
+     bool executeCycle(geometry_msgs::PoseStamped &goal);
-
-    /**
-     * @brief Create a new MoveBase
-     * @return A shared pointer to the new MoveBase
-     */
-    static move_base_core::BaseNavigation::Ptr create();
-
-  private:
 
     /**
      * @brief Set velocity for yaw goal tolerance of the robot
@@ -291,7 +306,7 @@ namespace move_base
 
   private:
     bool initialized_;
-    TFListenerPtr tf_;
+    robot::TFListenerPtr tf_;
     robot::NodeHandle private_nh_;
 
     boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -31,7 +31,7 @@ move_base::MoveBase::MoveBase()
 {
 }
 
-move_base::MoveBase::MoveBase(TFListenerPtr tf)
+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),
@@ -89,7 +89,7 @@ move_base::MoveBase::~MoveBase()
   tf_.reset();
 }
 
-void move_base::MoveBase::initialize(TFListenerPtr tf)
+void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
 {
   printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
   if (!initialized_)
@@ -334,10 +334,10 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
 
     // we'll start executing recovery behaviors at the beginning of our list
     recovery_index_ = 0;
-    nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
+    nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::PLANNING;
+      nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
       nav_feedback_->is_ready = true;
     }
     else
@@ -377,7 +377,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
   {
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::REJECTED;
+      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;
     }
@@ -416,7 +416,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
 
   if (nav_feedback_)
   {
-    nav_feedback_->navigation_state = move_base_core::State::PLANNING;
+    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
     nav_feedback_->feed_back_str = std::string("Planning");
     nav_feedback_->goal_checked = false;
   }
@@ -455,7 +455,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
   {
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::REJECTED;
+      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
       std::stringstream fb_ss;
       fb_ss << "The system has not been '" << maker << "'";
       nav_feedback_->feed_back_str = fb_ss.str();
@@ -472,7 +472,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
   {
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::REJECTED;
+      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;
     }
@@ -510,7 +510,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
 
   if (nav_feedback_)
   {
-    nav_feedback_->navigation_state = move_base_core::State::PLANNING;
+    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
     nav_feedback_->feed_back_str = std::string("Planning");
     nav_feedback_->goal_checked = false;
   }
@@ -539,7 +539,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
   {
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::REJECTED;
+      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;
     }
@@ -565,7 +565,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
 
   if (nav_feedback_)
   {
-    nav_feedback_->navigation_state = move_base_core::State::PLANNING;
+    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
     nav_feedback_->feed_back_str = std::string("Planning");
     nav_feedback_->goal_checked = false;
   }
@@ -587,7 +587,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
   {
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::REJECTED;
+      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;
     }
@@ -614,7 +614,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
 
   if (nav_feedback_)
   {
-    nav_feedback_->navigation_state = move_base_core::State::PLANNING;
+    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
     nav_feedback_->feed_back_str = std::string("Planning");
     nav_feedback_->goal_checked = false;
   }
@@ -625,20 +625,12 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
   {
     if (nav_feedback_)
     {
-      nav_feedback_->navigation_state = move_base_core::State::REJECTED;
+      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
       nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
       nav_feedback_->goal_checked = false;
     }
     return false;
   }
-
-  // move_base_msgs::MoveBaseActionGoal action_goal;
-  // action_goal.header.stamp = robot::Time::now();
-  // action_goal.goal.target_pose = goal;
-  // action_goal.goal.target_pose.pose.position.x = pose.pose.position.x + 0.05 * cos(tf2::getYaw(pose.pose.orientation));
-  // action_goal.goal.target_pose.pose.position.y = pose.pose.position.y + 0.05 * sin(tf2::getYaw(pose.pose.orientation));
-  // action_goal_pub_.publish(action_goal);
-
   lock.unlock();
   return true;
 }
@@ -1283,10 +1275,7 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
   geometry_msgs::PoseStamped goal_pose, global_pose;
   goal_pose = goal_pose_msg;
 
-  // just get the latest available transform... for accuracy they should send
-  // goals in the frame of the planner
   goal_pose.header.stamp = robot::Time();     // latest available
-
   try
   {
     tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp));
@@ -1307,15 +1296,32 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
     std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl;
     return goal_pose_msg;
   }
-
   return global_pose;
 }
 
+bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
+{
+  if (!twist_) {
+    return false;
+  }
+  twist = *twist_;
+  return true;
+}
+
+bool move_base::MoveBase::getFeedback(robot::move_base_core::NavFeedback &feedback)
+{
+  if (!nav_feedback_) {
+    return false;
+  }
+  feedback = *nav_feedback_;
+  return true;
+}
+
 // void wakePlanner(const robot::TimerEvent &event)
 // {
 // }
 
-move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
+robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
 {
   return std::make_shared<move_base::MoveBase>();
 }

src/Navigations/Packages/move_base/src/move_base_main.cpp

@@ -30,7 +30,7 @@
 #include <move_base/move_base.h>
 #include <iostream>
 #include <robot/node_handle.h>
-#include <nav_2d_utils/parameters.h>
+#include <robot_nav_2d_utils/parameters.h>
 #include <boost/dll/import.hpp>
 #include <robot/console.h>
 #include <tf3/buffer_core.h>
@@ -90,12 +90,12 @@ int main(int argc, char** argv)
     robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
     robot::PluginLoaderHelper loader;
     std::string path_file_so = loader.findLibraryPath("MoveBase");
-    auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
+    auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
       path_file_so,
       "MoveBase", boost::dll::load_mode::append_decorations);
     
     robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
-    move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
+    robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
     
     if (nav_ptr == nullptr) {
       robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__);