214 lines
9.5 KiB
C++
Executable File
214 lines
9.5 KiB
C++
Executable File
#ifndef MERGER_PATH_CALC_H_
|
|
#define MERGER_PATH_CALC_H_
|
|
#include <robot_geometry_msgs/PoseStamped.h>
|
|
#include <vector>
|
|
#include <cmath>
|
|
|
|
#include <tf3/convert.h>
|
|
|
|
#include "custom_planner/pose.h"
|
|
#include "custom_planner/Curve_common.h"
|
|
#include <data_convert/data_convert.h>
|
|
|
|
namespace custom_planner
|
|
{
|
|
//Lưu thông tin các cạnh
|
|
struct InfEdge{
|
|
int start_edge_idx;
|
|
int end_edge_idx;
|
|
bool isCurve;
|
|
double lenght_edge;
|
|
};
|
|
|
|
//Lưu trữ gosc chuyển hướng
|
|
struct RobotDirectionChangeAngle{
|
|
int start_edge_idx_first;
|
|
int end_edge_idx_first;
|
|
int start_edge_idx_second;
|
|
int end_edge_idx_second;
|
|
};
|
|
|
|
enum point_type
|
|
{
|
|
START = 0,
|
|
GOAL = 1,
|
|
UNKNOWN = 3
|
|
};
|
|
class MergePathCalc
|
|
{
|
|
public:
|
|
MergePathCalc();
|
|
~MergePathCalc();
|
|
|
|
/**
|
|
* \brief Tính toán bước nhảy thích hợp dựa trên độ dài đoạn và khoảng cách điểm mong muốn.
|
|
* \param segment_length Độ dài của đoạn đường.
|
|
* \param desired_point_spacing Khoảng cách giữa các điểm mong muốn (mặc định là 0.02).
|
|
* \return Trả về bước nhảy thích hợp để lấy mẫu điểm trên đoạn đường.
|
|
* \details
|
|
* Hàm này sẽ tính toán bước nhảy dựa trên độ dài đoạn đường và khoảng cách giữa các điểm mong muốn.
|
|
*/
|
|
inline double calculateAdaptiveStep(double segment_length,
|
|
double desired_point_spacing = 0.02)
|
|
{
|
|
return desired_point_spacing / segment_length;
|
|
}
|
|
|
|
/**
|
|
* \brief Tìm điểm gần nhất trên đường đi.
|
|
* \param pose Vị trí hiện tại của robot.
|
|
* \param posesOnPathWay Các điểm trên đường đi.
|
|
* \return Trả về chỉ mục của điểm gần nhất trong posesOnPathWay.
|
|
* \details
|
|
* Hàm này sẽ tìm điểm gần nhất trên đường đi so với vị trí hiện tại của robot.
|
|
* Nếu khoảng cách đến điểm gần nhất nhỏ hơn biding_distance, nó sẽ
|
|
* tiếp tục tìm điểm xa hơn để đảm bảo ràng buộc khoảng cách.
|
|
* Nếu tìm thấy, nó sẽ trả về chỉ mục của điểm gần nhất trong posesOnPathWay.
|
|
* Nếu không tìm thấy điểm nào thỏa mãn, nó sẽ trả về -1.
|
|
*/
|
|
int findNearestPointOnPath(Pose& pose,
|
|
std::vector<Pose>& posesOnPathWay,
|
|
point_type type);
|
|
|
|
/**
|
|
* \brief Tính độ dài của đường NURBS.
|
|
* \param curve_design Con trỏ đến đối tượng CurveCommon chứa các hàm tính toán đường cong.
|
|
* \param spline_inf Con trỏ đến đối tượng Spline_Inf chứa thông tin về đường cong NURBS.
|
|
* \param degree Bậc của đường NURBS (1, 2 hoặc 3).
|
|
* \param initial_step Bước nhảy ban đầu để lấy mẫu điểm trên đường cong.
|
|
* \return Trả về độ dài của đường NURBS.
|
|
* \details
|
|
* Hàm này sẽ tính độ dài của đường NURBS bằng cách lấy mẫu các điểm trên đường cong
|
|
* và tính khoảng cách giữa các điểm liên tiếp. Nó sẽ trả về tổng độ dài của đường cong.
|
|
*/
|
|
double calculateNURBSLength(Curve_common* curve_design,
|
|
Spline_Inf* spline_inf,
|
|
double initial_step = 0.01);
|
|
|
|
/**
|
|
* \brief Tìm các điểm điều khiển NURBS.
|
|
* \param start Vị trí bắt đầu.
|
|
* \param goal Vị trí kết thúc.
|
|
* \param posesOnPathWay Các điểm trên đường đi.
|
|
* \param degree Bậc của đường cong NURBS (2 hoặc 3).
|
|
* \return Trả về một vector chứa các điểm điều khiển NURBS.
|
|
* \details
|
|
* Hàm này tìm các điểm điều khiển NURBS dựa trên vị trí bắt đầu, kết thúc và các điểm trên đường đi.
|
|
* Nó sử dụng bậc của đường cong NURBS để xác định số lượng điểm điều khiển cần thiết.
|
|
*/
|
|
bool findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
|
|
const robot_geometry_msgs::PoseStamped& start,
|
|
std::vector<Pose>& posesOnPathWay,
|
|
point_type type);
|
|
|
|
/**
|
|
* \brief Tính toán đường đi NURBS.
|
|
* \param degree Bậc của đường NURBS (2 hoặc 3).
|
|
* \return Trả về một vector chứa các PoseStamped trên đường đi NURBS.
|
|
* \details
|
|
* Hàm này sẽ tính toán đường đi NURBS dựa trên các điểm điều khiển đã tìm được.
|
|
* Nó sẽ sử dụng các hàm từ CurveCommon và Spline_Inf để tính toán các điểm trên đường cong.
|
|
*/
|
|
std::vector<robot_geometry_msgs::PoseStamped> calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points,
|
|
bool reverse);
|
|
|
|
bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge,
|
|
vector<Pose>& posesOnEdge,
|
|
size_t current_edge_idx);
|
|
|
|
/**
|
|
* \brief Tính toán góc giữa hai điểm.
|
|
* \param x1 Tọa độ x của điểm đầu tiên.
|
|
* \param y1 Tọa độ y của điểm đầu tiên.
|
|
* \param x2 Tọa độ x của điểm thứ hai.
|
|
* \param y2 Tọa độ y của điểm thứ hai.
|
|
* \return Trả về góc tính bằng radian từ điểm đầu đến điểm thứ hai.
|
|
* \details
|
|
* Hàm này sẽ tính toán góc giữa hai điểm (x1, y1) và (x2, y2) theo công thức atan2.
|
|
*/
|
|
inline double calculateAngle(double x1, double y1, double x2, double y2)
|
|
{
|
|
double angle = atan2(y2 - y1, x2 - x1);
|
|
return 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;
|
|
}
|
|
|
|
/**
|
|
* \brief Tính góc có hướng giữa hai vector.
|
|
* \param start_pose Vị trí bắt đầu.
|
|
* \param posesOnPathWay Các điểm trên đường đi.
|
|
* \param idx Chỉ mục của điểm cần tính trong posesOnPathWay.
|
|
* \return Trả về góc có hướng giữa hai vector.
|
|
* \details
|
|
* Hàm này sẽ tính góc có hướng giữa vector từ start_pose đến điểm hiện tại
|
|
* và vector từ điểm hiện tại đến điểm tiếp theo trong posesOnPathWay.
|
|
*/
|
|
double signedAngle(Pose& start_pose,
|
|
std::vector<Pose>& posesOnPathWay,
|
|
point_type type,
|
|
int& idx);
|
|
|
|
double signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose);
|
|
|
|
// Chỉ mục của điểm mục tiêu trong posesOnPathWay
|
|
int start_target_idx_ = 0;
|
|
int goal_target_idx_ = 0;
|
|
int store_start_nearest_idx_ = 0;
|
|
int store_goal_nearest_idx_ = 0;
|
|
int end_idx_;
|
|
|
|
int edge_front_start_idx_ = 0;
|
|
int edge_front_end_idx_ = 0;
|
|
int edge_back_start_idx_ = 0;
|
|
int edge_back_end_idx_ = 0;
|
|
|
|
bool status_yaw_edge_end_ = true;
|
|
|
|
|
|
private:
|
|
// vector<robot_geometry_msgs::PoseStamped> control_points;
|
|
|
|
bool normal_plag = false;
|
|
|
|
// CurveCommon* curve_design = nullptr;
|
|
// Spline_Inf* spline_inf = nullptr;
|
|
Spline_Inf* spline_inf = nullptr;
|
|
Curve_common* curve_design = nullptr;
|
|
|
|
/**
|
|
* \brief Cập nhật hướng của các Pose trong đường đi NURBS.
|
|
* \param saved_poses Vector chứa các Pose đã lưu.
|
|
* \param nurbs_path Vector chứa đường đi NURBS.
|
|
* \details
|
|
* Hàm này sẽ cập nhật hướng (yaw) của các Pose trong saved_poses dựa trên hướng của đường đi NURBS.
|
|
* Nó sẽ tính toán góc yaw dựa trên vị trí của các Pose và cập nhật chúng.
|
|
*/
|
|
void updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
|
|
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
|
|
bool reverse);
|
|
|
|
/**
|
|
* \brief Thiết lập hướng (yaw) của các Pose trên đường NuRBS.
|
|
* \param Poses Vector chứa các PoseStamped.
|
|
* \param reverse Nếu true, sẽ đảo ngược hướng của các Pose.
|
|
* \details
|
|
* Hàm này sẽ thiết lập hướng (yaw) cho tất cả các Pose trong vector Poses.
|
|
* Nếu reverse là true, nó sẽ đảo ngược hướng của các Pose.
|
|
* Nếu reverse là false, nó sẽ giữ nguyên hướng của các Pose.
|
|
*/
|
|
static void setYawAllPosesOnEdge(std::vector<robot_geometry_msgs::PoseStamped>& Poses,
|
|
bool reverse = false);
|
|
|
|
|
|
};
|
|
|
|
} // namespace custom_planner
|
|
#endif // MERGE_PATH_CALC_H_
|