
#include <chrono>
#include <thread>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_msgs/Twist2D.h>
#include <angles/angles.h>

// pnkx core
#include <robot/robot.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_msgs/Twist2D.h>

#include "amr_control/amr_opc_ua_server_api.h"
#include "nova5_control/imr_nova_control.h"

#define STATUS_SUCCESSED 1
#define STATUS_ERROR -1

amr_control::OpcUAServerAPI::OpcUAServerAPI(const ros::NodeHandle &nh, std::shared_ptr<robot::move_base_core::BaseNavigation> move_base,
                                            std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<AmrMonitor> monitor)
{
  this->nh_ = nh;
  amr_control::OpcUAServerAPI::monitor_ptr_ = monitor;
  amr_control::OpcUAServerAPI::move_base_ptr_ = move_base;
  amr_control::OpcUAServerAPI::loc_base_ptr_ = loc_base;

  ros::NodeHandle nh_node;
  amr_control::OpcUAServerAPI::init_pub_ = nh_node.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);
}

amr_control::OpcUAServerAPI::~OpcUAServerAPI()
{
  pthread_join(amr_control::OpcUAServerAPI::hThread_, NULL);
  if (amr_control::OpcUAServerAPI::arm_thread_ptr_)
  {
    delete (amr_control::OpcUAServerAPI::arm_thread_ptr_);
  }
  if (amr_control::OpcUAServerAPI::belt_thread_ptr_)
  {
    delete (amr_control::OpcUAServerAPI::belt_thread_ptr_);
  }
  if (amr_control::OpcUAServerAPI::server_ptr_)
  {
    amr_control::OpcUAServerAPI::server_ptr_->stop();
    server_ptr_.reset();
  }
}

void amr_control::OpcUAServerAPI::start()
{
  amr_control::OpcUAServerAPI::server_ptr_ = std::make_shared<amr_comunication::AmrOpcUAServer>();
  server_ptr_->init(this->nh_);
  this->defineObjects();
  server_ptr_->start();
}

void amr_control::OpcUAServerAPI::defineObjects()
{
  do
  {
    sigset_t set;
    sigemptyset(&set);
    sigaddset(&set, SIGINT);
    pthread_sigmask(SIG_BLOCK, &set, NULL);
    pthread_create(&amr_control::OpcUAServerAPI::hThread_, NULL, &amr_control::OpcUAServerAPI::ThreadWorker, &set);
  } while (0);

  UA_NodeId slam_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes slamAttr = UA_ObjectAttributes_default;
  slamAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SLAM");
  UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"SLAM"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          slamAttr, NULL, &slam_Id);

  // amr_control::OpcUAServerAPI::addStartMappingMethod(this->server_ptr_->getServerObject(), slam_Id);
  // amr_control::OpcUAServerAPI::addStopMappingMethod(this->server_ptr_->getServerObject(), slam_Id);
  // amr_control::OpcUAServerAPI::addStartLocalizationMethod(this->server_ptr_->getServerObject(), slam_Id);
  // amr_control::OpcUAServerAPI::addStopLocalizationMethod(this->server_ptr_->getServerObject(), slam_Id);
  amr_control::OpcUAServerAPI::addListMapFilesMethod(this->server_ptr_->getServerObject(), slam_Id);
  amr_control::OpcUAServerAPI::addActivateMapMethod(this->server_ptr_->getServerObject(), slam_Id);
  amr_control::OpcUAServerAPI::addRobotPoseProperty(this->server_ptr_->getServerObject(), slam_Id);
  amr_control::OpcUAServerAPI::addSetInitialPoseMethod(this->server_ptr_->getServerObject(), slam_Id);

  UA_NodeId nav_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes navAttr = UA_ObjectAttributes_default;
  navAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Navigation");
  UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"Navigation"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          navAttr, NULL, &nav_Id);

  amr_control::OpcUAServerAPI::addMoveToNodeMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addDockToNodeMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addRotateToMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addMoveStraightMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addCancelMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addPauseMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addResumeMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addNavigationState(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addResetMethod(this->server_ptr_->getServerObject(), nav_Id);
  amr_control::OpcUAServerAPI::addWriteMutedMethod(this->server_ptr_->getServerObject(), nav_Id);

  UA_NodeId monitor_ptr_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes monitorAttr = UA_ObjectAttributes_default;
  monitorAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Monitoring");
  UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"Monitoring"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          monitorAttr, NULL, &monitor_ptr_Id);

  amr_control::OpcUAServerAPI::addVelocityCommand(this->server_ptr_->getServerObject(), monitor_ptr_Id);

  UA_NodeId amr_plus_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes amr_plusAttr = UA_ObjectAttributes_default;
  amr_plusAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"IMR+");
  UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"IMR+"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          amr_plusAttr, NULL, &amr_plus_Id);

  UA_NodeId dobot_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes dobotAttr = UA_ObjectAttributes_default;
  dobotAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot");
  UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, amr_plus_Id,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"Dobot"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          dobotAttr, NULL, &dobot_Id);

  amr_control::OpcUAServerAPI::addPickUpDobotMethod(this->server_ptr_->getServerObject(), dobot_Id);
  amr_control::OpcUAServerAPI::addPowerOnDobotMethod(this->server_ptr_->getServerObject(), dobot_Id);
  amr_control::OpcUAServerAPI::addContinueDobotMethod(this->server_ptr_->getServerObject(), dobot_Id);
  amr_control::OpcUAServerAPI::addGoHomeDobotMethod(this->server_ptr_->getServerObject(), dobot_Id);
  amr_control::OpcUAServerAPI::addCancelDobotMethod(this->server_ptr_->getServerObject(), dobot_Id);
  amr_control::OpcUAServerAPI::addCountMethod(this->server_ptr_->getServerObject(), dobot_Id);
  amr_control::OpcUAServerAPI::addDobotProperties(this->server_ptr_->getServerObject(), dobot_Id);

  UA_NodeId conveyor_belt_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes conveyor_beltAttr = UA_ObjectAttributes_default;
  conveyor_beltAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Conveyor Belts");
  UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, amr_plus_Id,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"Conveyor Belts"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          conveyor_beltAttr, NULL, &conveyor_belt_Id);

  amr_control::OpcUAServerAPI::addunLoadMethod(this->server_ptr_->getServerObject(), conveyor_belt_Id);
  amr_control::OpcUAServerAPI::addLoadMethod(this->server_ptr_->getServerObject(), conveyor_belt_Id);
  amr_control::OpcUAServerAPI::addResetConveyorBeltMethod(this->server_ptr_->getServerObject(), conveyor_belt_Id);
  amr_control::OpcUAServerAPI::addConveyorBeltState(this->server_ptr_->getServerObject(), conveyor_belt_Id);
}

