From c0ceccf32c4b764be092bdb77d4f42fbf7d7223e Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 6 Feb 2026 15:54:35 +0700 Subject: [PATCH] update --- CMakeLists.txt | 6 +- config/pnkx_local_planner_params.yaml | 11 ++-- .../diff/diff_predictive_trajectory.h | 9 +++ .../src/diff/diff_go_straight.cpp | 7 ++- .../src/diff/diff_predictive_trajectory.cpp | 56 +++++++++++++++++-- .../src/two_points_planner.cpp | 47 +++++++--------- .../src/pnkx_go_straight_local_planner.cpp | 7 ++- .../src/pnkx_rotate_local_planner.cpp | 2 +- .../Cores/move_base_core/CMakeLists.txt | 7 ++- .../include/move_base_core/navigation.h | 8 +-- .../Cores/move_base_core/package.xml | 2 + 11 files changed, 107 insertions(+), 55 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 68ea21d..478930f 100644 --- a/CMakeLists.txt +++ b/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) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3) -endif() +# if (NOT TARGET tf3) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3) +# endif() if (NOT TARGET robot_time) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time) diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index ca8748d..c422931 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/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) - min_journey_squared: 0.2 # 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) + lookahead_time: 2.0 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 1.0 # Minimum 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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index b223179..3232788 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 5a1ff43..23ce2c8 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/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; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 7f8690e..00bf276 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/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) { diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index d6f9a79..f3829bf 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/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 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) - || 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__); - return false; - } + // 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)); + // 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)) + // { + // robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__); + // 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); - tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, - goal.pose.orientation.z, goal.pose.orientation.w); - double goal_theta = tf3::getYaw(goal_quat); + theta = std::atan2(dy, dx); + // tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, + // goal.pose.orientation.z, goal.pose.orientation.w); + // 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(i) / num_points; - robot_geometry_msgs::PoseStamped pose; - pose.header.stamp = plan_time; - pose.header.frame_id = costmap_robot_->getGlobalFrameID(); - pose.pose.position.x = start.pose.position.x + fraction * dx; - pose.pose.position.y = start.pose.position.y + fraction * dy; - pose.pose.position.z = start.pose.position.z; + robot_nav_2d_msgs::Pose2DStamped pose2d; + pose2d.header.stamp = plan_time; + pose2d.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose2d.pose.x = start.pose.position.x + fraction * dx; + pose2d.pose.y = start.pose.position.y + fraction * dy; + pose2d.pose.theta = theta; // Nội suy hướng - tf3::Quaternion interpolated_quat; - 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(); - + robot_geometry_msgs::PoseStamped pose = robot_nav_2d_utils::pose2DToPoseStamped(pose2d); plan.push_back(pose); } plan.push_back(goal); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp index 612169b..c07c1c2 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp +++ b/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(nh_algorithm, algorithm_nav_name, tf, costmap_robot_, traj_generator_); + nav_algorithm_->initialize(parent_, 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; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp index 5e7272c..429d802 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp +++ b/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; diff --git a/src/Navigations/Cores/move_base_core/CMakeLists.txt b/src/Navigations/Cores/move_base_core/CMakeLists.txt index 65b0efc..043b7d2 100644 --- a/src/Navigations/Cores/move_base_core/CMakeLists.txt +++ b/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( diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index 824fc2f..49dbcb8 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -18,6 +18,7 @@ #include #include #include +#include // tf #include @@ -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); - - tf3::Quaternion q; - q.setRPY(0, 0, pose.theta); - goal.pose.orientation = tf3::toMsg(q); + goal.pose.orientation = robot_nav_2d_utils::pose2DToPose(pose).orientation; 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); } diff --git a/src/Navigations/Cores/move_base_core/package.xml b/src/Navigations/Cores/move_base_core/package.xml index a8efb81..3df053e 100644 --- a/src/Navigations/Cores/move_base_core/package.xml +++ b/src/Navigations/Cores/move_base_core/package.xml @@ -29,6 +29,7 @@ robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs + robot_nav_2d_utils robot_nav_msgs robot_sensor_msgs robot_map_msgs @@ -38,6 +39,7 @@ robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs + robot_nav_2d_utils robot_nav_msgs robot_sensor_msgs robot_map_msgs