#include "dock_planner/dock_calc.h" namespace dock_planner { DockCalc::DockCalc(/* args */) :costmap_(nullptr) { check_goal_ = false; // cost_threshold_ = 200; // Threshold for obstacle detection safety_distance_ = 0; // Safety distance from obstacles calib_safety_distance_ = 0; input_spline_inf = new Spline_Inf(); CurveDesign = new CurveCommon(); } DockCalc::~DockCalc() { delete CurveDesign; delete input_spline_inf; } robot_geometry_msgs::Pose DockCalc::offsetPoint(const robot_geometry_msgs::Pose& A, const double& theta, const double& d) { robot_geometry_msgs::Pose B; B.position.x = A.position.x + d * std::cos(theta); B.position.y = A.position.y + d * std::sin(theta); return B; } double DockCalc::pointToSegmentDistance(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) { double theta = signedAngle(A,B,C); double sin_theta = sin(fabs(theta)); double h = calc_distance(A,B); double distance = sin_theta * h; return distance; } double DockCalc::signedAngle(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) { double ABx = B.position.x - A.position.x; double ABy = B.position.y - A.position.y; double CBx = B.position.x - C.position.x; double CBy = B.position.y - C.position.y; double cross = ABx*CBy - ABy*CBx; double dot = ABx*CBx + ABy*CBy; double angle = atan2(cross,dot); return angle; } double DockCalc::compute_t(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) { constexpr double epsilon = 1e-6; double dx = C.position.x - B.position.x; double dy = C.position.y - B.position.y; double bx = B.position.x - A.position.x; double by = B.position.y - A.position.y; double numerator = dx * bx + dy * by; double denominator = dx * dx + dy * dy; if (denominator <= epsilon) { throw std::runtime_error("B and C are the same!"); } return -numerator / denominator; } robot_geometry_msgs::Pose DockCalc::compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C, const double& t) { robot_geometry_msgs::Pose D; D.position.x = B.position.x + t * (C.position.x - B.position.x); D.position.y = B.position.y + t * (C.position.y - B.position.y); return D; } RobotGeometry DockCalc::computeGeometry(const std::vector& fp) { double min_x = 1e9, max_x = -1e9; double min_y = 1e9, max_y = -1e9; double radius = 0.0; if((int)fp.size() >= 4) { for (const auto& p : fp) { min_x = std::min(min_x, p.x); max_x = std::max(max_x, p.x); min_y = std::min(min_y, p.y); max_y = std::max(max_y, p.y); radius = std::max(radius, std::hypot(p.x, p.y)); } } else return {0, 0, 0}; double length = std::max(max_x - min_x, max_y - min_y); double width = std::min(max_x - min_x, max_y - min_y); return {length, width, radius}; } bool DockCalc::makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& goal, const vector& footprint_robot, const vector& footprint_dock, const int& degree, const double& step, vector& planMoveToDock, bool reverse) { constexpr double epsilon = 1e-6; robot::log_error("[dock_calc] makePlanMoveToDock start"); planMoveToDock.clear(); if(save_goal_.pose.position.x != goal.pose.position.x || save_goal_.pose.position.y != goal.pose.position.x || data_convert::getYaw(save_goal_.pose.orientation) != data_convert::getYaw(goal.pose.orientation)) { // robot::log_error("[dock_calc] DEBUG 1000"); check_goal_ = true; save_goal_ == goal; } footprint_robot_ = footprint_robot; // Calculate footprint distances RobotGeometry robot = computeGeometry(footprint_robot); RobotGeometry dock = computeGeometry(footprint_dock); double distance_robot = robot.length; double distance_dock = dock.length; if (distance_robot <= epsilon) { robot::log_error("[dock_calc] Robot geometry is invalid, cannot calculate plan to dock."); return false; } // Calculate safety distance safety_distance_ = (robot.radius + calib_safety_distance_) * 2.0; if (safety_distance_ <= epsilon) { robot::log_error("[dock_calc] Safety distance is zero, cannot calculate plan to dock."); return false; } // Lambda: Generate NURBS path auto generateNURBSPath = [&](const vector& control_points, bool check_isFreeSpace, bool edge_end_plag, bool path_reverse) -> vector { vector result; bool obstacle_flag = false; // Convert to Eigen format std::vector> eigen_points; for (const auto& cp : control_points) { eigen_points.emplace_back(cp.position.x, cp.position.y, 0); } // Create knot vector std::vector knot_vector; int n = control_points.size() - 1; int m = n + degree + 1; for (int i = 0; i <= m; i++) { knot_vector.push_back(i <= degree ? 0.0 : 1.0); } // Setup spline std::vector weights(control_points.size(), 1.0); input_spline_inf->knot_vector.clear(); input_spline_inf->control_point.clear(); input_spline_inf->weight.clear(); CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_points, knot_vector); CurveDesign->ReadSplineInf(input_spline_inf, weights, false); // Calculate adaptive step double edge_length = calculateNURBSLength(CurveDesign, input_spline_inf, degree, step); double resolution = costmap_->getResolution(); double step_new = calculateAdaptiveStep(edge_length, resolution); // robot::log_error("[dock_calc] Adaptive step size: %f", step_new); // Generate path points vector saved_poses; for (double t = 0.0; t <= 1.0; t += step_new) { robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); if (!std::isnan(point.x) && !std::isnan(point.y)) { robot_geometry_msgs::Pose pose; pose.position = point; pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0); bool status_isFreeSpace = true; if(edge_end_plag) { double distance_to_goal = calc_distance(pose, goal.pose); if(distance_to_goal >= min_distance_to_goal_) { if(check_isFreeSpace) status_isFreeSpace = isFreeSpace(pose, footprint_robot_); } } else { if(check_isFreeSpace) status_isFreeSpace = isFreeSpace(pose, footprint_robot_); } if (status_isFreeSpace && t < 1.0) { saved_poses.push_back(pose); if (saved_poses.size() > 1) { updatePoseOrientation(saved_poses, result, path_reverse); } } if(!status_isFreeSpace) { obstacle_flag = true; break; } } } // Set final orientation if (!saved_poses.empty()) { double end_yaw = path_reverse ? calculateAngle(control_points.back().position.x, control_points.back().position.y, saved_poses.back().position.x, saved_poses.back().position.y) : calculateAngle(saved_poses.back().position.x, saved_poses.back().position.y, control_points.back().position.x, control_points.back().position.y); tf3::Quaternion q; q.setRPY(0, 0, end_yaw); saved_poses.back().orientation.x = q.x(); saved_poses.back().orientation.y = q.y(); saved_poses.back().orientation.z = q.z(); saved_poses.back().orientation.w = q.w(); result.push_back(saved_poses.back()); } if(obstacle_flag) { robot::log_error("[dock_calc] Obstacle detected, path generation interrupted."); result.clear(); } if (result.empty()) { robot::log_error("[dock_calc] No valid path generated."); } else { robot::log_info("[dock_calc] Path generated with %zu points.", result.size()); } return result; }; // Calculate minimum distance for pose C // Can safety_distance_ replace distance_robot double distance_min_for_pose_C = (distance_robot + distance_dock) / 2.0; min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0); // Calculate pose C (offset from goal) robot_geometry_msgs::Pose Goal_Pose = goal.pose; double yaw_goal = normalizeAngle(data_convert::getYaw(goal.pose.orientation)); robot_geometry_msgs::Pose Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C); // Compute t parameter and determine direction double t = compute_t(start.pose, goal.pose, Pose_C); /** * dir_robot_move_to_goal = false robot move backward * dir_robot_move_to_goal = true robot move forward */ bool dir_robot_move_to_goal = true; if (t < 0) { yaw_goal = (yaw_goal > 0) ? yaw_goal - M_PI : yaw_goal + M_PI; Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C); t = compute_t(start.pose, goal.pose, Pose_C); dir_robot_move_to_goal = false; } if (t <= 1) { robot::log_error("[dock_calc] t(%f) <= 1, cannot calculate plan to dock.", t); return false; } // Calculate pose D and distance AD robot_geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t); double distance_AD = pointToSegmentDistance(start.pose, goal.pose, Pose_C); // Calculate angle threshold with scaling const double MAX_DISTANCE = 1.0, MIN_DISTANCE = 0.1; const double MIN_SCALE = 0.2, MAX_SCALE = 1.0; const double calib_factor_alpha = 0.5; const double calib_factor_beta = 0.5; const double calib_factor_gamma = 0.77; double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * (MAX_DISTANCE - distance_AD) / (MAX_DISTANCE - MIN_DISTANCE); double angle_threshold = std::min((calib_factor_alpha + fabs(scale)) * calib_factor_beta * M_PI, calib_factor_gamma * M_PI); // Generate temporary path double distance_CD = calc_distance(Pose_C, Pose_D); double angle_alpha = (M_PI - angle_threshold) / 2.0; double distance_DF = distance_AD / tan(angle_alpha); double distance_min_for_pose_F = distance_min_for_pose_C + distance_CD + distance_DF; robot_geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F); robot_geometry_msgs::Pose mid_pose; mid_pose.position.x = (Pose_C.position.x + Pose_F.position.x) / 2.0; mid_pose.position.y = (Pose_C.position.y + Pose_F.position.y) / 2.0; vector control_points_tmp = {Pose_C, mid_pose, Pose_F}; vector saved_poses_tmp; if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) { robot::log_error("[dock_calc] Failed to find path with temporary control points."); return false; } // Find first segment control points int dir; vector control_points_1; if (!findNURBSControlPoints(dir, control_points_1, start.pose, saved_poses_tmp, angle_threshold, degree)) { robot::log_error("[dock_calc] Failed to find NURBS control points for first segment."); return false; } // Determine reverse direction for first segment bool first_segment_reverse = (dir == -1 && !dir_robot_move_to_goal) || ((dir == 1 || dir == 0) && dir_robot_move_to_goal); // Generate first segment vector first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end()); //Generate second segment (to goal) if (!planMoveToDock.empty()) { robot_geometry_msgs::Pose mid_control_point; mid_control_point.position.x = (planMoveToDock.back().position.x + goal.pose.position.x) / 2.0; mid_control_point.position.y = (planMoveToDock.back().position.y + goal.pose.position.y) / 2.0; vector control_points_2 = {planMoveToDock.back(), mid_control_point, goal.pose}; vector second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end()); planMoveToDock.insert(planMoveToDock.begin(),start.pose); planMoveToDock.insert(planMoveToDock.end(),goal.pose); } return !planMoveToDock.empty(); } bool DockCalc::findPathTMP(const vector& control_points, vector& saved_poses, const int& degree, double step) { // Chuyển đổi control points sang định dạng Eigen::Vector3d std::vector> eigen_control_points; for(const auto& cp : control_points) { Eigen::Vector3d point(cp.position.x, cp.position.y, 0); eigen_control_points.push_back(point); } // Tạo knot vector tự động std::vector knot_vector; int n = control_points.size() - 1; // n + 1 control points int m = n + degree + 1; // m + 1 knots // Tạo uniform knot vector for(int i = 0; i <= m; i++) { if(i <= degree) knot_vector.push_back(0.0); else knot_vector.push_back(1.0); } // Điều chỉnh weights để tăng ảnh hưởng của các control points std::vector weights(control_points.size(), 1.0); input_spline_inf->knot_vector.clear(); input_spline_inf->control_point.clear(); input_spline_inf->weight.clear(); // Cài đặt thông số cho spline CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_control_points, knot_vector); CurveDesign->ReadSplineInf(input_spline_inf, weights, false); // vector poses_tmp; for(double t = 0.0; t <= 1.0; t += step) { // robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t); robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); if(!std::isnan(point.x) && !std::isnan(point.y)) { // robot::log_error("[dock_calc][findPathTMP] point(%f, %f) at t(%f)", point.x, point.y, t); robot_geometry_msgs::Pose pose; pose.position = point; pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = 0.0; pose.orientation.w = 1.0; bool status_isFreeSpace = isFreeSpace(pose, footprint_robot_); if(status_isFreeSpace) { saved_poses.push_back(pose); } else { robot::log_error("[dock_calc][findPathTMP] Obstacle detected at t(%f)", t); break; } } else { robot::log_error("[dock_calc][findPathTMP] NaN point at t(%f)", t); } } if(saved_poses.empty()) { robot::log_error("[dock_calc][findPathTMP] No valid poses generated."); return false; } return true; } void DockCalc::updatePoseOrientation(std::vector& saved_poses, std::vector& nurbs_path, bool reverse) { double yaw; // robot::log_error("[merge_path_calc] DEBUG: reverse(%x)", reverse); if(!reverse) yaw = calculateAngle(saved_poses[saved_poses.size() - 2].position.x, saved_poses[saved_poses.size() - 2].position.y, saved_poses.back().position.x, saved_poses.back().position.y); else yaw = calculateAngle(saved_poses.back().position.x, saved_poses.back().position.y, saved_poses[saved_poses.size() - 2].position.x, saved_poses[saved_poses.size() - 2].position.y); tf3::Quaternion q; q.setRPY(0, 0, yaw); saved_poses[saved_poses.size() - 2].orientation.x = q.x(); saved_poses[saved_poses.size() - 2].orientation.y = q.y(); saved_poses[saved_poses.size() - 2].orientation.z = q.z(); saved_poses[saved_poses.size() - 2].orientation.w = q.w(); nurbs_path.push_back(saved_poses[saved_poses.size() - 2]); } double DockCalc::calculateNURBSLength(CurveCommon* curve_design, Spline_Inf* spline_inf, int degree, double initial_step) { double length = 0.0; robot_geometry_msgs::Point prev_point, curr_point; std::vector segment_lengths; static double step; // Đường cong bậc 1 (đường thẳng) thì bước nhảy bằng 0.1 if(degree == 1) step = 0.1; // Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step else step = initial_step; // Tính độ dài với step for(double u = 0; u <= 1; u += step) { // Lấy điểm trên đường cong tại tham số u curr_point = curve_design->CalculateCurvePoint(spline_inf, u, true); if(u > 0 && !std::isnan(curr_point.x) && !std::isnan(curr_point.y)) { double segment_length = std::sqrt(std::pow(curr_point.x - prev_point.x, 2) + std::pow(curr_point.y - prev_point.y, 2)); length += segment_length; segment_lengths.push_back(segment_length); } prev_point = curr_point; } return length; } bool DockCalc::findNURBSControlPoints(int& dir, vector& control_points, const robot_geometry_msgs::Pose& start_pose, const std::vector& saved_poses, const double& angle_threshold, const int& degree) { if (saved_poses.empty()) { robot::log_error("[dock_calc] No saved poses available to find NURBS control points."); return false; } control_points.clear(); int idx = findNearestPointOnPath(dir, start_pose, saved_poses, angle_threshold); // Helper lambda to create pose at position auto createPose = [](double x, double y, double yaw = 0.0) -> robot_geometry_msgs::Pose { robot_geometry_msgs::Pose pose; pose.position.x = x; pose.position.y = y; pose.orientation = data_convert::createQuaternionMsgFromYaw(0); return pose; }; if (idx != -1) { start_target_idx_ = idx; // Add start and middle poses control_points.push_back(start_pose); control_points.push_back(createPose(saved_poses[idx].position.x, saved_poses[idx].position.y)); // Calculate distance from start to middle point double dx = start_pose.position.x - saved_poses[idx].position.x; double dy = start_pose.position.y - saved_poses[idx].position.y; double target_distance = std::hypot(dx, dy); if (dir == 1) { // Find appropriate end point going backwards int end_idx = idx; const auto& mid_pose = control_points[1]; for (int i = idx; i >= 0; i--) { double dx_me = mid_pose.position.x - saved_poses[i].position.x; double dy_me = mid_pose.position.y - saved_poses[i].position.y; double dist_me = std::hypot(dx_me, dy_me); if (dist_me >= target_distance || i == 0) { end_idx = i; break; } } control_points.push_back(createPose(saved_poses[end_idx].position.x, saved_poses[end_idx].position.y)); start_target_idx_ = end_idx; } else if (dir == -1) { // robot::log_error("[dock_calc] DEBUG 200:"); // Use last pose as end point const auto& last_pose = saved_poses.back(); control_points.push_back(createPose(last_pose.position.x, last_pose.position.y)); start_target_idx_ = saved_poses.size() - 1; } } else { // robot::log_error("[dock_calc] DEBUG 300:"); // Create midpoint between start and nearest saved pose const auto& nearest_pose = saved_poses[store_start_nearest_idx_]; auto mid_pose = createPose( (start_pose.position.x + nearest_pose.position.x) / 2.0, (start_pose.position.y + nearest_pose.position.y) / 2.0 ); control_points.push_back(start_pose); control_points.push_back(mid_pose); control_points.push_back(nearest_pose); // robot::log_error("[dock_calc] DEBUG 1: start_pose(%f,%f)",start_pose.position.x, start_pose.position.y); // robot::log_error("[dock_calc] DEBUG 1: mid_pose(%f,%f)",mid_pose.position.x, mid_pose.position.y); // robot::log_error("[dock_calc] DEBUG 1: nearest_pose(%f,%f)",nearest_pose.position.x, nearest_pose.position.y); start_target_idx_ = store_start_nearest_idx_; } return true; } int DockCalc::findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose, const std::vector& saved_poses, const double& angle_threshol) { // Tìm điểm gần nhất int nearest_idx = 0; double min_dist = std::numeric_limits::max(); for (int i = 0; i < (int)saved_poses.size(); ++i) { double distance = std::hypot(saved_poses[i].position.x - pose.position.x, saved_poses[i].position.y - pose.position.y); if (distance < min_dist) { min_dist = distance; nearest_idx = i; } } // Lưu chỉ mục của điểm gần nhất store_start_nearest_idx_ = nearest_idx; bool found_pose_flag_1 = false; bool found_pose_flag_2 = false; for(int i = store_start_nearest_idx_; i >= 0; i--) { double angle = signedAngle(pose, saved_poses[i], saved_poses[i - 1]); if(fabs(angle) >= fabs(angle_threshol)) { nearest_idx = i; found_pose_flag_1 = true; break; } } double distance_1 = calc_distance(pose,saved_poses[nearest_idx]); double distance_2 = calc_distance(saved_poses[0],saved_poses[nearest_idx]); if(found_pose_flag_1 && (distance_2 >= distance_1)) { found_pose_flag_2 = true; dir = 1; } if(!found_pose_flag_2) { for(int i = store_start_nearest_idx_; i < (int)saved_poses.size(); i++) { double angle = signedAngle(pose, saved_poses[i], saved_poses[i + 1]); if(fabs(angle) >= (fabs(angle_threshol) - 0.05)) { nearest_idx = i; dir = -1; break; } if(i == (int)saved_poses.size() - 1) { dir = 0; return -1; } } double distance_1 = calc_distance(pose,saved_poses[nearest_idx]); double distance_2 = calc_distance(saved_poses[nearest_idx],saved_poses.back()); if(distance_2 + 0.1 < distance_1) { dir = 0; return -1; } } return nearest_idx; } bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose, const std::vector& footprint) { // static std::vector free_zone; // auto makePose = [](double x, double y, double yaw) // { // robot_geometry_msgs::Pose p; // p.position.x = x; // p.position.y = y; // p.position.z = 0.0; // tf3::Quaternion q; // q.setRPY(0.0, 0.0, yaw); // p.orientation.x = q.x(); // p.orientation.y = q.y(); // p.orientation.z = q.z(); // p.orientation.w = q.w(); // return p; // }; // unsigned int mx, my; // if (!costmap_->worldToMap(pose.position.x, pose.position.y, mx, my)) // { // // robot::log_error("[dock_calc][isFreeSpace]Debug 1"); // return false; // pose nằm ngoài bản đồ // } // unsigned char center_cost = costmap_->getCost(mx, my); // if (center_cost >= cost_threshold_) // { // // robot::log_error("[dock_calc][isFreeSpace]Debug 2"); // return false; // chính giữa đã bị chiếm // } // double resolution = costmap_->getResolution(); // if(check_goal_) // { // std::vector result_free_zone = calcFreeZone(pose, footprint); // if((int)result_free_zone.size() < 4) return false; // free_zone.push_back(makePose(result_free_zone[0].x, result_free_zone[0].y, 0.0)); // free_zone.push_back(makePose(result_free_zone[1].x, result_free_zone[1].y, 0.0)); // free_zone.push_back(makePose(result_free_zone[2].x, result_free_zone[2].y, 0.0)); // check_goal_ = false; // } // std::vector space_footprint = interpolateFootprint(pose, footprint, resolution); // for (int i = 0; i < (int)space_footprint.size(); i++) // { // robot_geometry_msgs::Pose Pose_on_footprint; // Pose_on_footprint.position.x = space_footprint[i].x; // Pose_on_footprint.position.y = space_footprint[i].y; // if((int)free_zone.size() < 3) return false; // double t_L = compute_t(Pose_on_footprint, free_zone[0], free_zone[1]); // double t_W = compute_t(Pose_on_footprint, free_zone[1], free_zone[2]); // if(t_L >= 0 && t_L <= 1 && t_W >= 0 && t_W <= 1) // { // return true; // } // if (!costmap_->worldToMap(space_footprint[i].x, space_footprint[i].y, mx, my)) // { // // robot::log_error("[dock_calc][isFreeSpace]Debug 3"); // return false; // pose nằm ngoài bản đồ // } // int check_x = mx; // int check_y = my; // if (check_x >= 0 && check_x < costmap_->getSizeInCellsX() && // check_y >= 0 && check_y < costmap_->getSizeInCellsY()) // { // unsigned char cost = costmap_->getCost(check_x, check_y); // if (cost >= robot_costmap_2d::LETHAL_OBSTACLE) // { // double dx = pose.position.x - space_footprint[i].x; // double dy = pose.position.y - space_footprint[i].y; // double dist = std::hypot(dx, dy); // if (dist <= (safety_distance_/2.0)) // { // // robot::log_error("[dock_calc][isFreeSpace]Debug dist: %f", dist); // return false; // } // } // } // } return true; } std::vector DockCalc::calcFreeZone(const robot_geometry_msgs::Pose& pose, const std::vector& footprint) { // Dịch sang tọa độ tuyệt đối std::vector free_zone; double cos_th = std::cos(data_convert::getYaw(pose.orientation)); double sin_th = std::sin(data_convert::getYaw(pose.orientation)); for (auto pt : footprint) { pt.x *= 1.2; pt.y *= 1.2; robot_geometry_msgs::Point abs_pt; abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th; abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th; free_zone.push_back(abs_pt); } return free_zone; } std::vector DockCalc::interpolateFootprint(const robot_geometry_msgs::Pose& pose, const std::vector& footprint, const double& resolution) { // Dịch sang tọa độ tuyệt đối std::vector abs_footprint; double cos_th = std::cos(data_convert::getYaw(pose.orientation)); double sin_th = std::sin(data_convert::getYaw(pose.orientation)); for (auto pt : footprint) { pt.x *= 1.1; pt.y *= 1.1; robot_geometry_msgs::Point abs_pt; abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th; abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th; abs_footprint.push_back(abs_pt); } std::vector points; for (size_t i = 0; i < (int)abs_footprint.size(); ++i) { const robot_geometry_msgs::Point &start = abs_footprint[i]; const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; double dx = end.x - start.x; double dy = end.y - start.y; double distance = std::hypot(dx, dy); int steps = std::max(1, static_cast(std::floor(distance / resolution))); for (int j = 0; j <= steps; ++j) { if (j == steps && i != (int)abs_footprint.size() - 1) continue; // tránh lặp double t = static_cast(j) / steps; robot_geometry_msgs::Point pt; pt.x = start.x + t * dx; pt.y = start.y + t * dy; points.push_back(pt); } } return points; } }