void amr_control::OpcUAServerAPI::addStartMappingMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Bắt đầu quá trình tạo bản đồ ");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartMapping");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"StartMapping"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"StartMapping"),
                          incAttr, &amr_control::OpcUAServerAPI::startMappingCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::startMappingCallBack(UA_Server *server,
                                                                const UA_NodeId *sessionId, void *sessionContext,
                                                                const UA_NodeId *methodId, void *methodContext,
                                                                const UA_NodeId *objectId, void *objectContext,
                                                                size_t inputSize, const UA_Variant *input,
                                                                size_t outputSize, UA_Variant *output)
{

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu tạo bản đồ  gọi thành công");

  std::thread([=]()
              {
      if (amr_control::OpcUAServerAPI::loc_base_ptr_)
        amr_control::OpcUAServerAPI::loc_base_ptr_->startMapping(); })
      .detach();

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ  gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ  bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addStopMappingMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Kết thúc quá trình tạo bản đồ ");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopMapping");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"StopMapping"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"StopMapping"),
                          incAttr, &amr_control::OpcUAServerAPI::stopMappingCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::stopMappingCallBack(UA_Server *server,
                                                               const UA_NodeId *sessionId, void *sessionContext,
                                                               const UA_NodeId *methodId, void *methodContext,
                                                               const UA_NodeId *objectId, void *objectContext,
                                                               size_t inputSize, const UA_Variant *input,
                                                               size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình tạo bản đồ gọi thành công");

  std::thread([=]()
              {
      if (amr_control::OpcUAServerAPI::loc_base_ptr_)
        amr_control::OpcUAServerAPI::loc_base_ptr_->stopMapping(); })
      .detach();

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Bắt đầu quá trình định vị bản đồ ");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartLocalization");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"StartLocalization"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"StartLocalization"),
                          incAttr, &amr_control::OpcUAServerAPI::startLocalizationCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::startLocalizationCallBack(UA_Server *server,
                                                                     const UA_NodeId *sessionId, void *sessionContext,
                                                                     const UA_NodeId *methodId, void *methodContext,
                                                                     const UA_NodeId *objectId, void *objectContext,
                                                                     size_t inputSize, const UA_Variant *input,
                                                                     size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ  gọi thành công");

  if (amr_control::OpcUAServerAPI::loc_base_ptr_)
    amr_control::OpcUAServerAPI::loc_base_ptr_->startLocalization();
  else
  {
    message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ  bị lỗi");
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ  gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ  bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Kết thúc quá trình định vị bản đồ ");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopLocalization");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"StopLocalization"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"StopLocalization"),
                          incAttr, &amr_control::OpcUAServerAPI::stopLocalizationCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::stopLocalizationCallBack(UA_Server *server,
                                                                    const UA_NodeId *sessionId, void *sessionContext,
                                                                    const UA_NodeId *methodId, void *methodContext,
                                                                    const UA_NodeId *objectId, void *objectContext,
                                                                    size_t inputSize, const UA_Variant *input,
                                                                    size_t outputSize, UA_Variant *output)
{

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ gọi thành công");

  if (amr_control::OpcUAServerAPI::loc_base_ptr_)
    amr_control::OpcUAServerAPI::loc_base_ptr_->stopLocalization();
  else
  {
    message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ  bị lỗi");
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addListMapFilesMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[1];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"List files");
  outputArguments[0].name = UA_STRING((char *)"List files");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Liệt kê các file bản đồ.");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GetMapFiles");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"GetMapFiles"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"GetMapFiles"),
                          incAttr, &amr_control::OpcUAServerAPI::listMapFilesCallBack,
                          0, NULL, 1, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::listMapFilesCallBack(UA_Server *server,
                                                                const UA_NodeId *sessionId, void *sessionContext,
                                                                const UA_NodeId *methodId, void *methodContext,
                                                                const UA_NodeId *objectId, void *objectContext,
                                                                size_t inputSize, const UA_Variant *input,
                                                                size_t outputSize, UA_Variant *output)
{

  UA_Variant_init(output);
  UA_String message = UA_STRING_ALLOC((char *)"");
  std::stringstream result;
  if (amr_control::OpcUAServerAPI::loc_base_ptr_)
  {
    amr_control::OpcUAServerAPI::loc_base_ptr_->listMapFiles(result);
    ROS_INFO_STREAM(result.str());
    message = UA_STRING_ALLOC((char *)result.str().c_str());
  }

  UA_Variant_setScalarCopy(output, &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Hoàn thành");
  return UA_STATUSCODE_GOOD;
}

void amr_control::OpcUAServerAPI::addActivateMapMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One input argument */
  UA_Argument inputArguments[1];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MapName");
  inputArguments[0].name = UA_STRING((char *)"MapName");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chọn một map file để định vị bản đồ ");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ActivateMap");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"ActivateMap"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"ActivateMap"),
                          incAttr, &amr_control::OpcUAServerAPI::activateMapCallBack,
                          1, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::activateMapCallBack(UA_Server *server,
                                                               const UA_NodeId *sessionId, void *sessionContext,
                                                               const UA_NodeId *methodId, void *methodContext,
                                                               const UA_NodeId *objectId, void *objectContext,
                                                               size_t inputSize, const UA_Variant *input,
                                                               size_t outputSize, UA_Variant *output)
{

  UA_String map_name = *(UA_String *)input[0].data;

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ  gọi thành công");
  bool result;
  if (amr_control::OpcUAServerAPI::loc_base_ptr_)
  {
    std::string map_name_str = std::string(reinterpret_cast<char *>(map_name.data), map_name.length);
    if (!map_name_str.empty())
      result = amr_control::OpcUAServerAPI::loc_base_ptr_->changeStaticMap(map_name_str);
    else
      message = UA_STRING_ALLOC((char *)"MapName chưa có thông tin");
  }

  if (!result)
  {
    message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ  bị lỗi");
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ  gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ  bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID)
{
  UA_Argument inputArguments[3];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X");
  inputArguments[0].name = UA_STRING((char *)"X");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[1]);
  inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y");
  inputArguments[1].name = UA_STRING((char *)"Y");
  inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[2]);
  inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"YAW");
  inputArguments[2].name = UA_STRING((char *)"YAW");
  inputArguments[2].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[2].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Set vị trí ban đầu của robot trên bản đồ.");

  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Initial Pose");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Initial Pose"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Initial Pose"),
                          incAttr, &amr_control::OpcUAServerAPI::setInitialPoseCallBack,
                          3, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::setInitialPoseCallBack(UA_Server *server,
                                                                  const UA_NodeId *sessionId, void *sessionContext,
                                                                  const UA_NodeId *methodId, void *methodContext,
                                                                  const UA_NodeId *objectId, void *objectContext,
                                                                  size_t inputSize, const UA_Variant *input,
                                                                  size_t outputSize, UA_Variant *output)
{

  UA_Double X = *(UA_Double *)input[0].data;
  UA_Double Y = *(UA_Double *)input[1].data;
  UA_Double Yaw = *(UA_Double *)input[2].data;

  std::string fixed_frame = "map";
  geometry_msgs::PoseWithCovarianceStamped pose;
  pose.header.frame_id = fixed_frame;
  pose.header.stamp = ros::Time::now();

  // set x,y coord
  pose.pose.pose.position.x = X;
  pose.pose.pose.position.y = Y;
  pose.pose.pose.position.z = 0.0;

  // set theta
  tf::Quaternion quat;
  quat.setRPY(0.0, 0.0, Yaw);
  tf::quaternionTFToMsg(quat, pose.pose.pose.orientation);

  pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5;
  pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5;
  pose.pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0;

  amr_control::OpcUAServerAPI::init_pub_.publish(pose);

  UA_Variant_init(output);

  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Initial Pose gọi thành công");

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose bị lỗi");
  return UA_STATUSCODE_BAD;
}

UA_NodeId *amr_control::OpcUAServerAPI::robotPoseX_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::robotPoseY_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::robotPoseYaw_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::currentActiveMap_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::slamState_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::workingDirectory_Id_ = new UA_NodeId();
void amr_control::OpcUAServerAPI::addRobotPoseProperty(UA_Server *server, UA_NodeId parentID)
{
  UA_NodeId robotPose_Id; /* get the nodeid assigned by the server */
  UA_ObjectAttributes slamAttr = UA_ObjectAttributes_default;
  slamAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RobotPose");
  UA_Server_addObjectNode(server, UA_NODEID_NULL, parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
                          UA_QUALIFIEDNAME(1, (char *)"RobotPose"),
                          UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
                          slamAttr, NULL, &robotPose_Id);

  // UA_NodeId robotPoseX_Id_;
  UA_VariableAttributes poseXAttr = UA_VariableAttributes_default;
  UA_Double pose_x = 0;
  UA_Variant_setScalar(&poseXAttr.value, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]);
  poseXAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X");
  poseXAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ X của robot.");
  poseXAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"X"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            poseXAttr, NULL, amr_control::OpcUAServerAPI::robotPoseX_Id_);

  // UA_NodeId robotPoseY_Id_;
  UA_VariableAttributes poseYAttr = UA_VariableAttributes_default;
  UA_Double pose_y = 0;
  UA_Variant_setScalar(&poseYAttr.value, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]);
  poseYAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y");
  poseYAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Y của robot.");
  poseYAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Y"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            poseYAttr, NULL, amr_control::OpcUAServerAPI::robotPoseY_Id_);

  // UA_NodeId robotPoseYaw_Id_;
  UA_VariableAttributes poseYawAttr = UA_VariableAttributes_default;
  UA_Double pose_yaw = 0;
  UA_Variant_setScalar(&poseYawAttr.value, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]);
  poseYawAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Yaw");
  poseYawAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Yaw của robot.");
  poseYawAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Yaw"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            poseYawAttr, NULL, amr_control::OpcUAServerAPI::robotPoseYaw_Id_);

  // UA_NodeId currentActiveMap_Id;
  UA_VariableAttributes currentActiveMapAttr = UA_VariableAttributes_default;
  UA_String current_active_map = UA_STRING_ALLOC((char *)" ");
  UA_Variant_setScalar(&currentActiveMapAttr.value, &current_active_map, &UA_TYPES[UA_TYPES_STRING]);
  currentActiveMapAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CurrentActiveMap");
  currentActiveMapAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến lưu trữ thông tin về map đang được active.");
  currentActiveMapAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            currentActiveMapAttr, NULL, amr_control::OpcUAServerAPI::currentActiveMap_Id_);

  // UA_NodeId slamState_Id_;
  UA_VariableAttributes slamStateAttr = UA_VariableAttributes_default;
  UA_String slam_state = UA_STRING_ALLOC((char *)" ");
  UA_Variant_setScalar(&slamStateAttr.value, &slam_state, &UA_TYPES[UA_TYPES_STRING]);
  slamStateAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SlamState");
  slamStateAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến trạng thái của module slam: Mapping, Localization, Calibrations, Ready, Error");
  slamStateAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            slamStateAttr, NULL, amr_control::OpcUAServerAPI::slamState_Id_);

  // UA_NodeId workingDirectory_Id_
  UA_VariableAttributes workingDirectoryAttr = UA_VariableAttributes_default;
  UA_String working_directory = UA_STRING_ALLOC((char *)" ");
  UA_Variant_setScalar(&workingDirectoryAttr.value, &working_directory, &UA_TYPES[UA_TYPES_STRING]);
  workingDirectoryAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"WorkingDirectories");
  workingDirectoryAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Đường dẫn lưu trữ các file bản đồ");
  workingDirectoryAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"WorkingDirectories"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            workingDirectoryAttr, NULL, amr_control::OpcUAServerAPI::workingDirectory_Id_);
}

