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

first commit

Browse Source
This commit is contained in:
duongtd
2025-12-19 16:45:50 +07:00
commit 377ebe3d6f
15 changed files with 5090 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
include/custom_planner/Curve_common.h Executable file
View File

@@ -0,0 +1,84 @@
#ifndef CUSTOM_PLANNER_CURVE_COMMON_H_
#define CUSTOM_PLANNER_CURVE_COMMON_H_
//#include "agv_path_smoothing/conversion.h"
#include <Eigen/Eigen>
//#include <vector>
// #include <deque>
// #include <iostream>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include "custom_planner/color.h"
struct Spline_Inf
{
int order;
std::vector<double> knot_vector;
std::vector<double> weight;
//std::vector<double> N;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point;
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > N;
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > dN;
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > ddN;
std::vector<double> N_double_vec;
std::vector<double> R_double_vec;
std::vector<double> dR_double_vec;
std::vector<double> ddR_double_vec;
// std::vector<double> ddN_double_vec;
//Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, Eigen::Dynamic> N; //bsaic function
Eigen::MatrixXd curvefit_N;
};
struct EigenTrajectoryPoint
{
typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint> > Vector;
double u_data;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector3d position;
//Eigen::VectorXd u_data;
};
class Curve_common
{
public:
Curve_common();
nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
void CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_data, int differential_times);
geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS);
geometry_msgs::Point CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS);
double CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
double CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
double CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS);
void ReadSplineInf(Spline_Inf *bspline_inf, int order, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point, std::vector<double> knot_vector);
void ReadSplineInf(Spline_Inf *bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector<double> file_discreate_point);
void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > *input_point, std::vector<double> file_discreate_point);
void ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point);
void ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
//TODO: move relate visualize function to new vislization.h
int print();
visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
private:
};
#endif //CUSTOM_PLANNER_CURVE_COMMON_H_

34
include/custom_planner/color.h Executable file
View File

@@ -0,0 +1,34 @@
#ifndef CUSTOM_PLANNER_COLOR_H_
#define CUSTOM_PLANNER_COLOR_H_
#include <std_msgs/ColorRGBA.h>
namespace agv_visualization
{
class Color : public std_msgs::ColorRGBA
{
public:
Color() : std_msgs::ColorRGBA() {}
Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {}
Color(double red, double green, double blue, double alpha) : Color() {
r = red;
g = green;
b = blue;
a = alpha;
}
static const Color White() { return Color(1.0, 1.0, 1.0); }
static const Color Black() { return Color(0.0, 0.0, 0.0); }
static const Color Gray() { return Color(0.5, 0.5, 0.5); }
static const Color Red() { return Color(1.0, 0.0, 0.0); }
static const Color Green() { return Color(0.0, 1.0, 0.0); }
static const Color Blue() { return Color(0.0, 0.0, 1.0); }
static const Color Yellow() { return Color(1.0, 1.0, 0.0); }
static const Color Orange() { return Color(1.0, 0.5, 0.0); }
static const Color Purple() { return Color(0.5, 0.0, 1.0); }
static const Color Chartreuse() { return Color(0.5, 1.0, 0.0); }
static const Color Teal() { return Color(0.0, 1.0, 1.0); }
static const Color Pink() { return Color(1.0, 0.0, 0.5); }
};
}
#endif //CUSTOM_PLANNER_COLOR_H_

43
include/custom_planner/conversion.h Executable file
View File

@@ -0,0 +1,43 @@
#ifndef CUSTOM_PLANNER_CONVERSION_H_
#define CUSTOM_PLANNER_CONVERSION_H_
#include "custom_planner/Curve_common.h"
#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
return Eigen::Vector3d(msg.x, msg.y, msg.z);
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;
for(int i = 0; i < static_cast<int>(plan.size()); i++)
{
eigen_trajectory_point.position(0) = plan.at(i).pose.position.x;
eigen_trajectory_point.position(1) = plan.at(i).pose.position.y;
eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
}
return eigen_trajectory_point_vec;
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;
for(int i = 0; i < static_cast<int>(discreate_point_vec.size()); i++)
{
eigen_trajectory_point.position(0) = discreate_point_vec.at(i).x;
eigen_trajectory_point.position(1) = discreate_point_vec.at(i).y;
eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
}
return eigen_trajectory_point_vec;
}
#endif //CUSTOM_PLANNER_CONVERSION_H_

