update

CMakeLists.txt

@@ -30,9 +30,9 @@ message(STATUS "========================================")
 # Build các packages theo thứ tự phụ thuộc
 # 1. Core libraries (header-only hoặc base libraries)
 
-if (NOT TARGET tf3)
+# if (NOT TARGET tf3)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3)
+#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3)
-endif()
+# endif()
 
 if (NOT TARGET robot_time)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time)

config/pnkx_local_planner_params.yaml

@@ -74,7 +74,6 @@ LimitedAccelGenerator:
 
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
   xy_local_goal_tolerance: 0.02
   angle_threshold: 0.47
   index_samples: 60
@@ -110,7 +109,6 @@ MKTAlgorithmDiffPredictiveTrajectory:
 
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
   xy_local_goal_tolerance: 0.02
   angle_threshold: 0.8
   index_samples: 60
@@ -121,11 +119,11 @@ MKTAlgorithmDiffGoStraight:
   # only when false:
   lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 1.0                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  lookahead_time: 2.0                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  min_journey_squared: 1.0                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 1.0                    # Maximum squared journey to consider for goal (default: 0.2) 
   max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
 
   # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
@@ -146,7 +144,6 @@ MKTAlgorithmDiffGoStraight:
 
 MKTAlgorithmDiffRotateToGoal:
   library_path: libmkt_algorithm_diff
-  avoid_obstacles: false
   xy_local_goal_tolerance: 0.02
   angle_threshold: 0.47
   index_samples: 60

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

@@ -177,6 +177,15 @@ namespace mkt_algorithm
         const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
         const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd);
 