void amr_control::OpcUAServerAPI::slamHandle()
{
  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr)
  {
    robot_geometry_msgs::Pose2D robot_pose;
    if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(robot_pose))
    {
      UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "getRobotPose: Is Not OKAY");
    }
    else
    {
      UA_Double pose_x = std::round(robot_pose.x * 10000.0) / 10000.0;
      UA_Double pose_y = std::round(robot_pose.y * 10000.0) / 10000.0;
      UA_Double pose_yaw = std::round(robot_pose.theta * 10000.0) / 10000.0;

      if (amr_control::OpcUAServerAPI::robotPoseX_Id_ != NULL &&
          amr_control::OpcUAServerAPI::robotPoseY_Id_ != NULL &&
          amr_control::OpcUAServerAPI::robotPoseYaw_Id_ != NULL &&
          amr_control::OpcUAServerAPI::currentActiveMap_Id_ != NULL &&
          amr_control::OpcUAServerAPI::slamState_Id_ != NULL &&
          amr_control::OpcUAServerAPI::workingDirectory_Id_ != NULL)
      {
        // Cập nhật giá trị X
        UA_Variant poseXVariant;
        UA_Variant_setScalar(&poseXVariant, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]);
        UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseX_Id_, poseXVariant); // Truyền đối tượng, không phải con trỏ

        // Cập nhật giá trị Y
        UA_Variant poseYVariant;
        UA_Variant_setScalar(&poseYVariant, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]);
        UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseY_Id_, poseYVariant); // Truyền đối tượng, không phải con trỏ

        // Cập nhật giá trị Yaw
        UA_Variant poseYawVariant;
        UA_Variant_setScalar(&poseYawVariant, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]);
        UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseYaw_Id_, poseYawVariant);

        if (amr_control::OpcUAServerAPI::loc_base_ptr_)
        {
          // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str());
          // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str());
          UA_Variant currentActiveMapVariant;
          UA_String activated_map_filename = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str());
          UA_Variant_setScalar(&currentActiveMapVariant, &activated_map_filename, &UA_TYPES[UA_TYPES_STRING]);
          UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::currentActiveMap_Id_, currentActiveMapVariant);

          UA_Variant workingDirectoryvariant;
          UA_String working_dir = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str());
          UA_Variant_setScalar(&workingDirectoryvariant, &working_dir, &UA_TYPES[UA_TYPES_STRING]);
          UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::workingDirectory_Id_, workingDirectoryvariant);

          UA_String slam_state = UA_STRING_ALLOC((char *)" ");
          switch (amr_control::OpcUAServerAPI::loc_base_ptr_->getState())
          {
          case loc_core::Mapping:
            slam_state = UA_STRING_ALLOC((char *)"Mapping");
            break;
          case loc_core::Localization:
            slam_state = UA_STRING_ALLOC((char *)"Localization");
            break;
          case loc_core::Calibrations:
            slam_state = UA_STRING_ALLOC((char *)"Calibrations");
            break;
          case loc_core::Ready:
            slam_state = UA_STRING_ALLOC((char *)"Ready");
            break;
          case loc_core::Error:
            slam_state = UA_STRING_ALLOC((char *)"Error");
            break;
          default:
            break;
          }
          UA_Variant slamStatevariant;
          UA_Variant_setScalar(&slamStatevariant, &slam_state, &UA_TYPES[UA_TYPES_STRING]);
          UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::slamState_Id_, slamStatevariant);
        }
      }
    }
  }
}

