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

44
src/merge_path_calc.cpp
View File

@@ -12,10 +12,10 @@ namespace custom_planner {
delete spline_inf;
}
std::vector<geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points, bool reverse)
std::vector<robot_geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points, bool reverse)
{
std::vector<geometry_msgs::PoseStamped> nurbs_path;
std::vector<geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path;
std::vector<robot_geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
if((int)nurbs_path.size() > 0)
{
@@ -61,10 +61,10 @@ namespace custom_planner {
for(double t = 0.0; t <= 1.0; t += step)
{
geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
robot_geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
if(!std::isnan(point.x) && !std::isnan(point.y))
{
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = robot::Time::now();
pose.pose.position = point;
pose.pose.orientation.x = 0.0;
@@ -112,7 +112,7 @@ namespace custom_planner {
double initial_step)
{
double length = 0.0;
geometry_msgs::Point prev_point, curr_point;
robot_geometry_msgs::Point prev_point, curr_point;
std::vector<double> segment_lengths;
static double step;
@@ -137,8 +137,8 @@ namespace custom_planner {
return length;
}
void MergePathCalc::updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses,
std::vector<geometry_msgs::PoseStamped>& nurbs_path,
void MergePathCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse)
{
double yaw;
@@ -312,8 +312,8 @@ namespace custom_planner {
return nearest_idx;
}
bool MergePathCalc::findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points,
const geometry_msgs::PoseStamped& pose,
bool MergePathCalc::findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const robot_geometry_msgs::PoseStamped& pose,
std::vector<Pose>& posesOnPathWay,
point_type type)
{
@@ -349,7 +349,7 @@ namespace custom_planner {
control_points.push_back(pose);
// Với bậc 2, thêm 1 điểm trung gian nằm trên đường thẳng
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = start_pose.getX() + dx/2.0;
mid_pose.pose.position.y = start_pose.getY() + dy/2.0;
mid_pose.pose.orientation = pose.pose.orientation;
@@ -358,7 +358,7 @@ namespace custom_planner {
control_points.push_back(mid_pose);
// Thêm điểm cuối
geometry_msgs::PoseStamped end_pose;
robot_geometry_msgs::PoseStamped end_pose;
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
@@ -376,7 +376,7 @@ namespace custom_planner {
// Tạo các control point cho NURBS
control_points.push_back(pose);
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
mid_pose.pose.orientation = pose.pose.orientation;
@@ -434,14 +434,14 @@ namespace custom_planner {
control_points.push_back(pose);
geometry_msgs::PoseStamped mid_pose_straight;
robot_geometry_msgs::PoseStamped mid_pose_straight;
mid_pose_straight.pose.position.x = start_pose.getX() + dx/2.0;
mid_pose_straight.pose.position.y = start_pose.getY() + dy/2.0;
mid_pose_straight.pose.orientation = pose.pose.orientation;
mid_pose_straight.header = pose.header;
control_points.push_back(mid_pose_straight);
geometry_msgs::PoseStamped end_pose_straight;
robot_geometry_msgs::PoseStamped end_pose_straight;
end_pose_straight.pose.position.x = posesOnPathWay[start_target_idx_].getX();
end_pose_straight.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
@@ -451,7 +451,7 @@ namespace custom_planner {
return true;
}
geometry_msgs::PoseStamped end_pose;
robot_geometry_msgs::PoseStamped end_pose;
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
@@ -482,7 +482,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
// Thêm điểm đầu từ đường đã có
geometry_msgs::PoseStamped start_pose;
robot_geometry_msgs::PoseStamped start_pose;
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -495,7 +495,7 @@ namespace custom_planner {
double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
// Thêm 1 điểm trung gian cho bậc 2
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx/2.0;
mid_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy/2.0;
mid_pose.pose.orientation = start_pose.pose.orientation;
@@ -553,7 +553,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
geometry_msgs::PoseStamped start_pose_straight;
robot_geometry_msgs::PoseStamped start_pose_straight;
start_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
start_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -563,7 +563,7 @@ namespace custom_planner {
double dx_straight = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
double dy_straight = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
geometry_msgs::PoseStamped mid_pose_straight;
robot_geometry_msgs::PoseStamped mid_pose_straight;
mid_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx_straight/2.0;
mid_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy_straight/2.0;
mid_pose_straight.pose.orientation = start_pose_straight.pose.orientation;
@@ -575,7 +575,7 @@ namespace custom_planner {
}
// Thêm điểm đầu từ đường đã có
geometry_msgs::PoseStamped start_pose;
robot_geometry_msgs::PoseStamped start_pose;
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -583,7 +583,7 @@ namespace custom_planner {
start_pose.header = pose.header;
control_points.push_back(start_pose);
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = posesOnPathWay[idx].getX();
mid_pose.pose.position.y = posesOnPathWay[idx].getY();
mid_pose.pose.orientation = pose.pose.orientation;