git commit -m "first commit for v2"

Controllers/Packages/amr_comunication/src/opc_ua/ua_server.cpp

@@ -0,0 +1,155 @@
+#include "amr_comunication/opc_ua/ua_server.h"
+#include <exception>
+
+std::function<void(int)> amr_comunication::AmrOpcUAServer::signalHandler_;
+volatile UA_Boolean amr_comunication::AmrOpcUAServer::running_ = true;
+
+amr_comunication::AmrOpcUAServer::AmrOpcUAServer(const ros::NodeHandle& nh)
+  : initalized_(false), server_ptr_(NULL), config_(NULL)
+{
+  if (!initalized_) this->init(nh);
+}
+
+amr_comunication::AmrOpcUAServer::AmrOpcUAServer()
+  : initalized_(false), server_ptr_(NULL), config_(NULL) {
+}
+
+amr_comunication::AmrOpcUAServer::~AmrOpcUAServer()
+{
+  this->stop();
+}
+
+void amr_comunication::AmrOpcUAServer::init(const ros::NodeHandle& nh)
+{
+  if (!initalized_)
+  {
+    server_ptr_ = UA_Server_new();
+    config_ = UA_Server_getConfig(server_ptr_);
+    retval_ = UA_ServerConfig_setDefault(config_);
+    if (retval_ != UA_STATUSCODE_GOOD) {
+      UA_Server_delete(server_ptr_);
+      UA_ServerConfig_clean(config_);
+      exit(1);
+    }
+    signalHandler_ = std::bind(&amr_comunication::AmrOpcUAServer::stopHandler, this, std::placeholders::_1);
+    signal(SIGINT, &AmrOpcUAServer::staticSignalHandler);
+    signal(SIGTERM, &AmrOpcUAServer::staticSignalHandler);
+
+    // Đặt URL máy chủ
+    url_ = UA_STRING((char*)"opc.tcp://0.0.0.0:4840");
+    config_->serverUrls = &url_;
+    config_->serverUrlsSize = 1;
+
+    // logins_.push_back({ UA_STRING_STATIC("admin"), UA_STRING_STATIC("robotics") });
+    // logins_.push_back({ UA_STRING_STATIC("HiepLM"), UA_STRING_STATIC("robotics") });
+    // /* Disable anonymous logins, enable two user/password logins */
+    // config_->accessControl.clear(&config_->accessControl);
+
+
+    // retval_ = UA_AccessControl_default(config_, false,
+    //   &config_->securityPolicies[config_->securityPoliciesSize - 1].policyUri,
+    //   (int)amr_comunication::AmrOpcUAServer::logins_.size(), amr_comunication::AmrOpcUAServer::logins_.data());
+
+    // if (retval_ != UA_STATUSCODE_GOOD)
+    //   exit(1);
+
+    /* Set accessControl functions for nodeManagement */
+    config_->accessControl.allowAddNode = amr_comunication::AmrOpcUAServer::allowAddNode;
+    config_->accessControl.allowAddReference = amr_comunication::AmrOpcUAServer::allowAddReference;
+    config_->accessControl.allowDeleteNode = amr_comunication::AmrOpcUAServer::allowDeleteNode;
+    config_->accessControl.allowDeleteReference = amr_comunication::AmrOpcUAServer::allowDeleteReference;
+
+    // retval_ = UA_Server_run(server_ptr_, &amr_comunication::AmrOpcUAServer::running_);
+    initalized_ = true;
+  }
+}
+
+void amr_comunication::AmrOpcUAServer::start()
+{
+  try
+  {
+    if (server_ptr_ != nullptr && amr_comunication::AmrOpcUAServer::running_)
+    {
+      retval_ = UA_Server_run(server_ptr_, &amr_comunication::AmrOpcUAServer::running_);
+    }
+  }
+  catch (const std::exception& e)
+  {
+    std::cerr << e.what() << '\n';
+  }
+}
+
+void amr_comunication::AmrOpcUAServer::stop()
+{
+  try
+  {
+    if (server_ptr_ != nullptr && config_ != nullptr)
+    {
+      UA_ServerConfig_clean(config_);
+      UA_Server_delete(server_ptr_);
+    }
+  }
+  catch (const std::exception& e)
+  {
+    std::cerr << e.what() << '\n';
+  }
+}
+
+UA_Server* amr_comunication::AmrOpcUAServer::getServerObject()
+{
+  return server_ptr_;
+}
+
+UA_StatusCode amr_comunication::AmrOpcUAServer::statusCode()
+{
+  return retval_;
+}
+
+
+UA_Boolean amr_comunication::AmrOpcUAServer::allowAddNode(UA_Server* server, UA_AccessControl* ac,
+  const UA_NodeId* sessionId, void* sessionContext,
+  const UA_AddNodesItem* item)
+{
+  printf("Called allowAddNode\n");
+  return UA_TRUE;
+}
+
+UA_Boolean amr_comunication::AmrOpcUAServer::allowAddReference(UA_Server* server, UA_AccessControl* ac,
+  const UA_NodeId* sessionId, void* sessionContext,
+  const UA_AddReferencesItem* item)
+{
+  printf("Called allowAddReference\n");
+  return UA_TRUE;
+}
+
+UA_Boolean amr_comunication::AmrOpcUAServer::allowDeleteNode(UA_Server* server, UA_AccessControl* ac,
+  const UA_NodeId* sessionId, void* sessionContext,
+  const UA_DeleteNodesItem* item)
+{
+  printf("Called allowDeleteNode\n");
+  return UA_FALSE; // Do not allow deletion from client
+}
+
+UA_Boolean amr_comunication::AmrOpcUAServer::allowDeleteReference(UA_Server* server, UA_AccessControl* ac,
+  const UA_NodeId* sessionId, void* sessionContext,
+  const UA_DeleteReferencesItem* item)
+{
+  printf("Called allowDeleteReference\n");
+  return UA_TRUE;
+}
+
+void amr_comunication::AmrOpcUAServer::stopHandler(int sign)
+{
+  UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, (char*)"received ctrl-c");
+  running_ = false;
+}
+
+void amr_comunication::AmrOpcUAServer::staticSignalHandler(int sign)
+{
+  if (signalHandler_)
+  {
+    signalHandler_(sign);
+  }
+}
+
+

