From 377ebe3d6f66c6a1c7f2d1a1d76fd1b330a93952 Mon Sep 17 00:00:00 2001 From: duongtd Date: Fri, 19 Dec 2025 16:45:50 +0700 Subject: [PATCH] first commit --- CMakeLists.txt | 110 ++ config/custom_global_params.yaml | 17 + include/custom_planner/Curve_common.h | 84 ++ include/custom_planner/color.h | 34 + include/custom_planner/conversion.h | 43 + include/custom_planner/custom_planner.h | 230 ++++ include/custom_planner/merge_path_calc.h | 214 ++++ include/custom_planner/pathway.h | 150 +++ include/custom_planner/pose.h | 52 + src/Curve_common.cpp | 1269 ++++++++++++++++++++++ src/custom_planner.cpp | 1195 ++++++++++++++++++++ src/merge_path_calc.cpp | 673 ++++++++++++ src/pathway.cc | 291 +++++ src/plan_for_retry.cpp | 686 ++++++++++++ src/pose.cc | 42 + 15 files changed, 5090 insertions(+) create mode 100755 CMakeLists.txt create mode 100755 config/custom_global_params.yaml create mode 100755 include/custom_planner/Curve_common.h create mode 100755 include/custom_planner/color.h create mode 100755 include/custom_planner/conversion.h create mode 100755 include/custom_planner/custom_planner.h create mode 100755 include/custom_planner/merge_path_calc.h create mode 100755 include/custom_planner/pathway.h create mode 100755 include/custom_planner/pose.h create mode 100755 src/Curve_common.cpp create mode 100755 src/custom_planner.cpp create mode 100755 src/merge_path_calc.cpp create mode 100755 src/pathway.cc create mode 100755 src/plan_for_retry.cpp create mode 100755 src/pose.cc diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..c01b697 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,110 @@ +# --- CMake version và project name --- +cmake_minimum_required(VERSION 3.10) +project(custom_planner) + +# --- C++ standard và position independent code --- +set(CMAKE_CXX_STANDARD 17) # Sử dụng C++17 +set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Thư viện có thể build thành shared lib + +# --- RPATH settings: ưu tiên thư viện build tại chỗ --- +# Dùng để runtime linker tìm thư viện đã build trước khi install +set(CMAKE_SKIP_BUILD_RPATH FALSE) +set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) +set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/custom_planner") +set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/custom_planner") +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + +# --- Dependencies --- +# Tìm các thư viện cần thiết +# find_package(tf3 REQUIRED) # Nếu dùng tf3 +find_package(Eigen3 REQUIRED) # Thư viện Eigen cho toán học +find_package(Boost REQUIRED COMPONENTS system thread filesystem) # Boost: system, thread, filesystem + +# --- Include directories --- +# Thêm các folder chứa header files +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# --- Eigen và PCL definitions --- +add_definitions(${EIGEN3_DEFINITIONS}) + +# --- Core library: custom_planner --- +# Tạo thư viện chính +add_library(custom_planner + src/custom_planner.cpp + src/pathway.cc + src/pose.cc + src/Curve_common.cpp + src/merge_path_calc.cpp +) + +# --- Link các thư viện phụ thuộc --- +target_link_libraries(custom_planner + ${Boost_LIBRARIES} # Boost + visualization_msgs + nav_msgs + tf3 + tf3_geometry_msgs + robot_time + data_convert + costmap_2d + nav_core + robot_protocol_msgs +) + +# --- Include directories cho target --- +target_include_directories(custom_planner + PUBLIC + ${Boost_INCLUDE_DIRS} # Boost headers + $ # Khi build từ source + $ # Khi install + ) + +# --- Cài đặt thư viện vào hệ thống khi chạy make install --- +install(TARGETS custom_planner + EXPORT custom_planner-targets + ARCHIVE DESTINATION lib # Thư viện tĩnh .a + LIBRARY DESTINATION lib # Thư viện động .so + RUNTIME DESTINATION bin # File thực thi (nếu có) + INCLUDES DESTINATION include # Cài đặt include +) + +# --- Xuất export set costmap_2dTargets thành file CMake module --- +# --- Tạo file lib/cmake/custom_planner/costmap_2dTargets.cmake --- +# --- File này chứa cấu hình giúp project khác có thể dùng --- +# --- Find_package(custom_planner REQUIRED) --- +# --- Target_link_libraries(my_app PRIVATE custom_planner::custom_planner) --- +install(EXPORT custom_planner-targets + FILE custom_planner-targets.cmake + NAMESPACE custom_planner:: + DESTINATION lib/cmake/custom_planner +) + +# --- Cài đặt headers --- +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +# # --- Plugin libraries --- +# # Tạo các plugin shared library +# add_library(plugins +# SHARED +# plugins/static_layer.cpp +# plugins/obstacle_layer.cpp +# plugins/inflation_layer.cpp +# plugins/voxel_layer.cpp +# plugins/critical_layer.cpp +# plugins/directional_layer.cpp +# plugins/preferred_layer.cpp +# plugins/unpreferred_layer.cpp +# ) + +# target_link_libraries(plugins +# custom_planner +# ${Boost_LIBRARIES} +# yaml-cpp +# robot_time +# ) \ No newline at end of file diff --git a/config/custom_global_params.yaml b/config/custom_global_params.yaml new file mode 100755 index 0000000..6536a6e --- /dev/null +++ b/config/custom_global_params.yaml @@ -0,0 +1,17 @@ +base_global_planner: CustomPlanner +CustomPlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: false + nominalvel_mpersecs: 0.8 + timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s + allow_unknown: true + directory_to_save_paths: "/init/paths" + pathway_filename: "pathway.txt" + current_pose_topic_name: "/amcl_pose" + map_frame_id: "map" + base_frame_id: "base_link" + diff --git a/include/custom_planner/Curve_common.h b/include/custom_planner/Curve_common.h new file mode 100755 index 0000000..bb9f5f8 --- /dev/null +++ b/include/custom_planner/Curve_common.h @@ -0,0 +1,84 @@ +#ifndef CUSTOM_PLANNER_CURVE_COMMON_H_ +#define CUSTOM_PLANNER_CURVE_COMMON_H_ + +//#include "agv_path_smoothing/conversion.h" +#include +//#include +// #include +// #include + +#include +#include +#include +#include "custom_planner/color.h" + +struct Spline_Inf +{ + int order; + std::vector knot_vector; + std::vector weight; + //std::vector N; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::vector > control_point; + std::vector > N; + std::vector > dN; + std::vector > ddN; + std::vector N_double_vec; + std::vector R_double_vec; + std::vector dR_double_vec; + std::vector ddR_double_vec; + // std::vector ddN_double_vec; + //Eigen::Matrix N; //bsaic function + Eigen::MatrixXd curvefit_N; + +}; + +struct EigenTrajectoryPoint +{ + typedef std::vector > 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 > control_point, std::vector knot_vector); + void ReadSplineInf(Spline_Inf *bspline_inf, std::vector weight_vector, bool use_limit_derivative_fitting); + void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector file_discreate_point); + void ReadDiscreate2DPointFromLaunch(std::vector > *input_point, std::vector file_discreate_point); + void ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point); + void ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector > 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 > discreate_point, const std::string& frame_id, const std::string& name, double scale); + visualization_msgs::Marker ShowDiscreatePoint2(std::vector > 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_ \ No newline at end of file diff --git a/include/custom_planner/color.h b/include/custom_planner/color.h new file mode 100755 index 0000000..0f7640d --- /dev/null +++ b/include/custom_planner/color.h @@ -0,0 +1,34 @@ +#ifndef CUSTOM_PLANNER_COLOR_H_ +#define CUSTOM_PLANNER_COLOR_H_ + +#include + +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_ \ No newline at end of file diff --git a/include/custom_planner/conversion.h b/include/custom_planner/conversion.h new file mode 100755 index 0000000..096bd9d --- /dev/null +++ b/include/custom_planner/conversion.h @@ -0,0 +1,43 @@ +#ifndef CUSTOM_PLANNER_CONVERSION_H_ +#define CUSTOM_PLANNER_CONVERSION_H_ +#include "custom_planner/Curve_common.h" + +#include +#include +#include + +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 &plan) +{ + EigenTrajectoryPoint::Vector eigen_trajectory_point_vec; + EigenTrajectoryPoint eigen_trajectory_point; + + for(int i = 0; i < static_cast(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 &discreate_point_vec) +{ + EigenTrajectoryPoint::Vector eigen_trajectory_point_vec; + EigenTrajectoryPoint eigen_trajectory_point; + + for(int i = 0; i < static_cast(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_ \ No newline at end of file diff --git a/include/custom_planner/custom_planner.h b/include/custom_planner/custom_planner.h new file mode 100755 index 0000000..c96a6b3 --- /dev/null +++ b/include/custom_planner/custom_planner.h @@ -0,0 +1,230 @@ +#ifndef CUSTOM_PLANNER_H +#define CUSTOM_PLANNER_H + +#include +#include +#include +#include + +using namespace std; + +// ROS +#include +#include + +// Costmap used for the map representation +#include + +// global representation +#include + +// tf +#include +#include + +// internal lib +#include +#include +#include "custom_planner/Curve_common.h" +#include "custom_planner/conversion.h" +#include "custom_planner/merge_path_calc.h" + +#include + +#include +#include + +#include +// #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& 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& 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& footprint, + std::vector& out_footprint); + + Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0); + + bool countRobotDirectionChangeAngle(vector& 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& 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 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& saveposesOnEdge, + int& total_edge); + + bool isThetaValid(double theta); + + bool curveIsValid(int degree, const std::vector &knot_vector, + vector>& control_points); + + void setYawAllPosesOnEdge(vector& posesOnEdge, bool reverse); + + 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 lưu thông tin các cạnh + std::vector edges_info_; + std::vector 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 posesOnPathWay; + Pose start_on_path; + std::map 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 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 service_servers_; + + // ros::Publisher sbpl_plan_footprint_pub_; + boost::mutex mutex_; +}; +} + +#endif + diff --git a/include/custom_planner/merge_path_calc.h b/include/custom_planner/merge_path_calc.h new file mode 100755 index 0000000..3ab36d4 --- /dev/null +++ b/include/custom_planner/merge_path_calc.h @@ -0,0 +1,214 @@ +#ifndef MERGER_PATH_CALC_H_ +#define MERGER_PATH_CALC_H_ +#include +#include +#include + +#include + +#include "custom_planner/pose.h" +#include "custom_planner/Curve_common.h" +#include + +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& 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& control_points, + const geometry_msgs::PoseStamped& start, + std::vector& 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 calculateNURBSPath(vector& control_points, + bool reverse); + + bool handleEdgeIntersection(vector& savePosesOnEdge, + vector& 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& 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 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& saved_poses, + std::vector& 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& Poses, + bool reverse = false); + + + }; + +} // namespace custom_planner +#endif // MERGE_PATH_CALC_H_ \ No newline at end of file diff --git a/include/custom_planner/pathway.h b/include/custom_planner/pathway.h new file mode 100755 index 0000000..3e70456 --- /dev/null +++ b/include/custom_planner/pathway.h @@ -0,0 +1,150 @@ +#ifndef CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H +#define CUSTOM_PLANNER_CUSTOM_PLANNER_PATHWAY_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 path_; + vector 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 getPosesOnPath(){return posesOnPath_;} + + void syncPosesAndPath(); + + void SavePathAsFile(const string& file_name); + + void LoadPathFromFile(const string& file_name); + + vector findIntersections(Edge L); + + vector> findIntersections(Edge L1, Edge L2); + + void testprint() + { + if(!path_.empty()){ + int i =0; + for(auto &edge : path_) + { + cout<<"edge "< +#include +#include +#include +#include +#include +#include +#include + +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 \ No newline at end of file diff --git a/src/Curve_common.cpp b/src/Curve_common.cpp new file mode 100755 index 0000000..4210292 --- /dev/null +++ b/src/Curve_common.cpp @@ -0,0 +1,1269 @@ +#include "custom_planner/Curve_common.h" +#include "custom_planner/conversion.h" + +#include + +#include + +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 line_result; + geometry_msgs::PoseStamped current_pose; + + line_result.header.frame_id = frame_id; + line_result.header.stamp = robot::Time::now(); + + current_pose.header.frame_id = frame_id; + + int segment = 1 / t_intervel; + double line_parameter = 0; + + for(int i = 0; i < segment; i++) + { + current_pose.header.seq = i; + current_pose.header.stamp = robot::Time::now(); + current_pose.pose.position.x = start_point.x * (1 - line_parameter) + end_point.x * (line_parameter); + current_pose.pose.position.y = start_point.y * (1 - line_parameter) + end_point.y * (line_parameter); + + line_result.poses.push_back(current_pose); + line_parameter += t_intervel; + } + + std::cout << "End cacluation" << "\n"; + return line_result; +} + +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; + EigenTrajectoryPoint::Vector temp_control_point_vec; + + bezier_curve_result.header.frame_id = frame_id; + bezier_curve_result.header.stamp = robot::Time::now(); + current_pose.header.frame_id = frame_id; + + int segment = 1 / t_intervel; + int control_point_length = control_point.size(); + double curve_parameter = 0; + temp_control_point_vec.reserve(control_point_length); + + for(int i = 0; i <= segment; i++) + { + temp_control_point_vec.assign(control_point.begin(), control_point.end()); + + for(int j = 1; j <= control_point_length - 1; j++) + { + for(int k = 1; k <= control_point_length - j; k++) + { + temp_control_point_vec.at(k - 1).position(0) = temp_control_point_vec.at(k - 1).position(0) * (1 - curve_parameter) + temp_control_point_vec.at(k).position(0) * curve_parameter; + temp_control_point_vec.at(k - 1).position(1) = temp_control_point_vec.at(k - 1).position(1) * (1 - curve_parameter) + temp_control_point_vec.at(k).position(1) * curve_parameter; + } + } + current_pose.header.seq = i; + current_pose.header.stamp = robot::Time::now(); + current_pose.pose.position.x = temp_control_point_vec.at(0).position(0); + current_pose.pose.position.y = temp_control_point_vec.at(0).position(1); + bezier_curve_result.poses.push_back(current_pose); + curve_parameter += t_intervel; + } + + std::cout << "End calculate Bezier Curve" << "\n"; + return bezier_curve_result; +} + +void Curve_common::ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector file_discreate_point) +{ + int index = 0; + Eigen::Vector3d read_point; + EigenTrajectoryPoint eigen_discreate_point; + input_point->reserve(static_cast(file_discreate_point.size()) / 2); + + for(int i = 0; i < static_cast(file_discreate_point.size()); i++) + { + if(i % 2 == 0) + { + read_point(0) = file_discreate_point[i]; + } + else + { + read_point(1) = file_discreate_point[i]; + eigen_discreate_point.position = read_point; + //input_point->emplace_back(dis_control_point); //I don't know why have error + input_point->push_back(eigen_discreate_point); + index++; + } + } + + //Debug use + // std::cout << "input control point size : " << input_control_point->size() << "\n"; + // for(int i = 0; i < input_control_point->size(); i++) + // { + // std::cout << "control point x : " << input_control_point->at(i).position(0) << "\n"; + // std::cout << "control point y : " << input_control_point->at(i).position(1) << "\n"; + // } +} + +void Curve_common::ReadDiscreate2DPointFromLaunch(std::vector > *input_point, std::vector file_discreate_point) +{ + Eigen::Vector3d read_point; + input_point->reserve(static_cast(file_discreate_point.size()) / 2); + + for(int i = 0; i < static_cast(file_discreate_point.size()); i++) + { + if(i % 2 == 0) + { + read_point(0) = file_discreate_point[i]; + } + else + { + read_point(1) = file_discreate_point[i]; + input_point->push_back(read_point); + } + } + + //Debug use + // std::cout << "input control point size : " << input_point->size() << "\n"; + // for(int i = 0; i < input_point->size(); i++) + // { + // std::cout << "control point x : " << input_point->at(i)(0) << "\n"; + // std::cout << "control point y : " << input_point->at(i)(1) << "\n"; + // } +} + +visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector > discreate_point, const std::string& frame_id, const std::string& name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = robot::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + //waypoints_marker.color = color; + waypoints_marker.color.r = 1; + waypoints_marker.color.g = 1; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(static_cast(discreate_point.size())); + + geometry_msgs::Point view_point; + for(int i = 0; i < static_cast(discreate_point.size()); i++) + { + view_point.x = discreate_point.at(i)(0); + view_point.y = discreate_point.at(i)(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = robot::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + waypoints_marker.color = point_color; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(static_cast(discreate_point.size())); + + geometry_msgs::Point view_point; + for(int i = 0; i < static_cast(discreate_point.size()); i++) + { + view_point.x = discreate_point.at(i)(0); + view_point.y = discreate_point.at(i)(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = robot::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + //waypoints_marker.color = color; + waypoints_marker.color.r = 1; + waypoints_marker.color.g = 1; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(static_cast(discreate_point.size())); + + 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); + view_point.y = discreate_point.at(i).position(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = robot::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + waypoints_marker.color = point_color; + // waypoints_marker.color.r = r; + // waypoints_marker.color.g = g; + // waypoints_marker.color.b = b; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(static_cast(discreate_point.size())); + + 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); + view_point.y = discreate_point.at(i).position(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) +{ + 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); + view_point.y = discreate_point.at(i).position(1); + points->points.push_back(view_point); + } +} + +void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector > discreate_point) +{ + geometry_msgs::Point view_point; + for(int i = 0; i < static_cast(discreate_point.size()); i++) + { + view_point.x = discreate_point.at(i)(0); + view_point.y = discreate_point.at(i)(1); + points->points.push_back(view_point); + } +} + +void Curve_common::ReadSplineInf(Spline_Inf *bspline_inf, int order, std::vector > control_point, std::vector knot_vector) +{ + bspline_inf->order = order; + bspline_inf->knot_vector.assign(knot_vector.begin(), knot_vector.end()); + bspline_inf->control_point.assign(control_point.begin(), control_point.end()); + + //Debug use + // std::cout << "knot vector size : " << bspline_inf->knot_vector.size() << "\n"; + // for(int i = 0; i < bspline_inf->knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << bspline_inf->knot_vector.at(i) << "\n"; + // } + + // std::cout << "input control point size : " << bspline_inf->control_point.size() << "\n"; + // for(int i = 0; i < bspline_inf->control_point.size(); i++) + // { + // std::cout << "control point x : " << bspline_inf->control_point.at(i)(0) << ", y = " << bspline_inf->control_point.at(i)(1) << "\n"; + // } +} + +void Curve_common::ReadSplineInf(Spline_Inf *spline_inf, std::vector weight_vector, bool use_limit_derivative_fitting) +{ + if(!use_limit_derivative_fitting) + { + if(static_cast(spline_inf->control_point.size()) != static_cast(weight_vector.size())) + { + std::cout << "weight vector size is wrong" << "\n"; + return; + } + } + else + { + for(int i = 0; i < 2; i++) + weight_vector.push_back(1); + } + + spline_inf->weight.assign(weight_vector.begin(), weight_vector.end()); + + + //Debug use + // std::cout << "knot vector size : " << spline_inf->knot_vector.size() << "\n"; + // for(int i = 0; i < spline_inf->knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << spline_inf->knot_vector.at(i) << "\n"; + // } + + // std::cout << "control point size : " << static_cast(spline_inf->control_point.size()) << "\n"; + // for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + // { + // std::cout << "control point x : " << spline_inf->control_point.at(i)(0) << ", y = " << spline_inf->control_point.at(i)(1) << "\n"; + // } +} + +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; + + bspline_curve_result.header.frame_id = frame_id; + bspline_curve_result.header.stamp = robot::Time::now(); + current_pose.header.frame_id = frame_id; + + int p_degree = bspline_inf.order - 1; + int n = bspline_inf.control_point.size() - 1; + //TODO: Check knot vector size and sequence is correect + int m = bspline_inf.knot_vector.size() - 1; //The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 100; + double curve_parameter = 0; + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + double sum_x = 0, sum_y = 0; + std::vector curve_parameter_vec; + Eigen::VectorXd temp_basic_function; + std::vector > temp_basic_function_eigen; + + //Debug use + // for(int i = 0; i < bspline_inf.knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << bspline_inf.knot_vector.at(i) << "\n"; + // } + + // std::cout << "input control point size : " << bspline_inf.control_point.size() << "\n"; + // for(int i = 0; i < bspline_inf.control_point.size(); i++) + // { + // std::cout << "control point x : " << bspline_inf.control_point.at(i)(0) << ", y = " << bspline_inf.control_point.at(i)(1) << "\n"; + // } + + //TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for(int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + bspline_inf.N.clear(); + temp_basic_function.resize(segment); + + //TODO: Check basic function boundary is correct + //Calculate degree = 0's basic function + for(int i = 0; i < m; i++) + { + if(bspline_inf.knot_vector.at(i) != bspline_inf.knot_vector.at(n)) + { + for(int u = 0; u < segment; u++) + { + if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] < bspline_inf.knot_vector.at(i+1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + //std::cout << "small temp basic function index : " << u << " small temp basic function value : " << temp_basic_function(u) << "\n"; + //std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + else + { + for(int u = 0; u < segment; u++) + { + if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= bspline_inf.knot_vector.at(i+1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + //std::cout << "temp basic function index : " << u << "temp basic function value : " << temp_basic_function(u) << "\n"; + //std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + + + // if(bspline_inf.knot_vector.at(i) < bspline_inf.knot_vector.at(i+1)) + // { + // if(bspline_inf.knot_vector.at(i) != bspline_inf.knot_vector.at(n)) + // { + // for(int u = 0; u < segment; u++) + // { + // if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] < bspline_inf.knot_vector.at(i+1)) + // temp_basic_function(u) = 1; + // else + // temp_basic_function(u) = 0; + + // std::cout << "small temp basic function index : " << u << " small temp basic function value : " << temp_basic_function(u) << "\n"; + // } + // } + // else + // { + // for(int u = 0; u < segment; u++) + // { + // if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= bspline_inf.knot_vector.at(i+1)) + // temp_basic_function(u) = 1; + // else + // temp_basic_function(u) = 0; + + // std::cout << "n basic function index : " << u << " n basic function value : " << temp_basic_function(u) << "\n"; + // //std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + // } + // } + // } + // else + // { + // for(int u = 0; u < segment; u++) + // { + // if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= bspline_inf.knot_vector.at(i+1)) + // temp_basic_function(u) = 1; + // else + // temp_basic_function(u) = 0; + + // std::cout << "temp basic function index : " << u << " temp basic function value : " << temp_basic_function(u) << "\n"; + // } + // } + bspline_inf.N.push_back(temp_basic_function); + //std::cout << i << "'s bspline_inf Ni,0 size " << temp_basic_function.size() << "\n"; + } + + //Calculate the rest of basic function + for(int p = 1; p <= p_degree; p++) + { + temp_basic_function_eigen.clear(); + for(int i = 0; i < (m - p); i++) + { + for(int u = 0; u < segment; u++) + { + left_denom = bspline_inf.knot_vector.at(p+i) - bspline_inf.knot_vector.at(i); + right_denom = bspline_inf.knot_vector.at(i+p+1) - bspline_inf.knot_vector.at(i+1); + + if(left_denom == 0) + left_term = 0; + else + left_term = (curve_parameter_vec[u] - bspline_inf.knot_vector.at(i)) * bspline_inf.N.at(i)(u) / left_denom; + + if(right_denom == 0) + right_term = 0; + else + right_term = (bspline_inf.knot_vector.at(i+p+1) - curve_parameter_vec[u]) * bspline_inf.N.at(i+1)(u) / right_denom; + + temp_basic_function(u) = left_term + right_term; + } + temp_basic_function_eigen.push_back(temp_basic_function); + } + bspline_inf.N = temp_basic_function_eigen; + } + + for(int u = 0; u < segment; u++) + { + sum_x = 0; + sum_y = 0; + for(int i = 0; i < static_cast(bspline_inf.N.size()); i++) + { + sum_x += bspline_inf.control_point.at(i)(0) * bspline_inf.N.at(i)(u); + sum_y += bspline_inf.control_point.at(i)(1) * bspline_inf.N.at(i)(u); + //std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + current_pose.header.seq = u; + current_pose.header.stamp = robot::Time::now(); + current_pose.pose.position.x = sum_x; + current_pose.pose.position.y = sum_y; + bspline_curve_result.poses.push_back(current_pose); + } + + std::cout << "End Calculate" << "\n"; + return bspline_curve_result; +} + +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; + + nurbs_curve_result.header.frame_id = frame_id; + nurbs_curve_result.header.stamp = robot::Time::now(); + current_pose.header.frame_id = frame_id; + + int p_degree = spline_inf.order - 1; + int n = spline_inf.control_point.size() - 1; + //TODO: Check knot vector size and sequence is correect + int m = spline_inf.knot_vector.size() - 1; //The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 100; + double curve_parameter = 0; + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + double sum_nurbs_denom = 0; + double sum_x = 0, sum_y = 0; + std::vector curve_parameter_vec; + Eigen::VectorXd temp_basic_function; + std::vector > temp_basic_function_eigen; + + //Debug use + // for(int i = 0; i < bspline_inf.knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << bspline_inf.knot_vector.at(i) << "\n"; + // } + + // std::cout << "input control point size : " << bspline_inf.control_point.size() << "\n"; + // for(int i = 0; i < bspline_inf.control_point.size(); i++) + // { + // std::cout << "control point x : " << bspline_inf.control_point.at(i)(0) << ", y = " << bspline_inf.control_point.at(i)(1) << "\n"; + // } + + if(spline_inf.weight.size() == 0) + { + std::cout << "weight vector size is zero, please call read ReadSplineInf function" << "\n"; + return nurbs_curve_result; + } + + //TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for(int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + spline_inf.N.clear(); + temp_basic_function.resize(segment); + + //TODO: Check basic function boundary is correct + //Calculate degree = 0's basic function + for(int i = 0; i < m; i++) + { + //if(spline_inf.knot_vector.at(i) != spline_inf.knot_vector.at(n)) + if(spline_inf.knot_vector.at(i) != spline_inf.knot_vector.at(i+1)) + { + for(int u = 0; u < segment; u++) + { + if(spline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] < spline_inf.knot_vector.at(i+1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + //std::cout << "small temp basic function index : " << u << " small temp basic function value : " << temp_basic_function(u) << "\n"; + //std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + else + { + for(int u = 0; u < segment; u++) + { + if(spline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= spline_inf.knot_vector.at(i+1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + //std::cout << "temp basic function index : " << u << "temp basic function value : " << temp_basic_function(u) << "\n"; + //std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + + spline_inf.N.push_back(temp_basic_function); + //std::cout << i << "'s bspline_inf Ni,0 size " << temp_basic_function.size() << "\n"; + } + + //Calculate the rest of basic function + for(int p = 1; p <= p_degree; p++) + { + temp_basic_function_eigen.clear(); + for(int i = 0; i < (m - p); i++) //m-p is calculate how many region between basis function + { + for(int u = 0; u < segment; u++) + { + left_denom = spline_inf.knot_vector.at(p+i) - spline_inf.knot_vector.at(i); + right_denom = spline_inf.knot_vector.at(i+p+1) - spline_inf.knot_vector.at(i+1); + + if(left_denom == 0) + left_term = 0; + else + left_term = (curve_parameter_vec[u] - spline_inf.knot_vector.at(i)) * spline_inf.N.at(i)(u) / left_denom; + + if(right_denom == 0) + right_term = 0; + else + right_term = (spline_inf.knot_vector.at(i+p+1) - curve_parameter_vec[u]) * spline_inf.N.at(i+1)(u) / right_denom; + + temp_basic_function(u) = left_term + right_term; + } + temp_basic_function_eigen.push_back(temp_basic_function); + } + spline_inf.N = temp_basic_function_eigen; + } + + for(int u = 0; u < segment; u++) + { + sum_nurbs_denom = 0; + sum_x = 0; + sum_y = 0; + for(int i = 0; i < static_cast(spline_inf.N.size()); i++) + { + sum_nurbs_denom += spline_inf.N.at(i)(u) * spline_inf.weight.at(i); + sum_x += spline_inf.control_point.at(i)(0) * spline_inf.N.at(i)(u) * spline_inf.weight.at(i); + sum_y += spline_inf.control_point.at(i)(1) * spline_inf.N.at(i)(u) * spline_inf.weight.at(i); + //std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + sum_x /= sum_nurbs_denom; + sum_y /= sum_nurbs_denom; + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + current_pose.header.seq = u; + current_pose.header.stamp = robot::Time::now(); + current_pose.pose.position.x = sum_x; + current_pose.pose.position.y = sum_y; + nurbs_curve_result.poses.push_back(current_pose); + } + + std::cout << "End calculate NURBS function" << "\n"; + return nurbs_curve_result; +} + +void Curve_common::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_data, int differential_times) +{ + 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 + int m = spline_inf->knot_vector.size() - 1; //The last knot vector index number + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + double derivative_left_term = 0, derivative_right_term = 0; + double derivative_value = 0; + std::vector temp_basic_function; + Eigen::VectorXd derivative_temp_basic_function; + double degree_basis_value; + std::vector > derivative_basic_function_eigen; + + //Calculate degree = 0's basic function + spline_inf->N_double_vec.clear(); + spline_inf->N_double_vec.resize(m); + for(int i = 0; i < m; i++) + { + // if(spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(n)) + if(spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(i+1)) + { + if(spline_inf->knot_vector.at(i) <= u_data && u_data < spline_inf->knot_vector.at(i+1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + else + { + if(spline_inf->knot_vector.at(i) <= u_data && u_data <= spline_inf->knot_vector.at(i+1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + + spline_inf->N_double_vec.at(i) = degree_basis_value; + } + + //Calculate the rest of basic function + spline_inf->dN.clear(); + for(int p = 1; p <= p_degree; p++) + { + temp_basic_function.clear(); + derivative_temp_basic_function.resize(m-p); + for(int i = 0; i < (m - p); i++) + { + left_denom = spline_inf->knot_vector.at(p+i) - spline_inf->knot_vector.at(i); + right_denom = spline_inf->knot_vector.at(i+p+1) - spline_inf->knot_vector.at(i+1); + + if(left_denom == 0) + { + left_term = 0; + derivative_left_term = 0; + } + else + { + left_term = (u_data - spline_inf->knot_vector.at(i)) * spline_inf->N_double_vec.at(i) / left_denom; + derivative_left_term = spline_inf->N_double_vec.at(i) / left_denom; + } + + if(right_denom == 0) + { + right_term = 0; + derivative_right_term = 0; + } + else + { + right_term = (spline_inf->knot_vector.at(i+p+1) - u_data) * spline_inf->N_double_vec.at(i+1) / right_denom; + derivative_right_term = spline_inf->N_double_vec.at(i+1) / right_denom; + } + + derivative_value = p * (derivative_left_term - derivative_right_term); + temp_basic_function.push_back(left_term + right_term); //Calculate Ni,p + derivative_temp_basic_function(i) = derivative_value; //Calculate dNi,p + } + spline_inf->N_double_vec.assign(temp_basic_function.begin(), temp_basic_function.end()); + spline_inf->dN.push_back(derivative_temp_basic_function); + } + + if(differential_times > 1) + { + spline_inf->ddN.clear(); + for(int p = 2; p <= p_degree; p++) + { + derivative_temp_basic_function.resize(m-p); + for(int i = 0; i < (m - p); i++) + { + left_denom = spline_inf->knot_vector.at(p+i) - spline_inf->knot_vector.at(i); + right_denom = spline_inf->knot_vector.at(i+p+1) - spline_inf->knot_vector.at(i+1); + + if(left_denom == 0) + derivative_left_term = 0; + else + derivative_left_term = spline_inf->dN.at(p-2)(i) / left_denom; + + if(right_denom == 0) + derivative_right_term = 0; + else + derivative_right_term = spline_inf->dN.at(p-2)(i+1) / right_denom; + + derivative_value = p * (derivative_left_term - derivative_right_term); + derivative_temp_basic_function(i) = derivative_value; //Calculate ddNi,p + } + spline_inf->ddN.push_back(derivative_temp_basic_function); + } + } +} + +geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS) +{ + geometry_msgs::Point derivative_curve_point; + int p_degree = spline_inf->order - 1; + double sum_x = 0, sum_y = 0; + double sum_denom = 0; + double sum_derivative_once_denom = 0; + double sum_derivative_twice_denom = 0; + double R_fraction = 0; + double dR_left_term_fraction = 0; + double dR_right_term = 0; + double ddR_left_term_fraction = 0; + double ddR_median_term_fraction = 0; + double ddR_right_term_fraction = 0; + + spline_inf->R_double_vec.clear(); + spline_inf->dR_double_vec.clear(); + spline_inf->ddR_double_vec.clear(); + + //Debug use + // std::cout << "N_double_vec :" << "\n"; + // for(int i = 0; i < spline_inf->N_double_vec.size(); i++) + // { + // std::cout << spline_inf->N_double_vec.at(i) << " "; + // } + // std::cout << "\n"; + + // std::cout << "dN :" << "\n"; + // for(int i = 0; i < spline_inf->dN.back().size(); i++) + // { + // std::cout << spline_inf->dN.back()(i) << " "; + // } + // std::cout << "\n"; + + //TODO: Safety check + if(UsingNURBS == false) + { + if(differential_times == 1) + { + for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->dN.back()(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->dN.back()(i); + // std::cout << i << "'s spline_inf.dN : " << spline_inf->dN.back()(i) << "\n"; + } + } + if(differential_times == 2) + { + for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->ddN.back()(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->ddN.back()(i); + // std::cout << i << "'s spline_inf.ddN : " << spline_inf->ddN.back()(i) << "\n"; + } + } + } + else + { + //TODO: Safety check differential_times + for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + { + sum_denom += spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + sum_derivative_once_denom += spline_inf->dN.back()(i) * spline_inf->weight.at(i); + if(differential_times == 2) + sum_derivative_twice_denom += spline_inf->ddN.back()(i) * spline_inf->weight.at(i); + } + for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + { + if(sum_denom != 0) + { + R_fraction = spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + + dR_left_term_fraction = spline_inf->dN.back()(i) * spline_inf->weight.at(i); + dR_right_term = R_fraction * sum_derivative_once_denom / std::pow(sum_denom, 2); + + if(differential_times == 2) + { + ddR_left_term_fraction = spline_inf->ddN.back()(i) * spline_inf->weight.at(i); + ddR_median_term_fraction = 2 * dR_left_term_fraction * sum_derivative_once_denom + R_fraction * sum_derivative_twice_denom; + ddR_right_term_fraction = 2 * R_fraction * std::pow(sum_derivative_once_denom, 2); + spline_inf->ddR_double_vec.push_back( (ddR_left_term_fraction / sum_denom) - (ddR_median_term_fraction / std::pow(sum_denom, 2)) + (ddR_right_term_fraction / std::pow(sum_denom, 3)) ); + } + + spline_inf->R_double_vec.push_back(R_fraction / sum_denom); + spline_inf->dR_double_vec.push_back((dR_left_term_fraction / sum_denom) - dR_right_term); + } + else + { + spline_inf->R_double_vec.push_back(0); + spline_inf->dR_double_vec.push_back(0); + spline_inf->ddR_double_vec.push_back(0); + } + } + + if(differential_times == 1) + { + for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->dR_double_vec.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->dR_double_vec.at(i); + //std::cout << i << "'s spline_inf.dN : " << spline_inf->dN.back()(i) << "\n"; + } + } + if(differential_times == 2) + { + for(int i = 0; i < static_cast(spline_inf->control_point.size()); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->ddR_double_vec.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->ddR_double_vec.at(i); + } + } + } + + derivative_curve_point.x = sum_x; + derivative_curve_point.y = sum_y; + derivative_curve_point.z = 0; + + //Debug use + // std::cout << u_data << "'s derivative current_pose x : " << derivative_curve_point.x << "\n"; + // std::cout << u_data << "'s derivative current_pose y : " << derivative_curve_point.y << "\n"; + + return derivative_curve_point; +} + +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; + nav_msgs::Path bspline_derivative_result; + geometry_msgs::PoseStamped current_pose; + + bspline_derivative_result.header.frame_id = frame_id; + bspline_derivative_result.header.stamp = robot::Time::now(); + current_pose.header.frame_id = frame_id; + + int p_degree = bspline_inf.order - 1; + int n = bspline_inf.control_point.size() - 1; + //TODO: Check knot vector size and sequence is correect + int m = bspline_inf.knot_vector.size() - 1; //The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 0; + double curve_parameter = 0; + std::vector curve_parameter_vec; + + //TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for(int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + //TODO: Can support high order derivative in loop + for(int u = 0; u < segment; u++) + { + CalculateDerivativeBasisFunc(&bspline_inf, curve_parameter_vec[u], differential_times); + derivative_point_result = CalculateDerivativeCurvePoint(&bspline_inf, curve_parameter_vec[u], differential_times, false); + current_pose.header.seq = u; + current_pose.header.stamp = robot::Time::now(); + current_pose.pose.position.x = derivative_point_result.x; + current_pose.pose.position.y = derivative_point_result.y; + bspline_derivative_result.poses.push_back(current_pose); + } + + return bspline_derivative_result; +} + +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; + nav_msgs::Path derivative_basis_result; + geometry_msgs::PoseStamped current_pose; + + derivative_basis_result.header.frame_id = frame_id; + derivative_basis_result.header.stamp = robot::Time::now(); + current_pose.header.frame_id = frame_id; + + //TODO: Add error ckeck function(below code with error) + // if(differential_times == 0 && index < bspline_inf.N_double_vec.size()) + // { + // std::cout << "Without this index's basis function, vector size is :" << bspline_inf.N_double_vec.size() << "\n"; + // return derivative_basis_result; + // } + // if(differential_times == 1 && index < bspline_inf.dN.back().size()) + // { + // std::cout << "Without this index's basis function, vector size is :" << bspline_inf.dN.back().size() << "\n"; + // return derivative_basis_result; + // } + // if(differential_times == 2 && index < bspline_inf.ddN.back().size()) + // { + // std::cout << "Without this index's basis function, vector size is :" << bspline_inf.ddN.back().size() << "\n"; + // return derivative_basis_result; + // } + + int p_degree = bspline_inf.order - 1; + int n = bspline_inf.control_point.size() - 1; + //TODO: Check knot vector size and sequence is correect + int m = bspline_inf.knot_vector.size() - 1; //The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 0; + double curve_parameter = 0; + std::vector curve_parameter_vec; + + //TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for(int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + //TODO: Can support high order derivative in loop + for(int u = 0; u < segment; u++) + { + CalculateDerivativeBasisFunc(&bspline_inf, curve_parameter_vec[u], differential_times); + current_pose.header.seq = u; + current_pose.header.stamp = robot::Time::now(); + current_pose.pose.position.x = curve_parameter_vec[u]; + if(differential_times == 0) + current_pose.pose.position.y = bspline_inf.N_double_vec.at(index); + else if(differential_times == 1) + current_pose.pose.position.y = bspline_inf.dN.back()(index); + else + current_pose.pose.position.y = bspline_inf.ddN.back()(index); + derivative_basis_result.poses.push_back(current_pose); + } + + return derivative_basis_result; +} + +double Curve_common::CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + if(u_data < 0) + { + std::cout << "Error, u_data need bigger than 0" << "\n"; + std::cout << "error u_data value is: " << u_data << "\n"; + return 0; + } + + if(u_data > 1) + { + std::cout << "Error!! because u_data bigger than 1, maybe is program numerical error" << "\n"; + u_data = 1; + } + + Eigen::Vector3d derivative_once_point; + Eigen::Vector3d derivative_twice_point; + + double curvature = 0; + double fraction = 0; + double denominator = 0; + + CalculateDerivativeBasisFunc(&spline_inf, u_data, 2); + derivative_once_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 1, UsingNURBS)); + derivative_twice_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 2, UsingNURBS)); + + fraction = derivative_once_point.cross(derivative_twice_point).lpNorm<2>(); + denominator = std::pow(derivative_once_point.lpNorm<2>(), 3); + + if(denominator == 0) + curvature = 0; + else + curvature = fraction / denominator; + + //TODO: Check curvature direction (but in nurbs planner not useful) + // Eigen::Vector3d direction_vector; //curvature direction + // direction_vector = derivative_once_point.cross(derivative_twice_point.cross(derivative_once_point)); + // std::cout << "direction vector x: " << direction_vector(0) << "\n"; + // std::cout << "direction vector y: " << direction_vector(1) << "\n"; + // std::cout << "direction vector z: " << direction_vector(2) << "\n"; + // if(direction_vector(0) < 0 && direction_vector(1) < 0 || direction_vector(0) > 0 && direction_vector(1) < 0) + // curvature *= -1; + + return curvature; +} + +double Curve_common::CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + if(u_data < 0) + { + std::cout << "Error, u_data need bigger than 0" << "\n"; + std::cout << "error u_data value is: " << u_data << "\n"; + return 0; + } + + if(u_data > 1) + { + std::cout << "Error!! because u_data bigger than 1, maybe is program numerical error" << "\n"; + u_data = 1; + } + + Eigen::Vector3d derivative_once_point; + Eigen::Vector3d derivative_twice_point; + + double curvature = 0; + double fraction = 0; + double denominator = 0; + + CalculateDerivativeBasisFunc(&spline_inf, u_data, 2); + derivative_once_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 1, UsingNURBS)); + derivative_twice_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 2, UsingNURBS)); + + fraction = derivative_once_point.cross(derivative_twice_point).lpNorm<2>(); + denominator = std::pow(derivative_once_point.lpNorm<2>(), 3); + + if(denominator == 0) + curvature = 0; + else + curvature = fraction / denominator; + + //TODO: Check curvature direction (but in nurbs planner not useful) + Eigen::Vector3d direction_vector; //curvature direction + direction_vector = derivative_once_point.cross(derivative_twice_point.cross(derivative_once_point)); + // std::cout << "direction vector x: " << direction_vector(0) << "\n"; + // std::cout << "direction vector y: " << direction_vector(1) << "\n"; + // std::cout << "direction vector z: " << direction_vector(2) << "\n"; + if(direction_vector(0) < 0 && direction_vector(1) < 0 || direction_vector(0) > 0 && direction_vector(1) < 0) + curvature *= -1; + + return curvature; +} + +Eigen::Vector3d Curve_common::CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + Eigen::Vector3d direction_vector; + if(u_data < 0) + { + std::cout << "Error, u_data need bigger than 0" << "\n"; + std::cout << "error u_data value is: " << u_data << "\n"; + return direction_vector; + } + + if(u_data > 1) + { + std::cout << "Error!! because u_data bigger than 1, maybe is program numerical error" << "\n"; + u_data = 1; + } + + + Eigen::Vector3d derivative_once_point; + Eigen::Vector3d derivative_twice_point; + + CalculateDerivativeBasisFunc(&spline_inf, u_data, 2); + derivative_once_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 1, UsingNURBS)); + derivative_twice_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 2, UsingNURBS)); + + direction_vector = derivative_once_point.cross(derivative_twice_point.cross(derivative_once_point)); + // std::cout << "direction vector x: " << direction_vector(0) << "\n"; + // std::cout << "direction vector y: " << direction_vector(1) << "\n"; + // std::cout << "direction vector z: " << direction_vector(2) << "\n"; + + return direction_vector; +} + +double Curve_common::CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + double curvature_radius = 0; + double curvature = 0; + + curvature = CalculateCurvature(spline_inf, u_data, UsingNURBS); + + if(curvature == 0) + return curvature_radius = 0; + else + return curvature_radius = 1 / curvature; +} + +double Curve_common::CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS) +{ + if(sub_intervals % 2 != 0) + { + std::cout << "Error! sub_intervals value is a even number" << "\n"; + return 0; + } + + Eigen::Vector3d eigen_derivative_point; + double sum_length = 0; + double interval = 0; + double u_index = 0; + + interval = (end_u - start_u) / (double)sub_intervals; + + for(int i = 0; i <= sub_intervals; i++) + { + u_index = start_u + interval * i; + CalculateDerivativeBasisFunc(&spline_inf, u_index, 1); + eigen_derivative_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_index, 1, UsingNURBS)); + + if(i == 0 || i == sub_intervals) + sum_length += eigen_derivative_point.lpNorm<2>(); + else if((i % 2) == 1) + sum_length += 4 * eigen_derivative_point.lpNorm<2>(); + else if((i % 2) == 0) + sum_length += 2 * eigen_derivative_point.lpNorm<2>(); + } + + sum_length *= interval / 3; + + return sum_length; +} + +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; + 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 + int m = spline_inf->knot_vector.size() - 1; //The last knot vector index number + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + std::vector temp_basic_function; + double degree_basis_value; + double sum_nurbs_denom = 0; + double sum_x = 0, sum_y = 0; + + //Calculate degree = 0's basic function + spline_inf->N_double_vec.clear(); + spline_inf->N_double_vec.resize(m); + for(int i = 0; i < m; i++) + { + // if(spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(n)) + if(spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(i+1)) + { + if(spline_inf->knot_vector.at(i) <= u_data && u_data < spline_inf->knot_vector.at(i+1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + else + { + if(spline_inf->knot_vector.at(i) <= u_data && u_data <= spline_inf->knot_vector.at(i+1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + + spline_inf->N_double_vec.at(i) = degree_basis_value; + } + + //Calculate the rest of basic function + for(int p = 1; p <= p_degree; p++) + { + temp_basic_function.clear(); + for(int i = 0; i < (m - p); i++) + { + left_denom = spline_inf->knot_vector.at(p+i) - spline_inf->knot_vector.at(i); + right_denom = spline_inf->knot_vector.at(i+p+1) - spline_inf->knot_vector.at(i+1); + + if(left_denom == 0) + { + left_term = 0; + } + else + { + left_term = (u_data - spline_inf->knot_vector.at(i)) * spline_inf->N_double_vec.at(i) / left_denom; + } + + if(right_denom == 0) + { + right_term = 0; + } + else + { + right_term = (spline_inf->knot_vector.at(i+p+1) - u_data) * spline_inf->N_double_vec.at(i+1) / right_denom; + } + temp_basic_function.push_back(left_term + right_term); //Calculate Ni,p + } + spline_inf->N_double_vec.assign(temp_basic_function.begin(), temp_basic_function.end()); + } + + if(!UsingNURBS) + { + for(int i = 0; i < static_cast(spline_inf->N_double_vec.size()); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->N_double_vec.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->N_double_vec.at(i); + //std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + curve_point.x = sum_x; + curve_point.y = sum_y; + } + else + { + if(spline_inf->weight.size() == 0) + { + std::cout << "weight vector size is zero, please call read ReadSplineInf function" << "\n"; + curve_point.x = 0; + curve_point.y = 0; + return curve_point; + } + + for(int i = 0; i < static_cast(spline_inf->N_double_vec.size()); i++) + { + sum_nurbs_denom += spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + //std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + sum_x /= sum_nurbs_denom; + sum_y /= sum_nurbs_denom; + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + curve_point.x = sum_x; + curve_point.y = sum_y; + } + + return curve_point; +} \ No newline at end of file diff --git a/src/custom_planner.cpp b/src/custom_planner.cpp new file mode 100755 index 0000000..4a0ceb8 --- /dev/null +++ b/src/custom_planner.cpp @@ -0,0 +1,1195 @@ +#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() + : initialized_(false), costmap_robot_(NULL) + { + } + + CustomPlanner::CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) + : initialized_(false), costmap_robot_(NULL) + { + initialize(name, costmap_robot); + } + + bool CustomPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) + { + if (!initialized_) + { + robot::NodeHandle private_nh("~/" + name); + robot::NodeHandle p_nh("~"); + printf("Name is %s", name.c_str()); + + pathway = new Pathway(); + startPose = new Pose(); + input_spline_inf = new Spline_Inf(); + CurveDesign = new Curve_common(); + + private_nh.getParam("planner_type", planner_type_, string("ARAPlanner")); + private_nh.getParam("allocated_time", allocated_time_, 10.0); + private_nh.getParam("initial_epsilon", initial_epsilon_, 3.0); + private_nh.getParam("environment_type", environment_type_, string("XYThetaLattice")); + private_nh.getParam("forward_search", forward_search_, bool(false)); + p_nh.getParam("primitive_filename", primitive_filename_, string("")); + private_nh.getParam("force_scratch_limit", force_scratch_limit_, 500); + + double nominalvel_mpersecs, timetoturn45degsinplace_secs; + private_nh.getParam("nominalvel_mpersecs", nominalvel_mpersecs, 0.4); + private_nh.getParam("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6); + + int lethal_obstacle; + private_nh.getParam("lethal_obstacle", lethal_obstacle, 20); + lethal_obstacle_ = (unsigned char)lethal_obstacle; + inscribed_inflated_obstacle_ = lethal_obstacle_ - 1; + + private_nh.getParam("publish_footprint_path", publish_footprint_path_, bool(true)); + private_nh.getParam("visualizer_skip_poses", visualizer_skip_poses_, 5); + private_nh.getParam("allow_unknown", allow_unknown_, bool(true)); + + name_ = name; + costmap_robot_ = costmap_robot; + + footprint_ = costmap_robot_->getRobotFootprint(); + + initialized_ = true; + printf("[custom_planner] Initialized successfully"); + } + } + + bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg, + const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + std::vector& plan) + { + plan.clear(); + + // Check for reinitialization needs + bool do_init = false; + if(!makePlanWithOrder(msg, start, goal)) return false; + if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() || + current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY()) { + printf("Costmap dimensions have changed from (%d x %d) to (%d x %d), reinitializing custom_planner.", + current_env_width_, current_env_height_, + costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY()); + do_init = true; + } else if (footprint_ != costmap_robot_->getRobotFootprint()) { + printf("Robot footprint has changed, reinitializing custom_planner."); + do_init = true; + } + + if (do_init) { + initialized_ = false; + initialize(name_, costmap_robot_); + } + + // ===== STEP 2: VALIDATE PATHWAY ===== + if (posesOnPathWay.empty()) { + printf("[custom_planner] posesOnPathWay is empty"); + return false; + } + + // ===== STEP 3: VALIDATE START/GOAL POSITIONS ===== + unsigned int mx_start, my_start, mx_end, my_end; + if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start) || + !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_end, my_end)) { + printf("[custom_planner] Cannot convert world coordinates to map coordinates"); + } + + // ===== STEP 4: FIND NEAREST POINT ON PATHWAY ===== + int nearest_idx = 0; + double min_dist = std::numeric_limits::max(); + for (int i = 0; i < static_cast(posesOnPathWay.size()); ++i) { + double distance = std::hypot(posesOnPathWay[i].getX() - start.pose.position.x, + posesOnPathWay[i].getY() - start.pose.position.y); + if (distance < min_dist) { + min_dist = distance; + nearest_idx = i; + } + } + merge_path_calc_.store_start_nearest_idx_ = nearest_idx; + + // ===== STEP 5: CALCULATE DISTANCES ===== + const double DISTANCE_THRESHOLD = 0.1; + + robot::Time plan_time = robot::Time::now(); + + // Calculate distance from start to nearest pathway point + double dx_start = posesOnPathWay[nearest_idx].getX() - start.pose.position.x; + double dy_start = posesOnPathWay[nearest_idx].getY() - start.pose.position.y; + double d_start_to_path = std::sqrt(dx_start * dx_start + dy_start * dy_start); + + // Calculate distance from pathway end to goal + double dx_goal = goal.pose.position.x - posesOnPathWay.back().getX(); + double dy_goal = goal.pose.position.y - posesOnPathWay.back().getY(); + double d_path_to_goal = std::sqrt(dx_goal * dx_goal + dy_goal * dy_goal); + + // ===== STEP 6: GENERATE PATH BASED ON DISTANCE CONDITIONS ===== + + if (d_start_to_path <= DISTANCE_THRESHOLD && d_path_to_goal <= DISTANCE_THRESHOLD) { + // CASE 1: Both start and goal are close to pathway - Direct path + printf("[custom_planner] Using direct pathway"); + + for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { + geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + + } else if (d_start_to_path > DISTANCE_THRESHOLD && d_path_to_goal <= DISTANCE_THRESHOLD) { + // 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; + bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints( + start_control_points, start, posesOnPathWay, START); + + if (valid_start_nurbs) { + 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; + new_pose.header.stamp = plan_time; + new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + new_pose.pose.position.z = 0; + plan.push_back(new_pose); + } + + // Add remaining pathway + for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay.size(); i++) { + geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + } else { + // Fallback to direct path + for (size_t i = nearest_idx; i < posesOnPathWay.size(); i++) { + geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + } + + } else if (d_start_to_path <= DISTANCE_THRESHOLD && d_path_to_goal > DISTANCE_THRESHOLD) { + // 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; + 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; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + + // Add NURBS path to goal + std::vector nurbs_path = + merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_); + + for (const auto& pose : nurbs_path) { + 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; + plan.push_back(new_pose); + } + } else { + // Fallback: use entire pathway + for (size_t i = 0; i < posesOnPathWay.size(); i++) { + geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + } + + } else { + // 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; + bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints( + start_control_points, start, posesOnPathWay, START); + bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints( + goal_control_points, goal, posesOnPathWay, GOAL); + + bool valid_index_order = merge_path_calc_.start_target_idx_ < merge_path_calc_.goal_target_idx_; + + if (!valid_index_order && valid_start_nurbs && valid_goal_nurbs) { + // Special case: Goal-only NURBS when indices are invalid + costmap_robot_->getRobotPose(goal_control_points[0]); + goal_control_points[goal_control_points.size() - 1] = goal; + + 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]; + new_pose.header.stamp = plan_time; + new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + new_pose.pose.position.z = 0; + plan.push_back(new_pose); + } + + } else if (valid_start_nurbs && valid_goal_nurbs && valid_index_order) { + // Dual NURBS: Start NURBS + Pathway + Goal NURBS + + // Add 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; + new_pose.header.stamp = plan_time; + new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + new_pose.pose.position.z = 0; + plan.push_back(new_pose); + } + + // 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; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + + // Add 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; + new_pose.header.stamp = plan_time; + new_pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + new_pose.pose.position.z = 0; + plan.push_back(new_pose); + } + + } else { + // 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; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = posesOnPathWay[i].getX(); + pose.pose.position.y = posesOnPathWay[i].getY(); + pose.pose.position.z = 0; + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[i].getYaw()); + plan.push_back(pose); + } + } + } + + // ===== STEP 7: ADD FINAL GOAL POSE ===== + geometry_msgs::PoseStamped pose_goal; + pose_goal.header.stamp = plan_time; + pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose_goal.pose = goal.pose; + plan.push_back(pose_goal); + + // ===== STEP 8: FINALIZE AND PUBLISH PLAN ===== + // Set sequence numbers + for (size_t i = 0; i < plan.size(); ++i) { + plan[i].header.seq = i; + } + + // Publish plan for visualization + nav_msgs::Path gui_path; + gui_path.header.frame_id = costmap_robot_->getGlobalFrameID(); + gui_path.header.stamp = plan_time; + gui_path.poses = plan; + // plan_pub_.publish(gui_path); + + printf("[custom_planner] Generated plan with %zu waypoints", plan.size()); + return true; + } + + void CustomPlanner::transformFootprintToEdges(const 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); + for (unsigned int i = 0; i < footprint.size(); i++) + { + out_footprint[2 * i].x = robot_pose.position.x + cos(yaw) * footprint[i].x - sin(yaw) * footprint[i].y; + out_footprint[2 * i].y = robot_pose.position.y + sin(yaw) * footprint[i].x + cos(yaw) * footprint[i].y; + if (i == 0) + { + out_footprint.back().x = out_footprint[i].x; + out_footprint.back().y = out_footprint[i].y; + } + else + { + out_footprint[2 * i - 1].x = out_footprint[2 * i].x; + out_footprint[2 * i - 1].y = out_footprint[2 * i].y; + } + } + } + + bool CustomPlanner::makePlanWithOrder(robot_protocol_msgs::Order msg, + const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal) + { + vector savePosesOnEdge; + int count_two_curve_idx = 0; + + //Xóa mảng nếu còn chứa phần tử không xác định + orderNodes.clear(); + posesOnPathWay.clear(); + edges_info_.clear(); + RobotDirectionChangeAngle_info_.clear(); + + std::string all_edges, all_nodes; + int total_edge = (int)msg.edges.size(); + int total_node = (int)msg.nodes.size(); + if(total_edge <= 0 && total_node <= 0) + { + double delta_x = start.pose.position.x - goal.pose.position.x; + double delta_y = start.pose.position.y - goal.pose.position.y; + double distance_start_to_goal = std::hypot(delta_x, delta_y); + if(distance_start_to_goal > 0.2) { + return false; + } else { + posesOnPathWay.push_back(Pose(start.pose.position.x, start.pose.position.y, data_convert::getYaw(start.pose.orientation))); + posesOnPathWay.push_back(Pose(goal.pose.position.x, goal.pose.position.y, data_convert::getYaw(goal.pose.orientation))); + return true; + } + } + + for(int i = 0; i < total_node; i++){ + all_nodes += "[" + std::to_string(i) + "] " + msg.nodes[i].nodeDescription.c_str(); + + if (i != total_node - 1) { + all_nodes += " | "; // phân cách giữa các edge + } + + orderNodes[msg.nodes[i].nodeId].nodeId = msg.nodes[i].nodeId; + orderNodes[msg.nodes[i].nodeId].sequenceId = msg.nodes[i].sequenceId; + orderNodes[msg.nodes[i].nodeId].position_x = (double)msg.nodes[i].nodePosition.x; + orderNodes[msg.nodes[i].nodeId].position_y = (double)msg.nodes[i].nodePosition.y; + orderNodes[msg.nodes[i].nodeId].theta = (double)msg.nodes[i].nodePosition.theta; + } + + for(int i = 0; i < total_edge; i++) + { + all_edges += "[" + std::to_string(i) + "] " + msg.edges[i].edgeDescription; + + if (i != total_edge - 1) { + all_edges += " | "; // phân cách giữa các edge + } + auto start_nodeId_it = orderNodes.find(msg.edges[i].startNodeId); + auto end_nodeId_it = orderNodes.find(msg.edges[i].endNodeId); + if(start_nodeId_it!=orderNodes.end()&&end_nodeId_it!=orderNodes.end()) + { + int degree = 0; + int order = 0; + + vector posesOnEdge; + vector> control_points; + + std::vector knot_vector; + std::vector weight_vector; + + control_points.reserve(msg.edges[i].trajectory.controlPoints.size()); + knot_vector.reserve(msg.edges[i].trajectory.knotVector.size()); + weight_vector.reserve(msg.edges[i].trajectory.controlPoints.size()); + + for(int j = 0;j<(int)msg.edges[i].trajectory.controlPoints.size();j++) + { + control_points.push_back(Eigen::Vector3d(msg.edges[i].trajectory.controlPoints[j].x, msg.edges[i].trajectory.controlPoints[j].y, 0)); + weight_vector.push_back(msg.edges[i].trajectory.controlPoints[j].weight); + } + for(int k = 0 ;k < (int)msg.edges[i].trajectory.knotVector.size();k++) + { + knot_vector.push_back(msg.edges[i].trajectory.knotVector[k]); + } + degree = (int)msg.edges[i].trajectory.degree; + + if(curveIsValid(degree, knot_vector, control_points)) + { + double t_intervel = 0.01; + order = degree + 1; + input_spline_inf->control_point.clear(); + input_spline_inf->knot_vector.clear(); + input_spline_inf->weight.clear(); + CurveDesign->ReadSplineInf(input_spline_inf, order, control_points, knot_vector); + CurveDesign->ReadSplineInf(input_spline_inf, weight_vector, false); + + //Tính độ dài của cạnh thứ i + double edge_length = merge_path_calc_.calculateNURBSLength(CurveDesign, input_spline_inf, t_intervel); + //Tính step của cạnh thứ i + if (!costmap_robot_ || !costmap_robot_->getCostmap()) { + return false; + } + double resolution = costmap_robot_->getCostmap()->getResolution(); + // double resolution = 0.05; // Default resolution, can be adjusted + double t_intervel_new = merge_path_calc_.calculateAdaptiveStep(edge_length, resolution); + + //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; + 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)); + } + if ((int)posesOnEdge.size() < 3) + { + printf("Edge has too few poses for angle calculation"); + continue; + } + + double angle_1 = calculateAngle(posesOnEdge.front().getX(),posesOnEdge.front().getY(), + posesOnEdge[(int)(posesOnEdge.size()/2)].getX(), + posesOnEdge[(int)(posesOnEdge.size()/2)].getY()); + + double angle_2 = calculateAngle(posesOnEdge[(int)(posesOnEdge.size()/2)].getX(), + posesOnEdge[(int)(posesOnEdge.size()/2)].getY(), + posesOnEdge.back().getX(),posesOnEdge.back().getY()); + + double delta_angle = fabs(fabs(angle_1) - fabs(angle_2)); + bool found_intersection = false; + if((int)edges_info_.size() > 0) + 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 ; + gp1.x = savePosesOnEdge[idx_segment_A].getX(); + gp1.y = savePosesOnEdge[idx_segment_A].getY(); + 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; + lp1.x = posesOnEdge[idx_segment_B].getX(); + lp1.y = posesOnEdge[idx_segment_B].getY(); + geometry_msgs::Point lp2; + lp2.x = posesOnEdge[idx_segment_B - 1].getX(); + lp2.y = posesOnEdge[idx_segment_B - 1].getY(); + + if (doIntersect(gp1, gp2, lp1, lp2)) + { + posesOnEdge.erase(posesOnEdge.begin(), posesOnEdge.begin() + idx_segment_B); + savePosesOnEdge.erase(savePosesOnEdge.begin() + idx_segment_A + 1, savePosesOnEdge.end()); + + auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2); + if (intersection_point) + { + 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)); + } + + int save_idx = -1; + for(int idx_edges_info = 0; idx_edges_info < (int)edges_info_.size(); idx_edges_info++) + { + if(idx_segment_A >= edges_info_[idx_edges_info].start_edge_idx && + idx_segment_A <= edges_info_[idx_edges_info].end_edge_idx) + { + save_idx = idx_edges_info; + break; + } + } + + if (save_idx != -1) + { + edges_info_.erase(edges_info_.begin() + save_idx + 1, edges_info_.end()); + if (!edges_info_.empty()) + edges_info_.back().end_edge_idx = idx_segment_A; + } + + found_intersection = true; + break; + } + } + if(found_intersection == true) break; + } + + savePosesOnEdge.insert(savePosesOnEdge.end(), posesOnEdge.begin(), posesOnEdge.end()); + + if (!posesOnEdge.empty()) + { + InfEdge edge_info; // tạo struct mới + + if (i == 0) + { + edge_info.start_edge_idx = 0; + edge_info.end_edge_idx = edge_info.start_edge_idx + (int)posesOnEdge.size() - 1; + + merge_path_calc_.edge_front_start_idx_ = edge_info.start_edge_idx; + merge_path_calc_.edge_front_end_idx_ = edge_info.end_edge_idx; + } + else + { + edge_info.start_edge_idx = edges_info_.back().end_edge_idx + 1; + edge_info.end_edge_idx = edge_info.start_edge_idx + (int)posesOnEdge.size() - 1; + + merge_path_calc_.edge_back_start_idx_ = edge_info.start_edge_idx; + merge_path_calc_.edge_back_end_idx_ = edge_info.end_edge_idx; + } + + // Gán isCurve + edge_info.isCurve = (delta_angle >= EPSILON); + + // Thêm vào vector + edges_info_.push_back(edge_info); + } + } + else{ + printf("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", msg.edges[i].edgeId.c_str(), + msg.edges[i].startNodeId.c_str(), msg.edges[i].endNodeId.c_str()); + return false; + } + + } + else{ + printf("Edge: %s not found startNodeId: %s or endNodeId: %s", msg.edges[i].edgeId.c_str(), + msg.edges[i].startNodeId.c_str(), msg.edges[i].endNodeId.c_str()); + return false; + } + } + printf("[custom_planner][makePlanWithOrder] All nodes: %s", all_nodes.c_str()); + printf("[custom_planner][makePlanWithOrder] All edges: %s", all_edges.c_str()); + // printf("[custom_planner][makePlanWithOrder] savePosesOnEdge: %d", (int)savePosesOnEdge.size()); + total_edge = (int)edges_info_.size(); + bool status_countRobotDirectionChangeAngle = countRobotDirectionChangeAngle(savePosesOnEdge, total_edge); + bool status_calcAllYaw = false; + // printf("[custom_planner][makePlanWithOrder] DEBUG 1000 total_edge: %d", total_edge); + if(status_countRobotDirectionChangeAngle) status_calcAllYaw = calcAllYaw(start, goal, savePosesOnEdge, total_edge); + // printf("[custom_planner][makePlanWithOrder] DEBUG 2000"); + 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) + { + auto orientation = [](const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point& c) + { + double val = (b.y - a.y) * (c.x - b.x) - + (b.x - a.x) * (c.y - b.y); + if (std::abs(val) < 1e-6) return 0; // colinear + return (val > 0) ? 1 : 2; // clockwise or counterclockwise + }; + + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + + 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) + { + double x1 = p1.x, y1 = p1.y; + double x2 = q1.x, y2 = q1.y; + double x3 = p2.x, y3 = p2.y; + double x4 = q2.x, y4 = q2.y; + + double denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); + + if (std::abs(denom) < 1e-8) + return std::nullopt; // Đường thẳng song song hoặc trùng nhau + + double pre = (x1 * y2 - y1 * x2); + double post = (x3 * y4 - y3 * x4); + + double x = (pre * (x3 - x4) - (x1 - x2) * post) / denom; + double y = (pre * (y3 - y4) - (y1 - y2) * post) / denom; + + // Kiểm tra xem điểm giao nằm trên cả hai đoạn không + auto between = [](double a, double b, double c) { + return std::min(a, b) - 1e-6 <= c && c <= std::max(a, b) + 1e-6; + }; + + if (between(x1, x2, x) && between(y1, y2, y) && + between(x3, x4, x) && between(y3, y4, y)) + { + geometry_msgs::Point pt; + pt.x = x; + pt.y = y; + pt.z = 0.0; + return pt; + } + + return std::nullopt; // Giao điểm nằm ngoài đoạn + } + + bool CustomPlanner::countRobotDirectionChangeAngle(vector& savePosesOnEdge, + int& total_edge) + { + if (savePosesOnEdge.empty() || total_edge == 0) + return false; + + skipEdge_flag_ = false; + + // Kiểm tra 2 cạnh đầu không phải curve + if (edges_info_.size() > 1 && !edges_info_[0].isCurve && !edges_info_[1].isCurve) + { + int idx0 = edges_info_[0].start_edge_idx; + int idx1 = edges_info_[1].start_edge_idx; + int idx2 = edges_info_[1].end_edge_idx; + if (idx0 >= 0 && idx1 >= 0 && idx2 < (int)savePosesOnEdge.size()) + { + double angle = merge_path_calc_.signedAngle(savePosesOnEdge[idx0], savePosesOnEdge[idx1], savePosesOnEdge[idx2]); + skipEdge_flag_ = (fabs(angle) < 0.15 * M_PI); + } + } + + for (int i = 0; i < total_edge; i++) + { + if (!edges_info_[i].isCurve) continue; + + // Tính các chỉ số cần thiết + int prev_idx = i - 1; + int next_idx = i + 1; + + // Tính connecting_angle_start nếu có cạnh trước + double connecting_angle_start = M_PI; + if (prev_idx >= 0) + { + int prev_end = edges_info_[prev_idx].end_edge_idx; + int curr_start = edges_info_[i].start_edge_idx; + connecting_angle_start = merge_path_calc_.signedAngle(savePosesOnEdge[prev_end - 2], + savePosesOnEdge[curr_start], + savePosesOnEdge[curr_start + 2]); + } + + // Tính connecting_angle_end nếu có cạnh sau + double connecting_angle_end = M_PI; + if (next_idx < total_edge) + { + int curr_end = edges_info_[i].end_edge_idx; + int next_start = edges_info_[next_idx].start_edge_idx; + connecting_angle_end = merge_path_calc_.signedAngle(savePosesOnEdge[curr_end - 2], + savePosesOnEdge[next_start], + savePosesOnEdge[next_start + 2]); + } + + // Hàm phụ để tính cost + auto calc_cost = [&](double angle) -> int + { + if (fabs(angle) <= (0.1 * M_PI)) return 20; + if (fabs(angle) < (0.45 * M_PI)) return 10; + return 0; + }; + + int cost_start = calc_cost(connecting_angle_start); + int cost_end = calc_cost(connecting_angle_end); + + // Gom phần kiểm tra cost_start và cost_end thành 1 block + auto process_cost = [&](int cost, int idx_first, int idx_second, int idx_first_end, int idx_second_end) + { + if (cost > 0) + { + // Tính toán các góc kiểm tra, lưu biến tạm nếu dùng nhiều lần + Pose pose_centre = pointOnAngleBisector(savePosesOnEdge[idx_first_end - 2], + savePosesOnEdge[idx_second], + savePosesOnEdge[idx_second + 2], 0.1); + double angle_check_1 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_first_end - 1]); + double angle_check_2 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_first_end - 3]); + double angle_check_3 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_second + 1]); + double angle_check_4 = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_second + 3]); + double delta_angle_1 = fabs(angle_check_2) - fabs(angle_check_1); + double delta_angle_2 = fabs(angle_check_4) - fabs(angle_check_3); + + if (delta_angle_1 > EPSILON || delta_angle_2 > EPSILON) cost += 20; + else + { + // Kiểm tra dấu + double angle_check_2b = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_first]); + double angle_check_4b = merge_path_calc_.signedAngle(pose_centre, savePosesOnEdge[idx_second], savePosesOnEdge[idx_second_end]); + if (isOppositeSign(angle_check_1, angle_check_2b) || isOppositeSign(angle_check_3, angle_check_4b)) cost += 10; + } + } + return cost; + }; + + // Xử lý cost_start + if (prev_idx >= 0) + { + cost_start = process_cost(cost_start, edges_info_[prev_idx].start_edge_idx, edges_info_[i].start_edge_idx, + edges_info_[prev_idx].end_edge_idx, edges_info_[i].end_edge_idx); + } + // Xử lý cost_end + if (next_idx < total_edge) + { + cost_end = process_cost(cost_end, edges_info_[i].start_edge_idx, edges_info_[next_idx].start_edge_idx, + edges_info_[i].end_edge_idx, edges_info_[next_idx].end_edge_idx); + } + + // Nếu cost đủ lớn thì lưu lại cặp cạnh cong + if (cost_start >= 30 && prev_idx >= 0) + { + RobotDirectionChangeAngle_info_.emplace_back( + RobotDirectionChangeAngle{edges_info_[prev_idx].start_edge_idx, + edges_info_[prev_idx].end_edge_idx, + edges_info_[i].start_edge_idx, + edges_info_[i].end_edge_idx}); + // count_two_curve_idx++; + } + if (cost_end >= 30 && next_idx < total_edge) + { + RobotDirectionChangeAngle_info_.emplace_back( + RobotDirectionChangeAngle{edges_info_[i].start_edge_idx, + edges_info_[i].end_edge_idx, + edges_info_[next_idx].start_edge_idx, + edges_info_[next_idx].end_edge_idx}); + // count_two_curve_idx++; + } + // Nếu đã lưu 1 cặp thì bỏ qua cạnh tiếp theo + if (cost_start >= 30 || cost_end >= 30) i++; + // else return false; + } + // printf("[custom_planner][countRobotDirectionChangeAngle] DEBUG: 400 count_two_curve_idx(%d)", count_two_curve_idx); + return true; + } + + bool CustomPlanner::calcAllYaw(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + vector& savePosesOnEdge, + int& total_edge) + { + int total_two_curve = (int)RobotDirectionChangeAngle_info_.size(); + // printf("[custom_planner][calcAllYaw] total_two_curve: %d", total_two_curve); + vector savePoseTMP; + if(!savePoseTMP.empty()) savePoseTMP.clear(); + + if(savePosesOnEdge.empty()) + { + printf("[custom_planner][calcAllYaw] savePosesOnEdge is empty!\n"); + return false; + } + + if(!(int)posesOnPathWay.empty()) posesOnPathWay.clear(); + + merge_path_calc_.status_yaw_edge_end_ = checkYawEdgeEnd(savePosesOnEdge[(int)savePosesOnEdge.size()-2], + savePosesOnEdge[(int)savePosesOnEdge.size()-1], goal); + if(!rotating_robot_plag_) + { + if(total_two_curve > 0) + { + if(total_two_curve == 1) + { + vector Pose_start_tmp; + vector Pose_goal_tmp; + vector::iterator start_it_start = savePosesOnEdge.begin(); + vector::iterator start_it_end = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.front().end_edge_idx_first + 1; + vector::iterator goal_it_start = savePosesOnEdge.begin() + RobotDirectionChangeAngle_info_.back().start_edge_idx_second; + vector::iterator goal_it_end = savePosesOnEdge.end(); + + Pose_start_tmp.insert(Pose_start_tmp.end(), start_it_start, start_it_end); + Pose_goal_tmp.insert(Pose_goal_tmp.end(), goal_it_start, goal_it_end); + + setYawAllPosesOnEdge(Pose_start_tmp,false); + setYawAllPosesOnEdge(Pose_goal_tmp,true); + + savePoseTMP.insert(savePoseTMP.begin(), Pose_start_tmp.begin(),Pose_start_tmp.end()); + savePoseTMP.insert(savePoseTMP.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end()); + reverse_ = false; + } + + else { + // Khởi tạo phần đầu path + vector Pose_start_tmp; + if (!savePosesOnEdge.empty() && !RobotDirectionChangeAngle_info_.empty()) + { + vector::iterator start_it_start = savePosesOnEdge.begin(); + vector::iterator start_it_end = savePosesOnEdge.begin() + + RobotDirectionChangeAngle_info_[0].end_edge_idx_first + 1; + + if (start_it_end <= savePosesOnEdge.end()) + { + Pose_start_tmp.insert(Pose_start_tmp.end(), start_it_start, start_it_end); + setYawAllPosesOnEdge(Pose_start_tmp, false); + savePoseTMP.insert(savePoseTMP.end(), Pose_start_tmp.begin(), Pose_start_tmp.end()); + } + } + + // Xử lý các đoạn giữa path + int flip = -1; + for(int i = 0; i < total_two_curve - 1; i++) + { + vector Pose_mid_tmp; + vector::iterator it_start = savePosesOnEdge.begin() + + RobotDirectionChangeAngle_info_[i].start_edge_idx_second; + vector::iterator it_end = savePosesOnEdge.begin() + + RobotDirectionChangeAngle_info_[i+1].end_edge_idx_first + 1; + + if (it_start < savePosesOnEdge.end() && it_end <= savePosesOnEdge.end()) + { + Pose_mid_tmp.insert(Pose_mid_tmp.end(), it_start, it_end); + flip *= -1; + setYawAllPosesOnEdge(Pose_mid_tmp, flip > 0); + + if (!Pose_mid_tmp.empty()) + { + savePoseTMP.insert(savePoseTMP.end(), + Pose_mid_tmp.begin(), + Pose_mid_tmp.end()); + } + } + } + vector Pose_goal_tmp; + if (!savePosesOnEdge.empty() && !RobotDirectionChangeAngle_info_.empty()) + { + vector::iterator goal_it_start = savePosesOnEdge.begin() + + RobotDirectionChangeAngle_info_.back().start_edge_idx_second; + vector::iterator goal_it_end = savePosesOnEdge.end(); + + if (goal_it_start < savePosesOnEdge.end()) + { + Pose_goal_tmp.insert(Pose_goal_tmp.end(), goal_it_start, goal_it_end); + flip *= -1; + setYawAllPosesOnEdge(Pose_goal_tmp, flip > 0); + + if(flip > 0) reverse_ = true; + else reverse_ = false; + if (!Pose_goal_tmp.empty()) + { + savePoseTMP.insert(savePoseTMP.end(), + Pose_goal_tmp.begin(), + Pose_goal_tmp.end()); + } + } + } + } + // posesOnPathWay.insert(posesOnPathWay.end(), Pose_goal_tmp.begin(),Pose_goal_tmp.end()); + } + else + { + if(merge_path_calc_.status_yaw_edge_end_) + { + reverse_ = true; + // printf("custom_planner][calcAllYaw] DEBUG 300"); + setYawAllPosesOnEdge(savePosesOnEdge,true); + } + else + { + reverse_ = false; + // printf("custom_planner][calcAllYaw] DEBUG 310"); + setYawAllPosesOnEdge(savePosesOnEdge,false); + } + savePoseTMP.insert(savePoseTMP.end(), savePosesOnEdge.begin(),savePosesOnEdge.end()); + } + vector> MatrixPose; + MatrixPose.resize(total_edge); + // Duyệt ngược để insert không làm lệch chỉ số các cạnh phía trước + for(int i = total_edge - 1; i >= 1; i--) + { + int idx_1 = edges_info_[i-1].end_edge_idx - 2; + int idx_2 = edges_info_[i].start_edge_idx; + int idx_3 = edges_info_[i].start_edge_idx + 2; + + // Kiểm tra indices hợp lệ + if (idx_1 < 0 || idx_1 >= static_cast(savePoseTMP.size()) || + idx_2 < 0 || idx_2 >= static_cast(savePoseTMP.size()) || + idx_3 < 0 || idx_3 >= static_cast(savePoseTMP.size())) + { + printf("Invalid indices: %d, %d, %d", idx_1, idx_2, idx_3); + continue; + } + + double delta_Angle = merge_path_calc_.signedAngle(savePoseTMP[idx_1], + savePoseTMP[idx_2], + savePoseTMP[idx_3]); + // Cần tính lại các góc quay + if(fabs(delta_Angle) > (0.45 * M_PI) && fabs(delta_Angle) < (0.85 * M_PI)) + { + MatrixPose[i].clear(); + + int max_step = (int)((M_PI - fabs(delta_Angle)) / (0.05 * M_PI)); + for(int j = 0; j <= max_step; j++) + { + Pose intermediate_pose; + intermediate_pose.setX(savePoseTMP[edges_info_[i].start_edge_idx].getX()); + intermediate_pose.setY(savePoseTMP[edges_info_[i].start_edge_idx].getY()); + if(delta_Angle < 0) + { + intermediate_pose.setYaw(savePoseTMP[edges_info_[i-1].end_edge_idx].getYaw() + j * (0.05 * M_PI)); + } + else + { + intermediate_pose.setYaw(savePoseTMP[edges_info_[i-1].end_edge_idx].getYaw() - j * (0.05 * M_PI)); + } + // intermediate_pose.getX(), intermediate_pose.getY(), intermediate_pose.getYaw()); + MatrixPose[i].push_back(intermediate_pose); + } + MatrixPose[i].shrink_to_fit(); + int insert_pos = edges_info_[i].start_edge_idx; + + savePoseTMP.insert(savePoseTMP.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end()); + } + } + posesOnPathWay.insert(posesOnPathWay.end(), + savePoseTMP.begin(), + savePoseTMP.end()); + } + else // rotating_robot_plag_ is true + { + if(merge_path_calc_.status_yaw_edge_end_) + { + reverse_ = false; + setYawAllPosesOnEdge(savePosesOnEdge, true); + } + else{ + reverse_ = true; + setYawAllPosesOnEdge(savePosesOnEdge, false); + } + + vector> MatrixPose; + MatrixPose.resize(total_edge); + // Duyệt ngược để insert không làm lệch chỉ số các cạnh phía trước + for(int i = total_edge - 1; i >= 1; i--) + { + int idx_1 = edges_info_[i-1].end_edge_idx - 2; + int idx_2 = edges_info_[i].start_edge_idx; + int idx_3 = edges_info_[i].start_edge_idx + 2; + double delta_Angle = merge_path_calc_.signedAngle(savePosesOnEdge[idx_1], + savePosesOnEdge[idx_2], + savePosesOnEdge[idx_3]); + delta_Angle = delta_Angle - M_PI; + + if(fabs(delta_Angle) > (0.05 * M_PI)) + { + MatrixPose[i].clear(); + int max_step = (int)((M_PI - fabs(delta_Angle)) / (0.05 * M_PI)); + for(int j = 0; j <= max_step; j++) { + Pose intermediate_pose; + intermediate_pose.setX(savePosesOnEdge[edges_info_[i].start_edge_idx].getX()); + intermediate_pose.setY(savePosesOnEdge[edges_info_[i].start_edge_idx].getY()); + if(delta_Angle > 0) { + intermediate_pose.setYaw(savePosesOnEdge[edges_info_[i-1].end_edge_idx].getYaw() + j * (0.05 * M_PI)); + } else { + intermediate_pose.setYaw(savePosesOnEdge[edges_info_[i-1].end_edge_idx].getYaw() - j * (0.05 * M_PI)); + } + MatrixPose[i].push_back(intermediate_pose); + } + int insert_pos = edges_info_[i].start_edge_idx; + savePosesOnEdge.insert(savePosesOnEdge.begin() + insert_pos, MatrixPose[i].begin(), MatrixPose[i].end()); + } + MatrixPose[i].shrink_to_fit(); + } + posesOnPathWay.insert(posesOnPathWay.end(), + savePosesOnEdge.begin(), + savePosesOnEdge.end()); + } + + if (skipEdge_flag_) + { + int nearest_idx = -1; + double min_dist = std::numeric_limits::max(); + int start_idx = edges_info_[0].end_edge_idx; + int end_idx = std::min(edges_info_[1].end_edge_idx, (int)posesOnPathWay.size()); + + for (int i = start_idx; i < end_idx; i++) + { + double dx = start.pose.position.x - posesOnPathWay[i].getX(); + double dy = start.pose.position.y - posesOnPathWay[i].getY(); + double dist = std::hypot(dx, dy); + + if (dist < min_dist) + { + min_dist = dist; + nearest_idx = i; + } + } + + if (nearest_idx > 0 && nearest_idx < (int)posesOnPathWay.size()) + { + posesOnPathWay.erase(posesOnPathWay.begin(), posesOnPathWay.begin() + nearest_idx); + merge_path_calc_.edge_front_start_idx_ = 0; + merge_path_calc_.edge_front_end_idx_ = end_idx - nearest_idx; + + int total = edges_info_.back().start_edge_idx - edges_info_.back().end_edge_idx; + merge_path_calc_.edge_back_end_idx_ = (int)posesOnPathWay.size() - 1; + merge_path_calc_.edge_back_start_idx_ = merge_path_calc_.edge_back_end_idx_ - total; + // posesOnPathWay.shrink_to_fit(); + // printf("[custom_planner][makePlanWithOrder] DEBUG: 2 posesOnPathWay size(%d)", (int)posesOnPathWay.size()); + } + } + return true; + } + + Pose CustomPlanner::pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length) + { + // Vector BA + double ux = A.getX() - B.getX(); + double uy = A.getY() - B.getY(); + double ul = std::hypot(ux, uy); + ux /= ul; + uy /= ul; + + // Vector BC + double vx = C.getX() - B.getX(); + double vy = C.getY() - B.getY(); + double vl = std::hypot(vx, vy); + vx /= vl; + vy /= vl; + + // Vector bisector + double wx = ux + vx; + double wy = uy + vy; + double wl = std::hypot(wx, wy); + + // Chuẩn hóa và nhân độ dài + Pose P; + P.setX(B.getX() + length * wx / wl); + P.setY(B.getY() + length * wy / wl); + return P; +} + + bool CustomPlanner::isThetaValid(double theta) + { + bool result = false; + if(theta < -M_PI || theta > M_PI) result = false; + else result = true; + return result; + } + + bool CustomPlanner::curveIsValid(int degree, const std::vector &knot_vector, + vector>& control_points) + { + if(degree < 1 || degree > 9) + { + printf("degree is invalid value\n"); + return false; + } + if(!((knot_vector.size() - degree - 1) == control_points.size())) + { + printf("relation between degree, number of knots, and number of control points is invalid\n"); + return false; + } + // if(std::is_sorted(knot_vector.begin(), knot_vector.end())) + // { + // printf("knot vector is not monotonic"); + // return false; + // } + return true; + } + + void CustomPlanner::setYawAllPosesOnEdge(vector& posesOnEdge, bool reverse) + { + if(!reverse) + { + if(!posesOnEdge.empty()){ + if(posesOnEdge.size()>2){ + for(int i = 0; i<((int)posesOnEdge.size()-1); i++) + { + double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(), + posesOnEdge[i+1].getX(), posesOnEdge[i+1].getY()); + posesOnEdge[i].setYaw(theta); + } + posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size()-2].getYaw()); + } + else if(posesOnEdge.size()==2) + { + if(posesOnEdge[0].getX()!=posesOnEdge[1].getX()) + { + double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(), + posesOnEdge[1].getX(), posesOnEdge[1].getY()); + posesOnEdge[0].setYaw(theta); + posesOnEdge[1].setYaw(theta); + } + } + } + } + else + { + if(!posesOnEdge.empty()){ + if(posesOnEdge.size()>2){ + for(int i = (int)posesOnEdge.size() -1; i>0; i--) + { + double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(), + posesOnEdge[i-1].getX(), posesOnEdge[i-1].getY()); + posesOnEdge[i].setYaw(theta); + } + posesOnEdge.front().setYaw(posesOnEdge[1].getYaw()); + } + else if(posesOnEdge.size()==2) + { + if(posesOnEdge[1].getX()!=posesOnEdge[0].getX()) + { + double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(), + posesOnEdge[0].getX(), posesOnEdge[0].getY()); + posesOnEdge[1].setYaw(theta); + posesOnEdge[0].setYaw(theta); + } + } + } + } + } + + bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const geometry_msgs::PoseStamped& goal) + { + double angle = calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY()); + + double yaw_goal = data_convert::getYaw(goal.pose.orientation); + // printf("[custom_planner] DEBUG: angle: %f; yaw_goal: %f",angle, yaw_goal); + double delta_angle = fabs((merge_path_calc_.normalizeAngle(yaw_goal)) - (merge_path_calc_.normalizeAngle(angle))); + return (delta_angle > (0.5*M_PI)); + } + +// Export factory function +static boost::shared_ptr custom_planner_plugin() { + return boost::make_shared(); +} + +// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) +BOOST_DLL_ALIAS(custom_planner_plugin, custom_planner) + +} diff --git a/src/merge_path_calc.cpp b/src/merge_path_calc.cpp new file mode 100755 index 0000000..ffcecfa --- /dev/null +++ b/src/merge_path_calc.cpp @@ -0,0 +1,673 @@ +#include"custom_planner/merge_path_calc.h" + +namespace custom_planner { + + MergePathCalc::MergePathCalc(){ + curve_design = new Curve_common(); + spline_inf = new Spline_Inf(); + } + MergePathCalc::~MergePathCalc(){ + // Giải phóng bộ nhớ + delete curve_design; + delete spline_inf; + } + + 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 + + if((int)nurbs_path.size() > 0) + { + nurbs_path.clear(); // Xóa đường cong cũ nếu có + saved_poses.clear(); // Xóa các pose đã lưu + } + + // Chuyển đổi control points sang định dạng Eigen::Vector3d + std::vector> eigen_control_points; + for(const auto& cp : control_points) + { + Eigen::Vector3d point(cp.pose.position.x, cp.pose.position.y, 0); + eigen_control_points.push_back(point); + } + // Tạo knot vector tự động + std::vector knot_vector; + int degree = 2; + int n = control_points.size() - 1; // n + 1 control points + int m = n + degree + 1; // m + 1 knots + // Tạo uniform knot vector + for(int i = 0; i <= m; i++) + { + if(i <= degree) knot_vector.push_back(0.0); + else knot_vector.push_back(1.0); + // else knot_vector.push_back((double)(i - degree)/(m - 2*degree)); + } + + // Điều chỉnh weights để tăng ảnh hưởng của các control points + std::vector weights(control_points.size(), 1.0); + + spline_inf->knot_vector.clear(); + spline_inf->control_point.clear(); + spline_inf->weight.clear(); + + // Cài đặt thông số cho spline + curve_design->ReadSplineInf(spline_inf, degree + 1, eigen_control_points, knot_vector); + curve_design->ReadSplineInf(spline_inf, weights, false); + + // Tính độ dài đường cong để xác định số điểm cần lấy mẫu + double NURBS_length = calculateNURBSLength(curve_design, spline_inf, 0.01); + double desired_spacing = 0.05; // Khoảng cách mong muốn giữa các điểm + double step = calculateAdaptiveStep(NURBS_length, desired_spacing); + + for(double t = 0.0; t <= 1.0; t += step) + { + geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true); + if(!std::isnan(point.x) && !std::isnan(point.y)) + { + geometry_msgs::PoseStamped pose; + pose.header.stamp = robot::Time::now(); + pose.pose.position = point; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // nurbs_path.push_back(pose); + saved_poses.push_back(pose); + if(saved_poses.size() > 1) + { + updatePoseOrientation(saved_poses, nurbs_path, reverse); + } + } + } + if(!saved_poses.empty()) + { + double end_yaw; + if(!reverse) + end_yaw = calculateAngle(saved_poses.back().pose.position.x, + saved_poses.back().pose.position.y, + control_points.back().pose.position.x, + control_points.back().pose.position.y); + else + end_yaw = calculateAngle(control_points.back().pose.position.x, + control_points.back().pose.position.y, + saved_poses.back().pose.position.x, + saved_poses.back().pose.position.y); + end_yaw = normalizeAngle(end_yaw); + + tf3::Quaternion q; + q.setRPY(0, 0, end_yaw); + saved_poses.back().pose.orientation.x = q.x(); + saved_poses.back().pose.orientation.y = q.y(); + saved_poses.back().pose.orientation.z = q.z(); + saved_poses.back().pose.orientation.w = q.w(); + + nurbs_path.push_back(saved_poses.back()); + } + return nurbs_path; + } + + double MergePathCalc::calculateNURBSLength(Curve_common* curve_design, + Spline_Inf* spline_inf, + double initial_step) + { + double length = 0.0; + geometry_msgs::Point prev_point, curr_point; + std::vector segment_lengths; + + static double step; + // Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step + step = initial_step; + + // Tính độ dài với step + for(double u = 0; u <= 1; u += step) + { + // Lấy điểm trên đường cong tại tham số u + curr_point = curve_design->CalculateCurvePoint(spline_inf, u, true); + + if(u > 0 && !std::isnan(curr_point.x) && !std::isnan(curr_point.y)) + { + double segment_length = std::sqrt(std::pow(curr_point.x - prev_point.x, 2) + + std::pow(curr_point.y - prev_point.y, 2)); + length += segment_length; + segment_lengths.push_back(segment_length); + } + prev_point = curr_point; + } + return length; + } + + void MergePathCalc::updatePoseOrientation(std::vector& saved_poses, + std::vector& nurbs_path, + bool reverse) + { + double yaw; + // ROS_WARN("[merge_path_calc] DEBUG: reverse(%x)", reverse); + if(!reverse) + yaw = calculateAngle(saved_poses[saved_poses.size() - 2].pose.position.x, + saved_poses[saved_poses.size() - 2].pose.position.y, + saved_poses.back().pose.position.x, + saved_poses.back().pose.position.y); + else + yaw = calculateAngle(saved_poses.back().pose.position.x, + saved_poses.back().pose.position.y, + saved_poses[saved_poses.size() - 2].pose.position.x, + saved_poses[saved_poses.size() - 2].pose.position.y); + + yaw = normalizeAngle(yaw); + tf3::Quaternion q; + q.setRPY(0, 0, yaw); + saved_poses[saved_poses.size() - 2].pose.orientation.x = q.x(); + saved_poses[saved_poses.size() - 2].pose.orientation.y = q.y(); + saved_poses[saved_poses.size() - 2].pose.orientation.z = q.z(); + saved_poses[saved_poses.size() - 2].pose.orientation.w = q.w(); + + nurbs_path.push_back(saved_poses[saved_poses.size() - 2]); + } + + int MergePathCalc::findNearestPointOnPath(Pose& pose, + std::vector& posesOnPathWay, + point_type type) + { + // Input validation - FIX: Thêm validation cơ bản + if (posesOnPathWay.empty()) { + return -1; + } + + // Các hằng số chung cho cả START và GOAL + static const double MAX_DISTANCE = 1.0; // Ngưỡng khoảng cách tối đa (m) + static const double MIN_DISTANCE = 0.1; // Khoảng cách tối thiểu (m) + static const double MIN_SCALE = 0.2; // Giá trị tối thiểu của hệ số + static const double MAX_SCALE = 1.0; // Giá trị tối đa của hệ số + static const double MAX_ANGLE_THRESHOLD = 0.77 * M_PI; // Ngưỡng góc tối đa + + int nearest_idx = 0; + + if(type == START) + { + // FIX: Validate edge_front_end_idx_ trước khi sử dụng + if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast(posesOnPathWay.size())) { + return -1; + } + + // === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN === + double min_dist = std::numeric_limits::max(); + double distance; + + for (int i = 0; i < edge_front_end_idx_; ++i) + { + distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(), + posesOnPathWay[i].getY() - pose.getY()); + + if (distance < min_dist) + { + min_dist = distance; + nearest_idx = i; + } + } + + // Lưu chỉ mục của điểm gần nhất + store_start_nearest_idx_ = nearest_idx; + + // === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG === + // Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng + double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * + (MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) / + (MAX_DISTANCE - MIN_DISTANCE); + + // === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC === + double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI; + if(angle_threshold >= MAX_ANGLE_THRESHOLD) + angle_threshold = MAX_ANGLE_THRESHOLD; + + // === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC === + for(int i = store_start_nearest_idx_; i <= edge_front_end_idx_; i++) + { + // Bounds checking đầy đủ trước khi truy cập + if (i + 1 < static_cast(posesOnPathWay.size()) && i + 1 <= edge_front_end_idx_) + { + double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]); + + if(std::abs(angle) >= angle_threshold) + { + nearest_idx = i; + break; + } + } + + + if(i >= edge_front_end_idx_ - 1) + { + // Đã đến cuối mà không tìm thấy điểm phù hợp + return -1; // Không tìm thấy điểm phù hợp + } + } + } + else if(type == GOAL) + { + // FIX: Validate edge_back_start_idx_ trước khi sử dụng + if (edge_back_start_idx_ < 0 || edge_back_start_idx_ >= static_cast(posesOnPathWay.size())) { + return -1; + } + + // === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN (DUYỆT NGƯỢC) === + double min_dist = std::numeric_limits::max(); + + for (int i = static_cast(posesOnPathWay.size()) - 1; i >= edge_back_start_idx_; --i) + { + double distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(), + posesOnPathWay[i].getY() - pose.getY()); + if (distance < min_dist) + { + min_dist = distance; + nearest_idx = i; + } + } + + // Lưu chỉ mục của điểm gần nhất + store_goal_nearest_idx_ = nearest_idx; + + // === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG (GIỐNG START) === + // Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng + double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * + (MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) / + (MAX_DISTANCE - MIN_DISTANCE); + + // === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC (GIỐNG START) === + double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI; + if(angle_threshold >= MAX_ANGLE_THRESHOLD) + angle_threshold = MAX_ANGLE_THRESHOLD; + + + // === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC (DUYỆT NGƯỢC) === + // Bắt đầu từ điểm đã tìm được, duyệt ngược để tìm điểm thỏa mãn góc + for(int i = nearest_idx; i >= edge_back_start_idx_; i--) + { + // FIX: Bounds checking đầy đủ trước khi truy cập + if (i - 1 >= edge_back_start_idx_ && i - 1 >= 0) + { + double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i - 1]); + + if(std::abs(angle) >= angle_threshold) + { + nearest_idx = i; + break; + } + } + + // FIX: Logic condition sửa lại để rõ ràng + if(i <= edge_back_start_idx_ + 1) + { + // Đã đến đầu mà không tìm thấy điểm phù hợp + return -1; + } + } + + } + // FIX: Thêm else case để handle invalid type + else { + return -1; + } + + return nearest_idx; + } + + bool MergePathCalc::findNURBSControlPoints(vector& control_points, + const geometry_msgs::PoseStamped& pose, + std::vector& posesOnPathWay, + point_type type) + { + // FIX: Input validation + if (posesOnPathWay.empty()) return false; + + // FIX: Chỉ clear một lần duy nhất thay vì clear nhiều lần + control_points.clear(); + + if(type == START) + { + Pose start_pose; + start_pose.setX(pose.pose.position.x); + start_pose.setY(pose.pose.position.y); + start_pose.setYaw(data_convert::getYaw(pose.pose.orientation)); + + int idx = findNearestPointOnPath(start_pose, posesOnPathWay, START); + + // Trường hợp tạo đường thẳng + if(idx == -1) + { + // FIX: Validate edge_front_end_idx_ trước khi sử dụng + if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast(posesOnPathWay.size())) { + return false; + } + + start_target_idx_ = edge_front_end_idx_ - 1; + + double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX(); + double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY(); + + // Thêm điểm đầu + 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; + 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; + // FIX: Copy header từ pose gốc + mid_pose.header = pose.header; + control_points.push_back(mid_pose); + + // Thêm điểm cuối + 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()); + // FIX: Copy header từ pose gốc + end_pose.header = pose.header; + control_points.push_back(end_pose); + + return true; + } + // Tìm đường cong + else + { + start_target_idx_ = idx; + + // Tạo các control point cho NURBS + control_points.push_back(pose); + + 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; + // FIX: Copy header từ pose gốc + mid_pose.header = pose.header; + control_points.push_back(mid_pose); + + + double dx_sm = start_pose.getX() - posesOnPathWay[start_target_idx_].getX(); + double dy_sm = start_pose.getY() - posesOnPathWay[start_target_idx_].getY(); + double dist_sm = std::hypot(dx_sm,dy_sm); + + // FIX: Bỏ biến end_idx_ không sử dụng + // end_idx_ = start_target_idx_; + + bool found_suitable_point = false; // FIX: Thêm flag để tránh goto + for(int i = start_target_idx_; i <= edge_front_end_idx_; i++) + { + // FIX: Validate bounds trước khi truy cập + if (i >= static_cast(posesOnPathWay.size())) { + break; + } + + double dx_me = posesOnPathWay[i].getX() - posesOnPathWay[start_target_idx_].getX(); + double dy_me = posesOnPathWay[i].getY() - posesOnPathWay[start_target_idx_].getY(); + double dist_me = std::hypot(dx_me,dy_me); + if(dist_me >= dist_sm) + { + start_target_idx_ = i; + found_suitable_point = true; + break; + } + if(i == edge_front_end_idx_) + { + // FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line + found_suitable_point = false; + break; + } + } + + // FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line + if (!found_suitable_point) { + // Clear và tạo straight line thay vì goto + control_points.clear(); + + // FIX: Validate lại edge_front_end_idx_ + if (edge_front_end_idx_ <= 0 || edge_front_end_idx_ > static_cast(posesOnPathWay.size())) { + return false; + } + + start_target_idx_ = edge_front_end_idx_ - 1; + + double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX(); + double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY(); + + control_points.push_back(pose); + + 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; + 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()); + end_pose_straight.header = pose.header; + control_points.push_back(end_pose_straight); + + return true; + } + + 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); + // FIX: Copy header từ pose gốc + end_pose.header = pose.header; + control_points.push_back(end_pose); + + return true; + } + } + else if(type == GOAL) + { + Pose goal_pose; + goal_pose.setX(pose.pose.position.x); + goal_pose.setY(pose.pose.position.y); + goal_pose.setYaw(data_convert::getYaw(pose.pose.orientation)); + + int idx = findNearestPointOnPath(goal_pose, posesOnPathWay, GOAL); + + if(idx == -1) + { + // Trường hợp tạo đường thẳng + // FIX: Validate size trước khi sử dụng + if (posesOnPathWay.empty()) { + return false; + } + + goal_target_idx_ = static_cast(posesOnPathWay.size()) - 1; + + // Thêm điểm đầu từ đường đã có + 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()); + // FIX: Copy header từ pose gốc + start_pose.header = pose.header; + control_points.push_back(start_pose); + + // Vector hướng từ goal tới start + double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX(); + 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; + 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; + // FIX: Copy header từ pose gốc + mid_pose.header = pose.header; + control_points.push_back(mid_pose); + + // Thêm điểm goal + control_points.push_back(pose); + return true; + } + else + { + // Trường hợp tạo đường cong + goal_target_idx_ = idx; + + double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX(); + double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY(); + double dist_me = std::hypot(dx, dy); + + bool found_suitable_point = false; // FIX: Thêm flag để tránh goto + for(int i = goal_target_idx_; i >= edge_back_start_idx_; i--) + { + // FIX: Validate bounds trước khi truy cập + if (i < 0 || i >= static_cast(posesOnPathWay.size())) { + break; + } + + double dx_sm = posesOnPathWay[i].getX() - posesOnPathWay[goal_target_idx_].getX(); + double dy_sm = posesOnPathWay[i].getY() - posesOnPathWay[goal_target_idx_].getY(); + double dist_sm = std::hypot(dx_sm,dy_sm); + if(dist_sm >= dist_me) + { + goal_target_idx_ = i; + found_suitable_point = true; + break; + } + if(i == edge_back_start_idx_) + { + // FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line + found_suitable_point = false; + break; + } + } + + // FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line + if (!found_suitable_point) { + // Clear và tạo straight line thay vì goto + control_points.clear(); + + // FIX: Validate lại size + if (posesOnPathWay.empty()) { + return false; + } + + goal_target_idx_ = static_cast(posesOnPathWay.size()) - 1; + + 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()); + start_pose_straight.header = pose.header; + control_points.push_back(start_pose_straight); + + 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; + 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; + mid_pose_straight.header = pose.header; + control_points.push_back(mid_pose_straight); + + control_points.push_back(pose); + return true; + } + + // Thêm điểm đầu từ đường đã có + 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()); + // FIX: Copy header từ pose gốc + start_pose.header = pose.header; + control_points.push_back(start_pose); + + 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; + // FIX: Copy header từ pose gốc + mid_pose.header = pose.header; + control_points.push_back(mid_pose); + + // Thêm điểm goal + control_points.push_back(pose); + return true; + } + } + // FIX: Thêm else case để handle invalid type + else { + return false; + } + + // FIX: Code không bao giờ reach được đây, nhưng để đảm bảo + // return false; // Không tìm thấy control points + } + + double MergePathCalc::signedAngle(Pose& pose, std::vector& posesOnPathWay, + point_type type, int& idx) + { + if(type == START) + { + // Vector AB + double bax = pose.getX() - posesOnPathWay[idx].getX(); + double bay = pose.getY() - posesOnPathWay[idx].getY(); + + // Vector BC + double bcx = posesOnPathWay[idx+1].getX() - posesOnPathWay[idx].getX(); + double bcy = posesOnPathWay[idx+1].getY() - posesOnPathWay[idx].getY(); + + // Tích vô hướng + double dot = bax * bcx + bay * bcy; + // Tích hướng 2D (z-component) + double cross = bax * bcy - bay * bcx; + + // Góc có hướng [-π, π] + double angle_rad = std::atan2(cross, dot); + return angle_rad; + } + else if(type == GOAL) + { + // Vector AB + double bax = posesOnPathWay[idx-1].getX() - posesOnPathWay[idx].getX(); + double bay = posesOnPathWay[idx-1].getY() - posesOnPathWay[idx].getY(); + + // Vector BC + double bcx = pose.getX() - posesOnPathWay[idx].getX(); + double bcy = pose.getY() - posesOnPathWay[idx].getY(); + + // Tích vô hướng + double dot = bax * bcx + bay * bcy; + // Tích hướng 2D (z-component) + double cross = bax * bcy - bay * bcx; + + // Góc có hướng [-π, π] + double angle_rad = std::atan2(cross, dot); + return angle_rad; + } + else return -1; // Trả về -1 nếu không phải START hoặc GOAL + + } + + double MergePathCalc::signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose) + { + // Vector AB + double bax = mid_pose.getX() - start_pose.getX(); + double bay = mid_pose.getY() - start_pose.getY(); + + // Vector BC + double bcx = mid_pose.getX() - end_pose.getX(); + double bcy = mid_pose.getY() - end_pose.getY(); + + // Tích vô hướng + double dot = bax * bcx + bay * bcy; + // Tích hướng 2D (z-component) + double cross = bax * bcy - bay * bcx; + + // Góc có hướng [-π, π] + double angle_rad = std::atan2(cross, dot); + return angle_rad; + } + +} // namespace custom_planner \ No newline at end of file diff --git a/src/pathway.cc b/src/pathway.cc new file mode 100755 index 0000000..4bf0185 --- /dev/null +++ b/src/pathway.cc @@ -0,0 +1,291 @@ +#include "custom_planner/pathway.h" + +using namespace custom_planner; + +bool Pathway::onSegment(Pose p, Pose q, Pose r) +{ + if (q.getX() <= max(p.getX(), r.getX()) && q.getX() >= min(p.getX(), r.getX()) && + q.getY() <= max(p.getY(), r.getY()) && q.getY() >= min(p.getY(), r.getY())) + return true; + + return false; +} + +int Pathway::orientation(Pose p, Pose q, Pose r) +{ + int val = (q.getY() - p.getY()) * (r.getX() - q.getX()) - + (q.getX() - p.getX()) * (r.getY() - q.getY()); + + if (val == 0) return 0; // collinear + + return (val > 0)? 1: 2; // clock or counterclock wise +} + +bool Pathway::doIntersect(Pose p1, Pose q1, Pose p2, Pose q2) +{ + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + + // General case + if (o1 != o2 && o3 != o4) + return true; + + // Special Cases + // p1, q1 and p2 are collinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + + // p1, q1 and q2 are collinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + + // p2, q2 and p1 are collinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + + // p2, q2 and q1 are collinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + + return false; // Doesn't fall in any of the above cases +} + +bool Pathway::isIntersect(Edge L) { + for (auto& segment : path_) { + if (doIntersect(L.getP1(), L.getP2(), segment.getP1(), segment.getP2())) { + return true; + } + } + return false; +} +bool Pathway::isPoseExisted(Pose p) +{ + for(auto& pose : posesOnPath_) + { + if(p.getX()==pose.getX() && p.getY()==pose.getY()) return true; + } + return false; +} +double Pathway::calculateAngle(Pose p1, Pose p2, Pose p3) +{ + // Tính vector 1 + double vector1x = p2.getX() - p1.getX(); + double vector1y = p2.getY() - p1.getY(); + + // Tính vector 2 + double vector2x = p3.getX() - p2.getX(); + double vector2y = p3.getY() - p2.getY(); + + // Tính độ dài của hai vector + double lengthVector1 = sqrt(vector1x * vector1x + vector1y * vector1y); + double lengthVector2 = sqrt(vector2x * vector2x + vector2y * vector2y); + + // Tính góc giữa hai vector + double dotProduct = vector1x * vector2x + vector1y * vector2y; + double cosTheta = dotProduct / (lengthVector1 * lengthVector2); + if(cosTheta>1) cosTheta = 1; + else if(cosTheta<(-1)) cosTheta = -1; + // Đổi radian sang độ + double angle = acos(cosTheta) * 180.0 / M_PI; + + return angle; +} +void Pathway::RecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin) +{ + if(!isPathInitilized_) + { + double InitialSegmentLength = sqrt(pow(currentPose.getX() - startPose.getX(), 2) + pow(currentPose.getY() - startPose.getY(), 2)); + if(InitialSegmentLength>=InitialSegmentLengthMin) + { + addEdge(startPose, currentPose); + isPathInitilized_ = true; + printf("path is initialized\n"); + } + + } + if(!path_.empty()) + { + double distanceCheck = sqrt(pow(currentPose.getX() - path_.back().getP2().getX(), 2) + pow(currentPose.getY() - path_.back().getP2().getY(), 2)); + if(distanceCheck>=SegmentLengthMin) + { + if(isNewPoseOnEdge(currentPose, delta_angle_th)){ + setP2LastEdgeOnPath(currentPose); + printf("add new pose on segment into path\n"); + } + else{ + addEdge(currentPose); + printf("add new segment into path\n"); + } + printf("Number of segment on path: %d\n",(int)path_.size()); + } + } + +} + +void Pathway::ResetAndRecordPath(Pose startPose, Pose currentPose, double delta_angle_th, double InitialSegmentLengthMin, double SegmentLengthMin) +{ + if(!isPathClear_){ + path_.clear(); + isPathClear_ = true; + } + RecordPath(startPose, currentPose, delta_angle_th, InitialSegmentLengthMin, SegmentLengthMin); +} + +bool Pathway::isNewPoseOnEdge(Pose p, double delta_angle_th) +{ + if(!path_.empty()) + { + double deltaAngle = calculateAngle(path_.back().getP1(), path_.back().getP2(), p); + // ROS_WARN("deltaAngle: %f",deltaAngle); + if(deltaAngle<=delta_angle_th) return true; + } + return false; +} + +void Pathway::setEdgeOnPath(uint32_t line_index, Pose p1, Pose p2){ + if(!path_.empty()&&line_index<=path_.size()) + { + path_.at(line_index).setEdge(p1, p2); + } +} + +void Pathway::syncPosesAndPath() +{ + if(!path_.empty()) + { + posesOnPath_.clear(); + for(auto& edge : path_) + { + if(!isPoseExisted(edge.getP1())) posesOnPath_.push_back(edge.getP1()); + if(!isPoseExisted(edge.getP2())) posesOnPath_.push_back(edge.getP2()); + } + } + else posesOnPath_.clear(); +} + +void Pathway::SavePathAsFile(const string& file_name) +{ + ofstream file(file_name); + + if (file.is_open()) + { + for (auto &edge : path_) + { + file << edge.getP1().getX() << " " << edge.getP1().getY() << " " << edge.getP1().getYaw() + << edge.getP2().getX() << " " << edge.getP2().getY() << " " << edge.getP2().getYaw() << "\n"; + } + + file.close(); + cout << "Pathway saved to file: " <> x1 >> y1 >> yaw1 >> x2 >> y2 >> yaw2) + { + Pose p1(x1, y1, yaw1); + Pose p2(x2, y2, yaw2); + + addEdge(p1, p2); + } + + file.close(); + cout << "Pathway loaded from file: " <=0&&alpha<=1&&beta>=0&&beta<=1) + { + double x0 = x1 + alpha*(x2 - x1); + double y0 = y1 + alpha*(y2 - y1); + resultPoint->setPoint(x0,y0); + return true; + } + else return false; +} + +vector Pathway::findIntersections(Edge L) { + vector intersectionPoints; + if(!path_.empty()){ + for (auto& segment : path_){ + Point result; + if(findIntersection(L,segment,&result)==true) + { + intersectionPoints.push_back(result); + } + } + } + return intersectionPoints; +} + +vector> Pathway::findIntersections(Edge L1, Edge L2) { + vector> intersectionPoints; + vector intersectionPointsL1; + vector intersectionPointsL2; + if(!path_.empty()){ + for (auto& segment : path_) { + Point result1; + if(findIntersection(L1,segment,&result1)==true) + { + intersectionPointsL1.push_back(result1); + } + Point result2; + if(findIntersection(L2,segment,&result2)==true) + { + intersectionPointsL2.push_back(result2); + } + } + } + intersectionPoints.push_back(intersectionPointsL1); + intersectionPoints.push_back(intersectionPointsL2); + + return intersectionPoints; +} \ No newline at end of file diff --git a/src/plan_for_retry.cpp b/src/plan_for_retry.cpp new file mode 100755 index 0000000..565e3a3 --- /dev/null +++ b/src/plan_for_retry.cpp @@ -0,0 +1,686 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; + +inline double calculateAngle(double xA, double yA, double xB, double yB) { + double deltaX = xB - xA; + double deltaY = yB - yA; + double angleRad = 0; + if(deltaX!=0) + { + angleRad = atan2(deltaY, deltaX); + // double angleDeg = angleRad * 180.0 / M_PI; + } + return angleRad; +} + +inline double getYaw(double x, double y, double z, double w){ + // yaw (z-axis rotation) + double siny_cosp = 2 * (w * z + x * y); + double cosy_cosp = 1 - 2 * (y * y + z * z); + double yaw = std::atan2(siny_cosp, cosy_cosp); + return yaw; +} + +bool isThetaValid(double theta) +{ + bool result = false; + if(theta < -M_PI || theta > M_PI) result = false; + else result = true; + return result; +} + +double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose) +{ + double delta_angle = 0; + if(isThetaValid(theta)) + { + double xAB = next_Pose.position.x - startPose.position.x; + double yAB = next_Pose.position.y - startPose.position.y; + double d = sqrt(xAB*xAB + yAB*yAB); + double xC = startPose.position.x + d*cos(theta); + double yC = startPose.position.y + d*sin(theta); + double xAC = xC-startPose.position.x; + double yAC = yC-startPose.position.y; + double dAB = sqrt(xAB*xAB + yAB*yAB); + double cos_a = (xAB*xAC + yAB*yAC)/(dAB*d); + if(cos_a>1) cos_a = 1; + else if(cos_a<(-1)) cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("xC: %f, yC: %f", xC, yC); + // ROS_WARN("dAB: %f", dAB); + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; +} + +double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose) +{ + double delta_angle = 0; + if(isThetaValid(theta)) + { + double xAB =endPose.position.x-prev_Pose.position.x; + double yAB = endPose.position.y-prev_Pose.position.y; + double d = sqrt(xAB*xAB + yAB*yAB); + double xC =endPose.position.x + d*cos(theta); + double yC = endPose.position.y + d*sin(theta); + double xBC = xC-endPose.position.x; + double yBC = yC-endPose.position.y; + double dAB = sqrt(xAB*xAB + yAB*yAB); + double cos_a = (xAB*xBC + yAB*yBC)/(dAB*d); + if(cos_a>1) cos_a = 1; + else if(cos_a<(-1)) cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("xC: %f, yC: %f", xC, yC); + // ROS_WARN("dAB: %f", dAB); + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; +} + +// 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; + 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); + if(length > d) + { + Poses.push_back(A); // Thêm điểm A vào vector trước khi chia + + int segments = length / d; + + // 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; + 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; + p.pose.position.y = p_y; + Poses.push_back(p); + } + + if(!Poses.empty()&&(Poses.back().pose.position.x!=B.pose.position.x || Poses.back().pose.position.y!=B.pose.position.y)) + { + Poses.push_back(B); // Thêm điểm B vào vector sau khi chia + } + + // Tính góc cho từng pose trên đoạn AB + if(//computeDeltaAngleStartOfPlan(getYaw(A.pose.orientation.x, A.pose.orientation.y, A.pose.orientation.z, A.pose.orientation.w), + //Poses.front().pose, Poses[1].pose) <= 0.872664626 && + computeDeltaAngleEndOfPlan(getYaw(B.pose.orientation.x, B.pose.orientation.y, B.pose.orientation.z, B.pose.orientation.w), + Poses.back().pose, Poses[Poses.size() - 2].pose) <= 1.3962634016) // <= 80 degree + { + double theta_tmp = 0; + for(int i = 0; i<((int)Poses.size()-1); i++) + { + if(Poses[i].pose.position.x!=Poses[i+1].pose.position.x) + { + double theta = calculateAngle(Poses[i].pose.position.x, Poses[i].pose.position.y, + Poses[i+1].pose.position.x, Poses[i+1].pose.position.y); + Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta); + theta_tmp = theta; + } + else + { + Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp); + } + } + Poses.back().pose.orientation = B.pose.orientation; + } + else if(//computeDeltaAngleStartOfPlan(getYaw(A.pose.orientation.x, A.pose.orientation.y, A.pose.orientation.z, A.pose.orientation.w), + //Poses.front().pose, Poses[1].pose) >= 2.2689280276 && + computeDeltaAngleEndOfPlan(getYaw(B.pose.orientation.x, B.pose.orientation.y, B.pose.orientation.z, B.pose.orientation.w), + Poses.back().pose, Poses[Poses.size() - 2].pose) >= 1.7453292526) // >= 100 degree + { + double theta_tmp = 0; + for(int i = (int)Poses.size() -1; i>0; i--) + { + if(Poses[i].pose.position.x!=Poses[i-1].pose.position.x) + { + double theta = calculateAngle(Poses[i].pose.position.x, Poses[i].pose.position.y, + Poses[i-1].pose.position.x, Poses[i-1].pose.position.y); + Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta); + theta_tmp = theta; + } + else + { + Poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp); + } + } + Poses.front().pose.orientation = A.pose.orientation; + } + } + else + { + Poses.push_back(A); + Poses.push_back(B); + } + return Poses; +} + +// Hàm gọi make plan : tạo tuyến đường đi quay lại điểm retry có trong plan hiện tại và đi tới điểm goal mới theo cung tròn. +// khi tạo thành công plan thì hàm trả về True, không thành công thì trả về False và có hiện cảnh báo nguyên nhân. + // current_plan: vector chứa plan hiện tại + // pose_A: điểm quay lại để retry + // pose_B: điểm goal mới, có hướng thẳng với palet hiện tại + // 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 result = false; + std::vector PlanRetry_1; + std::vector PlanRetry_2; + std::vector PlanRetry_3; + + if(current_plan.empty()||current_plan.size()<2) + { + ROS_WARN("current_plan is empty"); + return false; + } + + geometry_msgs::PoseStamped pose_A; + pose_A = current_plan[indexOfPoseA]; + + // Tính ra PlanRetry_1 điểm retry tại Pose_A + + PlanRetry_1.assign(current_plan.begin()+indexOfPoseA, current_plan.end()); + + if(!PlanRetry_1.empty()){ + std::reverse(PlanRetry_1.begin(), PlanRetry_1.end()); + } + + // Tính ra PlanRetry_2 với biên dạng cung tròn đi qua pose_A và pose_B, có tâm tại pose_C + + double xAB = pose_B.pose.position.x - pose_A.pose.position.x; + double yAB = pose_B.pose.position.y - pose_A.pose.position.y; + double d_AB = sqrt(xAB*xAB + yAB*yAB); + if(d_AB<=0.1) + { + ROS_WARN("Curve AB is too short, cannot compute plan"); + return false; + } + + // 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 độ -> cung tròn AB sẽ gần như là một đọan thẳng + if((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) >= 3.13 && + 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) <= M_PI) || + (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.1745 && + 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; + planSegment_AB = divideSegment(pose_A, pose_B, 0.1); + PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end()); + } + else + { + double xCA = pose_A.pose.position.x - pose_C.pose.position.x; + double yCA = pose_A.pose.position.y - pose_C.pose.position.y; + double xCB = pose_B.pose.position.x - pose_C.pose.position.x; + double yCB = pose_B.pose.position.y - pose_C.pose.position.y; + double rCA = sqrt(xCA*xCA + yCA*yCA); + double rCB = sqrt(xCB*xCB + yCB*yCB); + if(abs(rCA-rCB)>0.008) + { + ROS_WARN("pose_C is not Center of Curve AB"); + return false; + } + + double cos_ACB = (xCA*xCB + yCA*yCB)/(rCA*rCB); + if(cos_ACB>1) cos_ACB = 1; + else if(cos_ACB<(-1)) cos_ACB = -1; + double angleACB = acos(cos_ACB); + double angle_interval = 0.005; + // tính góc của vector CA: + double angleCA = atan2(yCA, xCA); + + // check thử xem chiều góc quét từ A -> B thì angleCA + delta_angle hay angleCA - delta_angle + bool is_increase_angle = false; + double check_angle = angleCA + 50*angle_interval*angleACB; + double xA1 = pose_C.pose.position.x + rCA*cos(check_angle); + double yA1 = pose_C.pose.position.y + rCA*sin(check_angle); + double xCA1 = xA1 - pose_C.pose.position.x; + double yCA1 = yA1 - pose_C.pose.position.y; + double cos_A1CB = (xCA1*xCB + yCA1*yCB)/(rCA*rCB); + if(cos_A1CB>1) cos_A1CB = 1; + else if(cos_A1CB<(-1)) cos_A1CB = -1; + double angleA1CB = acos(cos_A1CB); + if(angleA1CB>angleACB) + { + is_increase_angle = false; + } + else if(angleA1CB2) + { + for(int i = 0 ; i < PlanRetry_2.size(); i++) + { + ROS_INFO("Pose %d in PlanRetry : %f, %f", i, PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y); + } + if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w), + PlanRetry_2.front().pose, PlanRetry_2[1].pose) <= 1.3962634016 &&*/ + computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w), + PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) <= 1.3962634016 + ) // <= 80 degree + { + double theta_tmp = 0; + for(int i = 0; i<((int)PlanRetry_2.size()-1); i++) + { + if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i+1].pose.position.x) + { + double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y, + PlanRetry_2[i+1].pose.position.x, PlanRetry_2[i+1].pose.position.y); + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta); + theta_tmp = theta; + } + else + { + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp); + } + } + PlanRetry_2.back().pose.orientation = pose_B.pose.orientation; + } + else if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w), + PlanRetry_2.front().pose, PlanRetry_2[1].pose) >= 1.745329252 &&*/ + computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w), + PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) >= 1.745329252) // >= 100 degree + { + double theta_tmp = 0; + for(int i = (int)PlanRetry_2.size() -1; i>0; i--) + { + if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i-1].pose.position.x) + { + double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y, + PlanRetry_2[i-1].pose.position.x, PlanRetry_2[i-1].pose.position.y); + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta); + theta_tmp = theta; + } + else + { + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp); + } + } + PlanRetry_2.front().pose.orientation = PlanRetry_2[1].pose.orientation; + } + else + { + ROS_WARN("Pose_A yaw or Pose_B yaw is invalid value"); + return false; + } + } + else + { + ROS_WARN("Curve AB is too short, cannot compute plan"); + return false; + } + } + + // Tạo đoạn đường từ poseB -> poseB_behind + PlanRetry_3 = divideSegment(pose_B, pose_B_behind, 0.1); + + ros::Time plan_time = ros::Time::now(); + if(!PlanRetry_1.empty()&&!PlanRetry_2.empty()&&!PlanRetry_3.empty()) + { + for(int i = 0; i < (int)PlanRetry_1.size(); i++) + { + PlanRetry_1[i].header.stamp = plan_time; + result_plan.push_back(PlanRetry_1[i]); + } + for(int i = 0; i < (int)PlanRetry_2.size(); i++) + { + PlanRetry_2[i].header.stamp = plan_time; + PlanRetry_2[i].header.frame_id = PlanRetry_1.front().header.frame_id; + result_plan.push_back(PlanRetry_2[i]); + } + for(int i = 0; i < (int)PlanRetry_3.size(); i++) + { + PlanRetry_3[i].header.stamp = plan_time; + PlanRetry_3[i].header.frame_id = PlanRetry_1.front().header.frame_id; + result_plan.push_back(PlanRetry_3[i]); + } + result = true; + } + else{ + if(PlanRetry_3.empty()) + { + ROS_WARN("PlanRetry_3 is empty"); + } + if(PlanRetry_2.empty()) + { + ROS_WARN("PlanRetry_2 is empty"); + } + if(PlanRetry_1.empty()) + { + ROS_WARN("PlanRetry_1 is empty"); + } + } + return result; +} + +// Hàm gọi make plan : tạo tuyến đường đi quay lại điểm retry có trong plan hiện tại và đi tới điểm goal mới theo cung tròn. +// khi tạo thành công plan thì hàm trả về True, không thành công thì trả về False và có hiện cảnh báo nguyên nhân. + // current_plan: vector chứa plan hiện tại + // pose_A: điểm quay lại để retry + // pose_B: điểm goal mới, có hướng thẳng với palet hiện tại + // 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 result = false; + 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, + getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w), + pose_B.pose.position.x, pose_B.pose.position.y, + getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w)); + + // Tính ra PlanRetry_1 điểm retry tại Pose_A + + PlanRetry_1 = divideSegment(pose_B, pose_A, 0.1); + + // Tính ra PlanRetry_2 với biên dạng cung tròn đi qua pose_A và pose_B, có tâm tại pose_C + + double xAB = pose_B.pose.position.x - pose_A.pose.position.x; + double yAB = pose_B.pose.position.y - pose_A.pose.position.y; + double d_AB = sqrt(xAB*xAB + yAB*yAB); + if(d_AB<=0.1) + { + ROS_WARN("Curve AB is too short, cannot compute plan"); + return false; + } + + // 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 độ -> cung tròn AB sẽ gần như là một đọan thẳng + if((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) >= 3.13 && + 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) <= M_PI) || + (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.1745 && + 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; + planSegment_AB = divideSegment(pose_A, pose_B, 0.1); + PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end()); + } + else + { + double xCA = pose_A.pose.position.x - pose_C.pose.position.x; + double yCA = pose_A.pose.position.y - pose_C.pose.position.y; + double xCB = pose_B.pose.position.x - pose_C.pose.position.x; + double yCB = pose_B.pose.position.y - pose_C.pose.position.y; + double rCA = sqrt(xCA*xCA + yCA*yCA); + double rCB = sqrt(xCB*xCB + yCB*yCB); + if(abs(rCA-rCB)>0.008) + { + ROS_WARN("pose_C is not Center of Curve AB"); + return false; + } + + double cos_ACB = (xCA*xCB + yCA*yCB)/(rCA*rCB); + if(cos_ACB>1) cos_ACB = 1; + else if(cos_ACB<(-1)) cos_ACB = -1; + double angleACB = acos(cos_ACB); + double angle_interval = 0.005; + // tính góc của vector CA: + double angleCA = atan2(yCA, xCA); + + // check thử xem chiều góc quét từ A -> B thì angleCA + delta_angle hay angleCA - delta_angle + bool is_increase_angle = false; + double check_angle = angleCA + 50*angle_interval*angleACB; + double xA1 = pose_C.pose.position.x + rCA*cos(check_angle); + double yA1 = pose_C.pose.position.y + rCA*sin(check_angle); + double xCA1 = xA1 - pose_C.pose.position.x; + double yCA1 = yA1 - pose_C.pose.position.y; + double cos_A1CB = (xCA1*xCB + yCA1*yCB)/(rCA*rCB); + if(cos_A1CB>1) cos_A1CB = 1; + else if(cos_A1CB<(-1)) cos_A1CB = -1; + double angleA1CB = acos(cos_A1CB); + if(angleA1CB>angleACB) + { + is_increase_angle = false; + } + else if(angleA1CB2) + { + for(int i = 0 ; i < PlanRetry_2.size(); i++) + { + ROS_INFO("Pose %d in PlanRetry : %f, %f", i, PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y); + } + if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w), + PlanRetry_2.front().pose, PlanRetry_2[1].pose) <= 1.3962634016 &&*/ + computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w), + PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) <= 1.3962634016 + ) // <= 80 degree + { + double theta_tmp = 0; + for(int i = 0; i<((int)PlanRetry_2.size()-1); i++) + { + if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i+1].pose.position.x) + { + double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y, + PlanRetry_2[i+1].pose.position.x, PlanRetry_2[i+1].pose.position.y); + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta); + theta_tmp = theta; + } + else + { + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp); + } + } + PlanRetry_2.back().pose.orientation = pose_B.pose.orientation; + } + else if(/*computeDeltaAngleStartOfPlan(getYaw(pose_A.pose.orientation.x, pose_A.pose.orientation.y, pose_A.pose.orientation.z, pose_A.pose.orientation.w), + PlanRetry_2.front().pose, PlanRetry_2[1].pose) >= 1.745329252 &&*/ + computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w), + PlanRetry_2.back().pose, PlanRetry_2[PlanRetry_2.size() - 2].pose) >= 1.745329252) // >= 100 degree + { + double theta_tmp = 0; + for(int i = (int)PlanRetry_2.size() -1; i>0; i--) + { + if(PlanRetry_2[i].pose.position.x!=PlanRetry_2[i-1].pose.position.x) + { + double theta = calculateAngle(PlanRetry_2[i].pose.position.x, PlanRetry_2[i].pose.position.y, + PlanRetry_2[i-1].pose.position.x, PlanRetry_2[i-1].pose.position.y); + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta); + theta_tmp = theta; + } + else + { + PlanRetry_2[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta_tmp); + } + } + PlanRetry_2.front().pose.orientation = PlanRetry_2[1].pose.orientation; + } + else + { + ROS_WARN("Pose_A yaw or Pose_B yaw is invalid value"); + return false; + } + } + else + { + ROS_WARN("Curve AB is too short, cannot compute plan"); + return false; + } + } + + // Tạo đoạn đường từ poseB -> poseB_behind + PlanRetry_3 = divideSegment(pose_B, pose_B_behind, 0.1); + + ros::Time plan_time = ros::Time::now(); + if(!PlanRetry_1.empty()&&!PlanRetry_2.empty()&&!PlanRetry_3.empty()) + { + for(int i = 0; i < (int)PlanRetry_1.size(); i++) + { + PlanRetry_1[i].header.stamp = plan_time; + result_plan.push_back(PlanRetry_1[i]); + } + for(int i = 0; i < (int)PlanRetry_2.size(); i++) + { + PlanRetry_2[i].header.stamp = plan_time; + PlanRetry_2[i].header.frame_id = PlanRetry_1.front().header.frame_id; + result_plan.push_back(PlanRetry_2[i]); + } + for(int i = 0; i < (int)PlanRetry_3.size(); i++) + { + PlanRetry_3[i].header.stamp = plan_time; + PlanRetry_3[i].header.frame_id = PlanRetry_1.front().header.frame_id; + result_plan.push_back(PlanRetry_3[i]); + } + result = true; + } + else{ + if(PlanRetry_3.empty()) + { + ROS_WARN("PlanRetry_3 is empty"); + } + if(PlanRetry_2.empty()) + { + ROS_WARN("PlanRetry_2 is empty"); + } + if(PlanRetry_1.empty()) + { + ROS_WARN("PlanRetry_1 is empty"); + } + } + return result; +} + +// Hàm Tìm tâm C của cung tròn AB khi biết Pose tại điểm B: khi tìm thành công điểm C thì hàm trả về True + // pose_A: điểm start của cung tròn + // 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) +{ + // 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), + pose_B.pose, pose_A.pose) >= 3.13 && + 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) <= M_PI) || + (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.1745 && + 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)) + { + pose_C.pose.position.x = (pose_A.pose.position.x + pose_B.pose.position.x)/2; + pose_C.pose.position.y = (pose_A.pose.position.y + pose_B.pose.position.y)/2; + } + else + { + double x_R = pose_A.pose.position.x; + double y_R = pose_A.pose.position.y; + double x_G = pose_B.pose.position.x; + double y_G = pose_B.pose.position.y; + double phi_vG = getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w); + double x_H = (x_R+x_G)/2; + double y_H = (y_R+y_G)/2; + double m_vG = tan(phi_vG); + double m_G_n_vG = -1/m_vG; + double b_G_n_vG = y_G-m_G_n_vG*x_G; + double m_RG =(y_G-y_R)/(x_G-x_R); + double b_RG = y_R-m_RG*x_R; + double m_H_n_RG = -1/m_RG; + double b_H_n_RG = y_H-m_H_n_RG*x_H; + pose_C.pose.position.x = (b_H_n_RG-b_G_n_vG)/(m_G_n_vG-m_H_n_RG); + pose_C.pose.position.y = (b_H_n_RG*m_G_n_vG-b_G_n_vG*m_H_n_RG)/(m_G_n_vG-m_H_n_RG); + } + return true; +} diff --git a/src/pose.cc b/src/pose.cc new file mode 100755 index 0000000..63978f0 --- /dev/null +++ b/src/pose.cc @@ -0,0 +1,42 @@ +#include + +namespace custom_planner +{ + +bool Pose::SavePoseAsFile(const string& file_name) +{ + bool result = false; + ofstream file(file_name); + if (file.is_open()) + { + file << x_ << " " << y_ << " "<< yaw_ << "\n"; + file.close(); + result = true; + } + else + { + result = false; + } + return result; +} + +bool Pose::LoadPoseFromFile(const string& file_name) +{ + bool result = false; + ifstream file(file_name); + if (file.is_open()) + { + double x, y, yaw; + file >> x >> y >> yaw; + setPose(x, y, yaw); + file.close(); + result = true; + } + else + { + result = false; + } + return result; +} + +} \ No newline at end of file