fix move base

.gitignore

@@ -422,4 +422,3 @@ build
 install
 devel
 
-obstacle

CMakeLists.txt

@@ -138,22 +138,6 @@ if (NOT TARGET pnkx_local_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
 endif()
 
-if (NOT TARGET robot_angles)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles)
-endif()
-
-if (NOT TARGET grid_map_core)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core)
-endif()
-
-if (NOT TARGET robot_base_local_planner)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner)
-endif()
-
-if (NOT TARGET hybrid_local_planner)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner)
-endif()
-
 if (NOT TARGET robot_actionlib_msgs)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
 endif()

config/costmap_common_params.yaml

@@ -19,33 +19,17 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources:  l_scan_marking l_scan_clearing r_scan_marking r_scan_clearing b_scan_marking b_scan_clearing
+  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing 
-  l_scan_marking:
+  f_scan_marking:
-    topic: /l_scan
+    topic: /f_scan
     data_type: LaserScan
     clearing: false
     marking: true
     inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
-  l_scan_clearing:
+  f_scan_clearing:
-    topic: /l_scan
+    topic: /f_scan
-    data_type: LaserScan
-    clearing: true
-    marking: false
-    inf_is_valid: true
-    min_obstacle_height: 0.0
-    max_obstacle_height: 0.25
-  r_scan_marking:
-    topic: /r_scan
-    data_type: LaserScan
-    clearing: false
-    marking: true
-    inf_is_valid: true
-    min_obstacle_height: 0.0
-    max_obstacle_height: 0.25
-  r_scan_clearing:
-    topic: /r_scan
     data_type: LaserScan
     clearing: true
     marking: false
@@ -70,4 +54,3 @@ obstacles:
     max_obstacle_height: 0.25
 
 
-

config/costmap_global_params.yaml

@@ -4,7 +4,7 @@ global_costmap:
   global_frame: map
   update_frequency: 1.0
   publish_frequency: 1.0
-  raytrace_range: 3.5
+  raytrace_range: 2.0
   resolution: 0.05
   z_resolution: 0.2
   rolling_window: false

config/costmap_local_params.yaml

@@ -5,7 +5,7 @@ local_costmap:
   update_frequency: 6.0
   publish_frequency: 6.0
   rolling_window: true
-  raytrace_range: 3.5
+  raytrace_range: 2.0
   resolution: 0.05
   z_resolution: 0.15
   z_voxels: 8

config/hybrid_local_planner_params.yaml

@@ -1,59 +0,0 @@
-LocalPlannerAdapter:
-  library_path: liblocal_planner_adapter
-  yaw_goal_tolerance: 0.017  
-  xy_goal_tolerance: 0.03 
-  min_approach_linear_velocity: 0.06
-
-HybridLocalPlanner:
-# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
-# HybridLocalPlanner:
-    library_path: libhybrid_local_planner
-    odom_topic: odom
-    # Trajectory
-    max_global_plan_lookahead_dist: 4.0
-    global_plan_viapoint_sep: 0.5
-    global_plan_prune_distance: 0.0
-    global_plan_goal_sep: 0.05
-
-    # Robot
-
-    robot_max_v_ac: 0.4
-    robot_max_w_ac: 0.4
-    robot_max_v_pt: 1.0
-    robot_max_w_pt: 0.4
-    robot_max_v_backwards_pt: -0.2
-    acc_lim_x: 1.0
-    acc_lim_theta: 2.0
-    turn_around_priority: True
-    stop_dist: 0.5
-    dec_dist: 1.0
-
-
-    # GoalTolerance
-
-    xy_goal_tolerance: 0.1
-    yaw_goal_tolerance: 0.07
-
-    # Optimization
-
-    # PP Parameters
-    w_vel: 0.8
-    w_omega: 1.0
-    # DWA Parameters
-    enable_backward_motion: false
-    w_targetheading_ac: 1.7
-    w_velocity_ac: 0.2
-    w_clearance_ac: 0.2
-    w_pathDistance_ac: 0.05
-    w_smoothness_ac: 0.3
-    w_targetheading_pt: 0.2
-    w_velocity_pt: 0.8
-    w_clearance_pt: 0.1
-    w_pathDistance_pt: 2.1
-    w_smoothness_pt: 0.3
-    time_horizon: 3.0
-    velocity_resolution: 0.015
-
-    segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment
-    calibration_factor: 1.5 # Hệ số hiệu chuẩn
-    use_obstacle_avoidance: true # Bật tắt tránh vật cản

examples/NavigationExample/NavigationExample.csproj

@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net10.0</TargetFramework>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
   <ItemGroup>

src/APIs/c_api/src/nav_c_api.cpp

@@ -83,6 +83,7 @@ extern "C" NavigationHandle navigation_create(void)
     // Using default constructor - initialization will be done via navigation_initialize()
     robot::PluginLoaderHelper loader;
     std::string path_file_so = loader.findLibraryPath("MoveBase");
