DuongTD/dock_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2025-12-22 17:37:45 +07:00
commit 66be0d86cc
16 changed files with 3280 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

811
src/dock_calc.cpp Normal file
View 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;
}
}

124
src/dock_planner.cpp Normal file
View File

@@ -0,0 +1,124 @@
#include "dock_planner/dock_planner.h"
#include <cmath>
#include <algorithm>
namespace dock_planner
{
/**
* @brief Constructor mặc định
* Khởi tạo tất cả các pointer về nullptr và trạng thái chưa initialized
*/
DockPlanner::DockPlanner() : costmap_robot_(nullptr),
costmap_(nullptr),
initialized_(false),
check_free_space_(false),
cost_threshold_(200),
calib_safety_distance_(0.05) {}
/**
* @brief Constructor với parameters
* @param name Tên của planner
* @param costmap_robot Pointer tới costmap ROS wrapper
* Tự động gọi initialize() nếu được cung cấp costmap
*/
DockPlanner::DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
: costmap_robot_(nullptr),
costmap_(nullptr),
initialized_(false),
check_free_space_(false),
cost_threshold_(200),
calib_safety_distance_(0.05)
{
initialize(name, costmap_robot);
}
/**
* @brief Khởi tạo global planner
* @param name Tên của planner instance
* @param costmap_robot Pointer tới costmap ROS wrapper
*
* Thiết lập:
* - Liên kết với costmap
* - Tạo publisher cho visualization
* - Load parameters từ ROS parameter server
* - Đánh dấu trạng thái initialized
*/
bool DockPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
{
if (!initialized_)
{
costmap_robot_ = costmap_robot;
costmap_ = costmap_robot_->getCostmap();
frame_id_ = costmap_robot_->getGlobalFrameID();
calc_plan_to_dock_.costmap_ = costmap_robot_->getCostmap();
robot::NodeHandle private_nh("~/" + name);
// Load parameters from the private namespace
private_nh.getParam("check_free_space", check_free_space_);
private_nh.getParam("cost_threshold", cost_threshold_);
private_nh.getParam("calib_safety_distance", calib_safety_distance_);
calc_plan_to_dock_.cost_threshold_ = cost_threshold_;
calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;
// plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false);
for(auto pt : footprint_dock.points)
{
geometry_msgs::Point footprint_point;
footprint_point.x = pt.x;
footprint_point.y = pt.y;
footprint_point.z = 0.0;
footprint_dock_.push_back(footprint_point);
}
initialized_ = true;
// ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
}
}
bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
{
if(!initialized_) return false;
if(!plan.empty()) plan.clear();
vector<geometry_msgs::Pose> planMoveToDock;
std::vector<geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
// footprint_dock_ = costmap_robot_->getRobotFootprint();
int degree = 2;
double step = 0.02;
bool status_makePlanMoveToDock = calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true);
robot::Time plan_time = robot::Time::now();
if(!status_makePlanMoveToDock) return false;
for(int i = 0; i < (int)planMoveToDock.size(); i++)
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = planMoveToDock[i].position.x;
pose.pose.position.y = planMoveToDock[i].position.y;
pose.pose.position.z = 0;
pose.pose.orientation = planMoveToDock[i].orientation;
plan.push_back(pose);
}
publishPlan(plan);
return true;
}
// void DockPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan)
// {
// nav_msgs::Path path_msg;
// path_msg.header.stamp = robot::Time::now();
// path_msg.header.frame_id = frame_id_;
// path_msg.poses = plan;
// plan_pub_.publish(path_msg);
// }
} // namespace dock_planner

1279
src/utils/curve_common.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

26
src/utils/line_common.cpp Normal file
View File

@@ -0,0 +1,26 @@
#include "dock_planner/utils/line_common.h"
double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2)
{
double A = y2 - y1;
double B = -(x2 - x1);
double C = -A * x1 - B * y1;
return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B);
}
bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance)
{
double crossProduct = distanceFromPointToLine(x0, y0, x1, y1, x2, y2);
if (fabs(crossProduct) > fabs(tolerance))
return false;
// Kiểm tra tọa độ của P có nằm giữa A và B không
if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) &&
y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2))
{
return true;
}
return false;
}

39
src/utils/pose.cpp Normal file
View File

@@ -0,0 +1,39 @@
#include "dock_planner/utils/pose.h"
bool vda_5050::Pose::SavePoseAsFile(const std::string& file_name)
{
bool result = false;
std::ofstream file(file_name);
if (file.is_open())
{
file << x_ << " " << y_ << " " << yaw_ << "\n";
file.close();
result = true;
}
else
{
result = false;
}
return result;
}
bool vda_5050::Pose::LoadPoseFromFile(const std::string& file_name)
{
bool result = false;
std::ifstream file(file_name);
if (file.is_open())
{
double x, y, yaw;
file >> x >> y >> yaw;
setPose(x, y, yaw);
file.close();
result = true;
}
else
{
result = false;
}
return result;
}