230
include/custom_planner/custom_planner.h Executable file
View File

@@ -0,0 +1,230 @@
#ifndef CUSTOM_PLANNER_H
#define CUSTOM_PLANNER_H
#include <iostream>
#include <vector>
#include <cmath>
#include <map>
using namespace std;
// ROS
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_robot.h>
// global representation
#include <nav_core/base_global_planner.h>
// tf
#include <tf3/convert.h>
#include <tf3/buffer_core.h>
// internal lib
#include <custom_planner/pose.h>
#include <custom_planner/pathway.h>
#include "custom_planner/Curve_common.h"
#include "custom_planner/conversion.h"
#include "custom_planner/merge_path_calc.h"
#include <geometry_msgs/PoseArray.h>
#include <thread>
#include <boost/thread.hpp>
#include <robot_protocol_msgs/Order.h>
// #include "vda5050_msgs/Order.h"
// #include "vda5050_msgs/Trajectory.h"
// #include "vda5050_msgs/Edge.h"
// #include "vda5050_msgs/Node.h"
// #include "vda5050_msgs/ControlPoint.h"
// #include "vda5050_msgs/NodePosition.h"
const double EPSILON = 1e-6;
using namespace std;
namespace custom_planner{
struct OrderNode{
string nodeId;
uint32_t sequenceId;
double position_x;
double position_y;
double theta;
};
class CustomPlanner : public nav_core::BaseGlobalPlanner{
public:
/**
* @brief Default constructor for the NavFnROS object
*/
CustomPlanner();
/**
* @brief Constructor for the CustomPlanner object
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/
CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
/**
* @brief Initialization function for the CustomPlanner object
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/
virtual bool initialize(std::string name,
costmap_2d::Costmap2DROBOT* costmap_robot);
virtual bool makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
{
printf("[%s:%d] This function is not available!",__FILE__,__LINE__);
return false;
}
virtual ~CustomPlanner(){};
private:
void publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
static void transformFootprintToEdges(const geometry_msgs::Pose& robot_pose,
const std::vector<geometry_msgs::Point>& footprint,
std::vector<geometry_msgs::Point>& out_footprint);
Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0);
bool countRobotDirectionChangeAngle(vector<Pose>& savePosesOnEdge, int& total_edge);
inline double calculateAngle(double xA, double yA, double xB, double yB)
{
double deltaX = xB - xA;
double deltaY = yB - yA;
double angleRad = atan2(deltaY, deltaX);
// double angleDeg = angleRad * 180.0 / M_PI;
return angleRad;
}
inline bool isOppositeSign(double angleA, double angleB)
{
return (angleA > 0 && angleB < 0) || (angleA < 0 && angleB > 0);
}
bool checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose,
const geometry_msgs::PoseStamped &goal);
bool loadPathwayData(const string& filename);
bool findNearestPoseOfPath(vector<Pose>& posesOnPathWay, Pose& PoseToCheck, Pose& PoseResult);
// void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg);
bool makePlanWithOrder(robot_protocol_msgs::Order msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
bool doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2);
std::optional<geometry_msgs::Point> computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2);
bool calcAllYaw(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
vector<Pose>& saveposesOnEdge,
int& total_edge);
bool isThetaValid(double theta);
bool curveIsValid(int degree, const std::vector<double> &knot_vector,
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
void setYawAllPosesOnEdge(vector<Pose>& posesOnEdge, bool reverse);
double computeDeltaAngle(Pose& Pose1, Pose& Pose2);
vector<Pose> divideSegment(Pose& A, Pose& B, double d);
vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d);
// Vector lưu thông tin các cạnh
std::vector<InfEdge> edges_info_;
std::vector<RobotDirectionChangeAngle> RobotDirectionChangeAngle_info_;
MergePathCalc merge_path_calc_;
bool skipEdge_flag_ = false;
bool reverse_ = false;
Spline_Inf* input_spline_inf;
Curve_common* CurveDesign;
Pathway* pathway;
Pose* startPose;
vector<Pose> posesOnPathWay;
Pose start_on_path;
std::map<string, OrderNode> orderNodes;
// vda5050_msgs::Order order_msg_;
uint16_t start_on_path_index;
bool initialized_;
bool rotating_robot_plag_ = false; /**< whether the robot is rotating or not, used to determine the direction of the path */
std::string planner_type_; /**< sbpl method to use for planning. choices are ARAPlanner and ADPlanner */
double allocated_time_; /**< amount of time allowed for search */
double initial_epsilon_; /**< initial epsilon for beginning the anytime search */
std::string environment_type_; /** what type of environment in which to plan. choices are 2D and XYThetaLattice. */
std::string cost_map_topic_; /** what topic is being used for the costmap topic */
bool forward_search_; /** whether to use forward or backward search */
std::string primitive_filename_; /** where to find the motion primitives for the current robot */
int force_scratch_limit_; /** the number of cells that have to be changed in the costmap to force the planner to plan from scratch even if its an incremental planner */
unsigned char lethal_obstacle_;
unsigned char inscribed_inflated_obstacle_;
unsigned char circumscribed_cost_;
unsigned char sbpl_cost_multiplier_;
bool publish_footprint_path_;
int visualizer_skip_poses_;
bool allow_unknown_;
std::string name_;
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<geometry_msgs::Point> footprint_;
unsigned int current_env_width_;
unsigned int current_env_height_;
// ros::Subscriber order_msg_sub_;
// ros::Publisher plan_pub_;
// ros::Publisher stats_publisher_;
// vector<ros::ServiceServer> service_servers_;
// ros::Publisher sbpl_plan_footprint_pub_;
boost::mutex mutex_;
};
}
#endif

