diff --git a/README.md b/README.md index e69de29..7e8b3b4 100644 --- a/README.md +++ b/README.md @@ -0,0 +1,312 @@ +# Dock Planner + +Global planner tao duong di cho robot tiep can tram dock (tram sac, tram hang) bang duong cong NURBS. + +## Cau truc thu muc + +``` +dock_planner/ +├── include/dock_planner/ +│ ├── dock_planner.h # Lop DockPlanner - plugin interface +│ ├── dock_calc.h # Lop DockCalc - logic tinh toan duong di +│ └── utils/ +│ ├── curve_common.h # Thu vien NURBS curve +│ ├── pose.h # Chuyen doi pose/quaternion +│ └── common.h # Tien ich chung +├── src/ +│ ├── dock_planner.cpp # Khoi tao, load param, goi DockCalc +│ ├── dock_calc.cpp # Thuat toan chinh: NURBS path generation +│ └── utils/ +│ ├── curve_common.cpp +│ ├── line_common.cpp +│ └── pose.cpp +└── config/ + └── dock_global_params.yaml # Tham so cau hinh +``` + +## Pipeline tong quan + +```mermaid +flowchart TB + subgraph INIT["1. KHOI TAO - DockPlanner::initialize"] + I1["Lien ket costmap_robot"] --> I2["Load ROS params:
cost_threshold, calib_safety_distance"] + I2 --> I3["Load footprint_dock
tu param server"] + end + + subgraph PLAN["2. LAP KE HOACH - DockPlanner::makePlan"] + P1["Lay footprint_robot
tu costmap"] --> P2["Goi DockCalc::makePlanMoveToDock"] + P2 --> P3["Chuyen vector Pose
thanh vector PoseStamped"] + end + + subgraph CALC["3. TINH TOAN - DockCalc::makePlanMoveToDock"] + C1["Tinh RobotGeometry + safety_distance"] + C2["Xac dinh huong tiep can
forward / backward"] + C3["Tinh angle_threshold
heuristic"] + C4["Tao duong tam NURBS
findPathTMP"] + C5["Tim control points
segment 1"] + C6["Sinh NURBS segment 1"] + C7["Sinh NURBS segment 2"] + C8["Ghep path: start + seg1 + seg2 + goal"] + C1 --> C2 --> C3 --> C4 --> C5 --> C6 --> C7 --> C8 + end + + INIT --> PLAN --> CALC +``` + +## So do giai thuat chi tiet + +### 1. Khoi tao (initialize) + +```mermaid +flowchart LR + A["DockPlanner::initialize(name, costmap_robot)"] --> B{Da initialized?} + B -- Co --> Z["return true"] + B -- Chua --> C["Lien ket costmap_robot_
costmap_
frame_id_"] + C --> D["getParam:
check_free_space
cost_threshold
calib_safety_distance"] + D --> E["Copy params
sang DockCalc"] + E --> F["footprintFromParams
→ footprint_dock_"] + F --> G["initialized_ = true"] + G --> Z +``` + +### 2. Lap ke hoach (makePlan) + +```mermaid +flowchart TD + A["makePlan(start, goal, plan)"] --> B{initialized_?} + B -- false --> FAIL["return false"] + B -- true --> C["footprint_robot =
costmap_robot_->getRobotFootprint()"] + C --> D["makePlanMoveToDock
(start, goal, footprint_robot,
footprint_dock_, degree=2, step=0.02)"] + D --> E{Thanh cong?} + E -- false --> FAIL + E -- true --> F["Chuyen moi Pose thanh PoseStamped
(gan timestamp, frame_id)"] + F --> G["return true"] +``` + +### 3. Thuat toan chinh (makePlanMoveToDock) + +```mermaid +flowchart TD + START["makePlanMoveToDock(start, goal)"] --> GEOM + + subgraph GEOM["BUOC 1: Tinh hinh hoc"] + G1["computeGeometry(footprint_robot)
→ robot.length, robot.radius"] + G2["computeGeometry(footprint_dock)
→ dock.length"] + G3["safety_distance =
(robot.radius + calib) * 2"] + G1 --> G3 + G2 --> G3 + end + + GEOM --> VALID{robot.length > 0
AND safety > 0?} + VALID -- false --> ERR["return false"] + VALID -- true --> POSE_C + + subgraph POSE_C["BUOC 2: Tinh diem trung gian C"] + PC1["distance_min_C =
(robot.length + dock.length) / 2"] + PC2["yaw_goal = normalizeAngle(goal.yaw)"] + PC3["Pose_C = offsetPoint(goal, yaw_goal, distance_min_C)"] + PC1 --> PC3 + PC2 --> PC3 + end + + POSE_C --> DIR + + subgraph DIR["BUOC 3: Xac dinh huong di"] + D1["t = compute_t(start, goal, Pose_C)"] + D2{t < 0 ?} + D3["Lat yaw_goal 180 do
Tinh lai Pose_C va t
dir = backward"] + D4["dir = forward"] + D1 --> D2 + D2 -- Co --> D3 + D2 -- Khong --> D4 + end + + DIR --> T_CHECK{t > 1 ?} + T_CHECK -- false --> ERR + T_CHECK -- true --> ANGLE + + subgraph ANGLE["BUOC 4: Tinh angle threshold"] + A1["Pose_D = compute_D(goal, Pose_C, t)"] + A2["distance_AD =
pointToSegmentDistance(start, goal, Pose_C)"] + A3["scale = f(distance_AD)
angle_threshold =
min((alpha+|scale|)*beta*pi, gamma*pi)"] + A1 --> A3 + A2 --> A3 + end + + ANGLE --> TMP_PATH + + subgraph TMP_PATH["BUOC 5: Tao duong tam"] + T1["Tinh Pose_F = offsetPoint(goal, yaw, dist_F)"] + T2["mid = trung diem(C, F)"] + T3["findPathTMP({C, mid, F})
→ saved_poses_tmp"] + T1 --> T2 --> T3 + end + + TMP_PATH --> TMP_OK{Path tmp ok?} + TMP_OK -- false --> ERR + TMP_OK -- true --> SEG1 + + subgraph SEG1["BUOC 6: Sinh segment 1"] + S1["findNURBSControlPoints
(start, saved_poses_tmp, angle_threshold)
→ control_points_1, dir"] + S2["Xac dinh chieu reverse
cua segment 1"] + S3["generateNURBSPath(control_points_1)
→ first_segment"] + S1 --> S2 --> S3 + end + + SEG1 --> SEG1_OK{first_segment
khong rong?} + SEG1_OK -- false --> ERR + SEG1_OK -- true --> SEG2 + + subgraph SEG2["BUOC 7: Sinh segment 2"] + S4["mid_ctrl = trung diem
(first_segment.back, goal)"] + S5["generateNURBSPath
({last_of_seg1, mid_ctrl, goal})
→ second_segment"] + S4 --> S5 + end + + SEG2 --> ASSEMBLE["BUOC 8: Ghep duong di
start + seg1 + seg2 + goal"] + ASSEMBLE --> OK["return true"] +``` + +### 4. Sinh duong NURBS (generateNURBSPath) + +```mermaid +flowchart TD + A["generateNURBSPath
(control_points, check_free,
edge_end, reverse)"] --> B["setupSpline(control_points, degree)
Tao knot vector + weights
Nap vao Spline_Inf"] + B --> C["Tinh chieu dai NURBS
calculateNURBSLength"] + C --> D["step_new = resolution / length
(adaptive step)"] + D --> E["Duyet t = 0 → 1
(buoc step_new)"] + + E --> F["point = CalculateCurvePoint(t)"] + F --> G{point hop le?
khong NaN?} + G -- false --> E + G -- true --> H{Kiem tra
isFreeSpace?} + H -- Bi chan --> I["obstacle_flag = true
BREAK"] + H -- Thong --> J["Luu pose
Cap nhat orientation"] + J --> E + + I --> K["result.clear()"] + E -- t > 1.0 --> L["Gan orientation
cho diem cuoi"] + L --> M["return result"] + K --> M +``` + +### 5. Tim control points (findNURBSControlPoints) + +```mermaid +flowchart TD + A["findNURBSControlPoints
(start, saved_poses, angle_threshold)"] --> B["findNearestPointOnPath
→ idx, dir"] + B --> C{idx != -1 ?} + + C -- idx hop le --> D["control_points = {start, saved[idx]}"] + D --> E{dir == 1?} + E -- Co --> F["Tim diem lui ve
sao cho dist >= target
→ end_idx"] + F --> G["Them saved[end_idx]"] + E -- Khong, dir == -1 --> H["Them saved.back()"] + + C -- idx == -1 --> I["mid = trung diem
(start, nearest)"] + I --> J["control_points =
{start, mid, nearest}"] + + G --> K["return true"] + H --> K + J --> K +``` + +### 6. Tim diem gan nhat tren duong (findNearestPointOnPath) + +```mermaid +flowchart TD + A["findNearestPointOnPath
(pose, saved_poses, angle_threshold)"] --> B["Tim nearest_idx
(khoang cach nho nhat)"] + B --> C["Duyet nguoc tu nearest_idx → 1"] + C --> D{signedAngle >= threshold?} + D -- Co --> E["nearest_idx = i
found_flag_1 = true"] + D -- Khong --> C + + E --> F{Kiem tra khoang cach
distance_2 >= distance_1?} + F -- Co --> G["dir = 1
return nearest_idx"] + F -- Khong --> H["Duyet xuoi tu nearest_idx → cuoi"] + C -- Het vong --> H + + H --> I{signedAngle >= threshold - 0.05?} + I -- Co --> J["dir = -1
return nearest_idx"] + I -- Khong, i == cuoi --> K["dir = 0
return -1"] + + J --> L{dist hop ly?} + L -- false --> K + L -- true --> J +``` + +## Mo ta cac diem hinh hoc + +``` + F ──────── mid ──────── C ──────── Goal + | (duong tam) | | + | | | + | Pose_D | | + | ╲ | | + | ╲ distance_AD | | + | ╲ | | + Start ╲ | | + | | + ◄── distance_min_F ────►| | + ◄── dist_C ──►| +``` + +- **Goal**: Vi tri dock dich +- **Pose_C**: Diem offset tu Goal theo huong yaw_goal, cach `(robot.length + dock.length) / 2` +- **Pose_F**: Diem xa hon tren cung huong, cach Goal = `dist_C + dist_CD + dist_DF` +- **Pose_D**: Hinh chieu vuong goc tu Start len duong Goal→C keo dai +- **distance_AD**: Khoang cach vuong goc tu Start den duong Goal→C +- **Duong tam (C → mid → F)**: Duong NURBS tam de tim diem noi segment 1 + +## Luong du lieu giua cac lop + +```mermaid +flowchart LR + subgraph Plugin["DockPlanner (Plugin Interface)"] + init["initialize()"] + make["makePlan()"] + end + + subgraph Core["DockCalc (Core Algorithm)"] + calc["makePlanMoveToDock()"] + nurbs["generateNURBSPath()"] + tmp["findPathTMP()"] + ctrl["findNURBSControlPoints()"] + near["findNearestPointOnPath()"] + setup["setupSpline()"] + end + + subgraph Lib["Utils"] + curve["CurveCommon
(NURBS engine)"] + spline["Spline_Inf
(knot/ctrl/weight)"] + end + + init -->|"config params"| calc + make -->|"start, goal,
footprints"| calc + calc --> tmp + calc --> ctrl + calc --> nurbs + ctrl --> near + tmp --> setup + nurbs --> setup + setup --> curve + setup --> spline + curve -->|"CalculateCurvePoint"| nurbs + curve -->|"CalculateCurvePoint"| tmp +``` + +## Tham so cau hinh + +| Tham so | Kieu | Mac dinh | Mo ta | +|---------|------|----------|-------| +| `cost_threshold` | int | 200 | Nguong chi phi vat can (0-255) | +| `calib_safety_distance` | double | 0.0 | Khoang cach an toan bo sung (met) | +| `check_free_space` | bool | false | Bat/tat kiem tra vung tu do | +| `degree` (hardcode) | int | 2 | Bac cua duong cong NURBS | +| `step` (hardcode) | double | 0.02 | Buoc lay mau tham so t | + +## Ghi chu + +- **NURBS (Non-Uniform Rational B-Spline)**: Duong cong bac 2 voi 3 control points, knot vector dang clamped `[0,0,0,1,1,1]`. +- **Adaptive step**: Buoc lay mau duoc tinh tu `resolution / curve_length` de dam bao mat do diem dong deu theo do phan giai costmap. +- **2-segment path**: Duong di gom 2 doan NURBS noi tiep — segment 1 dua robot tu vi tri hien tai vao duong tiep can, segment 2 dua robot tu diem noi den dock. diff --git a/include/dock_planner/dock_calc.h b/include/dock_planner/dock_calc.h index 6fcad81..c873ad4 100644 --- a/include/dock_planner/dock_calc.h +++ b/include/dock_planner/dock_calc.h @@ -5,12 +5,9 @@ #include #include -#include -#include #include #include -#include #include @@ -19,10 +16,9 @@ #include "dock_planner/utils/common.h" #include +#include #include -using namespace std; - namespace dock_planner { struct RobotGeometry @@ -35,14 +31,11 @@ namespace dock_planner class DockCalc { private: - /* data */ - Spline_Inf* input_spline_inf; - CurveCommon* CurveDesign; + std::unique_ptr input_spline_inf; + std::unique_ptr CurveDesign; - vector posesOnPathWay; - vector footprint_robot_; + std::vector footprint_robot_; robot_geometry_msgs::PoseStamped save_goal_; - // std::vector free_zone_; bool check_goal_; int store_start_nearest_idx_; @@ -55,9 +48,10 @@ namespace dock_planner return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y); } - inline double calculateAdaptiveStep(double segment_length, double desired_point_spacing = 0.02) + inline double calculateAdaptiveStep(double segment_length, double desired_point_spacing = 0.02) { - return desired_point_spacing / segment_length; + if (segment_length <= 1e-9) return 1.0; + return desired_point_spacing / segment_length; } inline double calculateAngle(double x1, double y1, double x2, double y2) @@ -66,13 +60,9 @@ namespace dock_planner return angle; } - inline double normalizeAngle(double angle) + inline double normalizeAngle(double angle) { - while (angle > M_PI) - angle -= 2.0 * M_PI; - while (angle <= -M_PI) - angle += 2.0 * M_PI; - return angle; + return std::remainder(angle, 2.0 * M_PI); } double calculateNURBSLength(CurveCommon* curve_design, @@ -90,15 +80,17 @@ namespace dock_planner robot_geometry_msgs::Pose compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C,const double& t); + void setupSpline(const std::vector& control_points, int degree); + void updatePoseOrientation(std::vector& saved_poses, std::vector& nurbs_path, bool reverse = true); - bool findPathTMP(const vector& control_points, - vector& saved_poses, + bool findPathTMP(const std::vector& control_points, + std::vector& saved_poses, const int& degree, double step = 0.02); - bool findNURBSControlPoints(int& dir, vector& control_points, + bool findNURBSControlPoints(int& dir, std::vector& control_points, const robot_geometry_msgs::Pose& start_pose, const std::vector& posesOnPathWay, const double& angle_threshol, const int& degree); @@ -122,20 +114,20 @@ namespace dock_planner //Params robot_costmap_2d::Costmap2D* costmap_; - int cost_threshold_; // Threshold for obstacle detection - double safety_distance_; // Safety distance from obstacles - double calib_safety_distance_; + int cost_threshold_; // Threshold for obstacle detection + double safety_distance_; // Derived = (robot.radius + calib_safety_distance_) * 2.0, recomputed each plan + double calib_safety_distance_; // Configured via ROS param "calib_safety_distance" - DockCalc(/* args */); + DockCalc(); ~DockCalc(); bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& goal, - const vector& footprint_robot, - const vector& footprint_dock, + const std::vector& footprint_robot, + const std::vector& footprint_dock, const int& degree, const double& step, - vector& planMoveToDock, + std::vector& planMoveToDock, bool reverse = false); diff --git a/include/dock_planner/dock_planner.h b/include/dock_planner/dock_planner.h index 1dc87be..d0ed37d 100644 --- a/include/dock_planner/dock_planner.h +++ b/include/dock_planner/dock_planner.h @@ -27,7 +27,6 @@ namespace dock_planner { robot_costmap_2d::Costmap2D* costmap_; bool initialized_; std::string frame_id_; - // ros::Publisher plan_pub_; std::vector footprint_dock_; int cost_threshold_; diff --git a/src/dock_calc.cpp b/src/dock_calc.cpp index 0832ad3..8e197d0 100644 --- a/src/dock_calc.cpp +++ b/src/dock_calc.cpp @@ -2,22 +2,22 @@ namespace dock_planner { - DockCalc::DockCalc(/* args */) :costmap_(nullptr) + DockCalc::DockCalc() + : input_spline_inf(std::make_unique()), + CurveDesign(std::make_unique()), + check_goal_(false), + store_start_nearest_idx_(0), + start_target_idx_(0), + min_distance_to_goal_(0.0), + costmap_(nullptr), + cost_threshold_(200), + safety_distance_(0.0), + calib_safety_distance_(0.0) { - 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; } + DockCalc::~DockCalc() = default; + robot_geometry_msgs::Pose DockCalc::offsetPoint(const robot_geometry_msgs::Pose& A, const double& theta, const double& d) { robot_geometry_msgs::Pose B; @@ -77,47 +77,73 @@ namespace dock_planner return D; } + void DockCalc::setupSpline(const std::vector& control_points, int degree) + { + std::vector> eigen_points; + eigen_points.reserve(control_points.size()); + for (const auto& cp : control_points) + { + eigen_points.emplace_back(cp.position.x, cp.position.y, 0); + } + + const int n = static_cast(control_points.size()) - 1; + const int m = n + degree + 1; + std::vector knot_vector; + knot_vector.reserve(m + 1); + for (int i = 0; i <= m; i++) + { + knot_vector.push_back(i <= degree ? 0.0 : 1.0); + } + + std::vector weights(control_points.size(), 1.0); + + input_spline_inf->knot_vector.clear(); + input_spline_inf->control_point.clear(); + input_spline_inf->weight.clear(); + + CurveDesign->ReadSplineInf(input_spline_inf.get(), degree + 1, eigen_points, knot_vector); + CurveDesign->ReadSplineInf(input_spline_inf.get(), weights, false); + } + RobotGeometry DockCalc::computeGeometry(const std::vector& fp) { - double min_x = 1e9, max_x = -1e9; - double min_y = 1e9, max_y = -1e9; + if (fp.size() < 4) return {0, 0, 0}; + + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + double min_y = std::numeric_limits::max(); + double max_y = std::numeric_limits::lowest(); double radius = 0.0; - if((int)fp.size() >= 4) + + for (const auto& p : fp) { - 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)); - } + 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}; + return {max_x - min_x, max_y - min_y, radius}; } bool DockCalc::makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start, const robot_geometry_msgs::PoseStamped& goal, - const vector& footprint_robot, - const vector& footprint_dock, + const std::vector& footprint_robot, + const std::vector& footprint_dock, const int& degree, const double& step, - vector& planMoveToDock, + std::vector& planMoveToDock, bool reverse) { constexpr double epsilon = 1e-6; - robot::log_error("[dock_calc] makePlanMoveToDock start: (%f, %f), goal: (%f, %f), goal frame id: %s", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y, goal.header.frame_id.c_str()); + robot::log_info("[dock_calc] makePlanMoveToDock start: (%f, %f), goal: (%f, %f), goal frame id: %s", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y, goal.header.frame_id.c_str()); planMoveToDock.clear(); if(save_goal_.pose.position.x != goal.pose.position.x || save_goal_.pose.position.y != goal.pose.position.y || 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; + save_goal_ = goal; } footprint_robot_ = footprint_robot; @@ -132,58 +158,31 @@ namespace dock_planner return false; } - // Calculate safety distance safety_distance_ = (robot.radius + calib_safety_distance_) * 2.0; - if (safety_distance_ <= epsilon) + if (safety_distance_ <= epsilon) { robot::log_error("[dock_calc] Safety distance is zero, cannot calculate plan to dock."); return false; } // Lambda: Generate NURBS path - auto generateNURBSPath = [&](const vector& control_points, bool check_isFreeSpace, - bool edge_end_plag, bool path_reverse) -> vector + auto generateNURBSPath = [&](const std::vector& control_points, bool check_isFreeSpace, + bool edge_end_plag, bool path_reverse) -> std::vector { - vector result; + std::vector result; bool obstacle_flag = false; - // Convert to Eigen format - std::vector> eigen_points; - for (const auto& cp : control_points) - { - eigen_points.emplace_back(cp.position.x, cp.position.y, 0); - } - - // Create knot vector - std::vector knot_vector; - int n = control_points.size() - 1; - int m = n + degree + 1; - for (int i = 0; i <= m; i++) - { - knot_vector.push_back(i <= degree ? 0.0 : 1.0); - } - - // Setup spline - std::vector weights(control_points.size(), 1.0); - input_spline_inf->knot_vector.clear(); - input_spline_inf->control_point.clear(); - input_spline_inf->weight.clear(); - - CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_points, knot_vector); - CurveDesign->ReadSplineInf(input_spline_inf, weights, false); + setupSpline(control_points, degree); // Calculate adaptive step - double edge_length = calculateNURBSLength(CurveDesign, input_spline_inf, degree, step); + double edge_length = calculateNURBSLength(CurveDesign.get(), input_spline_inf.get(), degree, step); double resolution = costmap_->getResolution(); double step_new = calculateAdaptiveStep(edge_length, resolution); - // robot::log_error("[dock_calc] Adaptive step size: %f", step_new); - - // Generate path points - vector saved_poses; - for (double t = 0.0; t <= 1.0; t += step_new) + std::vector saved_poses; + for (double t = 0.0; t <= 1.0; t += step_new) { - robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); + robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf.get(), t, true); if (!std::isnan(point.x) && !std::isnan(point.y)) { @@ -256,7 +255,6 @@ namespace dock_planner }; // 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); @@ -291,16 +289,22 @@ namespace dock_planner 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; + // Angle threshold heuristic: linearly scale [MIN_DISTANCE..MAX_DISTANCE] of + // perpendicular distance AD into an angle in [alpha*beta*pi .. gamma*pi]. + // Closer to the line → larger angle threshold (more permissive turn). + constexpr double MAX_DISTANCE = 1.0; // distance at which scale = MIN_SCALE + constexpr double MIN_DISTANCE = 0.1; // distance at which scale = MAX_SCALE + constexpr double MIN_SCALE = 0.2; + constexpr double MAX_SCALE = 1.0; + constexpr double CALIB_FACTOR_ALPHA = 0.5; // base offset added to |scale| + constexpr double CALIB_FACTOR_BETA = 0.5; // multiplier applied to (alpha + |scale|) + constexpr double CALIB_FACTOR_GAMMA = 0.77; // upper clamp expressed as fraction of pi - double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * + const 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); + const double angle_threshold = std::min( + (CALIB_FACTOR_ALPHA + std::fabs(scale)) * CALIB_FACTOR_BETA * M_PI, + CALIB_FACTOR_GAMMA * M_PI); // Generate temporary path double distance_CD = calc_distance(Pose_C, Pose_D); @@ -313,8 +317,8 @@ namespace dock_planner mid_pose.position.x = (Pose_C.position.x + Pose_F.position.x) / 2.0; mid_pose.position.y = (Pose_C.position.y + Pose_F.position.y) / 2.0; - vector control_points_tmp = {Pose_C, mid_pose, Pose_F}; - vector saved_poses_tmp; + std::vector control_points_tmp = {Pose_C, mid_pose, Pose_F}; + std::vector saved_poses_tmp; if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) @@ -325,7 +329,7 @@ namespace dock_planner // Find first segment control points int dir; - vector control_points_1; + std::vector control_points_1; if (!findNURBSControlPoints(dir, control_points_1, start.pose, saved_poses_tmp, angle_threshold, degree)) { robot::log_error("[dock_calc] Failed to find NURBS control points for first segment."); @@ -337,70 +341,41 @@ namespace dock_planner ((dir == 1 || dir == 0) && dir_robot_move_to_goal); // Generate first segment - vector first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); + std::vector first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); + if (first_segment.empty()) return false; + + // Generate second segment (to goal) + robot_geometry_msgs::Pose mid_control_point; + mid_control_point.position.x = (first_segment.back().position.x + goal.pose.position.x) / 2.0; + mid_control_point.position.y = (first_segment.back().position.y + goal.pose.position.y) / 2.0; + + std::vector control_points_2 = {first_segment.back(), + mid_control_point, + goal.pose}; + + std::vector second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); + + // Assemble final path: start + first_segment + second_segment + goal + planMoveToDock.reserve(1 + first_segment.size() + second_segment.size() + 1); + planMoveToDock.push_back(start.pose); planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end()); + planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end()); + planMoveToDock.push_back(goal.pose); - //Generate second segment (to goal) - if (!planMoveToDock.empty()) - { - robot_geometry_msgs::Pose mid_control_point; - mid_control_point.position.x = (planMoveToDock.back().position.x + goal.pose.position.x) / 2.0; - mid_control_point.position.y = (planMoveToDock.back().position.y + goal.pose.position.y) / 2.0; - - vector control_points_2 = {planMoveToDock.back(), - mid_control_point, - goal.pose}; - - vector second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); - planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end()); - planMoveToDock.insert(planMoveToDock.begin(),start.pose); - planMoveToDock.insert(planMoveToDock.end(),goal.pose); - } - - return !planMoveToDock.empty(); + return true; } - bool DockCalc::findPathTMP(const vector& control_points, - vector& saved_poses, + bool DockCalc::findPathTMP(const std::vector& control_points, + std::vector& saved_poses, const int& degree, double step) { - // Chuyển đổi control points sang định dạng Eigen::Vector3d - std::vector> eigen_control_points; - for(const auto& cp : control_points) + setupSpline(control_points, degree); + + for(double t = 0.0; t <= 1.0; t += step) { - Eigen::Vector3d point(cp.position.x, cp.position.y, 0); - eigen_control_points.push_back(point); - } - // Tạo knot vector tự động - std::vector knot_vector; - int n = control_points.size() - 1; // n + 1 control points - int m = n + degree + 1; // m + 1 knots - // Tạo uniform knot vector - for(int i = 0; i <= m; i++) - { - if(i <= degree) knot_vector.push_back(0.0); - else knot_vector.push_back(1.0); - } - - // Điều chỉnh weights để tăng ảnh hưởng của các control points - std::vector weights(control_points.size(), 1.0); - - input_spline_inf->knot_vector.clear(); - input_spline_inf->control_point.clear(); - input_spline_inf->weight.clear(); - - // Cài đặt thông số cho spline - CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_control_points, knot_vector); - CurveDesign->ReadSplineInf(input_spline_inf, weights, false); - - // vector poses_tmp; - for(double t = 0.0; t <= 1.0; t += step) - { - // robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t); - robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); - if(!std::isnan(point.x) && !std::isnan(point.y)) + robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf.get(), 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; @@ -434,29 +409,23 @@ namespace dock_planner void DockCalc::updatePoseOrientation(std::vector& saved_poses, std::vector& nurbs_path, - bool reverse) + 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); + auto& prev = saved_poses[saved_poses.size() - 2]; + const auto& curr = saved_poses.back(); + + const double yaw = reverse + ? calculateAngle(curr.position.x, curr.position.y, prev.position.x, prev.position.y) + : calculateAngle(prev.position.x, prev.position.y, curr.position.x, curr.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(); + prev.orientation.x = q.x(); + prev.orientation.y = q.y(); + prev.orientation.z = q.z(); + prev.orientation.w = q.w(); - nurbs_path.push_back(saved_poses[saved_poses.size() - 2]); + nurbs_path.push_back(prev); } double DockCalc::calculateNURBSLength(CurveCommon* curve_design, @@ -466,33 +435,24 @@ namespace dock_planner { double length = 0.0; robot_geometry_msgs::Point prev_point, curr_point; - std::vector segment_lengths; - static double step; - // Đường cong bậc 1 (đường thẳng) thì bước nhảy bằng 0.1 - if(degree == 1) step = 0.1; - // Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step - else step = initial_step; + const double step = (degree == 1) ? 0.1 : initial_step; - // Tính độ dài với step - for(double u = 0; u <= 1; u += 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)) + 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); + length += std::hypot(curr_point.x - prev_point.x, + curr_point.y - prev_point.y); } prev_point = curr_point; } return length; } - bool DockCalc::findNURBSControlPoints(int& dir, vector& control_points, + bool DockCalc::findNURBSControlPoints(int& dir, std::vector& control_points, const robot_geometry_msgs::Pose& start_pose, const std::vector& saved_poses, const double& angle_threshold, const int& degree) @@ -506,8 +466,7 @@ namespace dock_planner 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 + auto createPose = [](double x, double y) -> robot_geometry_msgs::Pose { robot_geometry_msgs::Pose pose; pose.position.x = x; @@ -551,33 +510,25 @@ namespace dock_planner 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) + 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 + 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 + (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_; } @@ -607,7 +558,7 @@ namespace dock_planner bool found_pose_flag_1 = false; bool found_pose_flag_2 = false; - for(int i = store_start_nearest_idx_; i >= 0; i--) + for(int i = store_start_nearest_idx_; i >= 1; i--) { double angle = signedAngle(pose, saved_poses[i], saved_poses[i - 1]); @@ -629,7 +580,8 @@ namespace dock_planner if(!found_pose_flag_2) { - for(int i = store_start_nearest_idx_; i < (int)saved_poses.size(); i++) + const int last_idx = (int)saved_poses.size() - 1; + for(int i = store_start_nearest_idx_; i < last_idx; i++) { double angle = signedAngle(pose, saved_poses[i], saved_poses[i + 1]); @@ -639,7 +591,7 @@ namespace dock_planner dir = -1; break; } - if(i == (int)saved_poses.size() - 1) + if(i == last_idx - 1) { dir = 0; return -1; @@ -657,91 +609,10 @@ namespace dock_planner return nearest_idx; } - bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose, - const std::vector& footprint) + bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose, + const std::vector& footprint) { - // static std::vector free_zone; - - // auto makePose = [](double x, double y, double yaw) - // { - // robot_geometry_msgs::Pose p; - // p.position.x = x; - // p.position.y = y; - // p.position.z = 0.0; - - // tf3::Quaternion q; - // q.setRPY(0.0, 0.0, yaw); - // p.orientation.x = q.x(); - // p.orientation.y = q.y(); - // p.orientation.z = q.z(); - // p.orientation.w = q.w(); - // return p; - // }; - // unsigned int mx, my; - - // if (!costmap_->worldToMap(pose.position.x, pose.position.y, mx, my)) - // { - // // robot::log_error("[dock_calc][isFreeSpace]Debug 1"); - // return false; // pose nằm ngoài bản đồ - // } - - // unsigned char center_cost = costmap_->getCost(mx, my); - // if (center_cost >= cost_threshold_) - // { - // // robot::log_error("[dock_calc][isFreeSpace]Debug 2"); - // return false; // chính giữa đã bị chiếm - // } - - // double resolution = costmap_->getResolution(); - - // if(check_goal_) - // { - // std::vector result_free_zone = calcFreeZone(pose, footprint); - // if((int)result_free_zone.size() < 4) return false; - // free_zone.push_back(makePose(result_free_zone[0].x, result_free_zone[0].y, 0.0)); - // free_zone.push_back(makePose(result_free_zone[1].x, result_free_zone[1].y, 0.0)); - // free_zone.push_back(makePose(result_free_zone[2].x, result_free_zone[2].y, 0.0)); - // check_goal_ = false; - // } - // std::vector space_footprint = interpolateFootprint(pose, footprint, resolution); - // for (int i = 0; i < (int)space_footprint.size(); i++) - // { - // robot_geometry_msgs::Pose Pose_on_footprint; - // Pose_on_footprint.position.x = space_footprint[i].x; - // Pose_on_footprint.position.y = space_footprint[i].y; - // if((int)free_zone.size() < 3) return false; - // double t_L = compute_t(Pose_on_footprint, free_zone[0], free_zone[1]); - // double t_W = compute_t(Pose_on_footprint, free_zone[1], free_zone[2]); - // if(t_L >= 0 && t_L <= 1 && t_W >= 0 && t_W <= 1) - // { - // return true; - // } - // if (!costmap_->worldToMap(space_footprint[i].x, space_footprint[i].y, mx, my)) - // { - // // robot::log_error("[dock_calc][isFreeSpace]Debug 3"); - // return false; // pose nằm ngoài bản đồ - // } - // int check_x = mx; - // int check_y = my; - - // if (check_x >= 0 && check_x < costmap_->getSizeInCellsX() && - // check_y >= 0 && check_y < costmap_->getSizeInCellsY()) - // { - // unsigned char cost = costmap_->getCost(check_x, check_y); - // if (cost >= robot_costmap_2d::LETHAL_OBSTACLE) - // { - // double dx = pose.position.x - space_footprint[i].x; - // double dy = pose.position.y - space_footprint[i].y; - // double dist = std::hypot(dx, dy); - // if (dist <= (safety_distance_/2.0)) - // { - // // robot::log_error("[dock_calc][isFreeSpace]Debug dist: %f", dist); - // return false; - // } - // } - // } - // } - + // TODO: implement obstacle checking using costmap_ return true; } @@ -784,10 +655,11 @@ namespace dock_planner } std::vector points; - for (size_t i = 0; i < (int)abs_footprint.size(); ++i) + const size_t fp_size = abs_footprint.size(); + for (size_t i = 0; i < fp_size; ++i) { const robot_geometry_msgs::Point &start = abs_footprint[i]; - const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % fp_size]; double dx = end.x - start.x; double dy = end.y - start.y; @@ -796,13 +668,13 @@ namespace dock_planner for (int j = 0; j <= steps; ++j) { - if (j == steps && i != (int)abs_footprint.size() - 1) - continue; // tránh lặp - double t = static_cast(j) / steps; - robot_geometry_msgs::Point pt; - pt.x = start.x + t * dx; - pt.y = start.y + t * dy; - points.push_back(pt); + if (j == steps && i != fp_size - 1) + continue; // tránh lặp điểm chung giữa 2 cạnh liên tiếp + double t = static_cast(j) / steps; + robot_geometry_msgs::Point pt; + pt.x = start.x + t * dx; + pt.y = start.y + t * dy; + points.push_back(pt); } } return points; diff --git a/src/dock_planner.cpp b/src/dock_planner.cpp index 10af462..b1db3f0 100644 --- a/src/dock_planner.cpp +++ b/src/dock_planner.cpp @@ -9,12 +9,12 @@ 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), + DockPlanner::DockPlanner() : costmap_robot_(nullptr), + costmap_(nullptr), + initialized_(false), cost_threshold_(200), - calib_safety_distance_(0.0) {} + calib_safety_distance_(0.0), + check_free_space_(false) {} /** * @brief Constructor với parameters @@ -22,13 +22,13 @@ namespace dock_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, robot_costmap_2d::Costmap2DROBOT* costmap_robot) - : costmap_robot_(nullptr), - costmap_(nullptr), - initialized_(false), - check_free_space_(false), + DockPlanner::DockPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) + : costmap_robot_(nullptr), + costmap_(nullptr), + initialized_(false), cost_threshold_(200), - calib_safety_distance_(0.0) + calib_safety_distance_(0.0), + check_free_space_(false) { initialize(name, costmap_robot); } @@ -64,9 +64,7 @@ namespace dock_planner calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_; - // plan_pub_ = private_nh.advertise("plan", 1); - - robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false); + robot_nav_2d_msgs::Polygon2D footprint_dock = robot_nav_2d_utils::footprintFromParams(private_nh, false); for(auto pt : footprint_dock.points) { @@ -78,7 +76,6 @@ namespace dock_planner } initialized_ = true; - // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_); } return true; } @@ -88,49 +85,33 @@ namespace dock_planner std::vector& plan) { if(!initialized_) return false; - if(!plan.empty()) plan.clear(); - vector planMoveToDock; - // std::vector footprint_robot = costmap_robot_->getRobotFootprint(); - std::vector footprint_robot; - robot_geometry_msgs::Point p1; p1.x = 0.1; p1.y = -0.1; - robot_geometry_msgs::Point p2; p2.x = 0.1; p2.y = 0.1; - robot_geometry_msgs::Point p3; p3.x = -0.1; p3.y = 0.1; - robot_geometry_msgs::Point p4; p4.x = -0.1; p4.y = -0.1; - footprint_robot.push_back(p1); - footprint_robot.push_back(p2); - footprint_robot.push_back(p3); - footprint_robot.push_back(p4); + plan.clear(); - // 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(); + std::vector planMoveToDock; + std::vector footprint_robot = costmap_robot_->getRobotFootprint(); - if(!status_makePlanMoveToDock) return false; - for(int i = 0; i < (int)planMoveToDock.size(); i++) + constexpr int degree = 2; + constexpr double step = 0.02; + if (!calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true)) + return false; + + const robot::Time plan_time = robot::Time::now(); + const std::string& global_frame = costmap_robot_->getGlobalFrameID(); + plan.reserve(planMoveToDock.size()); + for (const auto& p : planMoveToDock) { - robot_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); + robot_geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = global_frame; + pose.pose.position.x = p.position.x; + pose.pose.position.y = p.position.y; + pose.pose.position.z = 0; + pose.pose.orientation = p.orientation; + plan.push_back(pose); } return true; } - // void DockPlanner::publishPlan(const std::vector& plan) - // { - // robot_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); - // } - // Export factory function robot_nav_core::BaseGlobalPlanner::Ptr DockPlanner::create() { return std::make_shared();