811 lines
33 KiB
C++
811 lines
33 KiB
C++
#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<robot_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 robot_geometry_msgs::PoseStamped& start,
|
|
const robot_geometry_msgs::PoseStamped& goal,
|
|
const vector<robot_geometry_msgs::Point>& footprint_robot,
|
|
const vector<robot_geometry_msgs::Point>& footprint_dock,
|
|
const int& degree, const double& step,
|
|
vector<robot_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<robot_geometry_msgs::Pose>& control_points, bool check_isFreeSpace,
|
|
bool edge_end_plag, bool path_reverse) -> vector<robot_geometry_msgs::Pose>
|
|
{
|
|
vector<robot_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<robot_geometry_msgs::Pose> 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<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
|
|
vector<robot_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<robot_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<robot_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())
|
|
{
|
|
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<robot_geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(),
|
|
mid_control_point,
|
|
goal.pose};
|
|
|
|
vector<robot_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<robot_geometry_msgs::Pose>& control_points,
|
|
vector<robot_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<robot_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);
|
|
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<robot_geometry_msgs::Pose>& saved_poses,
|
|
std::vector<robot_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;
|
|
robot_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<robot_geometry_msgs::Pose>& control_points,
|
|
const robot_geometry_msgs::Pose& start_pose,
|
|
const std::vector<robot_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) -> 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<robot_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 robot_geometry_msgs::Pose& pose,
|
|
const std::vector<robot_geometry_msgs::Point>& footprint)
|
|
{
|
|
// static std::vector<robot_geometry_msgs::Pose> 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<robot_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<robot_geometry_msgs::Point> 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<robot_geometry_msgs::Point> DockCalc::calcFreeZone(const robot_geometry_msgs::Pose& pose,
|
|
const std::vector<robot_geometry_msgs::Point>& footprint)
|
|
{
|
|
// Dịch sang tọa độ tuyệt đối
|
|
std::vector<robot_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;
|
|
|
|
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<robot_geometry_msgs::Point> DockCalc::interpolateFootprint(const robot_geometry_msgs::Pose& pose,
|
|
const std::vector<robot_geometry_msgs::Point>& footprint, const double& resolution)
|
|
{
|
|
|
|
// Dịch sang tọa độ tuyệt đối
|
|
std::vector<robot_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;
|
|
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<robot_geometry_msgs::Point> 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<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;
|
|
robot_geometry_msgs::Point pt;
|
|
pt.x = start.x + t * dx;
|
|
pt.y = start.y + t * dy;
|
|
points.push_back(pt);
|
|
}
|
|
}
|
|
return points;
|
|
}
|
|
|
|
} |