void amr_control::OpcUAServerAPI::addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID)
{
  UA_Argument inputArguments[2];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N");
  inputArguments[0].name = UA_STRING((char *)"Length");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[1]);
  inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]");
  inputArguments[1].name = UA_STRING((char *)"Nodes");
  inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_UInt32 pInputDimension[2] = {static_cast<UA_UInt32>(3), 5};
  inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS;
  inputArguments[1].arrayDimensionsSize = 2;
  inputArguments[1].arrayDimensions = pInputDimension;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message với độ chính xác mong muốn.");

  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToNode");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"MoveToNode"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"MoveToNode"),
                          incAttr, &amr_control::OpcUAServerAPI::moveToNodeCallBack,
                          2, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::moveToNodeCallBack(
    UA_Server *server,
    const UA_NodeId *sessionId, void *sessionContext,
    const UA_NodeId *methodId, void *methodContext,
    const UA_NodeId *objectId, void *objectContext,
    size_t inputSize, const UA_Variant *input,
    size_t outputSize, UA_Variant *output)
{
  UA_UInt32 length = *(UA_UInt32 *)input[0].data;
  UA_Double *nodes = (UA_Double *)input[1].data;

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"MoveToNode gọi thành công");

  if (input[1].arrayDimensionsSize != 2)
    return UA_STATUSCODE_BADINTERNALERROR;

  const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0];
  const int row = input[1].arrayDimensions[1];

  if (length > column)
  {
    message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử");
    goto STATUSCODE_BADREQUESTTOOLARGE;
  }

  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    UA_Double matrix[column][row];
    for (int i = 0; i < column; i++)
      for (int j = 0; j < row; j++)
        matrix[i][j] = nodes[i * row + j];

    robot_geometry_msgs::Pose2D goal_2d;
    goal_2d.x = matrix[column - 1][0];
    goal_2d.y = matrix[column - 1][1];
    goal_2d.theta = matrix[column - 1][2];

    const double velocity = matrix[column - 1][3];
    const double xy_tolerance = matrix[column - 1][4];
    const double yaw_tolerance = 0.025;

    const robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", robot::Time::now());
    amr_control::OpcUAServerAPI::move_base_ptr_->moveTo(goal, xy_tolerance, yaw_tolerance);

    amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity;
    amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4;

    robot_geometry_msgs::Vector3 linear;
    linear.x = velocity;
    amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear);
  }
  else
  {
    message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo");
    goto STATUSCODE_BADSTRUCTUREMISSING;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addDockToNodeMethod(UA_Server *server, UA_NodeId parentID)
{
  UA_Argument inputArguments[3];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N");
  inputArguments[0].name = UA_STRING((char *)"Length");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[1]);
  inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]");
  inputArguments[1].name = UA_STRING((char *)"Nodes");
  inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_UInt32 pInputDimension[2] = {static_cast<UA_UInt32>(3), 5};
  inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS;
  inputArguments[1].arrayDimensionsSize = 2;
  inputArguments[1].arrayDimensions = pInputDimension;

  UA_Argument_init(&inputArguments[2]);
  inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Maker");
  inputArguments[2].name = UA_STRING((char *)"Maker");
  inputArguments[2].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  inputArguments[2].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là lấy hàng ở một vị trí lận cận với điểm đích được order.");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToPickup");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"MoveToPickup"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"MoveToPickup"),
                          incAttr, &amr_control::OpcUAServerAPI::dockToNodeCallBack,
                          2, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::dockToNodeCallBack(
    UA_Server *server,
    const UA_NodeId *sessionId, void *sessionContext,
    const UA_NodeId *methodId, void *methodContext,
    const UA_NodeId *objectId, void *objectContext,
    size_t inputSize, const UA_Variant *input,
    size_t outputSize, UA_Variant *output)
{
  UA_UInt32 length = *(UA_UInt32 *)input[0].data;
  UA_Double *nodes = (UA_Double *)input[1].data;
  UA_String maker = *(UA_String *)input[2].data;


  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"MoveToPickUp gọi thành công");

  if (input[1].arrayDimensionsSize != 2)
    return UA_STATUSCODE_BADINTERNALERROR;

  const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0];
  const int row = input[1].arrayDimensions[1];

  if (length > column)
  {
    message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử");
    goto STATUSCODE_BADREQUESTTOOLARGE;
  }

  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    UA_Double matrix[column][row];
    for (int i = 0; i < column; i++)
      for (int j = 0; j < row; j++)
        matrix[i][j] = nodes[i * row + j];

    robot_geometry_msgs::Pose2D goal_2d;
    goal_2d.x = matrix[column - 1][0];
    goal_2d.y = matrix[column - 1][1];
    goal_2d.theta = matrix[column - 1][2];
    const double velocity = matrix[column - 1][3];
    const double xy_tolerance = matrix[column - 1][4];
    const double yaw_tolerance = 0.025;

    const robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", robot::Time::now());
    std::string maker_str(reinterpret_cast<char*>(maker.data), maker.length);
    amr_control::OpcUAServerAPI::move_base_ptr_->dockTo(maker_str, goal, xy_tolerance, yaw_tolerance);

    amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity;
    amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4;

    robot_geometry_msgs::Vector3 linear;
    linear.x = velocity;
    amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear);
  }
  else
  {
    message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo");
    goto STATUSCODE_BADSTRUCTUREMISSING;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToPickUp gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addRotateToMethod(UA_Server *server, UA_NodeId parentID)
{
  UA_Argument inputArguments[2];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Góc muốn quay trong giải từ -Pi đến Pi");
  inputArguments[0].name = UA_STRING((char *)"Yaw goal");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[1]);
  inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Sai số góc cho phép");
  inputArguments[1].name = UA_STRING((char *)"Yaw tolerance");
  inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_INT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Robot quay tại chỗ  một góc yaw.");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RotateTo");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"RotateTo"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"RotateTo"),
                          incAttr, &amr_control::OpcUAServerAPI::rotateToCallBack,
                          2, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::rotateToCallBack(
    UA_Server *server,
    const UA_NodeId *sessionId, void *sessionContext,
    const UA_NodeId *methodId, void *methodContext,
    const UA_NodeId *objectId, void *objectContext,
    size_t inputSize, const UA_Variant *input,
    size_t outputSize, UA_Variant *output)
{
  UA_Double Yaw = *(UA_Double *)input[0].data;
  UA_Double tolerance = *(UA_Double *)input[1].data;
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Successed");
  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    robot_geometry_msgs::PoseStamped goal;
    if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(goal))
    {
      message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ");
      goto STATUSCODE_BADINTERNALERROR;
    }
    tf2::Quaternion q;
    q.setRPY(0, 0, angles::normalize_angle(Yaw));
    goal.pose.orientation.x = q.x();
    goal.pose.orientation.y = q.y();
    goal.pose.orientation.z = q.z();
    goal.pose.orientation.w = q.w();
    amr_control::OpcUAServerAPI::move_base_ptr_->rotateTo(goal, tolerance);
  }
  else
  {
    return UA_STATUSCODE_BADSTRUCTUREMISSING;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo was called");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BAD:
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addMoveStraightMethod(UA_Server *server, UA_NodeId parentID)
{
  UA_Argument inputArguments[2];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Khoảng cách đi '+' là tiến '-' là lùi");
  inputArguments[0].name = UA_STRING((char *)"Distance");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[1]);
  inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc di chuyển");
  inputArguments[1].name = UA_STRING((char *)"Velocity linear");
  inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  inputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là trả hàng ở một vị trí lận cận với điểm đích được order.");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveStraight");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"MoveStraight"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"MoveStraight"),
                          incAttr, &amr_control::OpcUAServerAPI::moveStraighCallBack,
                          2, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::moveStraighCallBack(UA_Server *server,
                                                               const UA_NodeId *sessionId, void *sessionContext,
                                                               const UA_NodeId *methodId, void *methodContext,
                                                               const UA_NodeId *objectId, void *objectContext,
                                                               size_t inputSize, const UA_Variant *input,
                                                               size_t outputSize, UA_Variant *output)
{
  UA_Double distance = *(UA_Double *)input[0].data;
  UA_Double velocity = *(UA_Double *)input[1].data;

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"MoveStraight gọi thành công");

  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    robot_geometry_msgs::PoseStamped pose;
    if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(pose))
    {
      message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ");
      goto STATUSCODE_BADINTERNALERROR;
    }
    // ROS_INFO_STREAM(pose);
    robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
    amr_control::OpcUAServerAPI::move_base_ptr_->moveStraightTo(goal);
    
    amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity;
    amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4;

    robot_geometry_msgs::Vector3 linear;
    linear.x = velocity;
    amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear);
  }
  else
  {
    message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo");
    goto STATUSCODE_BADSTRUCTUREMISSING;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveStraight gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addCancelMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Hủy lệnh di chuyển của AMR");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Cancel");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Cancel"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Cancel"),
                          incAttr, &amr_control::OpcUAServerAPI::cancelCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::cancelCallBack(UA_Server *server,
                                                          const UA_NodeId *sessionId, void *sessionContext,
                                                          const UA_NodeId *methodId, void *methodContext,
                                                          const UA_NodeId *objectId, void *objectContext,
                                                          size_t inputSize, const UA_Variant *input,
                                                          size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Cancel gọi thành công");

  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    amr_control::OpcUAServerAPI::move_base_ptr_->cancel();
  }
  else
  {
    message = UA_STRING_ALLOC((char *)"Chức năng hủy lệnh đang bị lỗi khởi tạo");
    goto STATUSCODE_BADSTRUCTUREMISSING;
  }
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addPauseMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Lệnh tạm dừng di chuyển của AMR");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Pause");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Pause"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Pause"),
                          incAttr, &amr_control::OpcUAServerAPI::pauseCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::pauseCallBack(UA_Server *server,
                                                         const UA_NodeId *sessionId, void *sessionContext,
                                                         const UA_NodeId *methodId, void *methodContext,
                                                         const UA_NodeId *objectId, void *objectContext,
                                                         size_t inputSize, const UA_Variant *input,
                                                         size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Pause gọi thành công");

  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    amr_control::OpcUAServerAPI::move_base_ptr_->pause();
  }
  else
  {
    message = UA_STRING_ALLOC((char *)"Chức năng tạm dừng đang bị lỗi khởi tạo");
    goto STATUSCODE_BADSTRUCTUREMISSING;
  }
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addResumeMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Lệnh tiếp tục di chuyển của AMR");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Resume");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Resume"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Resume"),
                          incAttr, &amr_control::OpcUAServerAPI::resumeCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::resumeCallBack(UA_Server *server,
                                                          const UA_NodeId *sessionId, void *sessionContext,
                                                          const UA_NodeId *methodId, void *methodContext,
                                                          const UA_NodeId *objectId, void *objectContext,
                                                          size_t inputSize, const UA_Variant *input,
                                                          size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Resume gọi thành công");

  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr)
  {
    amr_control::OpcUAServerAPI::move_base_ptr_->resume();
  }
  else
  {
    message = UA_STRING_ALLOC((char *)"Chức năng tiếp tục đang bị lỗi khởi tạo");
    goto STATUSCODE_BADSTRUCTUREMISSING;
  }
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addResetMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Lệnh reset AMR");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Reset");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Reset"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Reset"),
                          incAttr, &amr_control::OpcUAServerAPI::resetCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::resetCallBack(UA_Server *server,
                                                         const UA_NodeId *sessionId, void *sessionContext,
                                                         const UA_NodeId *methodId, void *methodContext,
                                                         const UA_NodeId *objectId, void *objectContext,
                                                         size_t inputSize, const UA_Variant *input,
                                                         size_t outputSize, UA_Variant *output)
{

  // sleep(5);

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Reset gọi thành công");

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addWriteMutedMethod(UA_Server *server, UA_NodeId parentID)
{
  UA_Argument inputArguments[1];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Tích là bật hoặc ngược lại");
  inputArguments[0].name = UA_STRING((char *)"Input Value");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng bật/tắt vùng Muted Safety");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Muted Safety");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Muted Safety"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Muted Safety"),
                          incAttr, &amr_control::OpcUAServerAPI::writeMutedCallBack,
                          1, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::writeMutedCallBack(UA_Server *server,
                                                              const UA_NodeId *sessionId, void *sessionContext,
                                                              const UA_NodeId *methodId, void *methodContext,
                                                              const UA_NodeId *objectId, void *objectContext,
                                                              size_t inputSize, const UA_Variant *input,
                                                              size_t outputSize, UA_Variant *output)
{
  *amr_control::OpcUAServerAPI::muted_value_ = *(bool *)input[0].data;

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh Muted Safety gọi thành công");

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety bị lỗi");
  return UA_STATUSCODE_BAD;
}

