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

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:06:20 +07:00
parent 377ebe3d6f
commit 6d55c6c4be
8 changed files with 153 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
include/custom_planner/Curve_common.h
View File

@@ -8,7 +8,7 @@
// #include <iostream>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include "custom_planner/color.h"
@@ -47,7 +47,7 @@ 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_Line(robot_geometry_msgs::Point start_point, robot_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);
@@ -55,8 +55,8 @@ class Curve_common
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);
robot_geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS);
robot_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);

10
include/custom_planner/conversion.h
View File

@@ -3,14 +3,14 @@
#include "custom_planner/Curve_common.h"
#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Quaternion.h>
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const robot_geometry_msgs::Point& msg) {
return Eigen::Vector3d(msg.x, msg.y, msg.z);
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;
@@ -25,7 +25,7 @@ inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::v
return eigen_trajectory_point_vec;
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::Point> &discreate_point_vec)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;

48
include/custom_planner/custom_planner.h
View File

@@ -9,7 +9,7 @@
using namespace std;
// ROS
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
// Costmap used for the map representation
@@ -29,7 +29,7 @@ using namespace std;
#include "custom_planner/conversion.h"
#include "custom_planner/merge_path_calc.h"
#include <geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <thread>
#include <boost/thread.hpp>
@@ -82,9 +82,9 @@ public:
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);
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
@@ -92,9 +92,9 @@ public:
* @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)
virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
printf("[%s:%d] This function is not available!",__FILE__,__LINE__);
return false;
@@ -104,12 +104,12 @@ public:
private:
void publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
const robot_geometry_msgs::PoseStamped& start,
const robot_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);
static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
const std::vector<robot_geometry_msgs::Point>& footprint,
std::vector<robot_geometry_msgs::Point>& out_footprint);
Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0);
@@ -130,7 +130,7 @@ private:
}
bool checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose,
const geometry_msgs::PoseStamped &goal);
const robot_geometry_msgs::PoseStamped &goal);
bool loadPathwayData(const string& filename);
@@ -139,17 +139,17 @@ private:
// 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);
const robot_geometry_msgs::PoseStamped& start,
const robot_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);
bool doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_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);
std::optional<robot_geometry_msgs::Point> computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2);
bool calcAllYaw(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
bool calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& saveposesOnEdge,
int& total_edge);
@@ -163,7 +163,7 @@ private:
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<robot_geometry_msgs::PoseStamped> divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d);
// Vector lưu thông tin các cạnh
std::vector<InfEdge> edges_info_;
@@ -211,7 +211,7 @@ private:
std::string name_;
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<geometry_msgs::Point> footprint_;
std::vector<robot_geometry_msgs::Point> footprint_;
unsigned int current_env_width_;
unsigned int current_env_height_;

16
include/custom_planner/merge_path_calc.h
View File

@@ -1,6 +1,6 @@
#ifndef MERGER_PATH_CALC_H_
#define MERGER_PATH_CALC_H_
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <vector>
#include <cmath>
@@ -96,8 +96,8 @@ namespace custom_planner
* 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,
bool findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const robot_geometry_msgs::PoseStamped& start,
std::vector<Pose>& posesOnPathWay,
point_type type);
@@ -109,7 +109,7 @@ namespace custom_planner
* 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,
std::vector<robot_geometry_msgs::PoseStamped> calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points,
bool reverse);
bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge,
@@ -174,7 +174,7 @@ namespace custom_planner
private:
// vector<geometry_msgs::PoseStamped> control_points;
// vector<robot_geometry_msgs::PoseStamped> control_points;
bool normal_plag = false;
@@ -191,8 +191,8 @@ namespace custom_planner
* 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,
void updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse);
/**
@@ -204,7 +204,7 @@ namespace custom_planner
* 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,
static void setYawAllPosesOnEdge(std::vector<robot_geometry_msgs::PoseStamped>& Poses,
bool reverse = false);