first commit

include/dock_planner/dock_calc.h

@@ -0,0 +1,147 @@
+#ifndef DOCK_CALC_H
+#define DOCK_CALC_H
+
+#include <nav_2d_utils/footprint.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <nav_msgs/Path.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/GetPlan.h>
+
+#include <costmap_2d/costmap_2d_robot.h>
+#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/inflation_layer.h>
+
+#include <tf3/LinearMath/Quaternion.h>
+
+#include "dock_planner/utils/curve_common.h"
+#include "dock_planner/utils/pose.h"
+#include "dock_planner/utils/common.h"
+#include <robot/console.h>
+
+#include <vector>
+
+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<geometry_msgs::Pose> posesOnPathWay;
+        vector<geometry_msgs::Point> footprint_robot_;
+        geometry_msgs::PoseStamped save_goal_;
+        // std::vector<geometry_msgs::Pose> 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<geometry_msgs::Pose>& saved_poses,
+                                    std::vector<geometry_msgs::Pose>& nurbs_path,
+                                    bool reverse = true);
+
+        bool findPathTMP(const vector<geometry_msgs::Pose>& control_points,
+                        vector<geometry_msgs::Pose>& saved_poses,                
+                        const int& degree, double step = 0.02);
+
+        bool findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points,
+                                    const geometry_msgs::Pose& start_pose,
+                                    const std::vector<geometry_msgs::Pose>& posesOnPathWay,
+                                    const double& angle_threshol, const int& degree);
+
+        int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, 
+                                const std::vector<geometry_msgs::Pose>& posesOnPathWay, 
+                                const double& angle_threshol);
+
+        bool isFreeSpace(const geometry_msgs::Pose& pose, const std::vector<geometry_msgs::Point>& footprint);
+
+        std::vector<geometry_msgs::Point> calcFreeZone(const geometry_msgs::Pose& pose, 
+                                    const std::vector<geometry_msgs::Point>& footprint);
+
+        std::vector<geometry_msgs::Point> interpolateFootprint(const geometry_msgs::Pose& pose, 
+                const std::vector<geometry_msgs::Point>& footprint, const double& resolution);
+
+        RobotGeometry computeGeometry(const std::vector<geometry_msgs::Point>& 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<geometry_msgs::Point>& footprint_robot,
+                                const vector<geometry_msgs::Point>& footprint_dock,
+                                const int& degree, const double& step,
+                                vector<geometry_msgs::Pose>& planMoveToDock,
+                                bool reverse = false);
+        
+        
+    };
+
+}//namespace dock_planner
+
+
+#endif

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 <nav_core/base_global_planner.h>
+
+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<geometry_msgs::PoseStamped>& 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<geometry_msgs::Point> footprint_dock_;
+
+        int cost_threshold_;
+        double calib_safety_distance_;
+        bool check_free_space_;
+
+        DockCalc calc_plan_to_dock_;
+        
+
+        void publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan);
+    };
+} // namespace dock_planner
+
+#endif

include/dock_planner/utils/color.h

@@ -0,0 +1,34 @@
+#ifndef COLOR_H_
+#define COLOR_H_
+
+#include <std_msgs/ColorRGBA.h>
+
+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_

include/dock_planner/utils/common.h

@@ -0,0 +1,472 @@
+#ifndef _VDA_5050_COMMON_H_INCLUDE_
+#define _VDA_5050_COMMON_H_INCLUDE_
+#include <iostream>
+#include <vector>
+
+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<ActionParameter> actionParameters;
+  };
+
+  struct Node
+  {
+    std::string nodeId;
+    int sequenceId;
+    std::string nodeDescription;
+    bool released;
+    NodePosition nodePosition;
+    std::vector<std::shared_ptr<Action>> actions;
+  };
+
+  struct Corridor
+  {
+    double leftWidth;
+    double rightWidth;
+    std::string corridorRefPoint;
+  };
+
+  struct ControlPoint
+  {
+    double x, y, weight;
+  };
+
+  struct Trajectory
+  {
+    int degree;
+    std::vector<double> knotVector;
+    std::vector<ControlPoint> 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<Action> 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<Node> nodes;
+    std::vector<Edge> 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<std::shared_ptr<Action>> 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<ErrorReference> 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<InfoReference> 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<Error> 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<NodeState> nodeStates;
+    std::vector<EdgeState> edgeStates;
+    std::vector<std::shared_ptr<vda_5050::ActionState>> actionStates;
+    AGVPosition agvPosition;
+    Velocity velocity;
+    std::vector<Load> loads;
+    BatteryState batteryState;
+    std::vector<Error> errors;
+    std::vector<Info> 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_

include/dock_planner/utils/conversion.h

@@ -0,0 +1,43 @@
+#ifndef CONVERSION_H_
+#define CONVERSION_H_
+#include "curve_common.h"
+
+#include <Eigen/Geometry>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Quaternion.h>
+
+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<geometry_msgs::PoseStamped> &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<geometry_msgs::Point> &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_

include/dock_planner/utils/curve_common.h

@@ -0,0 +1,82 @@
+#ifndef CURVE_COMMON_H_
+#define CURVE_COMMON_H_
+
+#include <Eigen/Eigen>
+#include <nav_msgs/Path.h>
+#include <geometry_msgs/Point.h>
+#include <visualization_msgs/Marker.h>
+#include "color.h"
+#include <robot/console.h>
+
+struct Spline_Inf
+{
+  int order;
+  std::vector<double> knot_vector;
+  std::vector<double> weight;
+  //std::vector<double> N;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > N;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > dN;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > ddN;
+  std::vector<double> N_double_vec;
+  std::vector<double> R_double_vec;
+  std::vector<double> dR_double_vec;
+  std::vector<double> ddR_double_vec;
+  // std::vector<double> ddN_double_vec;
+  //Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, Eigen::Dynamic> N; //bsaic function
+  Eigen::MatrixXd curvefit_N;
+
+};
+
+struct EigenTrajectoryPoint
+{
+  typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint> > 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<double>& knot_vector,
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
+
+  void ReadSplineInf(Spline_Inf* bspline_inf, int order, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point, std::vector<double> knot_vector);
+  void ReadSplineInf(Spline_Inf* bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
+  void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector* input_point, std::vector<double> file_discreate_point);
+  void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >* input_point, std::vector<double> file_discreate_point);
+  void ShowDiscreatePoint(visualization_msgs::Marker* points, EigenTrajectoryPoint::Vector discreate_point);
+  void ShowDiscreatePoint(visualization_msgs::Marker* points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > 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<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
+  visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
+private:
+
+
+};
+
+#endif //CURVE_COMMON_H_

include/dock_planner/utils/line_common.h

@@ -0,0 +1,13 @@
+#ifndef __LINE_COMMON_H_
+#define __LINE_COMMON_H_
+
+#include <cmath>
+
+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

include/dock_planner/utils/pose.h

@@ -0,0 +1,66 @@
+#ifndef AMR_CONTROL_POSE_H
+#define AMR_CONTROL_POSE_H
+#include <stdio.h>
+#include <iostream>
+#include <stdlib.h>
+#include <unistd.h>
+#include <vector>
+#include <fstream>
+#include <math.h>
+#include <cmath>
+
+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