UA_NodeId *amr_control::OpcUAServerAPI::amr_status_str_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::amr_status_en_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ = new UA_NodeId();
void amr_control::OpcUAServerAPI::addNavigationState(UA_Server *server, UA_NodeId parentID)
{
  UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default;
  UA_String status_str = UA_STRING_ALLOC((char *)"");
  UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]);
  status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String");
  status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot.");
  status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Status String"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            status_str_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_str_Id_);

  UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default;
  UA_Int32 status_en = UA_Int32(0);
  UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]);
  status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum");
  status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot.");
  status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Status Enum"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            status_en_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_en_Id_);

  UA_VariableAttributes robotFeedBackAttr = UA_VariableAttributes_default;
  UA_String feed_back_str = UA_STRING_ALLOC((char *)"");
  UA_Variant_setScalar(&robotFeedBackAttr.value, &feed_back_str, &UA_TYPES[UA_TYPES_STRING]);
  robotFeedBackAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Feedback");
  robotFeedBackAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Thông tin phản hồi của robot.");
  robotFeedBackAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Feedback"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            robotFeedBackAttr, NULL, amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_);

  std::thread([=]()
              {
      ros::Rate r(10);
      while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning())
      {
        r.sleep();
        ros::spinOnce();
      } })
      .detach();
}

void amr_control::OpcUAServerAPI::navigationHandle()
{
  if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr)
  {
    if (amr_control::OpcUAServerAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      switch (amr_control::OpcUAServerAPI::move_base_ptr_->getFeedback()->navigation_state)
      {
      case robot::move_base_core::State::PENDING:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PENDING");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(0);
        break;
      case robot::move_base_core::State::ACTIVE:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ACTIVE");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(1);
        break;
      case robot::move_base_core::State::PREEMPTED:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTED");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(2);
        break;
      case robot::move_base_core::State::SUCCEEDED:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"SUCCEEDED");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(3);
        break;
      case robot::move_base_core::State::ABORTED:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ABORTED");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(4);
        break;
      case robot::move_base_core::State::REJECTED:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"REJECTED");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(5);
        break;
      case robot::move_base_core::State::PREEMPTING:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTING");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(6);
        break;
      case robot::move_base_core::State::RECALLING:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLING");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(7);
        break;
      case robot::move_base_core::State::RECALLED:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLED");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(8);
        break;
      case robot::move_base_core::State::LOST:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"LOST");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(9);
        break;
      case robot::move_base_core::State::PLANNING:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PLANNING");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(10);
        break;
      case robot::move_base_core::State::CONTROLLING:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CONTROLLING");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(11);
        break;
      case robot::move_base_core::State::CLEARING:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CLEARING");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(12);
        break;
      case robot::move_base_core::State::PAUSED:
        amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PAUSED");
        amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(13);
        break;
      default:
        break;
      }

      if (amr_control::OpcUAServerAPI::move_base_ptr_->getFeedback()->navigation_state == robot::move_base_core::State::CONTROLLING)
        amr_control::OpcUAServerAPI::amr_feedback_str_ = UA_STRING_ALLOC((char *)"");
      else
        amr_control::OpcUAServerAPI::amr_feedback_str_ =
            UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::move_base_ptr_->getFeedback()->feed_back_str.c_str());
    }

    if (amr_control::OpcUAServerAPI::amr_status_str_Id_ != NULL && amr_control::OpcUAServerAPI::amr_status_en_Id_ != NULL && amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ != NULL)
    {
      UA_Variant statusStrVariant;
      UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::amr_status_str_, &UA_TYPES[UA_TYPES_STRING]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ

      UA_Variant statusEnVariant;
      UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::amr_status_en_, &UA_TYPES[UA_TYPES_UINT32]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ

      UA_Variant feedbackVariant;
      UA_Variant_setScalar(&feedbackVariant, &amr_control::OpcUAServerAPI::amr_feedback_str_, &UA_TYPES[UA_TYPES_STRING]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_, feedbackVariant); // Truyền đối tượng, không phải con trỏ
    }
  }
}

UA_NodeId *amr_control::OpcUAServerAPI::vx_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::vy_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::omega_Id_ = new UA_NodeId();
void amr_control::OpcUAServerAPI::addVelocityCommand(UA_Server *server, UA_NodeId parentID)
{
  UA_VariableAttributes vxAttr = UA_VariableAttributes_default;
  UA_Double vx = UA_Double(0);
  UA_Variant_setScalar(&vxAttr.value, &vx, &UA_TYPES[UA_TYPES_DOUBLE]);
  vxAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vx");
  vxAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục X.");
  vxAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Vx"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            vxAttr, NULL, amr_control::OpcUAServerAPI::vx_Id_);

  UA_VariableAttributes vyAttr = UA_VariableAttributes_default;
  UA_Double vy = UA_Double(0);
  UA_Variant_setScalar(&vyAttr.value, &vy, &UA_TYPES[UA_TYPES_DOUBLE]);
  vyAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vy");
  vyAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục Y.");
  vyAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Vy"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            vyAttr, NULL, amr_control::OpcUAServerAPI::vy_Id_);

  UA_VariableAttributes omegaAttr = UA_VariableAttributes_default;
  UA_Double angular_vel = UA_Double(0);
  UA_Variant_setScalar(&omegaAttr.value, &angular_vel, &UA_TYPES[UA_TYPES_DOUBLE]);
  omegaAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Omega");
  omegaAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc quay theo trục Z.");
  omegaAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Omega"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            omegaAttr, NULL, amr_control::OpcUAServerAPI::omega_Id_);
}

void amr_control::OpcUAServerAPI::monitorHandle()
{
  if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr)
  {
    if (amr_control::OpcUAServerAPI::monitor_ptr_ != nullptr)
    {
      nav_2d_msgs::Twist2D velocity;
      if (monitor_ptr_->getVelocity(velocity))
      {
        UA_Double vx = std::round(velocity.x * 1000.0) / 1000.0;
        UA_Double vy = std::round(velocity.y * 1000.0) / 1000.0;
        UA_Double omega = std::round(velocity.theta * 1000.0) / 1000.0;

        if (amr_control::OpcUAServerAPI::vx_Id_ != NULL &&
            amr_control::OpcUAServerAPI::vy_Id_ != NULL &&
            amr_control::OpcUAServerAPI::omega_Id_ != NULL)
        {
          UA_Variant vxVariant;
          UA_Variant_setScalar(&vxVariant, &vx, &UA_TYPES[UA_TYPES_DOUBLE]);
          UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vx_Id_, vxVariant); // Truyền đối tượng, không phải con trỏ

          UA_Variant vyVariant;
          UA_Variant_setScalar(&vyVariant, &vy, &UA_TYPES[UA_TYPES_DOUBLE]);
          UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vy_Id_, vyVariant); // Truyền đối tượng, không phải con trỏ

          UA_Variant omegaVariant;
          UA_Variant_setScalar(&omegaVariant, &omega, &UA_TYPES[UA_TYPES_DOUBLE]);
          UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::omega_Id_, omegaVariant); // Truyền đối tượng, không phải con trỏ
        }
      }
    }
  }
}