214
include/custom_planner/merge_path_calc.h Executable file
View File

@@ -0,0 +1,214 @@
#ifndef MERGER_PATH_CALC_H_
#define MERGER_PATH_CALC_H_
#include <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<geometry_msgs::PoseStamped>& control_points,
const 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<geometry_msgs::PoseStamped> calculateNURBSPath(vector<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<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<geometry_msgs::PoseStamped>& saved_poses,
std::vector<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<geometry_msgs::PoseStamped>& Poses,
bool reverse = false);
};
} // namespace custom_planner
#endif // MERGE_PATH_CALC_H_

150
include/custom_planner/pathway.h Executable file
View File

@@ -0,0 +1,150 @@
#ifndef CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H
#define CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <unistd.h>
#include <vector>
#include <fstream>
#include <filesystem>
#include <math.h>
#include <cmath>
#include <custom_planner/pose.h>
using namespace std;
namespace custom_planner
{
class Point {
private:
double x_, y_;
public:
Point():
x_(0.0), y_(0.0) {};
Point(double x, double y):
x_(x), y_(y) {};
~Point() {};
inline void setX(double x) { x_ = x; }
inline void setY(double y) { y_ = y; }
inline void setPoint(double x, double y) { x_ = x, y_ = y; }
inline void setPoint(Point p) { x_ = p.x_, y_ = p.y_; }
inline double getX(void) { return x_; }
inline double getY(void) { return y_; }
inline Point getPoint(void) { return Point(x_, y_); }
}; // class Point
class Edge {
private:
Pose p1_;
Pose p2_;
public:
Edge(void):
p1_(Pose(0.0, 0.0, 0.0)),
p2_(Pose(0.0, 0.0, 0.0)) {}
Edge(Pose p1, Pose p2):
p1_(p1),
p2_(p2) {}
inline void setEdge(Pose p1, Pose p2){
p1_ = p1; p2_ = p2;
}
~Edge() {}
inline void setP1(Pose p1){p1_ = p1;}
inline void setP2(Pose p2){p2_ = p2;}
inline Pose getP1(void){return p1_;}
inline Pose getP2(void){return p2_;}
};
class Pathway {
private:
std::vector<Edge> path_;
vector<Pose> posesOnPath_;
Pose startPoseOfPath;
// Given three collinear poses p, q, r, the function checks if
// pose q lies on edge 'pr'
bool onSegment(Pose p, Pose q, Pose r);
//To find orientation of ordered triplet (p, q, r)
// The function returns following values
// 0 --> p, q and r are collinear
// 1 --> Clockwise
// 2 --> Counterclockwise
int orientation(Pose p, Pose q, Pose r);
bool doIntersect(Pose p1, Pose q1, Pose p2, Pose q2);
bool isIntersect(Edge L);
bool isPoseExisted(Pose p);
double calculateAngle(Pose p1, Pose p2, Pose p3);
bool findIntersection(Edge L1, Edge L2, Point* resultPoint);
public:
bool isPathInitilized_;
bool isPathClear_;
Pathway():isPathInitilized_(false),isPathClear_(false)
{
// startPointOfPath = Pose()
}
~Pathway(){
path_.clear();
posesOnPath_.clear();
}
void RecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin);
void ResetAndRecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin);
inline void clear(){path_.clear();}
inline bool isPathEmpty(){return path_.empty();}
inline void addEdge(Pose p1, Pose p2) {path_.push_back(Edge(p1, p2));}
inline void addEdge(Edge Edge) {path_.push_back(Edge);}
inline void addEdge(Pose p2) {if(!path_.empty()&&!isPoseExisted(p2))
path_.push_back(Edge(path_.back().getP2(),p2));}
bool isNewPoseOnEdge(Pose p, double delta_angle_th);
inline void setP2LastEdgeOnPath(Pose p2)
{
path_.back().setP2(p2);
}
void setEdgeOnPath(uint32_t line_index, Pose p1, Pose p2);
inline vector<Pose> getPosesOnPath(){return posesOnPath_;}
void syncPosesAndPath();
void SavePathAsFile(const string& file_name);
void LoadPathFromFile(const string& file_name);
vector<Point> findIntersections(Edge L);
vector<vector<Point>> findIntersections(Edge L1, Edge L2);
void testprint()
{
if(!path_.empty()){
int i =0;
for(auto &edge : path_)
{
cout<<"edge "<<i<<": "<<edge.getP1().getX()
<<","<<edge.getP1().getY()<<","<<edge.getP1().getYaw()<<" "
<<edge.getP2().getX()<<","<<edge.getP2().getY()<<","<<edge.getP2().getYaw()<<endl;
i++;
}
}
}
};
} // namespace custom_planner
#endif // CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H

