git commit -m "first commit for v2"

Controllers/Packages/amr_comunication/include/amr_comunication/opc_ua/ua_server.h

@@ -0,0 +1,197 @@
+/**
+ * @file amr_server.h
+ * @brief This file defines the AmrOpcUAServer class for managing an OPC UA server.
+ *
+ * The AmrOpcUAServer class provides functionalities to initialize, start, stop,
+ * and manage access control for an OPC UA server. It includes methods for
+ * handling user authentication and managing server operations.
+ *
+ * This library is part of the amr_comunication package, which is designed to
+ * facilitate the integration of ROS (Robot Operating System) with OPC UA
+ * for robotic applications.
+ *
+ * @note Ensure that the Open62541 library is properly linked and configured
+ *       to use this package effectively.
+ *
+ * @author HiepLM
+ * @date 16/10/2024
+ */
+
+#ifndef __AMR_COMUNICATION_UA_SERVER_H_INCLUDED_
+#define __AMR_COMUNICATION_UA_SERVER_H_INCLUDED_
+#include <ros/ros.h>
+#include <memory>
+#include <string>
+#include <iostream>
+#include <algorithm>
+#include <time.h>
+
+#include <thread>
+#include <chrono>
+
+#include <open62541/server.h>
+#include <open62541/server_config_default.h>
+#include <open62541/plugin/log_stdout.h>
+#include <open62541/plugin/accesscontrol_default.h>
+#include <signal.h>
+
+namespace amr_comunication
+{
+  class AmrOpcUAServer
+  {
+  public:
+    /**
+     * @brief Default constructor for AmrOpcUAServer
+     */
+    AmrOpcUAServer();
+
+    /**
+     * @brief Constructor for AmrOpcUAServer with ROS node handle
+     * @param nh ROS NodeHandle for initialization
+     */
+    AmrOpcUAServer(const ros::NodeHandle &nh);
+
+    /**
+     * @brief Virtual destructor for AmrOpcUAServer
+     */
+    virtual ~AmrOpcUAServer();
+
+    /**
+     * @brief Initialize the AmrOpcUAServer with a ROS node handle
+     * @param nh ROS NodeHandle for initialization
+     */
+    virtual void init(const ros::NodeHandle &nh);
+
+    /**
+     * @brief Stop the server operation.
+     *
+     * This method is responsible for safely shutting down the server,
+     * releasing any resources and stopping any ongoing processes.
+     */
+    virtual void stop(void);
+
+    /**
+     * @brief Start the server operation.
+     *
+     * This method initializes and starts the server, allowing it to
+     * begin processing requests from clients.
+     */
+    virtual void start(void);
+
+    /**
+     * @brief Get server object to redefine 
+     */
+    UA_Server *getServerObject();
+
+    /**
+     * @brief Get status code 
+     */
+    UA_StatusCode statusCode();
+
+    /**
+     * @brief Check server is running 
+     */
+    UA_Boolean isRunning() { return running_;}
+
+  private:
+    /**
+     * @brief Allow adding a node to the server.
+     *
+     * This method is called to determine if a node can be added to the server.
+     * It is part of the access control mechanism.
+     *
+     * @param server Pointer to the UA_Server instance.
+     * @param ac Pointer to the UA_AccessControl instance.
+     * @param sessionId Pointer to the session ID of the client.
+     * @param sessionContext Pointer to the session context.
+     * @param item Pointer to the item that contains information about the node to be added.
+     * @return UA_Boolean Returns UA_TRUE if the addition is allowed, otherwise UA_FALSE.
+     */
+    static UA_Boolean allowAddNode(UA_Server *server, UA_AccessControl *ac,
+                                   const UA_NodeId *sessionId, void *sessionContext,
+                                   const UA_AddNodesItem *item);
+
+    /**
+     * @brief Allow adding a reference to a node.
+     *
+     * This method is called to determine if a reference can be added to a node in the server.
+     * It is part of the access control mechanism.
+     *
+     * @param server Pointer to the UA_Server instance.
+     * @param ac Pointer to the UA_AccessControl instance.
+     * @param sessionId Pointer to the session ID of the client.
+     * @param sessionContext Pointer to the session context.
+     * @param item Pointer to the item that contains information about the reference to be added.
+     * @return UA_Boolean Returns UA_TRUE if the addition is allowed, otherwise UA_FALSE.
+     */
+    static UA_Boolean allowAddReference(UA_Server *server, UA_AccessControl *ac,
+                                        const UA_NodeId *sessionId, void *sessionContext,
+                                        const UA_AddReferencesItem *item);
+
+    /**
+     * @brief Allow deleting a node from the server.
+     *
+     * This method is called to determine if a node can be deleted from the server.
+     * It is part of the access control mechanism.
+     *
+     * @param server Pointer to the UA_Server instance.
+     * @param ac Pointer to the UA_AccessControl instance.
+     * @param sessionId Pointer to the session ID of the client.
+     * @param sessionContext Pointer to the session context.
+     * @param item Pointer to the item that contains information about the node to be deleted.
+     * @return UA_Boolean Returns UA_TRUE if the deletion is allowed, otherwise UA_FALSE.
+     */
+    static UA_Boolean allowDeleteNode(UA_Server *server, UA_AccessControl *ac,
+                                      const UA_NodeId *sessionId, void *sessionContext,
+                                      const UA_DeleteNodesItem *item);
+
+    /**
+     * @brief Allow deleting a reference from a node.
+     *
+     * This method is called to determine if a reference can be deleted from a node in the server.
+     * It is part of the access control mechanism.
+     *
+     * @param server Pointer to the UA_Server instance.
+     * @param ac Pointer to the UA_AccessControl instance.
+     * @param sessionId Pointer to the session ID of the client.
+     * @param sessionContext Pointer to the session context.
+     * @param item Pointer to the item that contains information about the reference to be deleted.
+     * @return UA_Boolean Returns UA_TRUE if the deletion is allowed, otherwise UA_FALSE.
+     */
+    static UA_Boolean allowDeleteReference(UA_Server *server, UA_AccessControl *ac,
+                                           const UA_NodeId *sessionId, void *sessionContext,
+                                           const UA_DeleteReferencesItem *item);
+
+    /**
+     * @brief Handle the stop signal.
+     *
+     * This method is called when a stop signal is received (e.g., SIGINT).
+     * It performs necessary cleanup and stops the server operation.
+     *
+     * @param sign The signal number received.
+     */
+    void stopHandler(int sign);
+
+    /**
+     * @brief Static signal handler for stopping the server.
+     *
+     * This static method is called when a stop signal is received (e.g., SIGINT).
+     * It invokes the instance method stopHandler to perform the cleanup.
+     *
+     * @param sign The signal number received.
+     */
+    static void staticSignalHandler(int sign);
+
+    bool initalized_;                               ///< Indicates whether the AmrOpcUAServer has been initialized.
+    static volatile UA_Boolean running_;            ///< Tracks the running state of the server; true if running, false if stopped.
+    UA_Server *server_ptr_;                         ///< Pointer to the UA_Server instance representing the OPC UA server.
+    UA_ServerConfig *config_;                       ///< Pointer to the server configuration (UA_ServerConfig).
+    UA_String url_;                                 ///< URL.
+    UA_StatusCode retval_;                          ///< Stores the return status code from server operations.
+    static std::function<void(int)> signalHandler_; ///< Static function to handle signals for stopping the server.
+    std::vector<UA_UsernamePasswordLogin> logins_;  ///< Vector containing user login credentials for the server.
+
+  }; // class AMRController
+} // namespace amr_comunication
+
+#endif // __AMR_COMUNICATION_UA_SERVER_H_INCLUDED_

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/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_

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/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_

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/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_

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/curve_common.h