void amr_control::OpcUAServerAPI::addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng lấy hàng bởi tay máy Dobot");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PickUp");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"PickUp"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"PickUp"),
                          incAttr, &amr_control::OpcUAServerAPI::PickUpDobotCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::PickUpDobotCallBack(UA_Server *server,
                                                               const UA_NodeId *sessionId, void *sessionContext,
                                                               const UA_NodeId *methodId, void *methodContext,
                                                               const UA_NodeId *objectId, void *objectContext,
                                                               size_t inputSize, const UA_Variant *input,
                                                               size_t outputSize, UA_Variant *output)
{

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động và chạy tay máy Dobot gọi thành công");

  if (!amr_control::OpcUAServerAPI::arm_function_ptr_)
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }
  if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable())
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận");
    return UA_STATUSCODE_GOOD;
  }
  else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable())
  {
    amr_control::OpcUAServerAPI::resetState();
    ROS_INFO("Dobot is running...");
    *amr_control::OpcUAServerAPI::arm_cancel_ = true;
    amr_control::OpcUAServerAPI::arm_function_ptr_();
  }
  else
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động và chạy tay máy Dobot gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh StartAndRunDobot bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng về gốc tay máy Dobot");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GoHomeDobot");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"GoHomeDobot"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"GoHomeDobot"),
                          incAttr, &amr_control::OpcUAServerAPI::goHomeDobotCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::goHomeDobotCallBack(UA_Server *server,
                                                               const UA_NodeId *sessionId, void *sessionContext,
                                                               const UA_NodeId *methodId, void *methodContext,
                                                               const UA_NodeId *objectId, void *objectContext,
                                                               size_t inputSize, const UA_Variant *input,
                                                               size_t outputSize, UA_Variant *output)
{

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh về gốc tay máy Dobot gọi thành công");

  if (!amr_control::OpcUAServerAPI::arm_function_ptr_)
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable())
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận");
    return UA_STATUSCODE_GOOD;
  }
  else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable())
  {
    amr_control::OpcUAServerAPI::resetState();
    *amr_control::OpcUAServerAPI::arm_go_home_ = true;
    *amr_control::OpcUAServerAPI::arm_cancel_ = true;
    ROS_INFO("Dobot is Homing...");
    amr_control::OpcUAServerAPI::arm_function_ptr_();
  }
  else
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addCountMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument inputArguments[2];
  UA_Argument_init(&inputArguments[0]);
  inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng OK cần lấy");
  inputArguments[0].name = UA_STRING((char *)"Count OK");
  inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  inputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&inputArguments[1]);
  inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng NG cần lấy");
  inputArguments[1].name = UA_STRING((char *)"Count NG");
  inputArguments[1].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  inputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng thay đổi số lượng hàng muốn lấy");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SetQuantityOfGoods");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"SetQuantityOfGoods"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"SetQuantityOfGoods"),
                          incAttr, &amr_control::OpcUAServerAPI::counterCallBack,
                          2, inputArguments, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::counterCallBack(UA_Server *server,
                                                           const UA_NodeId *sessionId, void *sessionContext,
                                                           const UA_NodeId *methodId, void *methodContext,
                                                           const UA_NodeId *objectId, void *objectContext,
                                                           size_t inputSize, const UA_Variant *input,
                                                           size_t outputSize, UA_Variant *output)
{

  UA_UInt32 count_ok = *(UA_UInt32 *)input[0].data;
  UA_UInt32 count_ng = *(UA_UInt32 *)input[1].data;

  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công");

  *amr_control::OpcUAServerAPI::count_ok_max_ = count_ok;
  *amr_control::OpcUAServerAPI::count_ng_max_ = count_ng;

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addCancelDobotMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng dừng tay máy Dobot");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CancelDobot");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"CancelDobot"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"CancelDobot"),
                          incAttr, &amr_control::OpcUAServerAPI::cancelDobotCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::cancelDobotCallBack(UA_Server *server,
                                                               const UA_NodeId *sessionId, void *sessionContext,
                                                               const UA_NodeId *methodId, void *methodContext,
                                                               const UA_NodeId *objectId, void *objectContext,
                                                               size_t inputSize, const UA_Variant *input,
                                                               size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh dừng tay máy Dobot gọi thành công");

  *amr_control::OpcUAServerAPI::arm_cancel_ = false;

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addContinueDobotMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng tiếp tục chạy tay máy Dobot");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ContinueDobot");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"ContinueDobot"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"ContinueDobot"),
                          incAttr, &amr_control::OpcUAServerAPI::continueDobotCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::continueDobotCallBack(UA_Server *server,
                                                                 const UA_NodeId *sessionId, void *sessionContext,
                                                                 const UA_NodeId *methodId, void *methodContext,
                                                                 const UA_NodeId *objectId, void *objectContext,
                                                                 size_t inputSize, const UA_Variant *input,
                                                                 size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh tiếp tục chạy tay máy Dobot gọi thành công");

  amr_control::OpcUAServerAPI::resetState();
  *amr_control::OpcUAServerAPI::arm_continue_ = true;

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng khởi động nguồn tay máy Dobot");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PowerOnDobot");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"PowerOnDobot"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"PowerOnDobot"),
                          incAttr, &amr_control::OpcUAServerAPI::powerOnDobotCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::powerOnDobotCallBack(UA_Server *server,
                                                                const UA_NodeId *sessionId, void *sessionContext,
                                                                const UA_NodeId *methodId, void *methodContext,
                                                                const UA_NodeId *objectId, void *objectContext,
                                                                size_t inputSize, const UA_Variant *input,
                                                                size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động nguồn tay máy Dobot gọi thành công");

  amr_control::OpcUAServerAPI::resetState();
  *amr_control::OpcUAServerAPI::arm_power_on_ = true;

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot bị lỗi");
  return UA_STATUSCODE_BAD;
}

UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ = new UA_NodeId();
void amr_control::OpcUAServerAPI::addDobotProperties(UA_Server *server, UA_NodeId parentID)
{

  UA_VariableAttributes dobot_state_str_Attr = UA_VariableAttributes_default;
  UA_String dobot_state_str = UA_STRING_ALLOC((char *)"PENDING");
  UA_Variant_setScalar(&dobot_state_str_Attr.value, &dobot_state_str, &UA_TYPES[UA_TYPES_STRING]);
  dobot_state_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State String");
  dobot_state_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot.");
  dobot_state_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"State String"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            dobot_state_str_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_);

  UA_VariableAttributes dobot_state_en_Attr = UA_VariableAttributes_default;
  UA_UInt32 dobot_state_en = UA_UInt32(amr_control::State::FAILED);
  UA_Variant_setScalar(&dobot_state_en_Attr.value, &dobot_state_en, &UA_TYPES[UA_TYPES_UINT32]);
  dobot_state_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State Enum");
  dobot_state_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot.");
  dobot_state_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"State Enum"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            dobot_state_en_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_);

  UA_VariableAttributes dobot_count_ok_max_Attr = UA_VariableAttributes_default;
  UA_UInt32 count_ok_max = *amr_control::OpcUAServerAPI::count_ok_max_;
  UA_Variant_setScalar(&dobot_count_ok_max_Attr.value, &count_ok_max, &UA_TYPES[UA_TYPES_UINT32]);
  dobot_count_ok_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count OK");
  dobot_count_ok_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng OK cần lấy");
  dobot_count_ok_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Count OK"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            dobot_count_ok_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_);

  UA_VariableAttributes dobot_count_ng_max_Attr = UA_VariableAttributes_default;
  UA_UInt32 count_ng_max = *amr_control::OpcUAServerAPI::count_ng_max_;
  UA_Variant_setScalar(&dobot_count_ng_max_Attr.value, &count_ng_max, &UA_TYPES[UA_TYPES_UINT32]);
  dobot_count_ng_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count NG");
  dobot_count_ng_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng NG cần lấy");
  dobot_count_ng_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Count NG"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            dobot_count_ng_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_);

  UA_VariableAttributes dobot_mode_ptr_Attr = UA_VariableAttributes_default;
  UA_Double *mode = amr_control::OpcUAServerAPI::mode_ptr_;
  UA_Variant_setScalar(&dobot_mode_ptr_Attr.value, mode, &UA_TYPES[UA_TYPES_DOUBLE]);
  dobot_mode_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Mode");
  dobot_mode_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Chế độ của tay máy Dobot");
  dobot_mode_ptr_Attr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Dobot Mode"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            dobot_mode_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_);

  UA_VariableAttributes dobot_status_code_ptr_Attr = UA_VariableAttributes_default;
  UA_UInt32 *status_code = amr_control::OpcUAServerAPI::status_code_ptr_;
  UA_Variant_setScalar(&dobot_status_code_ptr_Attr.value, status_code, &UA_TYPES[UA_TYPES_UINT32]);
  dobot_status_code_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Status Code");
  dobot_status_code_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Mã trạng thái của tay máy Dobot");
  dobot_status_code_ptr_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Dobot Status Code"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            dobot_status_code_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_);
}