+    robot::log_warning("%s", path_file_so.c_str());
     auto move_base_loader = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
         path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
     auto move_base_ptr = move_base_loader();

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -174,8 +174,9 @@ namespace two_points_planner
     // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
     const double dx = goal.pose.position.x - start.pose.position.x;
     const double dy = goal.pose.position.y - start.pose.position.y;
-    double distance = std::sqrt(dx * dx + dy * dy);
+    const double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
+
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
       theta = std::atan2(dy, dx);
@@ -187,8 +188,12 @@ namespace two_points_planner
     }
     else 
     { 
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
+      robot_geometry_msgs::PoseStamped pose = start;
-      return false;
+      pose.pose.position.x += 0.01 * cos(theta);
+      pose.pose.position.y += 0.01 * sin(theta);
+      plan.push_back(pose);
+      plan.push_back(goal);
+      return true;
     }
 
     // Lấy độ phân giải của costmap

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

@@ -398,7 +398,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
   else if (!ret_nav_)
   {
     traj = nav_algorithm_->calculator(pose, velocity);
-
     if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
     {
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
@@ -406,9 +405,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
         traj = dkpl_.front()->docking_nav_->calculator(pose, velocity);
       }
     }
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }
   else
+  {
     traj = rotate_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;

src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h

@@ -89,7 +89,7 @@ public:
 
 private:
   /**
-   * @brief Resolve library path (handle relative paths, search in search_paths)
+   * @brief Resolve library path (absolute, then PNKX_NAV_CORE_LIBRARY_PATH, search_paths_, cwd)
    * @param library_path Path from config (may be relative or absolute)
    * @return Resolved absolute path, or empty if not found
    */

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -98,7 +98,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
       }
     }
   }
-
   // Try LD_LIBRARY_PATH as fallback
   const char* ld_path = std::getenv("LD_LIBRARY_PATH");
   if (ld_path) {
@@ -199,21 +198,27 @@ std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_pa
     return "";
   }
 
-  // If relative path, search in search_paths (build directory is already added)
+  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
-  std::string build_dir = getBuildDirectory();
+  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
-  if (!build_dir.empty()) {
+  if (nav_lib_path_env) {
-    // First try in build directory
-    // Add .so extension if not present
     std::string lib_path_with_ext = library_path;
     if (lib_path_with_ext.find(".so") == std::string::npos) {
       lib_path_with_ext += ".so";
     }
-    std::filesystem::path full_path = std::filesystem::path(build_dir) / lib_path_with_ext;
+    std::string nav_lib_paths(nav_lib_path_env);
-    if (std::filesystem::exists(full_path)) {
+    std::stringstream ss(nav_lib_paths);
-      try {
+    std::string base_dir;
-        return std::filesystem::canonical(full_path).string();
+    while (std::getline(ss, base_dir, ':')) {
-      } catch (...) {
+      if (base_dir.empty()) {
-        return full_path.string();
+        continue;
+      }
+      std::filesystem::path full_path = std::filesystem::path(base_dir) / lib_path_with_ext;
+      if (std::filesystem::exists(full_path)) {
+        try {
+          return std::filesystem::canonical(full_path).string();
+        } catch (...) {
+          return full_path.string();
+        }
       }
     }
   }

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

@@ -271,6 +271,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(global_planner);
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, global_planner, boost::dll::load_mode::append_decorations);
 
@@ -314,6 +315,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(local_planner);
+      robot::log_info("[%s:%d]\n INFO: local_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       controller_loader_ =
           boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
               path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -401,7 +403,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(base_global_planner);
-
+      robot::log_info("[%s:%d]\n INFO: global_planner library path: %s", __FILE__, __LINE__, path_file_so.c_str());
       auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
           path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
       auto new_planner = new_loader();
@@ -2005,6 +2007,7 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
         std::string behavior_name = behavior["name"].as<std::string>();
         robot::PluginLoaderHelper loader;
         std::string path_file_so = loader.findLibraryPath(behavior_type);
+        robot::log_info("Loading recovery behavior '%s' of type '%s' from '%s'", behavior_name.c_str(), behavior_type.c_str(), path_file_so.c_str());
         // Load the factory function from the shared library
         recovery_loaders_[behavior_name] = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
             path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
@@ -2864,7 +2867,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         robot::log_debug("Goal reached!");
         resetState();
-        swapPlanner(default_config_.base_global_planner);
+        // swapPlanner(default_config_.base_global_planner);
         // disable the planner thread
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         runPlanner_ = false;
@@ -3070,7 +3073,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     {
       robot::log_debug("All recovery behaviors have failed, locking the planner and disabling it.");
       resetState();
-      swapPlanner(default_config_.base_global_planner);
+      // swapPlanner(default_config_.base_global_planner);
 
       // disable the planner thread
       boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);