From a43b714f010327fe37d551f85b52950061721de8 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 5 Feb 2026 14:13:40 +0700 Subject: [PATCH] update 5/2 --- config/move_base_common_params.yaml | 2 +- config/pnkx_local_planner_params.yaml | 103 +++++----- examples/CSharpExample.cs | 8 - .../src/diff/diff_go_straight.cpp | 93 ++++++++- .../src/diff/diff_predictive_trajectory.cpp | 2 +- .../src/two_points_planner.cpp | 16 +- .../src/pnkx_go_straight_local_planner.cpp | 15 +- .../src/pnkx_local_planner.cpp | 1 + .../src/pnkx_rotate_local_planner.cpp | 9 +- .../local_planner_adapter.h | 4 +- .../src/local_planner_adapter.cpp | 125 ++++++------ .../Packages/move_base/CMakeLists.txt | 6 +- .../Packages/move_base/src/move_base.cpp | 181 ++++++++++++++++-- 13 files changed, 409 insertions(+), 156 deletions(-) diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index 02f44d8..73110da 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -4,7 +4,7 @@ controller_frequency: 30.0 # run controller at 15.0 Hz controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan planner_frequency: 0.0 # don't continually replan (only when controller failed) planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s... -max_planning_retries: 5 # ... or after 10 attempts (whichever happens first) +max_planning_retries: 0 # ... or after 10 attempts (whichever happens first) oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s oscillation_distance: 0.5 ### recovery behaviors diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 238e214..b293eea 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -111,76 +111,75 @@ MKTAlgorithmDiffPredictiveTrajectory: MKTAlgorithmDiffGoStraight: library_path: libmkt_algorithm_diff avoid_obstacles: false - xy_local_goal_tolerance: 0.01 - angle_threshold: 0.6 - index_samples: 60 - follow_step_path: true - - # Lookahead - use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) - # 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: 0.325 # The minimum lookahead distance (m) threshold. (default: 0.3) - max_lookahead_dist: 1.5 # 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.3 # Minimum squared journey to consider for goal (default: 0.2) - max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2) - - # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true - use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) - # only when true: - rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) - - # stoped - rot_stopped_velocity: 0.03 - trans_stopped_velocity: 0.06 - - use_regulated_linear_velocity_scaling: false - use_cost_regulated_linear_velocity_scaling: false - -MKTAlgorithmDiffRotateToGoal: - library_path: libmkt_algorithm_diff - avoid_obstacles: false - xy_local_goal_tolerance: 0.01 - angle_threshold: 0.6 + xy_local_goal_tolerance: 0.02 + angle_threshold: 0.8 index_samples: 60 follow_step_path: true # Lookahead use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) # only when false: - lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3) - max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) - lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) - min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) - max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2) + min_lookahead_dist: 1.0 # 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) + 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 use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) # only when true: rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) - angular_decel_zone: 0.1 - + angular_decel_zone: 0.1 + # stoped - rot_stopped_velocity: 0.03 + rot_stopped_velocity: 0.05 trans_stopped_velocity: 0.06 - # Regulated linear velocity scaling - use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + use_final_heading_alignment: true + final_heading_xy_tolerance: 0.1 + final_heading_angle_tolerance: 0.05 + final_heading_min_velocity: 0.05 + final_heading_kp_angular: 2.0 + +MKTAlgorithmDiffRotateToGoal: + library_path: libmkt_algorithm_diff + avoid_obstacles: false + xy_local_goal_tolerance: 0.02 + angle_threshold: 0.47 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) - regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) - - # Inflation cost scaling (Limit velocity by proximity to obstacles) - use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) - inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 - cost_scaling_dist: 0.2 # (default: 0.6) - cost_scaling_gain: 2.0 # (default: 1.0) + 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) + 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 + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + angular_decel_zone: 0.1 + # stoped + rot_stopped_velocity: 0.05 + trans_stopped_velocity: 0.06 + + use_final_heading_alignment: true + final_heading_xy_tolerance: 0.1 + final_heading_angle_tolerance: 0.05 + final_heading_min_velocity: 0.05 + final_heading_kp_angular: 2.0 + GoalChecker: library_path: libmkt_plugins_goal_checker diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs index 2c363c6..5d60a56 100644 --- a/examples/CSharpExample.cs +++ b/examples/CSharpExample.cs @@ -469,14 +469,6 @@ namespace NavigationExample [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_get_local_data(IntPtr handle, ref PlannerDataOutput out_data); - - // ============================================================================ - // Navigation Commands with Order - // ============================================================================ - - - - // ============================================================================ // Helper Methods for String Conversion // ============================================================================ 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 31322b3..5a1ff43 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 @@ -108,16 +108,101 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( { twist = traj_->nextTwist(); } - - drive_cmd.x = sqrt(twist.x * twist.x); - robot_nav_2d_msgs::Path2D transformed_plan = transform_plan_; + double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max)); + robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; if (transformed_plan.poses.empty()) { - robot::log_warning("Transformed plan is empty. Cannot determine a local plan."); + robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); + stopWithAccLimits(pose, velocity, drive_cmd, dt); + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; + return result; + } + double lookahead_dist = getLookAheadDistance(velocity); + auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose); + + // === Final Heading Alignment Check === + double xy_error = 0.0, heading_error = 0.0; + if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) + { + // Use Arc Motion controller for final heading alignment + alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); +#ifdef BUILD_WITH_ROS + ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", + xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); +#endif + } + else + { + 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; + transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target); + carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + + // Normal Pure Pursuit + this->computePurePursuit( + carrot_pose, + drive_target, + velocity, + transformed_plan, + min_approach_linear_velocity_, + sign_x, + dt, + drive_cmd); + } + applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt); + + if (this->nav_stop_) + { + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd, dt)) + return result; + } + else + drive_cmd = {}; + result.velocity = drive_cmd; return result; } + Eigen::VectorXd y(2); + y << drive_cmd.x, drive_cmd.theta; + // Cập nhật lại A nếu dt thay đổi + for (int i = 0; i < n_; ++i) + for (int j = 0; j < n_; ++j) + A(i, j) = (i == j ? 1.0 : 0.0); + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + kf_->update(y, dt, A); + double v_min = min_approach_linear_velocity_; + drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); + drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); + + + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + result.poses.push_back(pose_stamped.pose); + if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && + fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && + fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6) + break; + } + + 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 3af32ea..7f8690e 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 @@ -627,7 +627,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; - robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta); + // robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( 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 ce49201..7313a8d 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 @@ -49,6 +49,8 @@ namespace two_points_planner initialized_ = true; return true; } + robot::log_info("[%s:%d]\n TwoPointsPlanner: Already initialized", __FILE__, __LINE__); + return true; } unsigned char TwoPointsPlanner::computeCircumscribedCost() @@ -104,7 +106,7 @@ namespace two_points_planner { if (!initialized_) { - std::cerr << "TwoPointsPlanner: Global planner is not initialized" << std::endl; + robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__); return false; } @@ -112,14 +114,14 @@ namespace two_points_planner if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() || current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY()) { - printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n", + robot::log_warning("[%s:%d]\n TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.", __FILE__, __LINE__, current_env_width_, current_env_height_, costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY()); do_init = true; } else if (footprint_ != costmap_robot_->getRobotFootprint()) { - printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n"); + robot::log_warning("[%s:%d]\n TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.", __FILE__, __LINE__); do_init = true; } @@ -131,7 +133,7 @@ namespace two_points_planner plan.clear(); plan.push_back(start); - printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n", + robot::log_info("[%s:%d]\n TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)", __FILE__, __LINE__, start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y); unsigned int mx_start, my_start; @@ -144,7 +146,7 @@ namespace two_points_planner goal.pose.position.y, mx_end, my_end)) { - std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl; + 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)); @@ -152,7 +154,7 @@ namespace two_points_planner 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)) { - std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl; + robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__); return false; } robot::Time plan_time = robot::Time::now(); @@ -171,7 +173,7 @@ namespace two_points_planner } else { - std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl; + robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__); return false; } 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 846f693..612169b 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 @@ -51,17 +51,18 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl parent_ = parent; planner_nh_ = robot::NodeHandle(parent_, name); - + robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str()); this->getParams(planner_nh_); // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window std::string traj_generator_name; planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator")); - - robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); + robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); + try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(traj_generator_name); traj_gen_loader_ = boost::dll::import_alias( path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); traj_generator_ = traj_gen_loader_(); @@ -84,7 +85,8 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(algorithm_nav_name); nav_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); nav_algorithm_ = nav_algorithm_loader_(); @@ -106,7 +108,8 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker")); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(goal_checker_name); goal_checker_loader_ = boost::dll::import_alias( path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); goal_checker_ = goal_checker_loader_(); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index ce00ff3..accc3b9 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -58,6 +58,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, parent_ = parent; planner_nh_ = robot::NodeHandle(parent_, name); + robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str()); this->getParams(planner_nh_); std::string traj_generator_name; 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 6e28f67..f298506 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 @@ -55,7 +55,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str()); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(traj_generator_name); traj_gen_loader_ = boost::dll::import_alias( path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); traj_generator_ = traj_gen_loader_(); @@ -77,7 +78,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate")); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name); rotate_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations); rotate_algorithm_ = rotate_algorithm_loader_(); @@ -99,7 +101,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker")); try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(goal_checker_name); goal_checker_loader_ = boost::dll::import_alias( path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); goal_checker_ = goal_checker_loader_(); diff --git a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h index 839f198..0bb1184 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h @@ -197,7 +197,8 @@ namespace robot_nav_core_adapter // Plugin handling boost::function planner_loader_; robot_nav_core2::LocalPlanner::Ptr planner_; - + + bool initialized_; // Pointer Copies TFListenerPtr tf_; @@ -211,7 +212,6 @@ namespace robot_nav_core_adapter robot_nav_core_adapter::NavCoreAdapterConfig last_config_; robot_nav_core_adapter::NavCoreAdapterConfig default_config_; - bool setup_; }; } // namespace robot_nav_core_adapter diff --git a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index 696c49e..eda7585 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -75,87 +75,104 @@ namespace robot_nav_core_adapter */ void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) { - tf_ = createSharedPointerWithNoDelete(tf); - costmap_robot_ = costmap_robot; - costmap_adapter_ = std::make_shared(); - costmap_adapter_->initialize(costmap_robot); - - private_nh_ = robot::NodeHandle("~"); - adapter_nh_ = robot::NodeHandle(private_nh_, name); - - std::string planner_name; - if (adapter_nh_.hasParam("planner_name")) + if (!initialized_) { - adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner")); + tf_ = createSharedPointerWithNoDelete(tf); + costmap_robot_ = costmap_robot; + costmap_adapter_ = std::make_shared(); + costmap_adapter_->initialize(costmap_robot); + + private_nh_ = robot::NodeHandle("~"); + adapter_nh_ = robot::NodeHandle(private_nh_, name); + + std::string planner_name; + if (adapter_nh_.hasParam("planner_name")) + { + adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner")); + } + else + { + planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); + adapter_nh_.setParam("planner_name", planner_name); + } + try + { + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(planner_name); + planner_loader_ = boost::dll::import_alias( + path_file_so, planner_name, boost::dll::load_mode::append_decorations); + planner_ = planner_loader_(); + if (!planner_) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str()); + exit(EXIT_FAILURE); + } + last_config_.planner_name = planner_name; + robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str()); + planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_); + robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str()); + } + catch (const boost::system::system_error &ex) + { + robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what()); + exit(EXIT_FAILURE); + } + catch (const std::exception &ex) + { + robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what()); + exit(EXIT_FAILURE); + } + + initialized_ = true; + robot::log_info("LocalPlannerAdapter initialized successfully"); } else { - planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); - adapter_nh_.setParam("planner_name", planner_name); - } - try - { - robot::PluginLoaderHelper loader; - std::string path_file_so = loader.findLibraryPath(planner_name); - planner_loader_ = boost::dll::import_alias( - path_file_so, planner_name, boost::dll::load_mode::append_decorations); - planner_ = planner_loader_(); - if (!planner_) - { - robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str()); - exit(EXIT_FAILURE); - } - robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str()); - planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_); - robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str()); - } - catch (const boost::system::system_error &ex) - { - robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what()); - exit(EXIT_FAILURE); - } - catch (const std::exception &ex) - { - robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what()); - exit(EXIT_FAILURE); + robot::log_info("LocalPlannerAdapter already initialized"); } } bool LocalPlannerAdapter::swapPlanner(std::string planner_name) { boost::recursive_mutex::scoped_lock l(configuration_mutex_); - if (!setup_) + if (!initialized_) { return false; } - if (planner_name != last_config_.planner_name) { - robot_nav_core2::LocalPlanner::Ptr new_planner; - + robot_nav_core2::LocalPlanner::Ptr old_planner = planner_; + auto old_loader = planner_loader_; try { - // Tạo planner mới - std::string path_file_so; - planner_loader_ = boost::dll::import_alias( + boost::unique_lock lock(configuration_mutex_); + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(planner_name); + auto new_loader = boost::dll::import_alias( path_file_so, planner_name, boost::dll::load_mode::append_decorations); - new_planner = planner_loader_(); + + auto new_planner = new_loader(); if (!new_planner) { - std::cerr << "Failed to load planner " << planner_name << std::endl; - exit(EXIT_FAILURE); + robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str()); + throw std::runtime_error("Failed to load planner " + planner_name); } - new_planner->initialize(adapter_nh_, planner_name, tf_, costmap_robot_); - if (planner_) - planner_.reset(); + new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_); planner_ = new_planner; - + planner_loader_ = new_loader; last_config_.planner_name = planner_name; robot::log_info("Loaded new planner: %s", planner_name.c_str()); + + // Release old planner while old loader is still alive. + old_planner.reset(); + old_loader = boost::function(); + lock.unlock(); } catch (const std::exception &ex) { - robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what()); + robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: %s", planner_name.c_str(), ex.what()); + planner_ = old_planner; + planner_loader_ = old_loader; return false; } } diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index bb6500e..ba6bd96 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -4,7 +4,7 @@ project(move_base VERSION 1.0.0 LANGUAGES CXX) if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) set(BUILDING_WITH_CATKIN TRUE) message(STATUS "Building move_base with Catkin") - + else() set(BUILDING_WITH_CATKIN FALSE) message(STATUS "Building move_base with Standalone CMake") @@ -90,6 +90,10 @@ add_library(${PROJECT_NAME} SHARED if(BUILDING_WITH_CATKIN) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_compile_definitions(${PROJECT_NAME} + PUBLIC BUILD_WITH_ROS + ) + target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index f261959..585a34d 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -4,6 +4,9 @@ * * Author: Kevin Lee *********************************************************************/ +#ifdef BUILD_WITH_ROS +#include +#endif #include #include #include @@ -231,6 +234,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); throw std::runtime_error("Failed to load global planner " + global_planner); } + last_config_.base_global_planner = global_planner; + default_config_.base_global_planner = global_planner; if (planner_->initialize(global_planner, planner_costmap_robot_)) robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__); else @@ -333,7 +338,54 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) void move_base::MoveBase::swapPlanner(std::string base_global_planner) { + if (base_global_planner != last_config_.base_global_planner) + { + robot_nav_core::BaseGlobalPlanner::Ptr old_planner = planner_; + auto old_loader = planner_loader_; + // initialize the global planner + try + { + // wait for the current planner to finish planning + boost::unique_lock lock(planner_mutex_); + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(base_global_planner); + auto new_loader = boost::dll::import_alias( + path_file_so, base_global_planner, boost::dll::load_mode::append_decorations); + auto new_planner = new_loader(); + if (!new_planner) + { + robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr", __FILE__, __LINE__); + throw std::runtime_error("Failed to load global planner " + base_global_planner); + } + + if (planner_plan_) + planner_plan_->clear(); + if (latest_plan_) + latest_plan_->clear(); + if (controller_plan_) + controller_plan_->clear(); + resetState(); + + new_planner->initialize(base_global_planner, planner_costmap_robot_); + + planner_ = new_planner; + planner_loader_ = new_loader; + last_config_.base_global_planner = base_global_planner; + + // Release old planner while old loader is still alive. + old_planner.reset(); + old_loader = boost::function(); + lock.unlock(); + } + catch (const std::exception &ex) + { + robot::log_error("[%s:%d]\n Failed to create the %s planner, are you sure it is properly registered and that the \ + containing library is built? Exception: %s",__FILE__, __LINE__, base_global_planner.c_str(), ex.what()); + planner_ = old_planner; + planner_loader_ = old_loader; + } + } } void move_base::MoveBase::setRobotFootprint(const std::vector &fprt) @@ -788,8 +840,9 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, } catch (const std::exception &e) { - robot::log_error("[MoveBase] Exception in swapPlanner: %s", e.what()); - throw std::exception(e); + robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; } if (nav_feedback_) @@ -897,8 +950,9 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_ } catch (const std::exception &e) { - std::cerr << e.what() << "\n"; - throw std::exception(e); + robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; } if (nav_feedback_) @@ -923,6 +977,7 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance) { + robot::log_info("[MoveBase::moveStraightTo] Entry"); if (setup_) { std::string global_planner = "TwoPointsPlanner"; @@ -938,12 +993,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped } return false; } - if (fabs(xy_goal_tolerance) > 0.001) this->setXyGoalTolerance(fabs(xy_goal_tolerance)); else this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); - if (!tc_) { throw std::runtime_error("Null 'tc_' pointer encountered"); @@ -954,7 +1007,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); - tc_->swapPlanner(go_straight_planner_name_); + + try + { + tc_->swapPlanner(go_straight_planner_name_); + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::moveStraightTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; + } if (nav_feedback_) { @@ -965,6 +1028,48 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped if (cancel_ctr_) cancel_ctr_ = false; + // Check if action server exists + if (!as_) + { + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld", + action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); + + // Clear Order message since this is a non-Order moveTo call + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_.reset(); + } + + robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server..."); + as_->processGoal(action_goal); + robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server"); + } + catch (const std::exception &e) + { + lock.unlock(); + robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what()); + return false; + } + lock.unlock(); return true; } @@ -987,7 +1092,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, return false; } - this->setXyGoalTolerance(0.25); + this->setXyGoalTolerance(std::numeric_limits::infinity()); if (fabs(yaw_goal_tolerance) > 0.001) this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); @@ -1003,7 +1108,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); } boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); - tc_->swapPlanner(rotate_planner_name_); + + try + { + tc_->swapPlanner(rotate_planner_name_); + } + catch (const std::exception &e) + { + robot::log_error("[MoveBase::rotateTo] Exception in swapPlanner: %s", e.what()); + lock.unlock(); + return false; + } if (nav_feedback_) { @@ -1013,17 +1128,49 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, } if (cancel_ctr_) cancel_ctr_ = false; - robot_geometry_msgs::PoseStamped pose; - if (!this->getRobotPose(pose)) + + // Check if action server exists + if (!as_) { - if (nav_feedback_) - { - nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; - nav_feedback_->feed_back_str = std::string("Coudn't get robot pose"); - nav_feedback_->goal_checked = false; - } + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); return false; } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld", + action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); + + // Clear Order message since this is a non-Order moveTo call + { + boost::unique_lock planner_lock(planner_mutex_); + planner_order_.reset(); + } + + robot::log_info("[MoveBase::rotateTo] Processing goal through action server..."); + as_->processGoal(action_goal); + robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server"); + } + catch (const std::exception &e) + { + lock.unlock(); + robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what()); + return false; + } + lock.unlock(); return true; }