void amr_control::OpcUAServerAPI::dobotPropertiesHandle()
{
  if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr)
  {

    if (amr_control::OpcUAServerAPI::arm_thread_ptr_)
    {
      // if (amr_control::OpcUAServerAPI::arm_joined_)
      // {
      amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::FINISHED;
      amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"FINISHED");
      amr_control::OpcUAServerAPI::resetState();
      // }

      // if (
      //     !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() &&
      //     !amr_control::OpcUAServerAPI::arm_joined_)
      // {
      //   amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::INITIALIZING;
      //   amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING");
      // }
      // else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ &&
      //          amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() &&
      //          !amr_control::OpcUAServerAPI::arm_joined_)
      // {
      //   amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::RUNNING;
      //   amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"RUNNING");
      // }
    }
    else
    {
      amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::WAITING;
      amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"WAITING");
    }

    if (amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ != NULL &&
        amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ != NULL &&
        amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ != NULL &&
        amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ != NULL &&
        amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ != NULL &&
        amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ != NULL)
    {
      UA_Variant dobot_state_str_Variant;
      UA_Variant_setScalar(&dobot_state_str_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_str_, &UA_TYPES[UA_TYPES_STRING]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_, dobot_state_str_Variant); // Truyền đối tượng, không phải con trỏ

      UA_Variant dobot_state_en_Variant;
      UA_Variant_setScalar(&dobot_state_en_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_en_, &UA_TYPES[UA_TYPES_UINT32]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_, dobot_state_en_Variant); // Truyền đối tượng, không phải con trỏ

      UA_Variant dobot_count_ok_max_Variant;
      UA_Variant_setScalar(&dobot_count_ok_max_Variant, &amr_control::OpcUAServerAPI::count_ok_max_, &UA_TYPES[UA_TYPES_UINT32]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_, dobot_count_ok_max_Variant); // Truyền đối tượng, không phải con trỏ

      UA_Variant dobot_count_ng_max_Variant;
      UA_Variant_setScalar(&dobot_count_ng_max_Variant, &amr_control::OpcUAServerAPI::count_ng_max_, &UA_TYPES[UA_TYPES_UINT32]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_, dobot_count_ng_max_Variant); // Truyền đối tượng, không phải con trỏ

      if (amr_control::OpcUAServerAPI::mode_ptr_)
      {
        UA_Variant dobot_mode_ptr_Variant;
        UA_Variant_setScalar(&dobot_mode_ptr_Variant, amr_control::OpcUAServerAPI::mode_ptr_, &UA_TYPES[UA_TYPES_DOUBLE]);
        UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_, dobot_mode_ptr_Variant);
      }

      if (amr_control::OpcUAServerAPI::status_code_ptr_)
      {
        UA_Variant dobot_status_Variant;
        UA_Variant_setScalar(&dobot_status_Variant, amr_control::OpcUAServerAPI::status_code_ptr_, &UA_TYPES[UA_TYPES_UINT32]);
        UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_, dobot_status_Variant);
      }
    }

    if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::RUNNING)
    {
      if (*(amr_control::OpcUAServerAPI::status_code_ptr_) != amr_control::OpcUAServerAPI::old_status_code_ &&
          *(amr_control::OpcUAServerAPI::status_code_ptr_) == imr_nova_control::ROBOT_DRAG)
      {
        amr_control::OpcUAServerAPI::resetState();
      }
    }
    else if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::FINISHED)
    {
      *(amr_control::OpcUAServerAPI::status_code_ptr_) = 0;
    }
    amr_control::OpcUAServerAPI::old_status_code_ = *(amr_control::OpcUAServerAPI::status_code_ptr_);
  }
}

void amr_control::OpcUAServerAPI::addunLoadMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng trả hàng");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"UnLoad");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"UnLoad"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"UnLoad"),
                          incAttr, &amr_control::OpcUAServerAPI::unLoadCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::unLoadCallBack(UA_Server *server,
                                                          const UA_NodeId *sessionId, void *sessionContext,
                                                          const UA_NodeId *methodId, void *methodContext,
                                                          const UA_NodeId *objectId, void *objectContext,
                                                          size_t inputSize, const UA_Variant *input,
                                                          size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh trả hàng gọi thành công");

  if (!amr_control::OpcUAServerAPI::unLoad_excuted_)
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable())
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận");
    return UA_STATUSCODE_GOOD;
  }
  else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable())
  {
    amr_control::OpcUAServerAPI::unLoad_excuted_();
  }
  else
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addLoadMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng nhận hàng");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Load");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"Load"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"Load"),
                          incAttr, &amr_control::OpcUAServerAPI::loadCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::loadCallBack(UA_Server *server,
                                                        const UA_NodeId *sessionId, void *sessionContext,
                                                        const UA_NodeId *methodId, void *methodContext,
                                                        const UA_NodeId *objectId, void *objectContext,
                                                        size_t inputSize, const UA_Variant *input,
                                                        size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh nhận hàng gọi thành công");

  if (!amr_control::OpcUAServerAPI::load_excuted_)
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable())
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận");
    return UA_STATUSCODE_GOOD;
  }
  else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable())
  {
    amr_control::OpcUAServerAPI::load_excuted_();
  }
  else
  {
    UA_Int32 status = STATUS_ERROR;
    UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ");
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
    UA_Int32_clear(&status);
    UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
    UA_String_clear(&message);
    UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ");
    return UA_STATUSCODE_GOOD;
  }

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh nhận hàng bị lỗi");
  return UA_STATUSCODE_BAD;
}

void amr_control::OpcUAServerAPI::addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID)
{
  /* One output argument */
  UA_Argument outputArguments[2];
  UA_Argument_init(&outputArguments[0]);
  outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status");
  outputArguments[0].name = UA_STRING((char *)"Status");
  outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  outputArguments[0].valueRank = UA_VALUERANK_SCALAR;

  UA_Argument_init(&outputArguments[1]);
  outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message");
  outputArguments[1].name = UA_STRING((char *)"Message");
  outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  outputArguments[1].valueRank = UA_VALUERANK_SCALAR;

  /* Add the method node */
  UA_MethodAttributes incAttr = UA_MethodAttributes_default;
  incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US",
                                         (char *)"Chức năng reset");
  incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ResetBelt");
  incAttr.executable = true;
  incAttr.userExecutable = true;
  UA_Server_addMethodNode(server,
                          UA_NODEID_STRING(1, (char *)"ResetBelt"),
                          parentID,
                          UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                          UA_QUALIFIEDNAME(1, (char *)"ResetBelt"),
                          incAttr, &amr_control::OpcUAServerAPI::resetConveyorBeltCallBack,
                          0, NULL, 2, outputArguments, NULL, NULL);
}

