From 6d55c6c4beb86ae77c20cba3f26c6231a16912fa Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 30 Dec 2025 09:06:20 +0700 Subject: [PATCH] Hiep update --- include/custom_planner/Curve_common.h | 8 +- include/custom_planner/conversion.h | 10 +-- include/custom_planner/custom_planner.h | 48 ++++++------ include/custom_planner/merge_path_calc.h | 16 ++-- src/Curve_common.cpp | 40 +++++----- src/custom_planner.cpp | 98 +++++++++++------------- src/merge_path_calc.cpp | 44 +++++------ src/plan_for_retry.cpp | 50 ++++++------ 8 files changed, 153 insertions(+), 161 deletions(-) diff --git a/include/custom_planner/Curve_common.h b/include/custom_planner/Curve_common.h index bb9f5f8..d4e9a23 100755 --- a/include/custom_planner/Curve_common.h +++ b/include/custom_planner/Curve_common.h @@ -8,7 +8,7 @@ // #include #include -#include +#include #include #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); diff --git a/include/custom_planner/conversion.h b/include/custom_planner/conversion.h index 096bd9d..ce758f3 100755 --- a/include/custom_planner/conversion.h +++ b/include/custom_planner/conversion.h @@ -3,14 +3,14 @@ #include "custom_planner/Curve_common.h" #include -#include -#include +#include +#include -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 &plan) +inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector &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 &discreate_point_vec) +inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector &discreate_point_vec) { EigenTrajectoryPoint::Vector eigen_trajectory_point_vec; EigenTrajectoryPoint eigen_trajectory_point; diff --git a/include/custom_planner/custom_planner.h b/include/custom_planner/custom_planner.h index c96a6b3..a5b09a2 100755 --- a/include/custom_planner/custom_planner.h +++ b/include/custom_planner/custom_planner.h @@ -9,7 +9,7 @@ using namespace std; // ROS -#include +#include #include // 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 +#include #include #include @@ -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& plan); + const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& 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& plan) + virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& 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& footprint, - std::vector& out_footprint); + static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose, + const std::vector& footprint, + std::vector& 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 computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1, - const geometry_msgs::Point& p2, const geometry_msgs::Point& q2); + std::optional 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& saveposesOnEdge, int& total_edge); @@ -163,7 +163,7 @@ private: double computeDeltaAngle(Pose& Pose1, Pose& Pose2); vector divideSegment(Pose& A, Pose& B, double d); - vector divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d); + vector 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 edges_info_; @@ -211,7 +211,7 @@ private: std::string name_; costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */ - std::vector footprint_; + std::vector footprint_; unsigned int current_env_width_; unsigned int current_env_height_; diff --git a/include/custom_planner/merge_path_calc.h b/include/custom_planner/merge_path_calc.h index 3ab36d4..46bcbea 100755 --- a/include/custom_planner/merge_path_calc.h +++ b/include/custom_planner/merge_path_calc.h @@ -1,6 +1,6 @@ #ifndef MERGER_PATH_CALC_H_ #define MERGER_PATH_CALC_H_ -#include +#include #include #include @@ -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& control_points, - const geometry_msgs::PoseStamped& start, + bool findNURBSControlPoints(vector& control_points, + const robot_geometry_msgs::PoseStamped& start, std::vector& 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 calculateNURBSPath(vector& control_points, + std::vector calculateNURBSPath(vector& control_points, bool reverse); bool handleEdgeIntersection(vector& savePosesOnEdge, @@ -174,7 +174,7 @@ namespace custom_planner private: - // vector control_points; + // vector 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& saved_poses, - std::vector& nurbs_path, + void updatePoseOrientation(std::vector& saved_poses, + std::vector& 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& Poses, + static void setYawAllPosesOnEdge(std::vector& Poses, bool reverse = false); diff --git a/src/Curve_common.cpp b/src/Curve_common.cpp index 4210292..ae8de29 100755 --- a/src/Curve_common.cpp +++ b/src/Curve_common.cpp @@ -1,7 +1,7 @@ #include "custom_planner/Curve_common.h" #include "custom_planner/conversion.h" -#include +#include #include @@ -10,10 +10,10 @@ Curve_common::Curve_common() } -nav_msgs::Path Curve_common::Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id) +nav_msgs::Path Curve_common::Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id) { nav_msgs::Path line_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; line_result.header.frame_id = frame_id; line_result.header.stamp = robot::Time::now(); @@ -41,7 +41,7 @@ nav_msgs::Path Curve_common::Generate_Line(geometry_msgs::Point start_point, geo nav_msgs::Path Curve_common::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id) { nav_msgs::Path bezier_curve_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; EigenTrajectoryPoint::Vector temp_control_point_vec; bezier_curve_result.header.frame_id = frame_id; @@ -154,7 +154,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector(discreate_point.size())); - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) { view_point.x = discreate_point.at(i)(0); @@ -181,7 +181,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector(discreate_point.size())); - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) { view_point.x = discreate_point.at(i)(0); @@ -210,7 +210,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint waypoints_marker.points.reserve(static_cast(discreate_point.size())); - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) { view_point.x = discreate_point.at(i).position(0); @@ -240,7 +240,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin waypoints_marker.points.reserve(static_cast(discreate_point.size())); - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) { view_point.x = discreate_point.at(i).position(0); @@ -253,7 +253,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) { - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) { view_point.x = discreate_point.at(i).position(0); @@ -264,7 +264,7 @@ void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenT void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector > discreate_point) { - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) { view_point.x = discreate_point.at(i)(0); @@ -329,7 +329,7 @@ void Curve_common::ReadSplineInf(Spline_Inf *spline_inf, std::vector wei nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id) { nav_msgs::Path bspline_curve_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; bspline_curve_result.header.frame_id = frame_id; bspline_curve_result.header.stamp = robot::Time::now(); @@ -503,7 +503,7 @@ nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, doubl nav_msgs::Path Curve_common::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id) { nav_msgs::Path nurbs_curve_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; nurbs_curve_result.header.frame_id = frame_id; nurbs_curve_result.header.stamp = robot::Time::now(); @@ -754,9 +754,9 @@ void Curve_common::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u } } -geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS) +robot_geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS) { - geometry_msgs::Point derivative_curve_point; + robot_geometry_msgs::Point derivative_curve_point; int p_degree = spline_inf->order - 1; double sum_x = 0, sum_y = 0; double sum_denom = 0; @@ -880,9 +880,9 @@ geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spl nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id) { - geometry_msgs::Point derivative_point_result; + robot_geometry_msgs::Point derivative_point_result; nav_msgs::Path bspline_derivative_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; bspline_derivative_result.header.frame_id = frame_id; bspline_derivative_result.header.stamp = robot::Time::now(); @@ -923,9 +923,9 @@ nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_ nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id) { - geometry_msgs::Point derivative_point_result; + robot_geometry_msgs::Point derivative_point_result; nav_msgs::Path derivative_basis_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; derivative_basis_result.header.frame_id = frame_id; derivative_basis_result.header.stamp = robot::Time::now(); @@ -1156,11 +1156,11 @@ double Curve_common::CalculateCurveLength(Spline_Inf spline_inf, double start_u, return sum_length; } -geometry_msgs::Point Curve_common::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS) +robot_geometry_msgs::Point Curve_common::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS) { //TODO: Check u = 1 bug, why x=nan and y=nan? - geometry_msgs::Point curve_point; + robot_geometry_msgs::Point curve_point; int p_degree = spline_inf->order - 1; int n = static_cast(spline_inf->control_point.size()) - 1; //TODO: Check knot vector size and sequence is correect diff --git a/src/custom_planner.cpp b/src/custom_planner.cpp index 4a0ceb8..b9d0873 100755 --- a/src/custom_planner.cpp +++ b/src/custom_planner.cpp @@ -1,20 +1,12 @@ #include #include - +#include #include #include #include using namespace std; -namespace geometry_msgs -{ - bool operator==(const Point &p1, const Point &p2) - { - return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z; - } -} - namespace custom_planner { CustomPlanner::CustomPlanner() @@ -73,9 +65,9 @@ namespace custom_planner } bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg, - const geometry_msgs::PoseStamped& start, - const geometry_msgs::PoseStamped& goal, - std::vector& plan) + const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& plan) { plan.clear(); @@ -146,7 +138,7 @@ namespace custom_planner printf("[custom_planner] Using direct pathway"); for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -160,17 +152,17 @@ namespace custom_planner // CASE 2: Start is far, goal is close - NURBS from start to pathway printf("[custom_planner] Using start NURBS path reverse_(%x)", reverse_); - std::vector start_control_points; + std::vector start_control_points; bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints( start_control_points, start, posesOnPathWay, START); if (valid_start_nurbs) { - std::vector nurbs_path = + std::vector nurbs_path = merge_path_calc_.calculateNURBSPath(start_control_points, reverse_); // Add NURBS path for (const auto& pose : nurbs_path) { - geometry_msgs::PoseStamped new_pose = pose; + robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; @@ -179,7 +171,7 @@ namespace custom_planner // Add remaining pathway for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay.size(); i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -191,7 +183,7 @@ namespace custom_planner } else { // Fallback to direct path for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -206,14 +198,14 @@ namespace custom_planner // CASE 3: Start is close, goal is far - NURBS from pathway to goal printf("[custom_planner] Using goal NURBS path"); - std::vector goal_control_points; + std::vector goal_control_points; bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints( goal_control_points, goal, posesOnPathWay, GOAL); if (valid_goal_nurbs) { // Add pathway up to goal connection point for (size_t i = nearest_idx; i < merge_path_calc_.goal_target_idx_; i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -224,11 +216,11 @@ namespace custom_planner } // Add NURBS path to goal - std::vector nurbs_path = + std::vector nurbs_path = merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); for (const auto& pose : nurbs_path) { - geometry_msgs::PoseStamped new_pose = pose; + robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; @@ -237,7 +229,7 @@ namespace custom_planner } else { // Fallback: use entire pathway for (size_t i = 0; i < posesOnPathWay.size(); i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -252,7 +244,7 @@ namespace custom_planner // CASE 4: Both start and goal are far - Dual NURBS or special handling printf("[custom_planner] Using dual NURBS path"); - std::vector start_control_points, goal_control_points; + std::vector start_control_points, goal_control_points; bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints( start_control_points, start, posesOnPathWay, START); bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints( @@ -265,12 +257,12 @@ namespace custom_planner costmap_robot_->getRobotPose(goal_control_points[0]); goal_control_points[goal_control_points.size() - 1] = goal; - std::vector nurbs_path = + std::vector nurbs_path = merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); // Skip first point to avoid duplication for (size_t i = 1; i < nurbs_path.size(); i++) { - geometry_msgs::PoseStamped new_pose = nurbs_path[i]; + robot_geometry_msgs::PoseStamped new_pose = nurbs_path[i]; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; @@ -281,10 +273,10 @@ namespace custom_planner // Dual NURBS: Start NURBS + Pathway + Goal NURBS // Add start NURBS path - std::vector start_nurbs_path = + std::vector start_nurbs_path = merge_path_calc_.calculateNURBSPath(start_control_points, false); for (const auto& pose : start_nurbs_path) { - geometry_msgs::PoseStamped new_pose = pose; + robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; @@ -293,7 +285,7 @@ namespace custom_planner // Add middle pathway segment for (size_t i = (merge_path_calc_.start_target_idx_ - 1); i < merge_path_calc_.goal_target_idx_; i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -304,10 +296,10 @@ namespace custom_planner } // Add goal NURBS path - std::vector goal_nurbs_path = + std::vector goal_nurbs_path = merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); for (const auto& pose : goal_nurbs_path) { - geometry_msgs::PoseStamped new_pose = pose; + robot_geometry_msgs::PoseStamped new_pose = pose; new_pose.header.stamp = plan_time; new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); new_pose.pose.position.z = 0; @@ -318,7 +310,7 @@ namespace custom_planner // Fallback: Direct pathway from nearest point printf("[custom_planner] NURBS control points not found, using fallback path"); for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = posesOnPathWay[i].getX(); @@ -331,7 +323,7 @@ namespace custom_planner } // ===== STEP 7: ADD FINAL GOAL POSE ===== - geometry_msgs::PoseStamped pose_goal; + robot_geometry_msgs::PoseStamped pose_goal; pose_goal.header.stamp = plan_time; pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID(); pose_goal.pose = goal.pose; @@ -354,9 +346,9 @@ namespace custom_planner return true; } - void CustomPlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose, - const std::vector &footprint, - std::vector &out_footprint) + void CustomPlanner::transformFootprintToEdges(const robot_geometry_msgs::Pose &robot_pose, + const std::vector &footprint, + std::vector &out_footprint) { out_footprint.resize(2 * footprint.size()); double yaw = data_convert::getYaw(robot_pose.orientation); @@ -378,8 +370,8 @@ namespace custom_planner } bool CustomPlanner::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) { vector savePosesOnEdge; int count_two_curve_idx = 0; @@ -479,7 +471,7 @@ namespace custom_planner //Tính các điểm theo step mới for(double u_test = 0; u_test <= 1; u_test += t_intervel_new) { - geometry_msgs::Point curve_point; + robot_geometry_msgs::Point curve_point; curve_point = CurveDesign->CalculateCurvePoint(input_spline_inf, u_test, true); if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y)) posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0)); @@ -504,19 +496,19 @@ namespace custom_planner if(edges_info_.back().isCurve || delta_angle > EPSILON) for (int idx_segment_A = 0; idx_segment_A + 1 < (int)savePosesOnEdge.size(); ++idx_segment_A) { - geometry_msgs::Point gp1 ; + robot_geometry_msgs::Point gp1 ; gp1.x = savePosesOnEdge[idx_segment_A].getX(); gp1.y = savePosesOnEdge[idx_segment_A].getY(); - geometry_msgs::Point gp2; + robot_geometry_msgs::Point gp2; gp2.x = savePosesOnEdge[idx_segment_A + 1].getX(); gp2.y = savePosesOnEdge[idx_segment_A + 1].getY(); for (int idx_segment_B = (int)posesOnEdge.size() - 1; idx_segment_B > 0; --idx_segment_B) { - geometry_msgs::Point lp1; + robot_geometry_msgs::Point lp1; lp1.x = posesOnEdge[idx_segment_B].getX(); lp1.y = posesOnEdge[idx_segment_B].getY(); - geometry_msgs::Point lp2; + robot_geometry_msgs::Point lp2; lp2.x = posesOnEdge[idx_segment_B - 1].getX(); lp2.y = posesOnEdge[idx_segment_B - 1].getY(); @@ -528,7 +520,7 @@ namespace custom_planner auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2); if (intersection_point) { - geometry_msgs::Point p = intersection_point.value(); + robot_geometry_msgs::Point p = intersection_point.value(); // savePosesOnEdge.push_back(Pose(p.x, p.y, 0)); posesOnEdge.insert(posesOnEdge.begin(), Pose(p.x, p.y, 0)); } @@ -613,10 +605,10 @@ namespace custom_planner return status_calcAllYaw; } - bool CustomPlanner::doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1, - const geometry_msgs::Point& p2, const geometry_msgs::Point& q2) + bool CustomPlanner::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) { - auto orientation = [](const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point& c) + auto orientation = [](const robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_geometry_msgs::Point& c) { double val = (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y); @@ -632,8 +624,8 @@ namespace custom_planner return (o1 != o2 && o3 != o4); } - std::optional CustomPlanner::computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1, - const geometry_msgs::Point& p2, const geometry_msgs::Point& q2) + std::optional CustomPlanner::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) { double x1 = p1.x, y1 = p1.y; double x2 = q1.x, y2 = q1.y; @@ -659,7 +651,7 @@ namespace custom_planner if (between(x1, x2, x) && between(y1, y2, y) && between(x3, x4, x) && between(y3, y4, y)) { - geometry_msgs::Point pt; + robot_geometry_msgs::Point pt; pt.x = x; pt.y = y; pt.z = 0.0; @@ -799,8 +791,8 @@ namespace custom_planner return true; } - bool CustomPlanner::calcAllYaw(const geometry_msgs::PoseStamped& start, - const geometry_msgs::PoseStamped& goal, + bool CustomPlanner::calcAllYaw(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, vector& savePosesOnEdge, int& total_edge) { @@ -1174,7 +1166,7 @@ namespace custom_planner } } - bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const geometry_msgs::PoseStamped& goal) + bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const robot_geometry_msgs::PoseStamped& goal) { double angle = calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY()); diff --git a/src/merge_path_calc.cpp b/src/merge_path_calc.cpp index ffcecfa..f4ae642 100755 --- a/src/merge_path_calc.cpp +++ b/src/merge_path_calc.cpp @@ -12,10 +12,10 @@ namespace custom_planner { delete spline_inf; } - std::vector MergePathCalc::calculateNURBSPath(vector& control_points, bool reverse) + std::vector MergePathCalc::calculateNURBSPath(vector& control_points, bool reverse) { - std::vector nurbs_path; - std::vector saved_poses; // Lưu các pose đã tính toán + std::vector nurbs_path; + std::vector 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 segment_lengths; static double step; @@ -137,8 +137,8 @@ namespace custom_planner { return length; } - void MergePathCalc::updatePoseOrientation(std::vector& saved_poses, - std::vector& nurbs_path, + void MergePathCalc::updatePoseOrientation(std::vector& saved_poses, + std::vector& nurbs_path, bool reverse) { double yaw; @@ -312,8 +312,8 @@ namespace custom_planner { return nearest_idx; } - bool MergePathCalc::findNURBSControlPoints(vector& control_points, - const geometry_msgs::PoseStamped& pose, + bool MergePathCalc::findNURBSControlPoints(vector& control_points, + const robot_geometry_msgs::PoseStamped& pose, std::vector& 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(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(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; diff --git a/src/plan_for_retry.cpp b/src/plan_for_retry.cpp index 565e3a3..1473ec9 100755 --- a/src/plan_for_retry.cpp +++ b/src/plan_for_retry.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include using namespace std; @@ -41,7 +41,7 @@ bool isThetaValid(double theta) return result; } -double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose) +double computeDeltaAngleStartOfPlan(double theta, robot_geometry_msgs::Pose& startPose, robot_geometry_msgs::Pose& next_Pose) { double delta_angle = 0; if(isThetaValid(theta)) @@ -66,7 +66,7 @@ double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose return delta_angle; } -double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose) +double computeDeltaAngleEndOfPlan(double theta, robot_geometry_msgs::Pose& endPose, robot_geometry_msgs::Pose& prev_Pose) { double delta_angle = 0; if(isThetaValid(theta)) @@ -92,8 +92,8 @@ double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, ge } // Hàm chia đoạn thẳng AB thành các đoạn có độ dài d -std::vector divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d) { - std::vector Poses; +std::vector divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d) { + std::vector Poses; double xAB = B.pose.position.x - A.pose.position.x; double yAB = B.pose.position.y - A.pose.position.y; double length = sqrt(xAB*xAB + yAB*yAB); @@ -106,7 +106,7 @@ std::vector divideSegment(geometry_msgs::PoseStamped // Tính toán tọa độ của các điểm trên đoạn AB double ratio = d / length; for (int i = 1; i <= segments; ++i) { - geometry_msgs::PoseStamped p; + robot_geometry_msgs::PoseStamped p; double p_x = A.pose.position.x + (B.pose.position.x - A.pose.position.x) * ratio * i; double p_y = A.pose.position.y + (B.pose.position.y - A.pose.position.y) * ratio * i; p.pose.position.x = p_x; @@ -181,14 +181,14 @@ std::vector divideSegment(geometry_msgs::PoseStamped // pose_C: tâm của cung tròn AB // result_plan: vector chứa plan kết quả -bool makePlanForRetry(std::vector& current_plan, - int indexOfPoseA, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind, - geometry_msgs::PoseStamped& pose_C, std::vector& result_plan) +bool makePlanForRetry(std::vector& current_plan, + int indexOfPoseA, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind, + robot_geometry_msgs::PoseStamped& pose_C, std::vector& result_plan) { bool result = false; - std::vector PlanRetry_1; - std::vector PlanRetry_2; - std::vector PlanRetry_3; + std::vector PlanRetry_1; + std::vector PlanRetry_2; + std::vector PlanRetry_3; if(current_plan.empty()||current_plan.size()<2) { @@ -196,7 +196,7 @@ bool makePlanForRetry(std::vector& current_plan, return false; } - geometry_msgs::PoseStamped pose_A; + robot_geometry_msgs::PoseStamped pose_A; pose_A = current_plan[indexOfPoseA]; // Tính ra PlanRetry_1 điểm retry tại Pose_A @@ -228,7 +228,7 @@ bool makePlanForRetry(std::vector& current_plan, computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w), pose_B.pose, pose_A.pose) >= 0)) { - std::vector planSegment_AB; + std::vector planSegment_AB; planSegment_AB = divideSegment(pose_A, pose_B, 0.1); PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end()); } @@ -285,7 +285,7 @@ bool makePlanForRetry(std::vector& current_plan, double angle_tmp = angleCA + angleACB*i; double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double yP = pose_C.pose.position.y + rCA*sin(angle_tmp); - geometry_msgs::PoseStamped p; + robot_geometry_msgs::PoseStamped p; p.pose.position.x = xP; p.pose.position.y = yP; p.pose.position.z = 0; @@ -299,7 +299,7 @@ bool makePlanForRetry(std::vector& current_plan, double angle_tmp = angleCA - angleACB*i; double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double yP = pose_C.pose.position.y + rCA*sin(angle_tmp); - geometry_msgs::PoseStamped p; + robot_geometry_msgs::PoseStamped p; p.pose.position.x = xP; p.pose.position.y = yP; p.pose.position.z = 0; @@ -420,13 +420,13 @@ bool makePlanForRetry(std::vector& current_plan, // pose_C: tâm của cung tròn AB // result_plan: vector chứa plan kết quả -bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind, - geometry_msgs::PoseStamped& pose_C, std::vector& result_plan) +bool makePlanForRetry(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind, + robot_geometry_msgs::PoseStamped& pose_C, std::vector& result_plan) { bool result = false; - std::vector PlanRetry_1; - std::vector PlanRetry_2; - std::vector PlanRetry_3; + std::vector PlanRetry_1; + std::vector PlanRetry_2; + std::vector PlanRetry_3; ROS_INFO("[makePlanForRetry] pose_A: %f, %f, %f pose_B: %f, %f, %f pose_C: %f, %f", pose_A.pose.position.x, pose_A.pose.position.y, @@ -459,7 +459,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w), pose_B.pose, pose_A.pose) >= 0)) { - std::vector planSegment_AB; + std::vector planSegment_AB; planSegment_AB = divideSegment(pose_A, pose_B, 0.1); PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end()); } @@ -516,7 +516,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta double angle_tmp = angleCA + angleACB*i; double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double yP = pose_C.pose.position.y + rCA*sin(angle_tmp); - geometry_msgs::PoseStamped p; + robot_geometry_msgs::PoseStamped p; p.pose.position.x = xP; p.pose.position.y = yP; p.pose.position.z = 0; @@ -530,7 +530,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta double angle_tmp = angleCA - angleACB*i; double xP = pose_C.pose.position.x + rCA*cos(angle_tmp); double yP = pose_C.pose.position.y + rCA*sin(angle_tmp); - geometry_msgs::PoseStamped p; + robot_geometry_msgs::PoseStamped p; p.pose.position.x = xP; p.pose.position.y = yP; p.pose.position.z = 0; @@ -648,7 +648,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta // pose_B: điểm đích trên cung tròn // pose_C: tâm của cung tròn AB (kết quả) -bool findCenterOfCurve(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_C) +bool findCenterOfCurve(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_C) { // nếu hướng của vector AB và hướng của pose_B tạo với nhau một góc ~0 độ hoặc ~180 độ -> điểm C sẽ gần xấp xỉ với trung điểm của đoạn thẳng AB. if((computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),