udpdate

config/costmap_global_params.yaml

@@ -1,5 +1,6 @@
 global_costmap:
-  path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so
+  library_path: libplugins
+  robot_base_frame: base_footprint
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0

config/costmap_local_params.yaml

@@ -1,6 +1,7 @@
 local_costmap:  
-  path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so
+  library_path: libplugins
   global_frame: odom
+  robot_base_frame: base_footprint
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true

config/custom_global_params.yaml

@@ -1,5 +1,6 @@
 base_global_planner: CustomPlanner
 CustomPlanner:
+  library_path: libcustom_planner
   environment_type: XYThetaLattice
   planner_type: ARAPlanner
   allocated_time: 10.0
@@ -9,9 +10,6 @@ CustomPlanner:
   nominalvel_mpersecs: 0.8
   timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
   allow_unknown: true
-  directory_to_save_paths: "/init/paths"
+
-  pathway_filename: "pathway.txt"
+
-  current_pose_topic_name: "/amcl_pose"
-  map_frame_id: "map"
-  base_frame_id: "base_link"
   

config/move_base_common_params.yaml

@@ -21,4 +21,5 @@ conservative_reset:
 aggressive_reset:
   reset_distance: 3.0
 
-
+MoveBase:
+  library_path: libmove_base

config/two_points_global_params.yaml

@@ -1,3 +1,4 @@
 base_global_planner: TwoPointsPlanner
 TwoPointsPlanner:
+  library_path: libtwo_points_planner
   lethal_obstacle: 20

examples/CSharpExample.cs

@@ -287,7 +287,16 @@ namespace NavigationExample
                 return;
             }
 
-                        if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_link",
+            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))
             {

examples/NavigationExample/Program.cs

@@ -287,7 +287,16 @@ namespace NavigationExample
                 return;
             }
 
