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

optimal and add file readme

Browse Source
This commit is contained in:
hoangson02
2026-04-16 02:38:25 +00:00
parent 03907b9613
commit 91ba7b1569
5 changed files with 518 additions and 362 deletions
Download Patch File Download Diff File Expand all files Collapse all files

312
README.md
View File

@@ -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:<br/>cost_threshold, calib_safety_distance"]
I2 --> I3["Load footprint_dock<br/>tu param server"]
end
subgraph PLAN["2. LAP KE HOACH - DockPlanner::makePlan"]
P1["Lay footprint_robot<br/>tu costmap"] --> P2["Goi DockCalc::makePlanMoveToDock"]
P2 --> P3["Chuyen vector Pose<br/>thanh vector PoseStamped"]
end
subgraph CALC["3. TINH TOAN - DockCalc::makePlanMoveToDock"]
C1["Tinh RobotGeometry + safety_distance"]
C2["Xac dinh huong tiep can<br/>forward / backward"]
C3["Tinh angle_threshold<br/>heuristic"]
C4["Tao duong tam NURBS<br/>findPathTMP"]
C5["Tim control points<br/>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_<br/>costmap_<br/>frame_id_"]
C --> D["getParam:<br/>check_free_space<br/>cost_threshold<br/>calib_safety_distance"]
D --> E["Copy params<br/>sang DockCalc"]
E --> F["footprintFromParams<br/>→ 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 =<br/>costmap_robot_->getRobotFootprint()"]
C --> D["makePlanMoveToDock<br/>(start, goal, footprint_robot,<br/>footprint_dock_, degree=2, step=0.02)"]
D --> E{Thanh cong?}
E -- false --> FAIL
E -- true --> F["Chuyen moi Pose thanh PoseStamped<br/>(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)<br/>→ robot.length, robot.radius"]
G2["computeGeometry(footprint_dock)<br/>→ dock.length"]
G3["safety_distance =<br/>(robot.radius + calib) * 2"]
G1 --> G3
G2 --> G3
end
GEOM --> VALID{robot.length > 0<br/>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 =<br/>(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<br/>Tinh lai Pose_C va t<br/>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 =<br/>pointToSegmentDistance(start, goal, Pose_C)"]
A3["scale = f(distance_AD)<br/>angle_threshold =<br/>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})<br/>→ 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<br/>(start, saved_poses_tmp, angle_threshold)<br/>→ control_points_1, dir"]
S2["Xac dinh chieu reverse<br/>cua segment 1"]
S3["generateNURBSPath(control_points_1)<br/>→ first_segment"]
S1 --> S2 --> S3
end
SEG1 --> SEG1_OK{first_segment<br/>khong rong?}
SEG1_OK -- false --> ERR
SEG1_OK -- true --> SEG2
subgraph SEG2["BUOC 7: Sinh segment 2"]
S4["mid_ctrl = trung diem<br/>(first_segment.back, goal)"]
S5["generateNURBSPath<br/>({last_of_seg1, mid_ctrl, goal})<br/>→ second_segment"]
S4 --> S5
end
SEG2 --> ASSEMBLE["BUOC 8: Ghep duong di<br/>start + seg1 + seg2 + goal"]
ASSEMBLE --> OK["return true"]
```
### 4. Sinh duong NURBS (generateNURBSPath)
```mermaid
flowchart TD
A["generateNURBSPath<br/>(control_points, check_free,<br/>edge_end, reverse)"] --> B["setupSpline(control_points, degree)<br/>Tao knot vector + weights<br/>Nap vao Spline_Inf"]
B --> C["Tinh chieu dai NURBS<br/>calculateNURBSLength"]
C --> D["step_new = resolution / length<br/>(adaptive step)"]
D --> E["Duyet t = 0 → 1<br/>(buoc step_new)"]
E --> F["point = CalculateCurvePoint(t)"]
F --> G{point hop le?<br/>khong NaN?}
G -- false --> E
G -- true --> H{Kiem tra<br/>isFreeSpace?}
H -- Bi chan --> I["obstacle_flag = true<br/>BREAK"]
H -- Thong --> J["Luu pose<br/>Cap nhat orientation"]
J --> E
I --> K["result.clear()"]
E -- t > 1.0 --> L["Gan orientation<br/>cho diem cuoi"]
L --> M["return result"]
K --> M
```
### 5. Tim control points (findNURBSControlPoints)
```mermaid
flowchart TD
A["findNURBSControlPoints<br/>(start, saved_poses, angle_threshold)"] --> B["findNearestPointOnPath<br/>→ 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<br/>sao cho dist >= target<br/>→ end_idx"]
F --> G["Them saved[end_idx]"]
E -- Khong, dir == -1 --> H["Them saved.back()"]
C -- idx == -1 --> I["mid = trung diem<br/>(start, nearest)"]
I --> J["control_points =<br/>{start, mid, nearest}"]
G --> K["return true"]
H --> K
J --> K
```
### 6. Tim diem gan nhat tren duong (findNearestPointOnPath)
```mermaid
flowchart TD
A["findNearestPointOnPath<br/>(pose, saved_poses, angle_threshold)"] --> B["Tim nearest_idx<br/>(khoang cach nho nhat)"]
B --> C["Duyet nguoc tu nearest_idx → 1"]
C --> D{signedAngle >= threshold?}
D -- Co --> E["nearest_idx = i<br/>found_flag_1 = true"]
D -- Khong --> C
E --> F{Kiem tra khoang cach<br/>distance_2 >= distance_1?}
F -- Co --> G["dir = 1<br/>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<br/>return nearest_idx"]
I -- Khong, i == cuoi --> K["dir = 0<br/>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<br/>(NURBS engine)"]
spline["Spline_Inf<br/>(knot/ctrl/weight)"]
end
init -->|"config params"| calc
make -->|"start, goal,<br/>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.

46
include/dock_planner/dock_calc.h
View File

@@ -5,12 +5,9 @@
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_nav_msgs/Path.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_nav_msgs/GetPlan.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <tf3/LinearMath/Quaternion.h>
@@ -19,10 +16,9 @@
#include "dock_planner/utils/common.h"
#include <robot/robot.h>
#include <memory>
#include <vector>
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<Spline_Inf> input_spline_inf;
std::unique_ptr<CurveCommon> CurveDesign;
vector<robot_geometry_msgs::Pose> posesOnPathWay;
vector<robot_geometry_msgs::Point> footprint_robot_;
std::vector<robot_geometry_msgs::Point> footprint_robot_;
robot_geometry_msgs::PoseStamped save_goal_;
// std::vector<robot_geometry_msgs::Pose> free_zone_;
bool check_goal_;
int store_start_nearest_idx_;
@@ -57,7 +50,8 @@ namespace dock_planner
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)
@@ -68,11 +62,7 @@ namespace dock_planner
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<robot_geometry_msgs::Pose>& control_points, int degree);
void updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
std::vector<robot_geometry_msgs::Pose>& nurbs_path,
bool reverse = true);
bool findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
vector<robot_geometry_msgs::Pose>& saved_poses,
bool findPathTMP(const std::vector<robot_geometry_msgs::Pose>& control_points,
std::vector<robot_geometry_msgs::Pose>& saved_poses,
const int& degree, double step = 0.02);
bool findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
bool findNURBSControlPoints(int& dir, std::vector<robot_geometry_msgs::Pose>& control_points,
const robot_geometry_msgs::Pose& start_pose,
const std::vector<robot_geometry_msgs::Pose>& 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<robot_geometry_msgs::Point>& footprint_robot,
const vector<robot_geometry_msgs::Point>& footprint_dock,
const std::vector<robot_geometry_msgs::Point>& footprint_robot,
const std::vector<robot_geometry_msgs::Point>& footprint_dock,
const int& degree, const double& step,
vector<robot_geometry_msgs::Pose>& planMoveToDock,
std::vector<robot_geometry_msgs::Pose>& planMoveToDock,
bool reverse = false);

