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 3b06c95..d3064b3 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 @@ -176,7 +176,9 @@ namespace two_points_planner const double dy = goal.pose.position.y - start.pose.position.y; const double distance = std::sqrt(dx * dx + dy * dy); double theta; - + // Lấy độ phân giải của costmap + double resolution = costmap_robot_->getCostmap()->getResolution(); + if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9) { theta = std::atan2(dy, dx); @@ -189,16 +191,12 @@ namespace two_points_planner else { robot_geometry_msgs::PoseStamped pose = start; - pose.pose.position.x += 0.01 * cos(theta); - pose.pose.position.y += 0.01 * sin(theta); + pose.pose.position.x += resolution * cos(theta); + pose.pose.position.y += resolution * sin(theta); plan.push_back(pose); plan.push_back(goal); return true; } - - // Lấy độ phân giải của costmap - double resolution = costmap_robot_->getCostmap()->getResolution(); - // Tính số điểm cần chia int num_points = std::ceil(distance / resolution); 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 4a8551a..d1a141a 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 @@ -250,17 +250,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() if(rotate_algorithm_) rotate_algorithm_->reset(); ret_nav_ = ret_angle_ = false; - robot::log_info_at(__FILE__, __LINE__, "Debug"); - parent_.printParams(); std::string algorithm_nav_name; planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); parent_.setParam(algorithm_nav_name, original_papams_); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); nh_algorithm.setParam("allow_rotate", false); - - robot::log_info_at(__FILE__, __LINE__, "Debug ở đây"); - parent_.printParams(); } void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) @@ -455,7 +450,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_ { robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta); robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta); - robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta); } } @@ -801,7 +795,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath( robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose); robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal); std::vector docking_plan; - + robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y); + robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y); if (!docking_planner_->makePlan(start, goal, docking_plan)) { throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed"); diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 19b58c9..5073b7a 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -3144,15 +3144,6 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro } std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); - // robot_geometry_msgs::PoseStamped goal_pose, global_pose; - // goal_pose = goal_pose_msg; - - // goal_pose.header.stamp = robot::Time(); // latest available - // try - // { - // tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time()); - // tf3::doTransform(goal_pose, global_pose, transform); - // } robot_geometry_msgs::PoseStamped global_pose; tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); robot_geometry_msgs::PoseStamped goal_pose; @@ -3218,17 +3209,16 @@ robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData() { robot_nav_msgs::Path path; tc_->getGlobalPlan(path.poses); - if (!path.poses.empty()) + robot_nav_msgs::Path global_path; + global_path.header.stamp = robot::Time::now(); + global_path.header.frame_id = planner_costmap_robot_->getGlobalFrameID(); + for(auto &p : path.poses) { - path.header.stamp = path.poses[0].header.stamp; - path.header.frame_id = path.poses[0].header.frame_id; + robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p); + pose.header.stamp = robot::Time::now(); + global_path.poses.push_back(goalToGlobalFrame(pose)); } - else - { - path.header.stamp = robot::Time::now(); - path.header.frame_id = planner_costmap_robot_->getGlobalFrameID(); - } - global_data_.plan = robot_nav_2d_utils::pathToPath(path); + global_data_.plan = robot_nav_2d_utils::pathToPath(global_path); } ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true); convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);