update custom

.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.05
-    yaw_goal_tolerance: 0.05
-
-    # 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

config/maker_sources.yaml

@@ -37,7 +37,7 @@ charger:
     - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
 
   charger:
-    maker_goal_frame: charger
+    maker_goal_frame: charger_goal
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60

config/pnkx_local_planner_params.yaml

@@ -48,7 +48,7 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.15    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
   max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity

src/APIs/c_api/src/nav_c_api.cpp

@@ -572,6 +572,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
     linear.x = linear_x;
     linear.y = linear_y;
     linear.z = linear_z;
+    robot::log_info("setTwistLinear %f", linear.x);
     return nav_ptr->setTwistLinear(linear);
   }
   catch (...)

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -117,9 +117,7 @@ namespace mkt_algorithm
        * @return lookahead point
        */
       std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-      getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, 
+      getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
-                        const double &lookahead_dist, 
-                        robot_nav_2d_msgs::Path2D& global_plan);
 
       /**
        * @brief Prune global plan

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -279,7 +279,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
                                                         const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                                                         double &x_direction, double &y_direction, double &theta_direction)
 {
-  // robot::log_error("DEBUG STEP 2.0");
   if (!initialized_)
   {
     robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
@@ -289,7 +288,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
   {
     last_actuator_update_ = robot::Time::now();
   }
-  // robot::log_error("DEBUG STEP 3.0");
+
   std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
   if (footprint.size() > 1)
   {
@@ -312,14 +311,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
     this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
   }
-  // robot::log_error("DEBUG STEP 4.0");
+
   if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
   {
     robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
     return false;
   }
   this->getParams();
-  // robot::log_error("DEBUG STEP 5.0");
+
   frame_id_path_ = global_plan.header.frame_id;
   goal_ = goal;
   global_plan_ = global_plan;
@@ -330,14 +329,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__);
     return false;
   }
-  // robot::log_error("DEBUG STEP 6.0");
+
   double S = std::numeric_limits<double>::infinity();
   S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0,
                costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0);
   const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_;
   S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S);
   compute_plan_.poses.clear();
-  // robot::log_error("DEBUG STEP 7.0");
+
   if ((unsigned int)global_plan_.poses.size() == 2)
   {
     double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x;
@@ -361,7 +360,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       return false;
     }
   }
-  // robot::log_error("DEBUG STEP 8.0");
 
   double lookahead_dist = this->getLookAheadDistance(velocity);
   transform_plan_.poses.clear();
@@ -370,7 +368,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
-  // robot::log_error("DEBUG STEP 9.0");
   const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
   if(fabs(carrot_pose.pose.y) > 0.2)
   {
@@ -381,15 +378,14 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       return false;
     } 
   }  
-  // robot::log_error("DEBUG STEP 10.0");
+
   x_direction = x_direction_;
   y_direction = y_direction_ = 0;
   theta_direction = theta_direction_;
-  // robot::log_error("DEBUG STATUS : %x, %x", (unsigned int)(compute_plan_.poses.size() > 1), journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution());
+
   if ((unsigned int)(compute_plan_.poses.size() > 1) && 
       journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution())
   {
-    // robot::log_error("DEBUG STEP 10.1");
     const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
     int index;
     for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
@@ -408,46 +404,44 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     }
     
   }
-  else
+  else if(compute_plan_.poses.size() == 1)
   {
-    // robot::log_error("DEBUG STEP 11.1");
     try
     {
+      // auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      // auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      // double distance_it = 0;
+      // for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
+      // {
+      //   double dx = it->pose.x - carrot_pose_it->pose.x;
+      //   double dy = it->pose.y - carrot_pose_it->pose.y;
+      //   distance_it += std::hypot(dx, dy);
+      //   if (distance_it > costmap_robot_->getCostmap()->getResolution())
+      //   {
+      //     prev_carrot_pose_it = it;
+      //     break;
+      //   }
+      // }
 
-      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
+      // robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
-      // robot::log_error("DEBUG STEP 11.2");
+      //                                 ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
-      auto prev_carrot_pose_it = transform_plan_.poses.begin();
+      //                                 : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
-      // robot::log_error("DEBUG STEP 11.2.1 carrot_pose_it: %d", (int)std::distance(transform_plan_.poses.begin(), carrot_pose_it));
-      double distance_it = 0;
 
-      auto it = carrot_pose_it == transform_plan_.poses.begin() ? transform_plan_.poses.end() : carrot_pose_it - 1;
+      // robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
-      for ( ; it != transform_plan_.poses.begin(); --it)
-      {
-        double dx = it->pose.x - carrot_pose_it->pose.x;
-        double dy = it->pose.y - carrot_pose_it->pose.y;
-        distance_it += std::hypot(dx, dy);
-        if (distance_it > costmap_robot_->getCostmap()->getResolution())
-        {
-          prev_carrot_pose_it = it;
-          break;
-        }
-      }
-      // robot::log_error("DEBUG STEP 11.3");
-
-      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
-                                      ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
-                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
-
-      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
-      // robot::log_error("DEBUG STEP 11.4");
       // teb_local_planner::PoseSE2 start_pose(front);
       // teb_local_planner::PoseSE2 goal_pose(back);
       // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
-      const double dir_path = 0.0;
+
+      
+      auto goal_pose = compute_plan_.poses.front().pose;
+      auto start_pose = pose.pose;
+      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
+      double dir_path = cos(fabs(angle_path - goal_pose.theta));
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
-      // robot::log_error("DEBUG STEP 11.5");
+      else
+        x_direction = 0.0;
     }
     catch (std::exception &e)
     {
@@ -455,7 +449,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
       return false;
     }
   }
-  // robot::log_error("DEBUG STEP 11.0");
+
   x_direction_ = x_direction;
   y_direction_ = y_direction;
   theta_direction_ = theta_direction;
@@ -790,7 +784,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
       const auto& p = global_plan.poses[i];
       const auto& dx = p.pose.x - p1.pose.x;
       const auto& dy = p.pose.y - p1.pose.y;
-      if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
+      if(std::hypot(dx, dy) > costmap_robot_->getCostmap()->getResolution())
       {
         if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
           continue;
@@ -801,9 +795,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   }
   
   // Whether we should rotate robot to rough path heading
-  if(sign_x < 0.0)
+  // if(sign_x < 0.0)
-    path_angle += std::copysign(M_PI, path_angle) * (-1.0);
+  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
-  angle_to_path = path_angle;
+  // angle_to_path = path_angle;
+  angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
   double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
   // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
   double heading_rotate = rotate_to_heading_min_angle_;
@@ -1086,8 +1081,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
 }
 
 std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, 
+mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
-  const double &lookahead_dist, robot_nav_2d_msgs::Path2D& global_plan)
 {
   if (global_plan.poses.empty())
     throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
@@ -1124,104 +1118,9 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
   if (goal_pose_it == global_plan.poses.end())
     goal_pose_it = std::prev(global_plan.poses.end());
 
-  // --- Final safety check ---
-  if (goal_pose_it < global_plan.poses.begin() || goal_pose_it >= global_plan.poses.end())
-  {
-    // fallback cuối cùng
-    goal_pose_it = std::prev(global_plan.poses.end());
-  }
-
   return goal_pose_it;
 }
 
-// std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
-// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(
-//     const robot_nav_2d_msgs::Twist2D &velocity,
-//     const double &lookahead_dist,
-//     robot_nav_2d_msgs::Path2D &global_plan)
-// {
-//   auto &poses = global_plan.poses;
-
-//   // --- Guard ---
-//   if (poses.empty())
-//     throw robot_nav_core2::PlannerTFException("The global plan is empty.");
-
-//   if (poses.size() == 1)
-//     return poses.begin();
-
-//   if (poses.size() == 2)
-//     return std::prev(poses.end());
-
-//   // --- Init ---
-//   size_t goal_index = poses.size() - 1;
-
-//   const auto &p0 = poses[0].pose;
-//   const auto &p1 = poses[1].pose;
-
-//   double start_angle = atan2(p1.y - p0.y, p1.x - p0.x);
-//   double turn_threshold = M_PI_2 * 0.6;
-
-//   // --- Detect turn ---
-//   for (size_t i = 1; i < poses.size(); ++i)
-//   {
-//     const auto &a = poses[i - 1].pose;
-//     const auto &b = poses[i].pose;
-
-//     double current_angle = atan2(b.y - a.y, b.x - a.x);
-//     double delta = angles::normalize_angle(current_angle - start_angle);
-
-//     goal_index = i;
-
-//     if (fabs(delta) >= turn_threshold)
-//       break;
-//   }
-
-//   // --- Clamp goal_index ---
-//   if (goal_index >= poses.size())
-//     goal_index = poses.size() - 1;
-
-//   // --- Safe search range ---
-//   auto search_begin = poses.begin();
-
-//   // ❗ IMPORTANT: +1 để iterator hợp lệ
-//   auto search_end = poses.begin() + goal_index + 1;
-
-//   if (search_end > poses.end())
-//     search_end = poses.end();
-
-//   // --- Find lookahead ---
-//   double accumulated_dist = 0.0;
-//   auto goal_pose_it = search_begin;
-//   for (auto it = search_begin + 1; it != search_end; ++it)
-//   {
-//     double dx = it->pose.x - std::prev(it)->pose.x;
-//     double dy = it->pose.y - std::prev(it)->pose.y;
-//     accumulated_dist += std::hypot(dx, dy);
-//     if (accumulated_dist >= lookahead_dist)
-//     {
-//       goal_pose_it = it;
-//       break;
-//     }
-//   }
-
-//   // --- Fallback an toàn ---
-//   if (goal_pose_it == search_begin)
-//   {
-//     goal_pose_it = std::prev(search_end); // safe vì search_end > search_begin
-//   }
-
-//   // --- Final safety check ---
-//   if (goal_pose_it < poses.begin() || goal_pose_it >= poses.end())
-//   {
-//     // fallback cuối cùng
-//     return std::prev(poses.end());
-//   }
-
-//   return goal_pose_it;
-// }
-
-
-
 bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot)
 {
   if (global_plan.poses.empty())

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

@@ -190,14 +190,9 @@ namespace two_points_planner
     }
     else 
     { 
-      auto goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);  // hoặc start.theta
+      robot_geometry_msgs::PoseStamped pose = start;
-      robot_geometry_msgs::PoseStamped pose = goal;
+      pose.pose.position.x += resolution * cos(theta);
-      pose.pose.position.x += resolution * std::cos(goal_2d.pose.theta);
+      pose.pose.position.y += resolution * sin(theta);
-      pose.pose.position.y += resolution * std::sin(goal_2d.pose.theta);
-      plan.push_back(pose);
-      pose = goal;
-      pose.pose.position.x -= resolution * std::cos(goal_2d.pose.theta);
-      pose.pose.position.y -= resolution * std::sin(goal_2d.pose.theta);
       plan.push_back(pose);
       plan.push_back(goal);
       return true;

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h

@@ -31,8 +31,6 @@ namespace pnkx_local_planner
     void initialize(robot::NodeHandle &parent, const std::string &name,
                     TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
-    void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
-
     /**
      * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *

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

@@ -240,12 +240,6 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &n
   }
 }
 
-void pnkx_local_planner::PNKXDockingLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path)
-{
-  this->reset();
-  pnkx_local_planner::PNKXLocalPlanner::setPlan(path);
-}
-
 void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 {
   robot::log_info_at(__FILE__, __LINE__, "New Docking Goal Received.");
@@ -261,12 +255,11 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
 
   parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-  // nh_algorithm.setParam("allow_rotate", false);
+  nh_algorithm.setParam("allow_rotate", false);
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {  
-  // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0");
   this->getParams(planner_nh_);
   if (update_costmap_before_planning_)
   {
@@ -280,12 +273,11 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
-  // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 2.0");
+
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
                              local_goal_pose = this->transformPoseToLocal(goal_pose_);
-  // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 3.0");
   if (start_docking_)
   {
     local_goal_pose = goal_pose_;
@@ -293,14 +285,6 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
   try
   {
-    // robot::log_error("local_start_pose (%f, %f, %f)", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
-    // robot::log_error("local_goal_pose (%f, %f, %f)", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
-    // for(size_t i = 0; i < global_plan_.poses.size(); i++)
-    // {
-    //   robot::log_error("global_plan_ [%zu] (%f, %f, %f)", i, global_plan_.poses[i].pose.x, global_plan_.poses[i].pose.y, global_plan_.poses[i].pose.theta);
-    // }
-    // robot::log_error("costmap_robot_->getGlobalFrameID(): %s", costmap_robot_->getGlobalFrameID().c_str());
-    
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
@@ -310,10 +294,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   }
 
   double x_direction, y_direction, theta_direction;
-  if (!ret_nav_ && !dkpl_.empty())
+  if (!ret_nav_)
-  {
-    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 4.0");
-    if(nav_algorithm_)
   {
     if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
@@ -322,10 +303,9 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     }
     // else
     //   ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
-      // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 5.0");
+
     if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
     {
-        // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 6.0");
       this->lock();
       robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
       traj_generator_->setTwistLinear(linear);
@@ -344,7 +324,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 
       parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
       parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
-        // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 7.0");
+
       if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
       {
 
@@ -362,9 +342,6 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
           robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
         }
       }
-        
-        // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 8.0");
-      }
     }
   }
   else
@@ -388,26 +365,14 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
 robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
                                                                                                  const robot_nav_2d_msgs::Twist2D &velocity)
 {
-  // // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
+  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
-  // for(size_t i = 0; i < global_plan_.poses.size(); i++)
-  // {
-  //   robot::log_error("computeVelocityCommands global_plan_ [%d] (%f, %f, %f)", i, global_plan_.poses[i].pose.x, global_plan_.poses[i].pose.y, global_plan_.poses[i].pose.theta);
-  // }
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
-  cmd_vel.header.stamp = robot::Time::now();
-  cmd_vel.velocity.x = 0.0;
-  cmd_vel.velocity.y = 0.0;
-  cmd_vel.velocity.theta = 0.0;
   try
   {
-    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0");
+    if (global_plan_.poses.empty())
-    if ( global_plan_.poses.empty())
       return cmd_vel;
-    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.11");
     this->prepare(pose, velocity);
-    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 2");
     cmd_vel = this->ScoreAlgorithm(pose, velocity);
-    // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 3");
     return cmd_vel;
   }
   catch (const robot_nav_core2::PlannerException &e)
@@ -454,11 +419,6 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::S
 
 bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
-  // if(global_plan_.poses.size() <= 2) 
-  // {
-  //   robot::log_error("DEBUG GOAL");
-  //   return true;
-  // }
   if (goal_pose_.header.frame_id == "")
   {
     robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
@@ -493,8 +453,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     }
   }
 
-  // if (ret_nav_ && !ret_angle_ && !dock_ok)
+  if (ret_nav_ && !ret_angle_ && !dock_ok)
-  if (ret_nav_ && !ret_angle_)
   {
     double delta_orient =
         fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
@@ -551,7 +510,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
                 dkpl_.front()->getLocalPath(local_pose, local_goal, path);
                 this->setPlan(robot_nav_2d_utils::pathToPath(path));
                 this->setGoalPose(local_goal);
-                robot::log_debug(__FILE__, __LINE__, "DEBUG 1 size path: %d", (int)path.poses.size());
               }
             }
             catch (const std::exception &e)
@@ -576,7 +534,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
                 path.poses.push_back(local_goal);
                 this->setPlan(path);
                 this->setGoalPose(local_goal);
-                robot::log_debug(__FILE__, __LINE__, "DEBUG 2 size path: %d", (int)path.poses.size());
               }
             }
             catch (const std::exception &e)
@@ -585,7 +542,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             }
           }
         }
-        
       }
       else
       {

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
   }
   // Try to read from NodeHandle
   std::string library_path;
+  robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
   if (nh_.hasParam(param_path)) {
     nh_.getParam(param_path, library_path, std::string(""));
     if (!library_path.empty()) {
@@ -343,7 +344,7 @@ std::string PluginLoaderHelper::getBuildDirectory()
 std::string PluginLoaderHelper::getWorkspacePath()
 {
   // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
-  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
+  const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
   if (workspace_path && std::filesystem::exists(workspace_path)) {
     return std::string(workspace_path);
   }

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -146,7 +146,7 @@ namespace robot_nav_core
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) {return;}
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
 
     /**
      * @brief  Constructs the local planner

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

@@ -103,7 +103,7 @@ namespace robot_nav_core2
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) {return;}
+    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
 
     /**
      * @brief Compute the best command given the current pose, velocity and goal

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

@@ -976,7 +976,6 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
-
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1117,6 +1116,7 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
+
     as_->processGoal(action_goal);
   }
   catch (const std::exception &e)
@@ -2495,7 +2495,6 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
     // the real work on pursuing a goal is done here
     bool done = executeCycle(goal);
 
-    // robot::log_debug("[MoveBase] Completed an execution cycle: ̀done=%s", done ? "true" : "false");
     // if we're done, then we'll return from execute
     if (done)
       return;
@@ -2715,7 +2714,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
     controller_plan_ = latest_plan_;
     latest_plan_ = temp_plan;
     lock.unlock();
-    robot::log_debug("pointers swapped!: %d", controller_plan_->size());
+    robot::log_debug("pointers swapped!");
 
     if (!tc_->setPlan(*controller_plan_))
     {
@@ -2736,7 +2735,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       // as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
       return true;
     }
-    // robot::log_debug("pointers swapped2!");
 
     // make sure to reset recovery_index_ since we were able to find a valid plan
     if (recovery_trigger_ == PLANNING_R)
@@ -2748,9 +2746,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
   if (cancel_ctr_ && tc_)
   {
     robot_geometry_msgs::Vector3 linear;
-    linear.x = 0.0;
-    linear.y = 0.0;
-    linear.z = 0.0;
     // ROS_INFO_THROTTLE(1.0,"MoveTo is Canling ....");
     tc_->setTwistLinear(linear);
     try
@@ -2920,12 +2915,10 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
 
     {
       boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
-      // robot::log_error("paused_: %s", paused_ ? "true" : "false");
       if (!paused_)
       {
         if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
         {
-          // robot::log_debug("Got a valid velocity command from the local planner start!");
           robot_nav_msgs::Path path;
           tc_->getPlan(path.poses);
           if (!path.poses.empty())
@@ -2951,7 +2944,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
           }
           if (recovery_trigger_ == CONTROLLING_R)
             recovery_index_ = 0;
-          // robot::log_debug("Got a valid velocity command from the local planner end!");
         }
         else
         {