1
include/dock_planner/dock_planner.h
View File

@@ -27,7 +27,6 @@ namespace dock_planner {
robot_costmap_2d::Costmap2D* costmap_;
bool initialized_;
std::string frame_id_;
// ros::Publisher plan_pub_;
std::vector<robot_geometry_msgs::Point> footprint_dock_;
int cost_threshold_;

408
src/dock_calc.cpp
View File

@@ -2,21 +2,21 @@
namespace dock_planner
{
DockCalc::DockCalc(/* args */) :costmap_(nullptr)
DockCalc::DockCalc()
: input_spline_inf(std::make_unique<Spline_Inf>()),
CurveDesign(std::make_unique<CurveCommon>()),
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)
{
@@ -77,47 +77,73 @@ namespace dock_planner
return D;
}
void DockCalc::setupSpline(const std::vector<robot_geometry_msgs::Pose>& control_points, int degree)
{
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> 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<int>(control_points.size()) - 1;
const int m = n + degree + 1;
std::vector<double> 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<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.get(), degree + 1, eigen_points, knot_vector);
CurveDesign->ReadSplineInf(input_spline_inf.get(), weights, false);
}
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;
if (fp.size() < 4) return {0, 0, 0};
double min_x = std::numeric_limits<double>::max();
double max_x = std::numeric_limits<double>::lowest();
double min_y = std::numeric_limits<double>::max();
double max_y = std::numeric_limits<double>::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<robot_geometry_msgs::Point>& footprint_robot,
const vector<robot_geometry_msgs::Point>& footprint_dock,
const std::vector<robot_geometry_msgs::Point>& footprint_robot,
const std::vector<robot_geometry_msgs::Point>& footprint_dock,
const int& degree, const double& step,
vector<robot_geometry_msgs::Pose>& planMoveToDock,
std::vector<robot_geometry_msgs::Pose>& 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,7 +158,6 @@ namespace dock_planner
return false;
}
// Calculate safety distance
safety_distance_ = (robot.radius + calib_safety_distance_) * 2.0;
if (safety_distance_ <= epsilon)
{
@@ -141,49 +166,23 @@ namespace dock_planner
}
// 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>
auto generateNURBSPath = [&](const std::vector<robot_geometry_msgs::Pose>& control_points, bool check_isFreeSpace,
bool edge_end_plag, bool path_reverse) -> std::vector<robot_geometry_msgs::Pose>
{
vector<robot_geometry_msgs::Pose> result;
std::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);
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<robot_geometry_msgs::Pose> saved_poses;
std::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);
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<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
vector<robot_geometry_msgs::Pose> saved_poses_tmp;
std::vector<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
std::vector<robot_geometry_msgs::Pose> 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<robot_geometry_msgs::Pose> control_points_1;
std::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,70 +341,41 @@ namespace dock_planner
((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);
std::vector<robot_geometry_msgs::Pose> 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<robot_geometry_msgs::Pose> control_points_2 = {first_segment.back(),
mid_control_point,
goal.pose};
std::vector<robot_geometry_msgs::Pose> 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<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();
return true;
}
bool DockCalc::findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
vector<robot_geometry_msgs::Pose>& saved_poses,
bool DockCalc::findPathTMP(const std::vector<robot_geometry_msgs::Pose>& control_points,
std::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);
}
setupSpline(control_points, degree);
// Đ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);
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;
@@ -436,27 +411,21 @@ namespace dock_planner
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);
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<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;
const double step = (degree == 1) ? 0.1 : 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);
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<robot_geometry_msgs::Pose>& control_points,
bool DockCalc::findNURBSControlPoints(int& dir, std::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)
@@ -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;
@@ -553,8 +512,6 @@ namespace dock_planner
}
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;
@@ -563,21 +520,15 @@ namespace dock_planner
}
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;
@@ -660,88 +612,7 @@ namespace dock_planner
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;
// }
// }
// }
// }
// TODO: implement obstacle checking using costmap_
return true;
}
@@ -784,10 +655,11 @@ namespace dock_planner
}
std::vector<robot_geometry_msgs::Point> 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<double>(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<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;

69
src/dock_planner.cpp
View File

@@ -12,9 +12,9 @@ namespace dock_planner
DockPlanner::DockPlanner() : costmap_robot_(nullptr),
costmap_(nullptr),
initialized_(false),
check_free_space_(false),
cost_threshold_(200),
calib_safety_distance_(0.0) {}
calib_safety_distance_(0.0),
check_free_space_(false) {}
/**
* @brief Constructor với parameters
@@ -26,9 +26,9 @@ namespace dock_planner
: costmap_robot_(nullptr),
costmap_(nullptr),
initialized_(false),
check_free_space_(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<robot_nav_msgs::Path>("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<robot_geometry_msgs::PoseStamped>& plan)
{
if(!initialized_) return false;
if(!plan.empty()) plan.clear();
vector<robot_geometry_msgs::Pose> planMoveToDock;
// std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
std::vector<robot_geometry_msgs::Point> 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<robot_geometry_msgs::Pose> planMoveToDock;
std::vector<robot_geometry_msgs::Point> 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<robot_geometry_msgs::PoseStamped>& 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<dock_planner::DockPlanner>();