@@ -0,0 +1,81 @@
+#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"
+
+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_

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/line_common.h

@@ -0,0 +1,14 @@
+#ifndef __LINE_COMMON_H_
+#define __LINE_COMMON_H_
+
+#include <ros/ros.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

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/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

Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/vda_5050_connector.h

@@ -0,0 +1,150 @@
+#ifndef _vda_5050_CLIENT_H_INCLUDED_
+#define _vda_5050_CLIENT_H_INCLUDED_
+#include <ros/ros.h>
+#include <mosquitto.h>
+#include <iostream>
+#include <nlohmann/json.hpp>
+#include <thread>
+#include <amr_comunication/vda_5050/utils/common.h>
+#include "amr_comunication/vda_5050/utils/curve_common.h"
+#include "amr_comunication/vda_5050/utils/pose.h"
+
+
+namespace amr_comunication
+{
+  class VDA5050Connector
+  {
+  public:
+    struct UserParams;
+
+    VDA5050Connector();
+    virtual ~VDA5050Connector();
+    
+    static std::unique_ptr<UserParams> userParams_;
+    static vda_5050::Order vda5050_order_;
+    static vda_5050::InstantAction vda5050_instant_action_;
+    static vda_5050::State vda5050_state_;
+    static vda_5050::Visualization vda5050_visualization_;
+    static std::mutex state_mutex; // Tạo mutex để bảo vệ vda5050_state_
+    static std::mutex visualization_mutex;// Tạo mutex để bảo vệ vda5050_visualization_
+    std::map<std::string ,std::shared_ptr<vda_5050::Action>> parallel_execution_list_;
+
+    /**
+     * @brief execute an Order
+     */
+    static std::function<void()> execute_order_;
+
+    /**
+     * @brief execute an instant action
+     */
+    static std::function<void()> execute_instant_action_;
+
+    /**
+     * @brief execute an parallel action
+     */
+    static std::function<void()> execute_parallel_action_;
+
+    /**
+     * @brief execute an velocity max action
+     */
+    static std::function<void(double)> velocity_max_action_;
+
+    /**
+     * @brief execute an angular max action
+     */
+    static std::function<void(double)> angular_max_action_;
+
+  private:
+
+    void mqtt_init();
+
+    void mqtt_cleanup();
+
+    void mqtt_connect();
+
+    void mqtt_reconnect(const std::string& new_host, int new_port);
+
+    std::string getCurrentTimestamp();
+
+    static void mqtt_on_connect_callback(struct mosquitto* mosq, void* userdata, int result);
+
+    static void mqtt_on_disconnect_callback(struct mosquitto* mosq, void* userdata, int rc);
+
+    static void mqtt_message_callback(struct mosquitto* mosq, void* userdata, const struct mosquitto_message* message);
+    
+    void publishMqttTask();
+
+    void updatingProgress();
+
+    void stateInProgress();
+
+    void orderMsgStream(vda_5050::Order order);
+
+    bool ParseOrderMsg(const std::string &payload, vda_5050::Order &order);
+
+    bool ParseInstantActionMsg(const std::string& payload, vda_5050::InstantAction &instantAction);
+
+    nlohmann::json vda5050VisualizationToJson(const vda_5050::Visualization& visualization);
+
+    nlohmann::json vda5050StateToJson(const vda_5050::State& state);
+
+    template <typename T> bool checkObjectStructure(nlohmann::json json, std::string obj);
+
+    static bool acceptingOrderOrOrderUpdate(const vda_5050::Order &received_order, vda_5050::Order &current_order);
+
+    static bool acceptingInstantActions(const vda_5050::InstantAction &received_instantAction, vda_5050::InstantAction &current_instantAction);
+
+    static bool isTheVehicleStillExecuting(); 
+
+    static bool isTheVehicleWaitingAnUpdate();
+
+    static bool isAGVOnNodeOrInDeviationRange();
+    
+    static bool isAGVOnNodeOrInDeviationRangeOfNewOrder(vda_5050::Order new_order);
+
+    static void populateNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state);
+
+    static void appendNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state);
+
+    static void populateEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state);
+
+    static void appendEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state);
+
+    static void populateActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state);
+
+    static void appendActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state);
+
+    static void actionsHandle();
+    // properties
+    struct mosquitto* mosq_{ nullptr };
+    std::thread publish_mqtt_thread_;
+
+  public:
+
+    // global variables
+    struct UserParams
+    {
+      UserParams() {
+        mqtt_hostname_ = "0.0.0.0";
+        mqtt_port_ = 1883;
+        mqtt_keepalive_ = 60;
+      }
+      void print_params()
+      {
+        std::cout << "[amr_control_node][Param] mqtt_hostname_: " << mqtt_hostname_ << std::endl;
+        std::cout << "[amr_control_node][Param] mqtt_port_: " << mqtt_port_ << std::endl;
+        std::cout << "[amr_control_node][Param] mqtt_keepalive_: " << mqtt_keepalive_ << std::endl;
+      }
+      std::string name_;
+      std::string mqtt_hostname_;
+      std::string mqtt_client_id_;
+      std::string user_name_;
+      std::string password_;
+      int mqtt_port_;
+      int mqtt_keepalive_;
+    };
+  };
+} // namespace amr_comunication
+
+
+#endif // _vda_5050_CLIENT_H_INCLUDED_