Controllers/Packages/amr_comunication/src/vda_5050/utils/line_common.cpp

@@ -0,0 +1,26 @@
+#include "amr_comunication/vda_5050/utils/line_common.h"
+
+double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2)
+{
+  double A = y2 - y1;
+  double B = -(x2 - x1);
+  double C = -A * x1 - B * y1;
+
+  return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B);
+}
+
+bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance)
+{
+  double crossProduct = distanceFromPointToLine(x0,  y0, x1, y1, x2, y2);
+  if (fabs(crossProduct) > fabs(tolerance)) 
+    return false;
+
+  // Kiểm tra tọa độ của P có nằm giữa A và B không
+  if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) &&
+      y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2))
+  {
+    return true;
+  }
+
+  return false;
+}

Controllers/Packages/amr_comunication/src/vda_5050/utils/pose.cpp

@@ -0,0 +1,39 @@
+#include "amr_comunication/vda_5050/utils/pose.h"
+
+
+bool vda_5050::Pose::SavePoseAsFile(const std::string& file_name)
+{
+  bool result = false;
+  std::ofstream file(file_name);
+  if (file.is_open())
+  {
+    file << x_ << " " << y_ << " " << yaw_ << "\n";
+    file.close();
+    result = true;
+  }
+  else
+  {
+    result = false;
+  }
+  return result;
+}
+
+bool vda_5050::Pose::LoadPoseFromFile(const std::string& file_name)
+{
+  bool result = false;
+  std::ifstream file(file_name);
+  if (file.is_open())
+  {
+    double x, y, yaw;
+    file >> x >> y >> yaw;
+    setPose(x, y, yaw);
+    file.close();
+    result = true;
+  }
+  else
+  {
+    result = false;
+  }
+  return result;
+}
+