-                        if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_link",
+            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))
             {

examples/run_example.sh

@@ -7,7 +7,7 @@ set -e
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 PROJECT_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
 BUILD_DIR="$PROJECT_ROOT/build"
-LIB_DIR="$BUILD_DIR/src/APIs/c_api"
+LIB_DIR="$BUILD_DIR"
 EXAMPLE_DIR="$SCRIPT_DIR"
 
 echo "=========================================="

src/APIs/c_api/CMakeLists.txt

@@ -37,6 +37,10 @@ target_link_libraries(nav_c_api
         ${PACKAGES_DIR}
 )
 
+set_target_properties(nav_c_api PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Set include directories
 target_include_directories(nav_c_api
     PUBLIC

src/APIs/c_api/src/nav_c_api.cpp

@@ -12,6 +12,7 @@
 #include <cstring>
 #include <cstdlib>
 #include <boost/dll/import.hpp>
+#include <robot/plugin_loader_helper.h>
 
 
 // ============================================================================
@@ -227,9 +228,10 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
         printf("[%s:%d]\n 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);
-        
+        robot::PluginLoaderHelper loader;
+        std::string lib_path = loader.findLibraryPath("MoveBase");
         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",
+            lib_path,
             "MoveBase", 
             boost::dll::load_mode::append_decorations);
             

src/Algorithms/Cores/score_algorithm/CMakeLists.txt

@@ -29,7 +29,12 @@ target_link_libraries(score_algorithm
   PRIVATE
     Boost::filesystem
     Boost::system
+
+    )
+set_target_properties(score_algorithm PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
 )
+
 target_include_directories(score_algorithm PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)

src/Algorithms/Libraries/kalman/CMakeLists.txt

@@ -32,6 +32,10 @@ target_link_libraries(kalman
         Eigen3::Eigen
 )
 
+set_target_properties(kalman PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Set include directories
 target_include_directories(kalman
     PUBLIC
@@ -48,6 +52,10 @@ target_link_libraries(kalman_node
         Eigen3::Eigen
 )
 
+set_target_properties(kalman_node PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Cài đặt header files
 install(DIRECTORY include/kalman
     DESTINATION include

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -57,6 +57,10 @@ target_link_libraries(mkt_algorithm_diff
         Eigen3::Eigen
 )
 
+set_target_properties(mkt_algorithm_diff PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Set include directories
 target_include_directories(mkt_algorithm_diff
     PUBLIC

src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt

@@ -55,6 +55,10 @@ target_link_libraries(${PROJECT_NAME}_goal_checker
     PRIVATE Boost::boost
 )
 
+set_target_properties(${PROJECT_NAME}_goal_checker PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 target_include_directories(${PROJECT_NAME}_goal_checker
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -74,6 +78,10 @@ target_link_libraries(${PROJECT_NAME}_standard_traj_generator
     PRIVATE Boost::boost
 )
 
+set_target_properties(${PROJECT_NAME}_standard_traj_generator PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 target_include_directories(${PROJECT_NAME}_standard_traj_generator
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt

@@ -46,6 +46,10 @@ target_link_libraries(two_points_planner
     PRIVATE Boost::boost
 )
 
+set_target_properties(two_points_planner PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Set include directories
 target_include_directories(two_points_planner
     PUBLIC

src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt

@@ -56,6 +56,10 @@ target_link_libraries(${PROJECT_NAME}_utils
     PRIVATE Boost::boost
 )
 
+set_target_properties(${PROJECT_NAME}_utils PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Set include directories cho utils
 target_include_directories(${PROJECT_NAME}_utils
     PUBLIC
@@ -78,6 +82,10 @@ target_link_libraries(${PROJECT_NAME}
     PRIVATE Boost::boost
 )
 
+set_target_properties(${PROJECT_NAME} PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Set include directories cho thư viện chính
 target_include_directories(${PROJECT_NAME}
     PUBLIC

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -60,7 +60,6 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
       throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
 
     parent_ = parent;
-    robot::log_warning_at(__FILE__, __LINE__, "Parent namespace: %s", parent_.getNamespace().c_str());
     planner_nh_ = robot::NodeHandle(parent_, name);
     // planner_nh_.printRootParams();
     this->getParams(planner_nh_);

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit b3be5da393abee4ef535f68f702cd84c02f3b98b
+Subproject commit 10521c1629301d3e81f040213f08de7881bc5e59

src/Libraries/nav_2d_utils/CMakeLists.txt

@@ -39,6 +39,10 @@ target_link_libraries(conversions
         Boost::thread
 )
 
+set_target_properties(conversions PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 add_library(path_ops src/path_ops.cpp)
 target_include_directories(path_ops
     PUBLIC
@@ -58,22 +62,28 @@ target_include_directories(polygons
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(polygons
-    PUBLIC
-        nav_2d_msgs
-        geometry_msgs
-        robot_cpp
-    PRIVATE
-        ${xmlrpcpp_LIBRARIES}
-)
 
 if(xmlrpcpp_FOUND)
     target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS})
+    target_link_libraries(polygons
+        PUBLIC nav_2d_msgs geometry_msgs robot_cpp
+        PRIVATE ${xmlrpcpp_LIBRARIES})
 elseif(XmlRpcCpp_FOUND)
     target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
-    target_link_libraries(polygons PRIVATE ${XmlRpcCpp_LIBRARIES})
+    target_link_libraries(polygons 
+        PUBLIC nav_2d_msgs geometry_msgs robot_cpp 
+        PRIVATE ${XmlRpcCpp_LIBRARIES})
+else()
+    target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS})
+    target_link_libraries(polygons
+        PUBLIC nav_2d_msgs geometry_msgs robot_cpp
+        PRIVATE ${xmlrpcpp_LIBRARIES})
 endif()
 
+set_target_properties(polygons PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 add_library(bounds src/bounds.cpp)
 target_include_directories(bounds
     PUBLIC
@@ -87,6 +97,10 @@ target_link_libraries(bounds
         robot_cpp
 )
 
+set_target_properties(bounds PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 add_library(tf_help src/tf_help.cpp)
 target_include_directories(tf_help
     PUBLIC
@@ -102,6 +116,10 @@ target_link_libraries(tf_help
         robot_cpp
 )
 
+set_target_properties(tf_help PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
 # Create an INTERFACE library that represents all nav_2d_utils libraries
 add_library(nav_2d_utils INTERFACE)
 target_include_directories(nav_2d_utils

src/Libraries/robot_cpp

@@ -1 +1 @@
-Subproject commit 754042654785374e19d614a2d5927362735ed7ca
+Subproject commit c54064a3196792b2ad4362fae2b2294aa9b6872c

src/Libraries/tf3/CMakeLists.txt

@@ -14,6 +14,9 @@ include_directories(include ${console_bridge_INCLUDE_DIRS})
 #CPP Libraries
 add_library(tf3 src/cache.cpp src/buffer_core.cpp src/static_cache.cpp)
 target_link_libraries(tf3 ${Boost_LIBRARIES} ${console_bridge_LIBRARIES})
+set_target_properties(tf3 PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
 target_include_directories(tf3 PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -21,7 +24,9 @@ target_include_directories(tf3 PUBLIC
 add_executable(simple_tf3_example examples/simple_tf3_example.cpp)
 target_include_directories(simple_tf3_example PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(simple_tf3_example PRIVATE tf3 pthread ${console_bridge_LIBRARIES})
-
+set_target_properties(simple_tf3_example PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
 install(TARGETS tf3
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

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

@@ -9,6 +9,9 @@
 #include <geometry_msgs/Pose2D.h>
 #include <geometry_msgs/Vector3.h>
 
+// robot protocol msgs
+#include <robot_protocol_msgs/Order.h>
+
 // tf
 #include <tf3/buffer_core.h>
 #include <tf3/utils.h>
@@ -60,35 +63,35 @@ namespace move_base_core
 		switch (state)
 		{
 		case State::PENDING:
-			return "PENDING"; 				// Chờ xử lý
+			return "PENDING"; // Chờ xử lý
 		case State::ACTIVE:
-			return "ACTIVE"; 					// Đang hoạt động
+			return "ACTIVE"; // Đang hoạt động
 		case State::PREEMPTED:
-			return "PREEMPTED"; 			// Đã bị huỷ bởi yêu cầu mới
+			return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
 		case State::SUCCEEDED:
-			return "SUCCEEDED"; 			// Thành công
+			return "SUCCEEDED"; // Thành công
 		case State::ABORTED:
-			return "ABORTED"; 				// Bị lỗi
+			return "ABORTED"; // Bị lỗi
 		case State::REJECTED:
-			return "REJECTED"; 				// Từ chối bởi planner hoặc controller
+			return "REJECTED"; // Từ chối bởi planner hoặc controller
 		case State::PREEMPTING:
-			return "PREEMPTING"; 			// Đang huỷ bỏ theo yêu cầu
+			return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
 		case State::RECALLING:
-			return "RECALLING"; 			// Đang huỷ bỏ nội bộ
+			return "RECALLING"; // Đang huỷ bỏ nội bộ
 		case State::RECALLED:
-			return "RECALLED"; 				// Đã được thu hồi
+			return "RECALLED"; // Đã được thu hồi
 		case State::LOST:
-			return "LOST"; 						// Mất trạng thái
+			return "LOST"; // Mất trạng thái
 		case State::PLANNING:
-			return "PLANNING"; 				// Đang lập kế hoạch đường đi
+			return "PLANNING"; // Đang lập kế hoạch đường đi
 		case State::CONTROLLING:
-			return "CONTROLLING"; 		// Đang điều khiển robot di chuyển theo plan
+			return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
 		case State::CLEARING:
-			return "CLEARING"; 				// Đang dọn dẹp bản đồ / costmap
+			return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
 		case State::PAUSED:
-			return "PAUSED"; 					// Tạm dừng
+			return "PAUSED"; // Tạm dừng
 		default:
-			return "UNKNOWN_STATE"; 	// Không xác định
+			return "UNKNOWN_STATE"; // Không xác định
 		}
 	}
 
@@ -196,6 +199,19 @@ namespace move_base_core
 												double xy_goal_tolerance = 0.0,
 												double yaw_goal_tolerance = 0.0) = 0;
 
+		/**
+		 * @brief Send a goal for the robot to navigate to.
+		 * @param msg               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.
+		 */
+		virtual bool moveTo(const robot_protocol_msgs::Order &msg,
+												const geometry_msgs::PoseStamped &goal,
+												double xy_goal_tolerance = 0.0,
+												double yaw_goal_tolerance = 0.0) = 0;
+
 		/**
 		 * @brief Send a docking goal to a predefined marker (e.g. charger).
 		 * @param maker             Marker name or ID.
@@ -209,6 +225,19 @@ namespace move_base_core
 												double xy_goal_tolerance = 0.0,
 												double yaw_goal_tolerance = 0.0) = 0;
 
+		/**
+		 * @brief Send a docking goal to a predefined marker (e.g. charger).
+		 * @param msg               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.
+		 */
+		virtual bool dockTo(const robot_protocol_msgs::Order &msg,
+												const geometry_msgs::PoseStamped &goal,
+												double xy_goal_tolerance = 0.0,
+												double yaw_goal_tolerance = 0.0) = 0;
+
 		/**
 		 * @brief Move straight toward the target position (X-axis).
 		 * @param goal              Target pose.

src/Navigations/Cores/nav_core_adapter/CMakeLists.txt

@@ -66,19 +66,13 @@ set(_NAV_CORE_ADAPTER_RPATH
 )
 
 set_target_properties(costmap_adapter PROPERTIES
-  BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
-  LINK_FLAGS "-Wl,--disable-new-dtags"
 )
 set_target_properties(local_planner_adapter PROPERTIES
-  BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
-  LINK_FLAGS "-Wl,--disable-new-dtags"
 )
 set_target_properties(global_planner_adapter PROPERTIES
-  BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
-  LINK_FLAGS "-Wl,--disable-new-dtags"
 )
 
 # Cài đặt header files

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

@@ -93,10 +93,9 @@ namespace nav_core_adapter
       planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
       adapter_nh_.setParam("planner_name", planner_name);
     }
-
-    std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
     try
     {
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
         path_file_so, planner_name, boost::dll::load_mode::append_decorations);
       planner_ = planner_loader_();

src/Navigations/Packages/move_base/CMakeLists.txt

@@ -157,9 +157,7 @@ else()
   
   # Set RPATH để ưu tiên thư viện build cục bộ
   set_target_properties(move_base PROPERTIES
-    BUILD_RPATH "${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"
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-    INSTALL_RPATH "${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"
-    LINK_FLAGS "-Wl,--disable-new-dtags"
   )
 endif()
 

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

@@ -27,6 +27,7 @@
 #include <costmap_2d/costmap_2d_robot.h>
 #include <costmap_2d/costmap_2d.h>
 #include <nav_msgs/GetPlan.h>
+#include <robot_protocol_msgs/Order.h>
 
 #include <robot/node_handle.h>
 #include <robot/console.h>
@@ -94,6 +95,19 @@ namespace move_base
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;
 
+    /**
+     * @brief Send a goal for the robot to navigate to.
+     * @param msg               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.
+     */
+    virtual bool moveTo(const robot_protocol_msgs::Order &msg,
+                        const geometry_msgs::PoseStamped &goal,
+                        double xy_goal_tolerance = 0.0,
+                        double yaw_goal_tolerance = 0.0) override;
+
     /**
      * @brief Send a docking goal to a predefined marker (e.g. charger).
      * @param maker             Marker name or ID.
@@ -107,8 +121,22 @@ namespace move_base
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;
 
+    /**
+     * @brief Send a docking goal to a predefined marker (e.g. charger).
+     * @param msg               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.
+     */
+    virtual bool dockTo(const robot_protocol_msgs::Order &msg,
+                        const geometry_msgs::PoseStamped &goal,
+                        double xy_goal_tolerance = 0.0,
+                        double yaw_goal_tolerance = 0.0) override;
+
     /**
      * @brief Move straight toward the target position (X-axis).
+     * @param msg               Order message.
      * @param goal              Target pose.
      * @param xy_goal_tolerance Acceptable positional error (meters).
      * @return True if command issued successfully.
@@ -206,22 +234,6 @@ namespace move_base
      */
     void swapPlanner(std::string base_global_planner);
 
-    // /**
-    //  * @brief  A service call that clears the costmaps of obstacles
-    //  * @param req The service request
-    //  * @param resp The service response
-    //  * @return True if the service call succeeds, false otherwise
-    //  */
-    // bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
-
-    // /**
-    //  * @brief  A service call that can be made when the action is inactive that will return a plan
-    //  * @param  req The goal request
-    //  * @param  resp The plan request
-    //  * @return True if planning succeeded, false otherwise
-    //  */
-    // bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
-
     /**
      * @brief  Make a new global plan
      * @param  goal The goal to plan to

src/Navigations/Packages/move_base/plugins.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="lib/libmove_base">
+    <class type="move_base::MoveBase" base_class_type="move_base_core::BaseNavigation">
+      <description></description>
+    </class>
+  </library>
+</class_libraries>

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

@@ -15,6 +15,7 @@
 #include <boost/thread.hpp>
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
+#include <robot/plugin_loader_helper.h>
 
 move_base::MoveBase::MoveBase()
 : initialized_(false),
@@ -156,34 +157,24 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
     // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
     //  From config param
     double inscribed_radius;
-    robot::log_info("[%s:%d] inscribed_radius: %f", __FILE__, __LINE__, inscribed_radius);
     private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
     double circumscribed_radius; 
     private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
-    robot::log_info("[%s:%d] circumscribed_radius: %f", __FILE__, __LINE__, circumscribed_radius);
     inscribed_radius_ = inscribed_radius;
     circumscribed_radius_ = circumscribed_radius;
     private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
-    robot::log_info("[%s:%d] clearing_radius: %f", __FILE__, __LINE__, clearing_radius_);
     double conservative_reset_dist;
     private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
-    robot::log_info("[%s:%d] conservative_reset_dist: %f", __FILE__, __LINE__, conservative_reset_dist);
     conservative_reset_dist_ = conservative_reset_dist;
-    robot::log_info("[%s:%d] shutdown_costmaps: %f", __FILE__, __LINE__, shutdown_costmaps_);
     private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
-    robot::log_info("[%s:%d] clearing_rotation_allowed: %f", __FILE__, __LINE__, clearing_rotation_allowed_);
     private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
-    robot::log_info("[%s:%d] recovery_behavior_enabled: %f", __FILE__, __LINE__, recovery_behavior_enabled_);
     private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
     
     // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
-    robot::log_info("[%s:%d] create the ros wrapper for the planner's costmap...");
     try
     {
       planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
-      robot::log_info("[%s:%d] planner_costmap_robot_->pause()");
       planner_costmap_robot_->pause();
-      robot::log_info("[%s:%d] planner_costmap_robot_->pause()");
     }
     catch (const std::exception &ex)
     {
@@ -193,10 +184,10 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
     // initialize the global planner
     try
     {
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath("CustomPlanner");
       planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
-          "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", 
+          path_file_so, global_planner, boost::dll::load_mode::append_decorations);
-          global_planner, 
-          boost::dll::load_mode::append_decorations);
 
       planner_ = planner_loader_();
       if (!planner_)
@@ -214,13 +205,22 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
       robot::log_error("[%s:%d]\n EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
       throw std::runtime_error("Failed to create the " + global_planner + " planner");
     }
-    // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+
-    controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+    try
-    controller_costmap_robot_->pause();
+    {
+      controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+      controller_costmap_robot_->pause();
+    }
+    catch (const std::exception &ex)
+    {
+      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
+      throw std::runtime_error("Failed to create the controller_costmap_robot_");
+    }
     // create a local planner
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
+      robot::PluginLoaderHelper loader;
+      std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
       controller_loader_ = 
         boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
           path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -363,15 +363,18 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
 
   if (cancel_ctr_)
     cancel_ctr_ = false;
-  // move_base_msgs::MoveBaseActionGoal action_goal;
-  // action_goal.header.stamp = robot::Time::now();
-  // action_goal.goal.target_pose = goal;
-  // action_goal_pub_.publish(action_goal);
-
   lock.unlock();
   return true;
 }
 
+bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
+                                 const geometry_msgs::PoseStamped &goal,
+                                 double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  return false;
+}
+
+
 bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::PoseStamped &goal,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
@@ -454,14 +457,17 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
   if (cancel_ctr_)
     cancel_ctr_ = false;
 
-  // move_base_msgs::MoveBaseActionGoal action_goal;
-  // action_goal.header.stamp = robot::Time::now();
-  // action_goal.goal.target_pose = goal;
-  // action_goal_pub_.publish(action_goal);
   lock.unlock();
   return true;
 }
 
+bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
+                                 const geometry_msgs::PoseStamped &goal,
+                                 double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  return false;
+}
+
 bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
 {
   if (setup_)
@@ -505,10 +511,6 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
   }
   if (cancel_ctr_)
     cancel_ctr_ = false;
-  // move_base_msgs::MoveBaseActionGoal action_goal;
-  // action_goal.header.stamp = robot::Time::now();
-  // action_goal.goal.target_pose = goal;
-  // action_goal_pub_.publish(action_goal);
 
   lock.unlock();
   return true;