+      /**
+       * @brief Generate trajectory
+       * @param path
+       * @param sign_x
+       * @return trajectory
+       */
+       robot_nav_2d_msgs::Path2D generateParallelPath(
+        const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
+
       /**
        * @brief Generate Hermite trajectory
        * @param pose

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

@@ -9,6 +9,8 @@ void mkt_algorithm::diff::GoStraight::initialize(
   {
     nh_ = nh;
     nh_priv_ = robot::NodeHandle(nh, name);
+    robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
+    robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
     name_ = name;
     tf_ = tf;
     traj_ = traj;
@@ -136,11 +138,14 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   }
   else
   {
+    // robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f", 
+    //   journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
     if(fabs(carrot_pose.pose.y) > 0.2)
     {
       lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
     }
     robot_nav_2d_msgs::Twist2D drive_target;
+    robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", transformed_plan.poses.front().pose.y, transformed_plan.poses.front().pose.theta);
     transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
     carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
 
@@ -200,7 +205,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
       fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
     break;
   }
-
+  drive_cmd.theta = std::clamp(drive_cmd.theta, -fabs(0.02), fabs(0.02));
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;

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

@@ -10,6 +10,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
   {
     nh_ = nh;
     nh_priv_ = robot::NodeHandle(nh, name);
+    robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
+    robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
     name_ = name;
     tf_ = tf;
     traj_ = traj;
@@ -952,7 +954,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob
     lookahead_dist = fabs(velocity.x) * lookahead_time_;
     lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_);
   }
-
   return lookahead_dist;
 }
 
@@ -1178,21 +1179,24 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     drive_cmd.theta = 0.0;
     return robot_nav_2d_msgs::Path2D();
   }
+  robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", path.poses.front().pose.y, path.poses.front().pose.theta);
   double max_kappa = calculateMaxKappa(path);
   const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
   drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
   drive_cmd.theta = max_vel_theta_;
-  if (max_kappa <= straight_threshold)
+
+  if (max_kappa <= straight_threshold) // nếu đường thẳng
   { 
     if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_)
     {
       drive_cmd.theta = 0.05;
-      // return path;
     }
-    return generateHermiteTrajectory(path.poses.back(), sign_x);
+    if(fabs(path.poses.front().pose.y) > 0.02)
-    
+      return generateHermiteTrajectory(path.poses.back(), sign_x);
+    else
+      return generateParallelPath(path, sign_x);
   }
-  else
+  else // nếu đường cong
   {
     if(fabs(drive_cmd.x) < min_speed_xy_)
       drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
@@ -1200,6 +1204,46 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   }
 }
 
+robot_nav_2d_msgs::Path2D generateParallelPath(
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
+{
+  robot_nav_2d_msgs::Path2D parallel_path;
+  parallel_path.header = path.header;
+  int N = path.poses.size();
+  if (N < 2) return parallel_path;
+
+  double offset_y = path.poses.front().pose.y;
+
+  parallel_path.poses.resize(N);
+  for (int i = 0; i < N; ++i)
+  {
+    const auto& p = path.poses[i].pose;
+
+    double dx, dy;
+    if (i == 0) {
+      dx = path.poses[i+1].pose.x - p.x;
+      dy = path.poses[i+1].pose.y - p.y;
+    } else if (i == N-1) {
+      dx = p.x - path.poses[i-1].pose.x;
+      dy = p.y - path.poses[i-1].pose.y;
+    } else {
+      dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
+      dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
+    }
+
+    double theta = atan2(dy, dx);
+    double x_off = p.x - offset_y * sin(theta);
+    double y_off = p.y + offset_y * cos(theta);
+
+    parallel_path.poses[i].pose.x = x_off;
+    parallel_path.poses[i].pose.y = y_off;
+    parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
+    parallel_path.poses[i].header = path.poses[i].header;
+  }
+
+  return parallel_path;
+}
+
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
   const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
 {

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

@@ -153,14 +153,14 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
       return false;
     }
-    unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
+    // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
-    unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
+    // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
-    if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-      || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
+    //   || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
-    {
+    // {
-      robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
+    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
-      return false;
+    //   return false;
-    }
+    // }
     robot::Time plan_time = robot::Time::now();
     // 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;
@@ -169,10 +169,11 @@ namespace two_points_planner
     double theta;
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
-      theta = atan2(dy, dx);
+      theta = std::atan2(dy, dx);
-      tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, 
+      // tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, 
-                                goal.pose.orientation.z, goal.pose.orientation.w);
+      //                           goal.pose.orientation.z, goal.pose.orientation.w);
-      double goal_theta = tf3::getYaw(goal_quat);
+      // double goal_theta = tf3::getYaw(goal_quat);
+      double goal_theta = robot_nav_2d_utils::poseStampedToPose2D(goal).pose.theta;
       if(cos(goal_theta - theta) < 0) theta += M_PI;
     }
     else 
@@ -192,23 +193,15 @@ namespace two_points_planner
     {
       double fraction = static_cast<double>(i) / num_points;
 
-      robot_geometry_msgs::PoseStamped pose;
+      robot_nav_2d_msgs::Pose2DStamped pose2d;
-      pose.header.stamp = plan_time;
+      pose2d.header.stamp = plan_time;
-      pose.header.frame_id = costmap_robot_->getGlobalFrameID();
+      pose2d.header.frame_id = costmap_robot_->getGlobalFrameID();
-      pose.pose.position.x = start.pose.position.x + fraction * dx;
+      pose2d.pose.x = start.pose.position.x + fraction * dx;
-      pose.pose.position.y = start.pose.position.y + fraction * dy;
+      pose2d.pose.y = start.pose.position.y + fraction * dy;
-      pose.pose.position.z = start.pose.position.z;
+      pose2d.pose.theta = theta;
 
       // Nội suy hướng
-      tf3::Quaternion interpolated_quat;
+      robot_geometry_msgs::PoseStamped pose = robot_nav_2d_utils::pose2DToPoseStamped(pose2d);
-      interpolated_quat.setRPY(0, 0, theta);
-
-      // Chuyển đổi trực tiếp từ tf3::Quaternion sang robot_geometry_msgs::Quaternion
-      pose.pose.orientation.x = interpolated_quat.x();
-      pose.pose.orientation.y = interpolated_quat.y();
-      pose.pose.orientation.z = interpolated_quat.z();
-      pose.pose.orientation.w = interpolated_quat.w();
-
       plan.push_back(pose);
     }
     plan.push_back(goal);

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

@@ -95,8 +95,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_nav_name.c_str());
         exit(1);
       }
-      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
+      nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
-      nav_algorithm_->initialize(nh_algorithm, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception& ex)
     {
@@ -209,6 +208,8 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
   robot_nav_2d_msgs::Twist2D twist;
   mkt_msgs::Trajectory2D traj;
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
+  cmd_vel.header.stamp = robot::Time::now();
+  
   if (ret_nav_)
   {
     local_plan_.poses.clear();
@@ -217,7 +218,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
 
   traj = nav_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_->getGlobalFrameID(), 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/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp

@@ -213,7 +213,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc
 
   traj = nav_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_->getGlobalFrameID(), 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/Navigations/Cores/move_base_core/CMakeLists.txt

@@ -31,6 +31,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_geometry_msgs
     robot_protocol_msgs
     robot_nav_2d_msgs
+    robot_nav_2d_utils
     robot_nav_msgs
     robot_sensor_msgs
   )
@@ -41,17 +42,19 @@ else()
 # Catkin specific configuration
 # ========================================================
   find_package(catkin REQUIRED COMPONENTS         
-    tf3
     robot_time
     robot_geometry_msgs
     robot_protocol_msgs
     robot_nav_2d_msgs
+    robot_nav_2d_utils
   )
+  find_package(tf3)
 
   catkin_package(
     INCLUDE_DIRS include
     # LIBRARIES không cần vì đây là header-only library
-    CATKIN_DEPENDS tf3 robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs
+    CATKIN_DEPENDS robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs
+    DEPENDS tf3
   )
 
   include_directories(

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

@@ -18,6 +18,7 @@
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_protocol_msgs/Order.h>
+#include <robot_nav_2d_utils/conversions.h>
 
 // tf
 #include <tf3/buffer_core.h>
@@ -138,10 +139,7 @@ namespace robot
 			goal.header.stamp = robot::Time::now();
 			goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
 			goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
-
+			goal.pose.orientation = robot_nav_2d_utils::pose2DToPose(pose).orientation;
-			tf3::Quaternion q;
-			q.setRPY(0, 0, pose.theta);
-			goal.pose.orientation = tf3::toMsg(q);
 			return goal;
 		}
 
@@ -161,7 +159,7 @@ namespace robot
 			robot_geometry_msgs::Pose2D pose2d;
 			pose2d.x = pose.pose.position.x;
 			pose2d.y = pose.pose.position.y;
-			// pose2d.theta = tf2::getYaw(pose.pose.orientation);
+			pose2d.theta = robot_nav_2d_utils::poseStampedToPose2D(pose).pose.theta;
 			return offset_goal(pose2d, pose.header.frame_id, offset_distance);
 		}
 

src/Navigations/Cores/move_base_core/package.xml

@@ -29,6 +29,7 @@
   <build_depend>robot_geometry_msgs</build_depend>
   <build_depend>robot_protocol_msgs</build_depend>
   <build_depend>robot_nav_2d_msgs</build_depend>
+  <build_depend>robot_nav_2d_utils</build_depend>
   <build_depend>robot_nav_msgs</build_depend>
   <build_depend>robot_sensor_msgs</build_depend>
   <build_depend>robot_map_msgs</build_depend>
@@ -38,6 +39,7 @@
   <run_depend>robot_geometry_msgs</run_depend>
   <run_depend>robot_protocol_msgs</run_depend>
   <run_depend>robot_nav_2d_msgs</run_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
   <run_depend>robot_nav_msgs</run_depend>
   <run_depend>robot_sensor_msgs</run_depend>
   <run_depend>robot_map_msgs</run_depend>