commit 66be0d86cc1e63653a1c2011b0be57e64b138bd0 Author: duongtd Date: Mon Dec 22 17:37:45 2025 +0700 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..98962c8 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,90 @@ +# --- CMake version và project name --- +cmake_minimum_required(VERSION 3.10) +project(dock_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}/dock_planner") +set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/dock_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: dock_planner --- +# Tạo thư viện chính +add_library(dock_planner + src/dock_planner.cpp + src/dock_calc.cpp + src/utils/curve_common.cpp + src/utils/pose.cpp + src/utils/line_common.cpp +) + +# --- Link các thư viện phụ thuộc --- +target_link_libraries(dock_planner + ${Boost_LIBRARIES} # Boost + visualization_msgs + nav_msgs + tf3 + tf3_geometry_msgs + robot_time + data_convert + costmap_2d + nav_core + robot::console + nav_2d_utils +) + +# --- Include directories cho target --- +target_include_directories(dock_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 dock_planner + EXPORT dock_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/dock_planner/costmap_2dTargets.cmake --- +# --- File này chứa cấu hình giúp project khác có thể dùng --- +# --- Find_package(dock_planner REQUIRED) --- +# --- Target_link_libraries(my_app PRIVATE dock_planner::dock_planner) --- +install(EXPORT dock_planner-targets + FILE dock_planner-targets.cmake + NAMESPACE dock_planner:: + DESTINATION lib/cmake/dock_planner +) + +# --- Cài đặt headers --- +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/config/dock_global_params.yaml b/config/dock_global_params.yaml new file mode 100644 index 0000000..1c5332d --- /dev/null +++ b/config/dock_global_params.yaml @@ -0,0 +1,12 @@ +base_global_planner: "DockPlanner" +DockPlanner: + # File: config/planner_params.yaml + MyGlobalPlanner: + cost_threshold: 200 # Ngưỡng chi phí vật cản (0-255) + safety_distance: 2 # Khoảng cách an toàn (cells) + use_dijkstra: false # Sử dụng Dijkstra thay vì A* + + # File: config/costmap_params.yaml + global_costmap: + inflation_radius: 0.3 # Bán kính phình vật cản + cost_scaling_factor: 10.0 # Hệ số tỷ lệ chi phí \ No newline at end of file diff --git a/include/dock_planner/dock_calc.h b/include/dock_planner/dock_calc.h new file mode 100644 index 0000000..42c2a33 --- /dev/null +++ b/include/dock_planner/dock_calc.h @@ -0,0 +1,147 @@ +#ifndef DOCK_CALC_H +#define DOCK_CALC_H + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include "dock_planner/utils/curve_common.h" +#include "dock_planner/utils/pose.h" +#include "dock_planner/utils/common.h" +#include + +#include + +using namespace std; + +namespace dock_planner +{ + struct RobotGeometry + { + double length; // dọc trục X + double width; // dọc trục Y + double radius; // circle around base_link + }; + + class DockCalc + { + private: + /* data */ + Spline_Inf* input_spline_inf; + CurveCommon* CurveDesign; + + vector posesOnPathWay; + vector footprint_robot_; + geometry_msgs::PoseStamped save_goal_; + // std::vector free_zone_; + + bool check_goal_; + int store_start_nearest_idx_; + int start_target_idx_; + double min_distance_to_goal_; + + // Tính khoảng cách giữa 2 điểm + inline double calc_distance(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) + { + return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y); + } + + inline double calculateAdaptiveStep(double segment_length, double desired_point_spacing = 0.02) + { + return desired_point_spacing / segment_length; + } + + 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; + } + + double calculateNURBSLength(CurveCommon* curve_design, + Spline_Inf* spline_inf, + int degree, + double initial_step = 0.02); + + geometry_msgs::Pose offsetPoint(const geometry_msgs::Pose& A,const double& theta, const double& d); + + double pointToSegmentDistance(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C); + + double signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C); + + double compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C); + + geometry_msgs::Pose compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C,const double& t); + + void updatePoseOrientation(std::vector& saved_poses, + std::vector& nurbs_path, + bool reverse = true); + + bool findPathTMP(const vector& control_points, + vector& saved_poses, + const int& degree, double step = 0.02); + + bool findNURBSControlPoints(int& dir, vector& control_points, + const geometry_msgs::Pose& start_pose, + const std::vector& posesOnPathWay, + const double& angle_threshol, const int& degree); + + int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, + const std::vector& posesOnPathWay, + const double& angle_threshol); + + bool isFreeSpace(const geometry_msgs::Pose& pose, const std::vector& footprint); + + std::vector calcFreeZone(const geometry_msgs::Pose& pose, + const std::vector& footprint); + + std::vector interpolateFootprint(const geometry_msgs::Pose& pose, + const std::vector& footprint, const double& resolution); + + RobotGeometry computeGeometry(const std::vector& fp); + + + public: + //Params + costmap_2d::Costmap2D* costmap_; + + int cost_threshold_; // Threshold for obstacle detection + double safety_distance_; // Safety distance from obstacles + double calib_safety_distance_; + + DockCalc(/* args */); + + ~DockCalc(); + + bool makePlanMoveToDock(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + const vector& footprint_robot, + const vector& footprint_dock, + const int& degree, const double& step, + vector& planMoveToDock, + bool reverse = false); + + + }; + +}//namespace dock_planner + + +#endif \ No newline at end of file diff --git a/include/dock_planner/dock_planner.h b/include/dock_planner/dock_planner.h new file mode 100644 index 0000000..d69c632 --- /dev/null +++ b/include/dock_planner/dock_planner.h @@ -0,0 +1,42 @@ +#ifndef DOCK_PLANNER_H +#define DOCK_PLANNER_H + + + +#include "dock_planner/dock_calc.h" +#include + +namespace dock_planner { + class DockPlanner : public nav_core::BaseGlobalPlanner + { + public: + DockPlanner(); + DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot); + + bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot); + + bool makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + std::vector& plan); + + private: + // Core components + costmap_2d::Costmap2DROBOT* costmap_robot_; + costmap_2d::Costmap2D* costmap_; + bool initialized_; + std::string frame_id_; + // ros::Publisher plan_pub_; + std::vector footprint_dock_; + + int cost_threshold_; + double calib_safety_distance_; + bool check_free_space_; + + DockCalc calc_plan_to_dock_; + + + void publishPlan(const std::vector& plan); + }; +} // namespace dock_planner + +#endif \ No newline at end of file diff --git a/include/dock_planner/utils/color.h b/include/dock_planner/utils/color.h new file mode 100644 index 0000000..263d901 --- /dev/null +++ b/include/dock_planner/utils/color.h @@ -0,0 +1,34 @@ +#ifndef COLOR_H_ +#define 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 //COLOR_H_ \ No newline at end of file diff --git a/include/dock_planner/utils/common.h b/include/dock_planner/utils/common.h new file mode 100644 index 0000000..30f4a6d --- /dev/null +++ b/include/dock_planner/utils/common.h @@ -0,0 +1,472 @@ +#ifndef _VDA_5050_COMMON_H_INCLUDE_ +#define _VDA_5050_COMMON_H_INCLUDE_ +#include +#include + +namespace vda_5050 +{ + enum class OrderType + { + Base, + Horizon + }; + + struct NodePosition + { + double x, y, theta; + double allowedDeviationXY, allowedDeviationTheta; + std::string mapId, mapDescription; + }; + + struct ActionParameter + { + std::string key; + std::string value; + }; + + struct Action + { + std::string actionType; + std::string actionId; + std::string actionDescription; + std::string blockingType; + std::vector actionParameters; + }; + + struct Node + { + std::string nodeId; + int sequenceId; + std::string nodeDescription; + bool released; + NodePosition nodePosition; + std::vector> actions; + }; + + struct Corridor + { + double leftWidth; + double rightWidth; + std::string corridorRefPoint; + }; + + struct ControlPoint + { + double x, y, weight; + }; + + struct Trajectory + { + int degree; + std::vector knotVector; + std::vector controlPoints; + }; + + struct Edge + { + std::string edgeId; + int sequenceId; + std::string edgeDescription; + bool released; + std::string startNodeId; + std::string endNodeId; + double maxSpeed; + double maxHeight; + double minHeight; + double orientation; + std::string orientationType; + std::string direction; + bool rotationAllowed; + double maxRotationSpeed; + Trajectory trajectory; + double length; + Corridor corridor; + std::vector actions; + }; + + struct Order + { + int headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + std::string orderId; + int orderUpdateId; + std::string zoneSetId; + std::vector nodes; + std::vector edges; + + OrderType getTypeOrder() + { + for (auto &edge : edges) + { + if (!edge.released) + return OrderType::Horizon; + } + for (auto &node : nodes) + { + if (!node.released) + return OrderType::Horizon; + } + return OrderType::Base; + } + }; + + struct InstantAction + { + int headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + std::vector> actions; + }; + + struct AGVPosition + { + double x; + double y; + double theta; + std::string mapId; + bool positionInitialized; + double localizationScore; + double deviationRange; + }; + + struct Velocity + { + double vx; + double vy; + double omega; + }; + + struct BatteryState + { + double batteryCharge; + double batteryVoltage; + int8_t batteryHealth; + bool charging; + uint32_t reach; + }; + + struct SafetyState + { + std::string eStop; + bool fieldViolation; + std::string AUTO_ACK = "autoAck"; + std::string MANUAL = "manual"; + std::string REMOTE = "remote"; + std::string NONE = "none"; + }; + + struct ErrorReference + { + std::string referenceKey; + std::string referenceValue; + }; + + struct Error + { + struct ErrorLevel + { + enum ErrorLevel_EN + { + WARNING, + FATAL + }; + + operator ErrorLevel_EN() const { return level; } + + ErrorLevel &operator=(const ErrorLevel_EN &newType) + { + level = newType; + return *this; + } + + private: + ErrorLevel_EN level; + + public: + // Function to convert ActionStatus to string + std::string _toString() const + { + switch (level) + { + case WARNING: + return "WARNING"; + case FATAL: + return "FATAL"; + default: + return ""; + } + } + }; + + std::string errorType; + std::vector errorReferences; + std::string errorDescription; + ErrorLevel errorLevel; + }; + + struct NodeState + { + std::string nodeId; + int32_t sequenceId; + std::string nodeDescription; + NodePosition nodePosition; + bool released; + }; + + struct EdgeState + { + std::string edgeId; + uint32_t sequenceId; + std::string edgeDescription; + bool released; + Trajectory trajectory; + }; + + struct ActionState + { + struct ActionStatus + { + public: + enum ActionStatus_EN + { + WAITING, + INITIALIZING, + RUNNING, + PAUSED, + FINISHED, + FAILED + }; + + operator ActionStatus_EN() const { return type; } + + ActionStatus &operator=(const ActionStatus_EN &newType) + { + type = newType; + return *this; + } + + bool operator==(const ActionStatus_EN &otherType) const + { + return type == otherType; + } + + private: + ActionStatus_EN type; + + public: + // Function to convert ActionStatus to string + std::string _toString() const + { + switch (type) + { + case WAITING: + return "WAITING"; + case INITIALIZING: + return "INITIALIZING"; + case RUNNING: + return "RUNNING"; + case PAUSED: + return "PAUSED"; + case FINISHED: + return "FINISHED"; + case FAILED: + return "FAILED"; + default: + return ""; + } + } + }; + + std::string actionId; + std::string actionType; + std::string actionDescription; + ActionStatus actionStatus; + std::string resultDescription; + }; + + struct InfoReference + { + std::string referenceKey; + std::string referenceValue; + }; + + struct Info + { + std::string infoType; + std::vector infoReferences; + std::string infoDescription; + std::string infoLevel; + + std::string DEBUG = "DEBUG"; + std::string INFO = "INFO"; + }; + + struct BoundingBoxReference + { + double x; + double y; + double z; + double theta; + }; + + struct LoadDimensions + { + double length; + double width; + double height; + }; + + struct Load + { + std::string loadId; + std::string loadType; + std::string loadPosition; + BoundingBoxReference boundingBoxReference; + LoadDimensions loadDimensions; + uint32_t weight; + }; + + struct State + { + struct OperatingMode + { + enum OperatingMode_EN + { + AUTOMATIC, + SEMIAUTOMATIC, + MANUAL, + SERVICE, + TEACHING + }; + + private: + OperatingMode_EN type; + + public: + // Constructor mặc định + OperatingMode() : type(SERVICE) {} + + // Toán tử chuyển đổi sang enum + operator OperatingMode_EN() const { return type; } + + OperatingMode &operator=(const OperatingMode_EN &newType) + { + type = newType; + return *this; + } + + bool operator==(const OperatingMode_EN &otherType) const + { + return type == otherType; + } + + // Chuyển đổi sang chuỗi + std::string _toString() const + { + switch (type) + { + case AUTOMATIC: + return "AUTOMATIC"; + case SEMIAUTOMATIC: + return "SEMIAUTOMATIC"; + case MANUAL: + return "MANUAL"; + case SERVICE: + return "SERVICE"; + case TEACHING: + return "TEACHING"; + default: + return "SERVICE"; + } + } + }; + + State() + { + headerId = 1; + version = "1.0.0"; + manufacturer = "Phenikaa-X"; + serialNumber = ""; + velocity.vx = 0; + velocity.vy = 0; + velocity.omega = 0; + batteryState.batteryCharge = 100; + batteryState.batteryVoltage = 48; + batteryState.batteryHealth = 100; + batteryState.charging = false; + batteryState.reach = 0; + std::vector errors_; + errors = errors_; + } + + uint32_t headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + std::string orderId; + uint32_t orderUpdateId; + std::string zoneSetId; + std::string lastNodeId; + uint32_t lastNodeSequenceId; + bool driving; + bool paused; + bool newBaseRequest; + double distanceSinceLastNode; + OperatingMode operatingMode; + std::vector nodeStates; + std::vector edgeStates; + std::vector> actionStates; + AGVPosition agvPosition; + Velocity velocity; + std::vector loads; + BatteryState batteryState; + std::vector errors; + std::vector information; + SafetyState safetyState; + + OrderType getTypeOrder() + { + for (auto &edge : edgeStates) + { + if (!edge.released) + return OrderType::Horizon; + } + for (auto &node : nodeStates) + { + if (!node.released) + return OrderType::Horizon; + } + return OrderType::Base; + } + }; + + struct Visualization + { + Visualization() + { + headerId = 1; + version = "1.0.0"; + manufacturer = "Phenikaa-X"; + serialNumber = "AMR2WS"; + velocity.vx = 0; + velocity.vy = 0; + velocity.omega = 0; + } + + uint32_t headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + AGVPosition agvPosition; + Velocity velocity; + }; +} + +#endif // _VDA_5050_COMMON_H_INCLUDE_ \ No newline at end of file diff --git a/include/dock_planner/utils/conversion.h b/include/dock_planner/utils/conversion.h new file mode 100644 index 0000000..b70d885 --- /dev/null +++ b/include/dock_planner/utils/conversion.h @@ -0,0 +1,43 @@ +#ifndef CONVERSION_H_ +#define CONVERSION_H_ +#include "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 < 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 < 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 //CONVERSION_H_ \ No newline at end of file diff --git a/include/dock_planner/utils/curve_common.h b/include/dock_planner/utils/curve_common.h new file mode 100644 index 0000000..3ff0dee --- /dev/null +++ b/include/dock_planner/utils/curve_common.h @@ -0,0 +1,82 @@ +#ifndef CURVE_COMMON_H_ +#define CURVE_COMMON_H_ + +#include +#include +#include +#include +#include "color.h" +#include + +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 CurveCommon +{ +public: + CurveCommon(); + 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); + bool curveIsValid(int degree, const std::vector& knot_vector, + std::vector>& control_points); + + 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 //CURVE_COMMON_H_ \ No newline at end of file diff --git a/include/dock_planner/utils/line_common.h b/include/dock_planner/utils/line_common.h new file mode 100644 index 0000000..e2fdaca --- /dev/null +++ b/include/dock_planner/utils/line_common.h @@ -0,0 +1,13 @@ +#ifndef __LINE_COMMON_H_ +#define __LINE_COMMON_H_ + +#include + +namespace vda_5050_utils +{ + double distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2); + + bool isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance); +} + +#endif \ No newline at end of file diff --git a/include/dock_planner/utils/pose.h b/include/dock_planner/utils/pose.h new file mode 100644 index 0000000..64da7b2 --- /dev/null +++ b/include/dock_planner/utils/pose.h @@ -0,0 +1,66 @@ +#ifndef AMR_CONTROL_POSE_H +#define AMR_CONTROL_POSE_H +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vda_5050 +{ + class Pose { + private: + double x_, y_, yaw_; + double v_max_, accuracy_; + + 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), v_max_(0.0), accuracy_(0.0) { + }; + + Pose(double x, double y, double yaw) : + x_(x), y_(y), yaw_(yaw), v_max_(0.0), accuracy_(0.0) { + }; + + Pose(double x, double y, double yaw, double v_max, double accuracy) : + x_(x), y_(y), yaw_(yaw), v_max_(v_max), accuracy_(accuracy) { + }; + + ~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 setVmax(double v_max) { v_max_ = v_max; } + inline void setAccuracy(double accuracy) { accuracy_ = accuracy; } + inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); } + inline void setPose(double x, double y, double yaw, double v_max, double accuracy) + { + x_ = x, y_ = y, yaw_ = yaw, v_max_ = v_max, accuracy_ = accuracy, 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 double getVmax(void) { return v_max_; } + inline double getAccuracy(void) { return accuracy_; } + inline Pose getPose(void) { return Pose(x_, y_, yaw_, v_max_, accuracy_); } + bool SavePoseAsFile(const std::string& file_name); + bool LoadPoseFromFile(const std::string& file_name); + + }; // class Pose + +} + +#endif \ No newline at end of file diff --git a/src/dock_calc.cpp b/src/dock_calc.cpp new file mode 100644 index 0000000..b643908 --- /dev/null +++ b/src/dock_calc.cpp @@ -0,0 +1,811 @@ +#include "dock_planner/dock_calc.h" + +namespace dock_planner +{ + DockCalc::DockCalc(/* args */) :costmap_(nullptr) + { + check_goal_ = false; + // cost_threshold_ = 200; // Threshold for obstacle detection + safety_distance_ = 0; // Safety distance from obstacles + calib_safety_distance_ = 0; + input_spline_inf = new Spline_Inf(); + CurveDesign = new CurveCommon(); + } + + DockCalc::~DockCalc() + { + delete CurveDesign; + delete input_spline_inf; + } + + geometry_msgs::Pose DockCalc::offsetPoint(const geometry_msgs::Pose& A, const double& theta, const double& d) + { + geometry_msgs::Pose B; + B.position.x = A.position.x + d * std::cos(theta); + B.position.y = A.position.y + d * std::sin(theta); + return B; + } + + double DockCalc::pointToSegmentDistance(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) + { + double theta = signedAngle(A,B,C); + double sin_theta = sin(fabs(theta)); + double h = calc_distance(A,B); + double distance = sin_theta * h; + return distance; + } + + double DockCalc::signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) + { + double ABx = B.position.x - A.position.x; + double ABy = B.position.y - A.position.y; + + double CBx = B.position.x - C.position.x; + double CBy = B.position.y - C.position.y; + + double cross = ABx*CBy - ABy*CBx; + double dot = ABx*CBx + ABy*CBy; + + double angle = atan2(cross,dot); + return angle; + } + + double DockCalc::compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) + { + constexpr double epsilon = 1e-6; + double dx = C.position.x - B.position.x; + double dy = C.position.y - B.position.y; + double bx = B.position.x - A.position.x; + double by = B.position.y - A.position.y; + + double numerator = dx * bx + dy * by; + double denominator = dx * dx + dy * dy; + + if (denominator <= epsilon) + { + throw std::runtime_error("B and C are the same!"); + } + + return -numerator / denominator; + } + + geometry_msgs::Pose DockCalc::compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C, const double& t) + { + geometry_msgs::Pose D; + D.position.x = B.position.x + t * (C.position.x - B.position.x); + D.position.y = B.position.y + t * (C.position.y - B.position.y); + return D; + } + + RobotGeometry DockCalc::computeGeometry(const std::vector& fp) + { + double min_x = 1e9, max_x = -1e9; + double min_y = 1e9, max_y = -1e9; + double radius = 0.0; + if((int)fp.size() >= 4) + { + for (const auto& p : fp) + { + min_x = std::min(min_x, p.x); + max_x = std::max(max_x, p.x); + min_y = std::min(min_y, p.y); + max_y = std::max(max_y, p.y); + radius = std::max(radius, std::hypot(p.x, p.y)); + } + } + else return {0, 0, 0}; + double length = std::max(max_x - min_x, max_y - min_y); + double width = std::min(max_x - min_x, max_y - min_y); + return {length, width, radius}; + } + + bool DockCalc::makePlanMoveToDock(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + const vector& footprint_robot, + const vector& footprint_dock, + const int& degree, const double& step, + vector& planMoveToDock, + bool reverse) + { + constexpr double epsilon = 1e-6; + robot::log_error("[dock_calc] makePlanMoveToDock start"); + planMoveToDock.clear(); + + if(save_goal_.pose.position.x != goal.pose.position.x || + save_goal_.pose.position.y != goal.pose.position.x || + data_convert::getYaw(save_goal_.pose.orientation) != data_convert::getYaw(goal.pose.orientation)) + { + // robot::log_error("[dock_calc] DEBUG 1000"); + check_goal_ = true; + save_goal_ == goal; + } + + footprint_robot_ = footprint_robot; + // Calculate footprint distances + RobotGeometry robot = computeGeometry(footprint_robot); + RobotGeometry dock = computeGeometry(footprint_dock); + double distance_robot = robot.length; + double distance_dock = dock.length; + if (distance_robot <= epsilon) + { + robot::log_error("[dock_calc] Robot geometry is invalid, cannot calculate plan to dock."); + return false; + } + + // Calculate safety distance + safety_distance_ = (robot.radius + calib_safety_distance_) * 2.0; + if (safety_distance_ <= epsilon) + { + robot::log_error("[dock_calc] Safety distance is zero, cannot calculate plan to dock."); + return false; + } + + // Lambda: Generate NURBS path + auto generateNURBSPath = [&](const vector& control_points, bool check_isFreeSpace, + bool edge_end_plag, bool path_reverse) -> vector + { + vector result; + bool obstacle_flag = false; + + // Convert to Eigen format + std::vector> eigen_points; + for (const auto& cp : control_points) + { + eigen_points.emplace_back(cp.position.x, cp.position.y, 0); + } + + // Create knot vector + std::vector knot_vector; + int n = control_points.size() - 1; + int m = n + degree + 1; + for (int i = 0; i <= m; i++) + { + knot_vector.push_back(i <= degree ? 0.0 : 1.0); + } + + // Setup spline + std::vector weights(control_points.size(), 1.0); + input_spline_inf->knot_vector.clear(); + input_spline_inf->control_point.clear(); + input_spline_inf->weight.clear(); + + CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_points, knot_vector); + CurveDesign->ReadSplineInf(input_spline_inf, weights, false); + + // Calculate adaptive step + double edge_length = calculateNURBSLength(CurveDesign, input_spline_inf, degree, step); + double resolution = costmap_->getResolution(); + double step_new = calculateAdaptiveStep(edge_length, resolution); + + // robot::log_error("[dock_calc] Adaptive step size: %f", step_new); + + // Generate path points + vector saved_poses; + for (double t = 0.0; t <= 1.0; t += step_new) + { + geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); + + if (!std::isnan(point.x) && !std::isnan(point.y)) + { + geometry_msgs::Pose pose; + pose.position = point; + pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0); + + bool status_isFreeSpace = true; + if(edge_end_plag) + { + double distance_to_goal = calc_distance(pose, goal.pose); + if(distance_to_goal >= min_distance_to_goal_) + { + if(check_isFreeSpace) status_isFreeSpace = isFreeSpace(pose, footprint_robot_); + } + } + else + { + if(check_isFreeSpace) status_isFreeSpace = isFreeSpace(pose, footprint_robot_); + } + + if (status_isFreeSpace && t < 1.0) + { + saved_poses.push_back(pose); + if (saved_poses.size() > 1) + { + updatePoseOrientation(saved_poses, result, path_reverse); + } + } + if(!status_isFreeSpace) + { + obstacle_flag = true; + break; + } + } + } + + // Set final orientation + if (!saved_poses.empty()) + { + double end_yaw = path_reverse ? + calculateAngle(control_points.back().position.x, control_points.back().position.y, + saved_poses.back().position.x, saved_poses.back().position.y) : + calculateAngle(saved_poses.back().position.x, saved_poses.back().position.y, + control_points.back().position.x, control_points.back().position.y); + + tf3::Quaternion q; + q.setRPY(0, 0, end_yaw); + saved_poses.back().orientation.x = q.x(); + saved_poses.back().orientation.y = q.y(); + saved_poses.back().orientation.z = q.z(); + saved_poses.back().orientation.w = q.w(); + + result.push_back(saved_poses.back()); + } + if(obstacle_flag) + { + robot::log_error("[dock_calc] Obstacle detected, path generation interrupted."); + result.clear(); + } + if (result.empty()) + { + robot::log_error("[dock_calc] No valid path generated."); + } + else + { + robot::log_info("[dock_calc] Path generated with %zu points.", result.size()); + } + return result; + }; + + // Calculate minimum distance for pose C + // Can safety_distance_ replace distance_robot + double distance_min_for_pose_C = (distance_robot + distance_dock) / 2.0; + min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0); + + // Calculate pose C (offset from goal) + geometry_msgs::Pose Goal_Pose = goal.pose; + double yaw_goal = normalizeAngle(data_convert::getYaw(goal.pose.orientation)); + geometry_msgs::Pose Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C); + + // Compute t parameter and determine direction + double t = compute_t(start.pose, goal.pose, Pose_C); + /** + * dir_robot_move_to_goal = false robot move backward + * dir_robot_move_to_goal = true robot move forward + */ + bool dir_robot_move_to_goal = true; + + if (t < 0) + { + yaw_goal = (yaw_goal > 0) ? yaw_goal - M_PI : yaw_goal + M_PI; + Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C); + t = compute_t(start.pose, goal.pose, Pose_C); + dir_robot_move_to_goal = false; + } + + if (t <= 1) + { + robot::log_error("[dock_calc] t(%f) <= 1, cannot calculate plan to dock.", t); + return false; + } + + // Calculate pose D and distance AD + geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t); + double distance_AD = pointToSegmentDistance(start.pose, goal.pose, Pose_C); + + // Calculate angle threshold with scaling + const double MAX_DISTANCE = 1.0, MIN_DISTANCE = 0.1; + const double MIN_SCALE = 0.2, MAX_SCALE = 1.0; + const double calib_factor_alpha = 0.5; + const double calib_factor_beta = 0.5; + const double calib_factor_gamma = 0.77; + + double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * + (MAX_DISTANCE - distance_AD) / (MAX_DISTANCE - MIN_DISTANCE); + double angle_threshold = std::min((calib_factor_alpha + fabs(scale)) * calib_factor_beta * M_PI, calib_factor_gamma * M_PI); + + // Generate temporary path + double distance_CD = calc_distance(Pose_C, Pose_D); + double angle_alpha = (M_PI - angle_threshold) / 2.0; + double distance_DF = distance_AD / tan(angle_alpha); + double distance_min_for_pose_F = distance_min_for_pose_C + distance_CD + distance_DF; + + geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F); + geometry_msgs::Pose mid_pose; + mid_pose.position.x = (Pose_C.position.x + Pose_F.position.x) / 2.0; + mid_pose.position.y = (Pose_C.position.y + Pose_F.position.y) / 2.0; + + vector control_points_tmp = {Pose_C, mid_pose, Pose_F}; + vector saved_poses_tmp; + + + if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) + { + robot::log_error("[dock_calc] Failed to find path with temporary control points."); + return false; + } + + // Find first segment control points + int dir; + vector control_points_1; + if (!findNURBSControlPoints(dir, control_points_1, start.pose, saved_poses_tmp, angle_threshold, degree)) + { + robot::log_error("[dock_calc] Failed to find NURBS control points for first segment."); + return false; + } + + // Determine reverse direction for first segment + bool first_segment_reverse = (dir == -1 && !dir_robot_move_to_goal) || + ((dir == 1 || dir == 0) && dir_robot_move_to_goal); + + // Generate first segment + vector first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); + planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end()); + + //Generate second segment (to goal) + if (!planMoveToDock.empty()) + { + geometry_msgs::Pose mid_control_point; + mid_control_point.position.x = (planMoveToDock.back().position.x + goal.pose.position.x) / 2.0; + mid_control_point.position.y = (planMoveToDock.back().position.y + goal.pose.position.y) / 2.0; + + vector control_points_2 = {planMoveToDock.back(), + mid_control_point, + goal.pose}; + + vector second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); + planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end()); + planMoveToDock.insert(planMoveToDock.begin(),start.pose); + planMoveToDock.insert(planMoveToDock.end(),goal.pose); + } + + return !planMoveToDock.empty(); + } + + bool DockCalc::findPathTMP(const vector& control_points, + vector& saved_poses, + const int& degree, double step) + { + // 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.position.x, cp.position.y, 0); + eigen_control_points.push_back(point); + } + // Tạo knot vector tự động + std::vector knot_vector; + 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); + } + + // Điều chỉnh weights để tăng ảnh hưởng của các control points + std::vector weights(control_points.size(), 1.0); + + input_spline_inf->knot_vector.clear(); + input_spline_inf->control_point.clear(); + input_spline_inf->weight.clear(); + + // Cài đặt thông số cho spline + CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_control_points, knot_vector); + CurveDesign->ReadSplineInf(input_spline_inf, weights, false); + + // vector poses_tmp; + for(double t = 0.0; t <= 1.0; t += step) + { + // robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t); + geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); + if(!std::isnan(point.x) && !std::isnan(point.y)) + { + // robot::log_error("[dock_calc][findPathTMP] point(%f, %f) at t(%f)", point.x, point.y, t); + geometry_msgs::Pose pose; + pose.position = point; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + + bool status_isFreeSpace = isFreeSpace(pose, footprint_robot_); + if(status_isFreeSpace) + { + saved_poses.push_back(pose); + } + else + { + robot::log_error("[dock_calc][findPathTMP] Obstacle detected at t(%f)", t); + break; + } + } + else + { + robot::log_error("[dock_calc][findPathTMP] NaN point at t(%f)", t); + } + } + if(saved_poses.empty()) + { + robot::log_error("[dock_calc][findPathTMP] No valid poses generated."); + return false; + } + return true; + } + + void DockCalc::updatePoseOrientation(std::vector& saved_poses, + std::vector& nurbs_path, + bool reverse) + { + double yaw; + // robot::log_error("[merge_path_calc] DEBUG: reverse(%x)", reverse); + if(!reverse) + yaw = calculateAngle(saved_poses[saved_poses.size() - 2].position.x, + saved_poses[saved_poses.size() - 2].position.y, + saved_poses.back().position.x, + saved_poses.back().position.y); + else + yaw = calculateAngle(saved_poses.back().position.x, + saved_poses.back().position.y, + saved_poses[saved_poses.size() - 2].position.x, + saved_poses[saved_poses.size() - 2].position.y); + + tf3::Quaternion q; + q.setRPY(0, 0, yaw); + saved_poses[saved_poses.size() - 2].orientation.x = q.x(); + saved_poses[saved_poses.size() - 2].orientation.y = q.y(); + saved_poses[saved_poses.size() - 2].orientation.z = q.z(); + saved_poses[saved_poses.size() - 2].orientation.w = q.w(); + + nurbs_path.push_back(saved_poses[saved_poses.size() - 2]); + } + + double DockCalc::calculateNURBSLength(CurveCommon* curve_design, + Spline_Inf* spline_inf, + int degree, + 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 1 (đường thẳng) thì bước nhảy bằng 0.1 + if(degree == 1) step = 0.1; + // Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step + else 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; + } + + bool DockCalc::findNURBSControlPoints(int& dir, vector& control_points, + const geometry_msgs::Pose& start_pose, + const std::vector& saved_poses, + const double& angle_threshold, const int& degree) + { + if (saved_poses.empty()) + { + robot::log_error("[dock_calc] No saved poses available to find NURBS control points."); + return false; + } + + control_points.clear(); + int idx = findNearestPointOnPath(dir, start_pose, saved_poses, angle_threshold); + + // Helper lambda to create pose at position + auto createPose = [](double x, double y, double yaw = 0.0) -> geometry_msgs::Pose + { + geometry_msgs::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.orientation = data_convert::createQuaternionMsgFromYaw(0); + return pose; + }; + + if (idx != -1) + { + start_target_idx_ = idx; + + // Add start and middle poses + control_points.push_back(start_pose); + control_points.push_back(createPose(saved_poses[idx].position.x, saved_poses[idx].position.y)); + + // Calculate distance from start to middle point + double dx = start_pose.position.x - saved_poses[idx].position.x; + double dy = start_pose.position.y - saved_poses[idx].position.y; + double target_distance = std::hypot(dx, dy); + + if (dir == 1) + { + // Find appropriate end point going backwards + int end_idx = idx; + const auto& mid_pose = control_points[1]; + + for (int i = idx; i >= 0; i--) + { + double dx_me = mid_pose.position.x - saved_poses[i].position.x; + double dy_me = mid_pose.position.y - saved_poses[i].position.y; + double dist_me = std::hypot(dx_me, dy_me); + + if (dist_me >= target_distance || i == 0) + { + end_idx = i; + break; + } + } + + control_points.push_back(createPose(saved_poses[end_idx].position.x, saved_poses[end_idx].position.y)); + start_target_idx_ = end_idx; + } + else if (dir == -1) + { + // robot::log_error("[dock_calc] DEBUG 200:"); + // Use last pose as end point + const auto& last_pose = saved_poses.back(); + control_points.push_back(createPose(last_pose.position.x, last_pose.position.y)); + start_target_idx_ = saved_poses.size() - 1; + } + + } + else + { + // robot::log_error("[dock_calc] DEBUG 300:"); + // Create midpoint between start and nearest saved pose + const auto& nearest_pose = saved_poses[store_start_nearest_idx_]; + auto mid_pose = createPose( + (start_pose.position.x + nearest_pose.position.x) / 2.0, + (start_pose.position.y + nearest_pose.position.y) / 2.0 + ); + + control_points.push_back(start_pose); + control_points.push_back(mid_pose); + control_points.push_back(nearest_pose); + // robot::log_error("[dock_calc] DEBUG 1: start_pose(%f,%f)",start_pose.position.x, start_pose.position.y); + // robot::log_error("[dock_calc] DEBUG 1: mid_pose(%f,%f)",mid_pose.position.x, mid_pose.position.y); + // robot::log_error("[dock_calc] DEBUG 1: nearest_pose(%f,%f)",nearest_pose.position.x, nearest_pose.position.y); + + start_target_idx_ = store_start_nearest_idx_; + } + + return true; + } + + int DockCalc::findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, + const std::vector& saved_poses, + const double& angle_threshol) + { + // Tìm điểm gần nhất + int nearest_idx = 0; + double min_dist = std::numeric_limits::max(); + for (int i = 0; i < (int)saved_poses.size(); ++i) + { + double distance = std::hypot(saved_poses[i].position.x - pose.position.x, saved_poses[i].position.y - pose.position.y); + + 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; + bool found_pose_flag_1 = false; + bool found_pose_flag_2 = false; + + for(int i = store_start_nearest_idx_; i >= 0; i--) + { + double angle = signedAngle(pose, saved_poses[i], saved_poses[i - 1]); + + if(fabs(angle) >= fabs(angle_threshol)) + { + nearest_idx = i; + found_pose_flag_1 = true; + break; + } + } + + double distance_1 = calc_distance(pose,saved_poses[nearest_idx]); + double distance_2 = calc_distance(saved_poses[0],saved_poses[nearest_idx]); + if(found_pose_flag_1 && (distance_2 >= distance_1)) + { + found_pose_flag_2 = true; + dir = 1; + } + + if(!found_pose_flag_2) + { + for(int i = store_start_nearest_idx_; i < (int)saved_poses.size(); i++) + { + double angle = signedAngle(pose, saved_poses[i], saved_poses[i + 1]); + + if(fabs(angle) >= (fabs(angle_threshol) - 0.05)) + { + nearest_idx = i; + dir = -1; + break; + } + if(i == (int)saved_poses.size() - 1) + { + dir = 0; + return -1; + } + } + double distance_1 = calc_distance(pose,saved_poses[nearest_idx]); + double distance_2 = calc_distance(saved_poses[nearest_idx],saved_poses.back()); + if(distance_2 + 0.1 < distance_1) + { + dir = 0; + return -1; + } + } + + return nearest_idx; + } + + bool DockCalc::isFreeSpace(const geometry_msgs::Pose& pose, + const std::vector& footprint) + { + // static std::vector free_zone; + + // auto makePose = [](double x, double y, double yaw) + // { + // geometry_msgs::Pose p; + // p.position.x = x; + // p.position.y = y; + // p.position.z = 0.0; + + // tf3::Quaternion q; + // q.setRPY(0.0, 0.0, yaw); + // p.orientation.x = q.x(); + // p.orientation.y = q.y(); + // p.orientation.z = q.z(); + // p.orientation.w = q.w(); + // return p; + // }; + // unsigned int mx, my; + + // if (!costmap_->worldToMap(pose.position.x, pose.position.y, mx, my)) + // { + // // robot::log_error("[dock_calc][isFreeSpace]Debug 1"); + // return false; // pose nằm ngoài bản đồ + // } + + // unsigned char center_cost = costmap_->getCost(mx, my); + // if (center_cost >= cost_threshold_) + // { + // // robot::log_error("[dock_calc][isFreeSpace]Debug 2"); + // return false; // chính giữa đã bị chiếm + // } + + // double resolution = costmap_->getResolution(); + + // if(check_goal_) + // { + // std::vector result_free_zone = calcFreeZone(pose, footprint); + // if((int)result_free_zone.size() < 4) return false; + // free_zone.push_back(makePose(result_free_zone[0].x, result_free_zone[0].y, 0.0)); + // free_zone.push_back(makePose(result_free_zone[1].x, result_free_zone[1].y, 0.0)); + // free_zone.push_back(makePose(result_free_zone[2].x, result_free_zone[2].y, 0.0)); + // check_goal_ = false; + // } + // std::vector space_footprint = interpolateFootprint(pose, footprint, resolution); + // for (int i = 0; i < (int)space_footprint.size(); i++) + // { + // geometry_msgs::Pose Pose_on_footprint; + // Pose_on_footprint.position.x = space_footprint[i].x; + // Pose_on_footprint.position.y = space_footprint[i].y; + // if((int)free_zone.size() < 3) return false; + // double t_L = compute_t(Pose_on_footprint, free_zone[0], free_zone[1]); + // double t_W = compute_t(Pose_on_footprint, free_zone[1], free_zone[2]); + // if(t_L >= 0 && t_L <= 1 && t_W >= 0 && t_W <= 1) + // { + // return true; + // } + // if (!costmap_->worldToMap(space_footprint[i].x, space_footprint[i].y, mx, my)) + // { + // // robot::log_error("[dock_calc][isFreeSpace]Debug 3"); + // return false; // pose nằm ngoài bản đồ + // } + // int check_x = mx; + // int check_y = my; + + // if (check_x >= 0 && check_x < costmap_->getSizeInCellsX() && + // check_y >= 0 && check_y < costmap_->getSizeInCellsY()) + // { + // unsigned char cost = costmap_->getCost(check_x, check_y); + // if (cost >= costmap_2d::LETHAL_OBSTACLE) + // { + // double dx = pose.position.x - space_footprint[i].x; + // double dy = pose.position.y - space_footprint[i].y; + // double dist = std::hypot(dx, dy); + // if (dist <= (safety_distance_/2.0)) + // { + // // robot::log_error("[dock_calc][isFreeSpace]Debug dist: %f", dist); + // return false; + // } + // } + // } + // } + + return true; + } + + std::vector DockCalc::calcFreeZone(const geometry_msgs::Pose& pose, + const std::vector& footprint) + { + // Dịch sang tọa độ tuyệt đối + std::vector free_zone; + double cos_th = std::cos(data_convert::getYaw(pose.orientation)); + double sin_th = std::sin(data_convert::getYaw(pose.orientation)); + for (auto pt : footprint) + { + pt.x *= 1.2; + pt.y *= 1.2; + + geometry_msgs::Point abs_pt; + abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th; + abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th; + free_zone.push_back(abs_pt); + } + return free_zone; + } + + std::vector DockCalc::interpolateFootprint(const geometry_msgs::Pose& pose, + const std::vector& footprint, const double& resolution) + { + + // Dịch sang tọa độ tuyệt đối + std::vector abs_footprint; + double cos_th = std::cos(data_convert::getYaw(pose.orientation)); + double sin_th = std::sin(data_convert::getYaw(pose.orientation)); + for (auto pt : footprint) + { + pt.x *= 1.1; + pt.y *= 1.1; + geometry_msgs::Point abs_pt; + abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th; + abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th; + abs_footprint.push_back(abs_pt); + } + + std::vector points; + for (size_t i = 0; i < (int)abs_footprint.size(); ++i) + { + const geometry_msgs::Point &start = abs_footprint[i]; + const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + + double dx = end.x - start.x; + double dy = end.y - start.y; + double distance = std::hypot(dx, dy); + int steps = std::max(1, static_cast(std::floor(distance / resolution))); + + for (int j = 0; j <= steps; ++j) + { + if (j == steps && i != (int)abs_footprint.size() - 1) + continue; // tránh lặp + double t = static_cast(j) / steps; + geometry_msgs::Point pt; + pt.x = start.x + t * dx; + pt.y = start.y + t * dy; + points.push_back(pt); + } + } + return points; + } + +} \ No newline at end of file diff --git a/src/dock_planner.cpp b/src/dock_planner.cpp new file mode 100644 index 0000000..93c25b7 --- /dev/null +++ b/src/dock_planner.cpp @@ -0,0 +1,124 @@ +#include "dock_planner/dock_planner.h" +#include +#include + +namespace dock_planner +{ + /** + * @brief Constructor mặc định + * Khởi tạo tất cả các pointer về nullptr và trạng thái chưa initialized + */ + DockPlanner::DockPlanner() : costmap_robot_(nullptr), + costmap_(nullptr), + initialized_(false), + check_free_space_(false), + cost_threshold_(200), + calib_safety_distance_(0.05) {} + + /** + * @brief Constructor với parameters + * @param name Tên của planner + * @param costmap_robot Pointer tới costmap ROS wrapper + * Tự động gọi initialize() nếu được cung cấp costmap + */ + DockPlanner::DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) + : costmap_robot_(nullptr), + costmap_(nullptr), + initialized_(false), + check_free_space_(false), + cost_threshold_(200), + calib_safety_distance_(0.05) + { + initialize(name, costmap_robot); + } + + /** + * @brief Khởi tạo global planner + * @param name Tên của planner instance + * @param costmap_robot Pointer tới costmap ROS wrapper + * + * Thiết lập: + * - Liên kết với costmap + * - Tạo publisher cho visualization + * - Load parameters từ ROS parameter server + * - Đánh dấu trạng thái initialized + */ + bool DockPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) + { + if (!initialized_) + { + costmap_robot_ = costmap_robot; + costmap_ = costmap_robot_->getCostmap(); + frame_id_ = costmap_robot_->getGlobalFrameID(); + calc_plan_to_dock_.costmap_ = costmap_robot_->getCostmap(); + + robot::NodeHandle private_nh("~/" + name); + + // Load parameters from the private namespace + private_nh.getParam("check_free_space", check_free_space_); + private_nh.getParam("cost_threshold", cost_threshold_); + private_nh.getParam("calib_safety_distance", calib_safety_distance_); + + calc_plan_to_dock_.cost_threshold_ = cost_threshold_; + calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_; + + + // plan_pub_ = private_nh.advertise("plan", 1); + + nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false); + + for(auto pt : footprint_dock.points) + { + geometry_msgs::Point footprint_point; + footprint_point.x = pt.x; + footprint_point.y = pt.y; + footprint_point.z = 0.0; + footprint_dock_.push_back(footprint_point); + } + + initialized_ = true; + // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_); + } + } + + bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + std::vector& plan) + { + if(!initialized_) return false; + if(!plan.empty()) plan.clear(); + vector planMoveToDock; + std::vector footprint_robot = costmap_robot_->getRobotFootprint(); + + // footprint_dock_ = costmap_robot_->getRobotFootprint(); + int degree = 2; + double step = 0.02; + bool status_makePlanMoveToDock = calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true); + robot::Time plan_time = robot::Time::now(); + + if(!status_makePlanMoveToDock) return false; + for(int i = 0; i < (int)planMoveToDock.size(); i++) + { + geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = planMoveToDock[i].position.x; + pose.pose.position.y = planMoveToDock[i].position.y; + pose.pose.position.z = 0; + pose.pose.orientation = planMoveToDock[i].orientation; + plan.push_back(pose); + } + publishPlan(plan); + return true; + } + + // void DockPlanner::publishPlan(const std::vector& plan) + // { + // nav_msgs::Path path_msg; + // path_msg.header.stamp = robot::Time::now(); + // path_msg.header.frame_id = frame_id_; + // path_msg.poses = plan; + // plan_pub_.publish(path_msg); + // } + +} // namespace dock_planner \ No newline at end of file diff --git a/src/utils/curve_common.cpp b/src/utils/curve_common.cpp new file mode 100644 index 0000000..b5bcfbe --- /dev/null +++ b/src/utils/curve_common.cpp @@ -0,0 +1,1279 @@ +#include "dock_planner/utils/curve_common.h" +#include "dock_planner/utils/conversion.h" +#include +#include + +CurveCommon::CurveCommon() +{ +} + +nav_msgs::Path CurveCommon::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 CurveCommon::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 CurveCommon::ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector file_discreate_point) +{ + int index = 0; + Eigen::Vector3d read_point; + EigenTrajectoryPoint eigen_discreate_point; + input_point->reserve(file_discreate_point.size() / 2); + + for (int i = 0; i < 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 CurveCommon::ReadDiscreate2DPointFromLaunch(std::vector> *input_point, std::vector file_discreate_point) +{ + Eigen::Vector3d read_point; + input_point->reserve(file_discreate_point.size() / 2); + + for (int i = 0; i < 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 CurveCommon::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(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < 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 CurveCommon::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(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < 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 CurveCommon::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(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < 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 CurveCommon::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(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < 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 CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) +{ + geometry_msgs::Point view_point; + for (int i = 0; i < 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 CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector> discreate_point) +{ + geometry_msgs::Point view_point; + for (int i = 0; i < 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 CurveCommon::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 CurveCommon::ReadSplineInf(Spline_Inf *spline_inf, std::vector weight_vector, bool use_limit_derivative_fitting) +{ + if (!use_limit_derivative_fitting) + { + if (spline_inf->control_point.size() != 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 : " << spline_inf->control_point.size() << "\n"; + // for(int i = 0; i < 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 CurveCommon::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 < 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 CurveCommon::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 < 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 CurveCommon::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_data, int differential_times) +{ + 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 + 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 CurveCommon::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 < 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 < 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 < 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 < 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 < 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 < 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 CurveCommon::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 CurveCommon::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 CurveCommon::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 CurveCommon::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 CurveCommon::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 CurveCommon::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 CurveCommon::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; +} + +bool CurveCommon::curveIsValid(int degree, const std::vector &knot_vector, + std::vector> &control_points) +{ + if (degree < 1 || degree > 9) + { + robot::log_warning("degree is invalid value"); + return false; + } + if (!((knot_vector.size() - degree - 1) == control_points.size())) + { + robot::log_warning("relation between degree, number of knots, and number of control points is invalid"); + return false; + } + return true; +} + +geometry_msgs::Point CurveCommon::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 = 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 < 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 < 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/utils/line_common.cpp b/src/utils/line_common.cpp new file mode 100644 index 0000000..bdc2218 --- /dev/null +++ b/src/utils/line_common.cpp @@ -0,0 +1,26 @@ +#include "dock_planner/utils/line_common.h" + +double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2) +{ + double A = y2 - y1; + double B = -(x2 - x1); + double C = -A * x1 - B * y1; + + return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B); +} + +bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance) +{ + double crossProduct = distanceFromPointToLine(x0, y0, x1, y1, x2, y2); + if (fabs(crossProduct) > fabs(tolerance)) + return false; + + // Kiểm tra tọa độ của P có nằm giữa A và B không + if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) && + y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2)) + { + return true; + } + + return false; +} \ No newline at end of file diff --git a/src/utils/pose.cpp b/src/utils/pose.cpp new file mode 100644 index 0000000..4d9701d --- /dev/null +++ b/src/utils/pose.cpp @@ -0,0 +1,39 @@ +#include "dock_planner/utils/pose.h" + + +bool vda_5050::Pose::SavePoseAsFile(const std::string& file_name) +{ + bool result = false; + std::ofstream file(file_name); + if (file.is_open()) + { + file << x_ << " " << y_ << " " << yaw_ << "\n"; + file.close(); + result = true; + } + else + { + result = false; + } + return result; +} + +bool vda_5050::Pose::LoadPoseFromFile(const std::string& file_name) +{ + bool result = false; + std::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; +} +