52
include/custom_planner/pose.h Executable file
View File

@@ -0,0 +1,52 @@
#ifndef CUSTOM_PLANNER_CUSTOM_PLANNER_POSE_H
#define CUSTOM_PLANNER_CUSTOM_PLANNER_POSE_H
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <unistd.h>
#include <vector>
#include <fstream>
#include <math.h>
#include <cmath>
using namespace std;
namespace custom_planner{
class Pose {
private:
double x_, y_, yaw_;
void modifyYaw(void) {
while (yaw_ < -M_PI)
yaw_ += 2.0 * M_PI;
while (yaw_ > M_PI)
yaw_ -= 2.0 * M_PI;
}
public:
Pose():
x_(0.0), y_(0.0), yaw_(0.0) {};
Pose(double x, double y, double yaw):
x_(x), y_(y), yaw_(yaw) {};
~Pose() {};
inline void setX(double x) { x_ = x; }
inline void setY(double y) { y_ = y; }
inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); }
inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); }
inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); }
inline double getX(void) { return x_; }
inline double getY(void) { return y_; }
inline double getYaw(void) { return yaw_; }
inline Pose getPose(void) { return Pose(x_, y_, yaw_); }
bool SavePoseAsFile(const string& file_name);
bool LoadPoseFromFile(const string& file_name);
}; // class Pose
}
#endif