UA_StatusCode amr_control::OpcUAServerAPI::resetConveyorBeltCallBack(UA_Server *server,
                                                                     const UA_NodeId *sessionId, void *sessionContext,
                                                                     const UA_NodeId *methodId, void *methodContext,
                                                                     const UA_NodeId *objectId, void *objectContext,
                                                                     size_t inputSize, const UA_Variant *input,
                                                                     size_t outputSize, UA_Variant *output)
{
  UA_Variant_init(output);
  UA_Int32 status = STATUS_SUCCESSED;
  UA_String message = UA_STRING_ALLOC((char *)"Lệnh reset gọi thành công");

  *amr_control::OpcUAServerAPI::belt_cancel_ = true;

  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset gọi thành công");
  return UA_STATUSCODE_GOOD;

STATUSCODE_BADINTERNALERROR:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADINTERNALERROR;

STATUSCODE_BADREQUESTTOOLARGE:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADREQUESTTOOLARGE;

STATUSCODE_BADSTRUCTUREMISSING:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  return UA_STATUSCODE_BADSTRUCTUREMISSING;

STATUSCODE_BAD:
  status = STATUS_ERROR;
  UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]);
  UA_Int32_clear(&status);
  UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]);
  UA_String_clear(&message);
  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset bị lỗi");
  return UA_STATUSCODE_BAD;
}

UA_NodeId *amr_control::OpcUAServerAPI::belt_status_str_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::belt_status_en_Id_ = new UA_NodeId();
UA_NodeId *amr_control::OpcUAServerAPI::have_goods_Id_ = new UA_NodeId();
void amr_control::OpcUAServerAPI::addConveyorBeltState(UA_Server *server, UA_NodeId parentID)
{

  UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default;
  UA_String status_str = UA_STRING_ALLOC((char *)"READY");
  UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]);
  status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String");
  status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt.");
  status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Status String"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            status_str_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_str_Id_);

  UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default;
  UA_Int32 status_en = UA_Int32(*cur_belt_state_en_);
  UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]);
  status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum");
  status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt.");
  status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Status Enum"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            status_en_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_en_Id_);

  UA_VariableAttributes have_goods_Attr = UA_VariableAttributes_default;
  UA_Int32 have_goods = UA_Boolean(false);
  UA_Variant_setScalar(&have_goods_Attr.value, &have_goods, &UA_TYPES[UA_TYPES_BOOLEAN]);
  have_goods_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Have Goods");
  have_goods_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái đã nhận hàng chưa");
  have_goods_Attr.dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId;
  UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID,
                            UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                            UA_QUALIFIEDNAME(1, (char *)"Have Goods"),
                            UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
                            have_goods_Attr, NULL, amr_control::OpcUAServerAPI::have_goods_Id_);
}

void amr_control::OpcUAServerAPI::ConveyorBeltHandle()
{
  if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr)
  {
    switch (*amr_control::OpcUAServerAPI::cur_belt_state_en_)
    {
    case amr_control::WAITING:
      amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"WAITING");
      break;
    case amr_control::INITIALIZING:
      amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING");
      break;
    case amr_control::RUNNING:
      amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"RUNNING");
      break;
    case amr_control::PAUSED:
      amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"PAUSED");
      break;
    case amr_control::FINISHED:
      amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FINISHED");
      break;
    case amr_control::FAILED:
      amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FAILED");
      break;
    default:
      break;
    }

    if (amr_control::OpcUAServerAPI::belt_status_str_Id_ != NULL &&
        amr_control::OpcUAServerAPI::belt_status_en_Id_ != NULL &&
        amr_control::OpcUAServerAPI::have_goods_Id_ != NULL)
    {
      UA_Variant statusStrVariant;
      UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::belt_state_str_, &UA_TYPES[UA_TYPES_STRING]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ

      UA_Variant statusEnVariant;
      UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::cur_belt_state_en_, &UA_TYPES[UA_TYPES_UINT32]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ

      UA_Variant haveGoodsVariant;
      UA_Variant_setScalar(&haveGoodsVariant, &amr_control::OpcUAServerAPI::have_goods_, &UA_TYPES[UA_TYPES_BOOLEAN]);
      UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::have_goods_Id_, haveGoodsVariant); // Truyền đối tượng, không phải con trỏ
    }
  }
}

void *amr_control::OpcUAServerAPI::ThreadWorker(void *_)
{
  ros::Rate rate(10);

  while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning())
  {
    amr_control::OpcUAServerAPI::slamHandle();
    amr_control::OpcUAServerAPI::monitorHandle();
    amr_control::OpcUAServerAPI::navigationHandle();
    amr_control::OpcUAServerAPI::dobotPropertiesHandle();
    amr_control::OpcUAServerAPI::ConveyorBeltHandle();

    const UA_AsyncOperationRequest *request = NULL;
    void *context = NULL;
    UA_AsyncOperationType type;
    if (UA_Server_getAsyncOperationNonBlocking(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &type, &request, &context, NULL) == true)
    {
      UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Got entry: OKAY");

      UA_CallMethodResult response = UA_Server_call(
          amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &request->callMethodRequest);

      UA_Server_setAsyncOperationResult(
          amr_control::OpcUAServerAPI::server_ptr_->getServerObject(),
          (UA_AsyncOperationResponse *)&response, context);

      UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Call done: OKAY");
      UA_CallMethodResult_clear(&response);
    }
    rate.sleep();
    ros::spinOnce();
  }
  return 0;
}

void amr_control::OpcUAServerAPI::resetState()
{
  *amr_control::OpcUAServerAPI::arm_go_home_ = false;
  *amr_control::OpcUAServerAPI::arm_continue_ = false;
  *amr_control::OpcUAServerAPI::arm_power_on_ = false;
}

ros::Publisher amr_control::OpcUAServerAPI::init_pub_;
std::shared_ptr<amr_comunication::AmrOpcUAServer> amr_control::OpcUAServerAPI::server_ptr_ = nullptr;
std::shared_ptr<robot::move_base_core::BaseNavigation> amr_control::OpcUAServerAPI::move_base_ptr_ = nullptr;
std::shared_ptr<loc_core::BaseLocalization> amr_control::OpcUAServerAPI::loc_base_ptr_ = nullptr;
std::shared_ptr<amr_control::AmrMonitor> amr_control::OpcUAServerAPI::monitor_ptr_ = nullptr;
nav_2d_msgs::Twist2D amr_control::OpcUAServerAPI::cmd_vel_max_;

std::thread *amr_control::OpcUAServerAPI::arm_thread_ptr_;
std::function<void()> amr_control::OpcUAServerAPI::arm_function_ptr_;
amr_control::State amr_control::OpcUAServerAPI::arm_dobot_state_en_;
UA_String amr_control::OpcUAServerAPI::arm_dobot_state_str_;
unsigned int *amr_control::OpcUAServerAPI::count_ng_max_ = new unsigned int;
unsigned int *amr_control::OpcUAServerAPI::count_ok_max_ = new unsigned int;
bool *amr_control::OpcUAServerAPI::arm_cancel_ = new bool;
bool *amr_control::OpcUAServerAPI::arm_continue_ = new bool;
bool *amr_control::OpcUAServerAPI::arm_go_home_ = new bool;
bool *amr_control::OpcUAServerAPI::arm_power_on_ = new bool;
double *amr_control::OpcUAServerAPI::mode_ptr_;
unsigned int *amr_control::OpcUAServerAPI::status_code_ptr_ = new unsigned int(0);
unsigned int amr_control::OpcUAServerAPI::old_status_code_ = 0;

pthread_t amr_control::OpcUAServerAPI::hThread_;
UA_String amr_control::OpcUAServerAPI::amr_feedback_str_;
UA_String amr_control::OpcUAServerAPI::amr_status_str_;
UA_UInt32 amr_control::OpcUAServerAPI::amr_status_en_;
bool *amr_control::OpcUAServerAPI::muted_value_ = new bool;

amr_control::State *amr_control::OpcUAServerAPI::cur_belt_state_en_ = new amr_control::State;
UA_String amr_control::OpcUAServerAPI::belt_state_str_;
UA_Boolean amr_control::OpcUAServerAPI::have_goods_;
bool *amr_control::OpcUAServerAPI::belt_cancel_ = new bool;
std::function<void()> amr_control::OpcUAServerAPI::unLoad_excuted_;
std::function<void()> amr_control::OpcUAServerAPI::load_excuted_;
std::thread *amr_control::OpcUAServerAPI::belt_thread_ptr_;
