From ece9154a8448f5fb71e80b1b9a4a44283b117195 Mon Sep 17 00:00:00 2001 From: hoangson02 Date: Fri, 24 Apr 2026 06:48:26 +0000 Subject: [PATCH] update bug crash miss docking --- .../diff/diff_predictive_trajectory.h | 4 +- .../src/diff/diff_predictive_trajectory.cpp | 131 ++++++++++++++++-- .../Packages/global_planners/custom_planner | 2 +- .../pnkx_docking_local_planner.h | 2 + .../src/pnkx_docking_local_planner.cpp | 127 +++++++++++------ .../Packages/move_base/src/move_base.cpp | 11 +- 6 files changed, 221 insertions(+), 56 deletions(-) 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 936d3be..3ca1bc6 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 @@ -117,7 +117,9 @@ namespace mkt_algorithm * @return lookahead point */ std::vector::iterator - getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan); + getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, + const double &lookahead_dist, + robot_nav_2d_msgs::Path2D& global_plan); /** * @brief Prune global plan 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 a873f8c..8927d16 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 @@ -279,6 +279,7 @@ 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__); @@ -288,7 +289,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 footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) { @@ -311,14 +312,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; @@ -329,14 +330,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::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; @@ -360,6 +361,7 @@ 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(); @@ -368,6 +370,7 @@ 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) { @@ -378,14 +381,15 @@ 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--) @@ -406,12 +410,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: } else { + // robot::log_error("DEBUG STEP 11.1"); try { + auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + // robot::log_error("DEBUG STEP 11.2"); auto prev_carrot_pose_it = transform_plan_.poses.begin(); + // 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; - for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) + + auto it = carrot_pose_it == transform_plan_.poses.begin() ? transform_plan_.poses.end() : carrot_pose_it - 1; + + 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; @@ -422,19 +433,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: 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; 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"); } catch (std::exception &e) { @@ -442,7 +455,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; @@ -1073,7 +1086,8 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob } std::vector::iterator -mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) +mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, + 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."); @@ -1110,9 +1124,104 @@ 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::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()) diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 7c84705..62a493a 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 7c84705ce737736d53829916eb36c9860c10c418 +Subproject commit 62a493a892e6a0936cc584a434ac694f9bbf0f5a diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h index 36b3f23..422bc80 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h @@ -31,6 +31,8 @@ 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 * diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 8f79329..69b253f 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -240,6 +240,12 @@ 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."); @@ -260,6 +266,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() 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_) { @@ -273,11 +280,12 @@ 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_; @@ -285,6 +293,14 @@ 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"); } @@ -294,53 +310,60 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg } double x_direction, y_direction, theta_direction; - if (!ret_nav_) + if (!ret_nav_ && !dkpl_.empty()) { - if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) + // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 4.0"); + if(nav_algorithm_) { - robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); - throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); - } - // else - // ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction); - - if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_) - { - this->lock(); - robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_; - traj_generator_->setTwistLinear(linear); - linear.x *= (-1); - traj_generator_->setTwistLinear(linear); - traj_generator_->setTwistAngular(dkpl_.front()->angular_); - - std::string algorithm_nav_name; - planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); - robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); - nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_); - nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_); - nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_); - nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_); - nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_); - - parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); - parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); - - if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) + if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); + throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); + } + // 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); + linear.x *= (-1); + traj_generator_->setTwistLinear(linear); + traj_generator_->setTwistAngular(dkpl_.front()->angular_); - if (dkpl_.front()->following_) + std::string algorithm_nav_name; + planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); + nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_); + nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_); + nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_); + nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_); + nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_); + + 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_) { - robot_nav_2d_msgs::Pose2DStamped follow_pose; - if (dkpl_.front()->geLocalGoal(local_goal_pose)) + + if (dkpl_.front()->following_) { - local_goal_pose = follow_pose; + robot_nav_2d_msgs::Pose2DStamped follow_pose; + if (dkpl_.front()->geLocalGoal(local_goal_pose)) + { + local_goal_pose = follow_pose; + } + } + if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) + { + throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); + robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); } } - if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) - { - throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); - 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"); } } } @@ -365,14 +388,26 @@ 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 { - if (global_plan_.poses.empty()) + // robot::log_debug_at(__FILE__, __LINE__, "DEBUG STEP 1.0"); + 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) @@ -419,6 +454,11 @@ 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!"); @@ -511,6 +551,7 @@ 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) @@ -535,6 +576,7 @@ 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) @@ -543,6 +585,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ } } } + } else { diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 3a8fdc1..88451f9 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -1278,6 +1278,7 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry } 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) @@ -2494,6 +2495,7 @@ 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; @@ -2713,7 +2715,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!"); + robot::log_debug("pointers swapped!: %d", controller_plan_->size()); if (!tc_->setPlan(*controller_plan_)) { @@ -2734,6 +2736,7 @@ 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) @@ -2745,6 +2748,9 @@ 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 @@ -2914,10 +2920,12 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) { boost::unique_lock 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()) @@ -2943,6 +2951,7 @@ 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 {