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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:11:02 +07:00
parent c5c55147b7
commit 92264eaffc
8 changed files with 141 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
src/dock_calc.cpp
View File

@@ -18,15 +18,15 @@ namespace dock_planner
delete input_spline_inf;
}
geometry_msgs::Pose DockCalc::offsetPoint(const geometry_msgs::Pose& A, const double& theta, const double& d)
robot_geometry_msgs::Pose DockCalc::offsetPoint(const robot_geometry_msgs::Pose& A, const double& theta, const double& d)
{
geometry_msgs::Pose B;
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 geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C)
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));
@@ -35,7 +35,7 @@ namespace dock_planner
return distance;
}
double DockCalc::signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C)
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;
@@ -50,7 +50,7 @@ namespace dock_planner
return angle;
}
double DockCalc::compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C)
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;
@@ -69,15 +69,15 @@ namespace dock_planner
return -numerator / denominator;
}
geometry_msgs::Pose DockCalc::compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C, const double& t)
robot_geometry_msgs::Pose DockCalc::compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C, const double& t)
{
geometry_msgs::Pose D;
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<geometry_msgs::Point>& fp)
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;
@@ -99,12 +99,12 @@ namespace dock_planner
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,
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<geometry_msgs::Pose>& planMoveToDock,
vector<robot_geometry_msgs::Pose>& planMoveToDock,
bool reverse)
{
constexpr double epsilon = 1e-6;
@@ -141,10 +141,10 @@ namespace dock_planner
}
// 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>
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<geometry_msgs::Pose> result;
vector<robot_geometry_msgs::Pose> result;
bool obstacle_flag = false;
// Convert to Eigen format
@@ -180,14 +180,14 @@ namespace dock_planner
// robot::log_error("[dock_calc] Adaptive step size: %f", step_new);
// Generate path points
vector<geometry_msgs::Pose> saved_poses;
vector<robot_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);
robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
if (!std::isnan(point.x) && !std::isnan(point.y))
{
geometry_msgs::Pose pose;
robot_geometry_msgs::Pose pose;
pose.position = point;
pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
@@ -261,9 +261,9 @@ namespace dock_planner
min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0);
// Calculate pose C (offset from goal)
geometry_msgs::Pose Goal_Pose = goal.pose;
robot_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);
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);
@@ -288,7 +288,7 @@ namespace dock_planner
}
// Calculate pose D and distance AD
geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t);
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
@@ -308,13 +308,13 @@ namespace dock_planner
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;
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<geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
vector<geometry_msgs::Pose> saved_poses_tmp;
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))
@@ -325,7 +325,7 @@ namespace dock_planner
// Find first segment control points
int dir;
vector<geometry_msgs::Pose> control_points_1;
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.");
@@ -337,21 +337,21 @@ namespace dock_planner
((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);
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())
{
geometry_msgs::Pose mid_control_point;
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<geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(),
vector<robot_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);
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);
@@ -360,8 +360,8 @@ namespace dock_planner
return !planMoveToDock.empty();
}
bool DockCalc::findPathTMP(const vector<geometry_msgs::Pose>& control_points,
vector<geometry_msgs::Pose>& saved_poses,
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
@@ -393,15 +393,15 @@ namespace dock_planner
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;
// 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);
geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true);
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);
geometry_msgs::Pose pose;
robot_geometry_msgs::Pose pose;
pose.position = point;
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
@@ -432,8 +432,8 @@ namespace dock_planner
return true;
}
void DockCalc::updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses,
std::vector<geometry_msgs::Pose>& nurbs_path,
void DockCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
std::vector<robot_geometry_msgs::Pose>& nurbs_path,
bool reverse)
{
double yaw;
@@ -465,7 +465,7 @@ namespace dock_planner
double initial_step)
{
double length = 0.0;
geometry_msgs::Point prev_point, curr_point;
robot_geometry_msgs::Point prev_point, curr_point;
std::vector<double> segment_lengths;
static double step;
@@ -492,9 +492,9 @@ namespace dock_planner
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,
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())
@@ -507,9 +507,9 @@ namespace dock_planner
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
auto createPose = [](double x, double y, double yaw = 0.0) -> robot_geometry_msgs::Pose
{
geometry_msgs::Pose pose;
robot_geometry_msgs::Pose pose;
pose.position.x = x;
pose.position.y = y;
pose.orientation = data_convert::createQuaternionMsgFromYaw(0);
@@ -584,8 +584,8 @@ namespace dock_planner
return true;
}
int DockCalc::findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Pose>& saved_poses,
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
@@ -657,14 +657,14 @@ namespace dock_planner
return nearest_idx;
}
bool DockCalc::isFreeSpace(const geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint)
bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose,
const std::vector<robot_geometry_msgs::Point>& footprint)
{
// static std::vector<geometry_msgs::Pose> free_zone;
// static std::vector<robot_geometry_msgs::Pose> free_zone;
// auto makePose = [](double x, double y, double yaw)
// {
// geometry_msgs::Pose p;
// robot_geometry_msgs::Pose p;
// p.position.x = x;
// p.position.y = y;
// p.position.z = 0.0;
@@ -696,17 +696,17 @@ namespace dock_planner
// if(check_goal_)
// {
// std::vector<geometry_msgs::Point> result_free_zone = calcFreeZone(pose, footprint);
// 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<geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution);
// std::vector<robot_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;
// 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;
@@ -745,11 +745,11 @@ namespace dock_planner
return true;
}
std::vector<geometry_msgs::Point> DockCalc::calcFreeZone(const geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint)
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<geometry_msgs::Point> free_zone;
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)
@@ -757,7 +757,7 @@ namespace dock_planner
pt.x *= 1.2;
pt.y *= 1.2;
geometry_msgs::Point abs_pt;
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);
@@ -765,29 +765,29 @@ namespace dock_planner
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)
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<geometry_msgs::Point> abs_footprint;
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;
geometry_msgs::Point abs_pt;
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<geometry_msgs::Point> points;
std::vector<robot_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()];
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;
@@ -799,7 +799,7 @@ namespace dock_planner
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;
robot_geometry_msgs::Point pt;
pt.x = start.x + t * dx;
pt.y = start.y + t * dy;
points.push_back(pt);