first commit
This commit is contained in:
811
src/dock_calc.cpp
Normal file
811
src/dock_calc.cpp
Normal file
@@ -0,0 +1,811 @@
|
||||
#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;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose DockCalc::offsetPoint(const geometry_msgs::Pose& A, const double& theta, const double& d)
|
||||
{
|
||||
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 geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const 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 geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const 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 geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const 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;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose DockCalc::compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C, const double& t)
|
||||
{
|
||||
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<geometry_msgs::Point>& 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 geometry_msgs::PoseStamped& start,
|
||||
const geometry_msgs::PoseStamped& goal,
|
||||
const vector<geometry_msgs::Point>& footprint_robot,
|
||||
const vector<geometry_msgs::Point>& footprint_dock,
|
||||
const int& degree, const double& step,
|
||||
vector<geometry_msgs::Pose>& 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<geometry_msgs::Pose>& control_points, bool check_isFreeSpace,
|
||||
bool edge_end_plag, bool path_reverse) -> vector<geometry_msgs::Pose>
|
||||
{
|
||||
vector<geometry_msgs::Pose> result;
|
||||
bool obstacle_flag = false;
|
||||
|
||||
// Convert to Eigen format
|
||||
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_points;
|
||||
for (const auto& cp : control_points)
|
||||
{
|
||||
eigen_points.emplace_back(cp.position.x, cp.position.y, 0);
|
||||
}
|
||||
|
||||
// Create knot vector
|
||||
std::vector<double> 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<double> 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<geometry_msgs::Pose> saved_poses;
|
||||
for (double t = 0.0; t <= 1.0; t += step_new)
|
||||
{
|
||||
geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
|
||||
|
||||
if (!std::isnan(point.x) && !std::isnan(point.y))
|
||||
{
|
||||
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)
|
||||
geometry_msgs::Pose Goal_Pose = goal.pose;
|
||||
double yaw_goal = normalizeAngle(data_convert::getYaw(goal.pose.orientation));
|
||||
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
|
||||
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;
|
||||
|
||||
geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F);
|
||||
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<geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
|
||||
vector<geometry_msgs::Pose> 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<geometry_msgs::Pose> 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<geometry_msgs::Pose> 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())
|
||||
{
|
||||
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<geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(),
|
||||
mid_control_point,
|
||||
goal.pose};
|
||||
|
||||
vector<geometry_msgs::Pose> 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<geometry_msgs::Pose>& control_points,
|
||||
vector<geometry_msgs::Pose>& saved_poses,
|
||||
const int& degree, double step)
|
||||
{
|
||||
// Chuyển đổi control points sang định dạng Eigen::Vector3d
|
||||
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> 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<double> 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<double> 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<geometry_msgs::Pose> poses_tmp;
|
||||
for(double t = 0.0; t <= 1.0; t += step)
|
||||
{
|
||||
// robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t);
|
||||
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);
|
||||
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<geometry_msgs::Pose>& saved_poses,
|
||||
std::vector<geometry_msgs::Pose>& 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;
|
||||
geometry_msgs::Point prev_point, curr_point;
|
||||
std::vector<double> 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<geometry_msgs::Pose>& control_points,
|
||||
const geometry_msgs::Pose& start_pose,
|
||||
const std::vector<geometry_msgs::Pose>& 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) -> geometry_msgs::Pose
|
||||
{
|
||||
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 geometry_msgs::Pose& pose,
|
||||
const std::vector<geometry_msgs::Pose>& saved_poses,
|
||||
const double& angle_threshol)
|
||||
{
|
||||
// Tìm điểm gần nhất
|
||||
int nearest_idx = 0;
|
||||
double min_dist = std::numeric_limits<double>::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 geometry_msgs::Pose& pose,
|
||||
const std::vector<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
// static std::vector<geometry_msgs::Pose> free_zone;
|
||||
|
||||
// auto makePose = [](double x, double y, double yaw)
|
||||
// {
|
||||
// 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<geometry_msgs::Point> 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<geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution);
|
||||
// for (int i = 0; i < (int)space_footprint.size(); i++)
|
||||
// {
|
||||
// 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 >= 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<geometry_msgs::Point> DockCalc::calcFreeZone(const geometry_msgs::Pose& pose,
|
||||
const std::vector<geometry_msgs::Point>& footprint)
|
||||
{
|
||||
// Dịch sang tọa độ tuyệt đối
|
||||
std::vector<geometry_msgs::Point> 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;
|
||||
|
||||
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<geometry_msgs::Point> DockCalc::interpolateFootprint(const geometry_msgs::Pose& pose,
|
||||
const std::vector<geometry_msgs::Point>& footprint, const double& resolution)
|
||||
{
|
||||
|
||||
// Dịch sang tọa độ tuyệt đối
|
||||
std::vector<geometry_msgs::Point> 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;
|
||||
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<geometry_msgs::Point> points;
|
||||
for (size_t i = 0; i < (int)abs_footprint.size(); ++i)
|
||||
{
|
||||
const geometry_msgs::Point &start = abs_footprint[i];
|
||||
const 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<int>(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<double>(j) / steps;
|
||||
geometry_msgs::Point pt;
|
||||
pt.x = start.x + t * dx;
|
||||
pt.y = start.y + t * dy;
|
||||
points.push_back(pt);
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user