git commit -m "first commit for v2"

.gitignore

@@ -0,0 +1,18 @@
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+Test
+# CMakeLists.txt
+Managerments/maps/activated_map.txt
+Managerments/maps/initial_pose.txt
+pnkx_nav/Navigations/Libraries/Ros/geometry2/tf2_ros/src/tf2_ros/*

Controllers/Packages/amr_comunication/CMakeLists.txt

@@ -0,0 +1,246 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(amr_comunication)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(PkgConfig REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+pkg_check_modules(open62541 REQUIRED open62541)
+pkg_check_modules(MOSQUITTO REQUIRED libmosquitto)
+
+include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES opc_ua_server vda_5050_connector ${PROJECT_NAME}_utils
+  CATKIN_DEPENDS roscpp std_msgs 
+  DEPENDS Boost open62541 MOSQUITTO EIGEN3
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+##  ${MOSQUITTO_INCLUDE_DIRS}
+  ${BOOST_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}_utils
+  src/vda_5050/utils/curve_common.cpp
+  src/vda_5050/utils/pose.cpp
+  src/vda_5050/utils/line_common.cpp
+)
+
+add_library(opc_ua_server
+  src/opc_ua/ua_server.cpp
+)
+
+add_library(vda_5050_connector
+  src/vda_5050/vda_5050_connector.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(opc_ua_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_utils ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(vda_5050_connector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(vda_5050_connector_node test/vda_5050_connector_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(opc_ua_server
+  ${FreeOpcUa_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  open62541
+)
+
+target_link_libraries(${PROJECT_NAME}_utils
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${Eigen_LIBRARIES}
+)
+
+target_link_libraries(vda_5050_connector
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${PROJECT_NAME}_utils
+  ${MOSQUITTO_LIBRARIES}
+)
+
+target_link_libraries(vda_5050_connector_node
+  vda_5050_connector
+  ${catkin_LIBRARIES}
+)
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS opc_ua_server vda_5050_connector ${PROJECT_NAME}_utils
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_comunication.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

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

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

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

@@ -0,0 +1,34 @@
+#ifndef COLOR_H_
+#define COLOR_H_
+
+#include <std_msgs/ColorRGBA.h>
+
+namespace agv_visualization
+{
+  class Color : public std_msgs::ColorRGBA
+  {
+  public:
+    Color() : std_msgs::ColorRGBA() {}
+    Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {}
+    Color(double red, double green, double blue, double alpha) : Color() {
+      r = red;
+      g = green;
+      b = blue;
+      a = alpha;
+    }
+
+    static const Color White() { return Color(1.0, 1.0, 1.0); }
+    static const Color Black() { return Color(0.0, 0.0, 0.0); }
+    static const Color Gray() { return Color(0.5, 0.5, 0.5); }
+    static const Color Red() { return Color(1.0, 0.0, 0.0); }
+    static const Color Green() { return Color(0.0, 1.0, 0.0); }
+    static const Color Blue() { return Color(0.0, 0.0, 1.0); }
+    static const Color Yellow() { return Color(1.0, 1.0, 0.0); }
+    static const Color Orange() { return Color(1.0, 0.5, 0.0); }
+    static const Color Purple() { return Color(0.5, 0.0, 1.0); }
+    static const Color Chartreuse() { return Color(0.5, 1.0, 0.0); }
+    static const Color Teal() { return Color(0.0, 1.0, 1.0); }
+    static const Color Pink() { return Color(1.0, 0.0, 0.5); }
+  };
+}
+#endif //COLOR_H_

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

@@ -0,0 +1,472 @@
+#ifndef _VDA_5050_COMMON_H_INCLUDE_
+#define _VDA_5050_COMMON_H_INCLUDE_
+#include <iostream>
+#include <vector>
+
+namespace vda_5050
+{
+  enum class OrderType
+  {
+    Base,
+    Horizon
+  };
+
+  struct NodePosition
+  {
+    double x, y, theta;
+    double allowedDeviationXY, allowedDeviationTheta;
+    std::string mapId, mapDescription;
+  };
+
+  struct ActionParameter
+  {
+    std::string key;
+    std::string value;
+  };
+
+  struct Action
+  {
+    std::string actionType;
+    std::string actionId;
+    std::string actionDescription;
+    std::string blockingType;
+    std::vector<ActionParameter> actionParameters;
+  };
+
+  struct Node
+  {
+    std::string nodeId;
+    int sequenceId;
+    std::string nodeDescription;
+    bool released;
+    NodePosition nodePosition;
+    std::vector<std::shared_ptr<Action>> actions;
+  };
+
+  struct Corridor
+  {
+    double leftWidth;
+    double rightWidth;
+    std::string corridorRefPoint;
+  };
+
+  struct ControlPoint
+  {
+    double x, y, weight;
+  };
+
+  struct Trajectory
+  {
+    int degree;
+    std::vector<double> knotVector;
+    std::vector<ControlPoint> controlPoints;
+  };
+
+  struct Edge
+  {
+    std::string edgeId;
+    int sequenceId;
+    std::string edgeDescription;
+    bool released;
+    std::string startNodeId;
+    std::string endNodeId;
+    double maxSpeed;
+    double maxHeight;
+    double minHeight;
+    double orientation;
+    std::string orientationType;
+    std::string direction;
+    bool rotationAllowed;
+    double maxRotationSpeed;
+    Trajectory trajectory;
+    double length;
+    Corridor corridor;
+    std::vector<Action> actions;
+  };
+
+  struct Order
+  {
+    int headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    std::string orderId;
+    int orderUpdateId;
+    std::string zoneSetId;
+    std::vector<Node> nodes;
+    std::vector<Edge> edges;
+
+    OrderType getTypeOrder()
+    {
+      for (auto &edge : edges)
+      {
+        if (!edge.released)
+          return OrderType::Horizon;
+      }
+      for (auto &node : nodes)
+      {
+        if (!node.released)
+          return OrderType::Horizon;
+      }
+      return OrderType::Base;
+    }
+  };
+
+  struct InstantAction
+  {
+    int headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    std::vector<std::shared_ptr<Action>> actions;
+  };
+
+  struct AGVPosition
+  {
+    double x;
+    double y;
+    double theta;
+    std::string mapId;
+    bool positionInitialized;
+    double localizationScore;
+    double deviationRange;
+  };
+
+  struct Velocity
+  {
+    double vx;
+    double vy;
+    double omega;
+  };
+
+  struct BatteryState
+  {
+    double batteryCharge;
+    double batteryVoltage;
+    int8_t batteryHealth;
+    bool charging;
+    uint32_t reach;
+  };
+
+  struct SafetyState
+  {
+    std::string eStop;
+    bool fieldViolation;
+    std::string AUTO_ACK = "autoAck";
+    std::string MANUAL = "manual";
+    std::string REMOTE = "remote";
+    std::string NONE = "none";
+  };
+
+  struct ErrorReference
+  {
+    std::string referenceKey;
+    std::string referenceValue;
+  };
+
+  struct Error
+  {
+    struct ErrorLevel
+    {
+      enum ErrorLevel_EN
+      {
+        WARNING,
+        FATAL
+      };
+
+      operator ErrorLevel_EN() const { return level; }
+
+      ErrorLevel &operator=(const ErrorLevel_EN &newType)
+      {
+        level = newType;
+        return *this;
+      }
+
+    private:
+      ErrorLevel_EN level;
+
+    public:
+      // Function to convert ActionStatus to string
+      std::string _toString() const
+      {
+        switch (level)
+        {
+        case WARNING:
+          return "WARNING";
+        case FATAL:
+          return "FATAL";
+        default:
+          return "";
+        }
+      }
+    };
+
+    std::string errorType;
+    std::vector<ErrorReference> errorReferences;
+    std::string errorDescription;
+    ErrorLevel errorLevel;
+  };
+
+  struct NodeState
+  {
+    std::string nodeId;
+    int32_t sequenceId;
+    std::string nodeDescription;
+    NodePosition nodePosition;
+    bool released;
+  };
+
+  struct EdgeState
+  {
+    std::string edgeId;
+    uint32_t sequenceId;
+    std::string edgeDescription;
+    bool released;
+    Trajectory trajectory;
+  };
+
+  struct ActionState
+  {
+    struct ActionStatus
+    {
+    public:
+      enum ActionStatus_EN
+      {
+        WAITING,
+        INITIALIZING,
+        RUNNING,
+        PAUSED,
+        FINISHED,
+        FAILED
+      };
+
+      operator ActionStatus_EN() const { return type; }
+
+      ActionStatus &operator=(const ActionStatus_EN &newType)
+      {
+        type = newType;
+        return *this;
+      }
+
+      bool operator==(const ActionStatus_EN &otherType) const
+      {
+        return type == otherType;
+      }
+
+    private:
+      ActionStatus_EN type;
+
+    public:
+      // Function to convert ActionStatus to string
+      std::string _toString() const
+      {
+        switch (type)
+        {
+        case WAITING:
+          return "WAITING";
+        case INITIALIZING:
+          return "INITIALIZING";
+        case RUNNING:
+          return "RUNNING";
+        case PAUSED:
+          return "PAUSED";
+        case FINISHED:
+          return "FINISHED";
+        case FAILED:
+          return "FAILED";
+        default:
+          return "";
+        }
+      }
+    };
+
+    std::string actionId;
+    std::string actionType;
+    std::string actionDescription;
+    ActionStatus actionStatus;
+    std::string resultDescription;
+  };
+
+  struct InfoReference
+  {
+    std::string referenceKey;
+    std::string referenceValue;
+  };
+
+  struct Info
+  {
+    std::string infoType;
+    std::vector<InfoReference> infoReferences;
+    std::string infoDescription;
+    std::string infoLevel;
+
+    std::string DEBUG = "DEBUG";
+    std::string INFO = "INFO";
+  };
+
+  struct BoundingBoxReference
+  {
+    double x;
+    double y;
+    double z;
+    double theta;
+  };
+
+  struct LoadDimensions
+  {
+    double length;
+    double width;
+    double height;
+  };
+
+  struct Load
+  {
+    std::string loadId;
+    std::string loadType;
+    std::string loadPosition;
+    BoundingBoxReference boundingBoxReference;
+    LoadDimensions loadDimensions;
+    uint32_t weight;
+  };
+
+  struct State
+  {
+    struct OperatingMode
+    {
+      enum OperatingMode_EN
+      {
+        AUTOMATIC,
+        SEMIAUTOMATIC,
+        MANUAL,
+        SERVICE,
+        TEACHING
+      };
+
+    private:
+      OperatingMode_EN type;
+
+    public:
+      // Constructor mặc định
+      OperatingMode() : type(SERVICE) {}
+
+      // Toán tử chuyển đổi sang enum
+      operator OperatingMode_EN() const { return type; }
+
+      OperatingMode &operator=(const OperatingMode_EN &newType)
+      {
+        type = newType;
+        return *this;
+      }
+
+      bool operator==(const OperatingMode_EN &otherType) const
+      {
+        return type == otherType;
+      }
+
+      // Chuyển đổi sang chuỗi
+      std::string _toString() const
+      {
+        switch (type)
+        {
+        case AUTOMATIC:
+          return "AUTOMATIC";
+        case SEMIAUTOMATIC:
+          return "SEMIAUTOMATIC";
+        case MANUAL:
+          return "MANUAL";
+        case SERVICE:
+          return "SERVICE";
+        case TEACHING:
+          return "TEACHING";
+        default:
+          return "SERVICE";
+        }
+      }
+    };
+
+    State()
+    {
+      headerId = 1;
+      version = "1.0.0";
+      manufacturer = "Phenikaa-X";
+      serialNumber = "";
+      velocity.vx = 0;
+      velocity.vy = 0;
+      velocity.omega = 0;
+      batteryState.batteryCharge = 100;
+      batteryState.batteryVoltage = 48;
+      batteryState.batteryHealth = 100;
+      batteryState.charging = false;
+      batteryState.reach = 0;
+      std::vector<Error> errors_;
+      errors = errors_;
+    }
+
+    uint32_t headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    std::string orderId;
+    uint32_t orderUpdateId;
+    std::string zoneSetId;
+    std::string lastNodeId;
+    uint32_t lastNodeSequenceId;
+    bool driving;
+    bool paused;
+    bool newBaseRequest;
+    double distanceSinceLastNode;
+    OperatingMode operatingMode;
+    std::vector<NodeState> nodeStates;
+    std::vector<EdgeState> edgeStates;
+    std::vector<std::shared_ptr<vda_5050::ActionState>> actionStates;
+    AGVPosition agvPosition;
+    Velocity velocity;
+    std::vector<Load> loads;
+    BatteryState batteryState;
+    std::vector<Error> errors;
+    std::vector<Info> information;
+    SafetyState safetyState;
+
+    OrderType getTypeOrder()
+    {
+      for (auto &edge : edgeStates)
+      {
+        if (!edge.released)
+          return OrderType::Horizon;
+      }
+      for (auto &node : nodeStates)
+      {
+        if (!node.released)
+          return OrderType::Horizon;
+      }
+      return OrderType::Base;
+    }
+  };
+
+  struct Visualization
+  {
+    Visualization()
+    {
+      headerId = 1;
+      version = "1.0.0";
+      manufacturer = "Phenikaa-X";
+      serialNumber = "AMR2WS";
+      velocity.vx = 0;
+      velocity.vy = 0;
+      velocity.omega = 0;
+    }
+
+    uint32_t headerId;
+    std::string timestamp;
+    std::string version;
+    std::string manufacturer;
+    std::string serialNumber;
+    AGVPosition agvPosition;
+    Velocity velocity;
+  };
+}
+
+#endif // _VDA_5050_COMMON_H_INCLUDE_

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

@@ -0,0 +1,43 @@
+#ifndef CONVERSION_H_
+#define CONVERSION_H_
+#include "curve_common.h"
+
+#include <Eigen/Geometry>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Quaternion.h>
+
+inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
+  return Eigen::Vector3d(msg.x, msg.y, msg.z);
+}
+
+inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
+{
+  EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
+  EigenTrajectoryPoint eigen_trajectory_point;
+
+  for(int i = 0; i < plan.size(); i++)
+  {
+    eigen_trajectory_point.position(0) = plan.at(i).pose.position.x;
+    eigen_trajectory_point.position(1) = plan.at(i).pose.position.y;
+    eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
+  }
+
+  return eigen_trajectory_point_vec;
+}
+
+inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
+{
+  EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
+  EigenTrajectoryPoint eigen_trajectory_point;
+
+  for(int i = 0; i < discreate_point_vec.size(); i++)
+  {
+    eigen_trajectory_point.position(0) = discreate_point_vec.at(i).x;
+    eigen_trajectory_point.position(1) = discreate_point_vec.at(i).y;
+    eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
+  }
+  
+  return eigen_trajectory_point_vec;
+}
+
+#endif //CONVERSION_H_

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

@@ -0,0 +1,81 @@
+#ifndef CURVE_COMMON_H_
+#define CURVE_COMMON_H_
+
+#include <Eigen/Eigen>
+#include <nav_msgs/Path.h>
+#include <geometry_msgs/Point.h>
+#include <visualization_msgs/Marker.h>
+#include "color.h"
+
+struct Spline_Inf
+{
+  int order;
+  std::vector<double> knot_vector;
+  std::vector<double> weight;
+  //std::vector<double> N;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > N;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > dN;
+  std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > ddN;
+  std::vector<double> N_double_vec;
+  std::vector<double> R_double_vec;
+  std::vector<double> dR_double_vec;
+  std::vector<double> ddR_double_vec;
+  // std::vector<double> ddN_double_vec;
+  //Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, Eigen::Dynamic> N; //bsaic function
+  Eigen::MatrixXd curvefit_N;
+
+};
+
+struct EigenTrajectoryPoint
+{
+  typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint> > Vector;
+  double u_data;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Eigen::Vector3d position;
+  //Eigen::VectorXd u_data;
+};
+
+class CurveCommon
+{
+public:
+  CurveCommon();
+  nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
+  nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
+  nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
+  nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
+  nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
+  nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
+
+  void CalculateDerivativeBasisFunc(Spline_Inf* spline_inf, double u_data, int differential_times);
+  geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf* spline_inf, double u_data, int differential_times, bool UsingNURBS);
+  geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, bool UsingNURBS);
+  double CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
+  double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
+  Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
+  double CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
+  double CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS);
+  bool curveIsValid(int degree, const std::vector<double>& knot_vector,
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
+
+  void ReadSplineInf(Spline_Inf* bspline_inf, int order, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point, std::vector<double> knot_vector);
+  void ReadSplineInf(Spline_Inf* bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
+  void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector* input_point, std::vector<double> file_discreate_point);
+  void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >* input_point, std::vector<double> file_discreate_point);
+  void ShowDiscreatePoint(visualization_msgs::Marker* points, EigenTrajectoryPoint::Vector discreate_point);
+  void ShowDiscreatePoint(visualization_msgs::Marker* points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
+
+  //TODO: move relate visualize function to new vislization.h 
+  int print();
+  visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
+  visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
+  visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
+  visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
+private:
+
+
+};
+
+#endif //CURVE_COMMON_H_

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

@@ -0,0 +1,14 @@
+#ifndef __LINE_COMMON_H_
+#define __LINE_COMMON_H_
+
+#include <ros/ros.h>
+#include <cmath>
+
+namespace vda_5050_utils
+{
+  double distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2);
+
+  bool isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance);
+}
+
+#endif

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

@@ -0,0 +1,66 @@
+#ifndef AMR_CONTROL_POSE_H
+#define AMR_CONTROL_POSE_H
+#include <stdio.h>
+#include <iostream>
+#include <stdlib.h>
+#include <unistd.h>
+#include <vector>
+#include <fstream>
+#include <math.h>
+#include <cmath>
+
+namespace vda_5050
+{
+  class Pose {
+  private:
+    double x_, y_, yaw_;
+    double v_max_, accuracy_;
+
+    void modifyYaw(void) {
+      while (yaw_ < -M_PI)
+        yaw_ += 2.0 * M_PI;
+      while (yaw_ > M_PI)
+        yaw_ -= 2.0 * M_PI;
+    }
+
+  public:
+    Pose() :
+      x_(0.0), y_(0.0), yaw_(0.0), v_max_(0.0), accuracy_(0.0) {
+    };
+
+    Pose(double x, double y, double yaw) :
+      x_(x), y_(y), yaw_(yaw), v_max_(0.0), accuracy_(0.0) {
+    };
+
+    Pose(double x, double y, double yaw, double v_max, double accuracy) :
+      x_(x), y_(y), yaw_(yaw), v_max_(v_max), accuracy_(accuracy) {
+    };
+
+    ~Pose() {};
+
+    inline void setX(double x) { x_ = x; }
+    inline void setY(double y) { y_ = y; }
+    inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); }
+    inline void setVmax(double v_max) { v_max_ = v_max; }
+    inline void setAccuracy(double accuracy) { accuracy_ = accuracy; }
+    inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); }
+    inline void setPose(double x, double y, double yaw, double v_max, double accuracy)
+    {
+      x_ = x, y_ = y, yaw_ = yaw, v_max_ = v_max, accuracy_ = accuracy, modifyYaw();
+    }
+    inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); }
+
+    inline double getX(void) { return x_; }
+    inline double getY(void) { return y_; }
+    inline double getYaw(void) { return yaw_; }
+    inline double getVmax(void) { return v_max_; }
+    inline double getAccuracy(void) { return accuracy_; }
+    inline Pose getPose(void) { return Pose(x_, y_, yaw_, v_max_, accuracy_); }
+    bool SavePoseAsFile(const std::string& file_name);
+    bool LoadPoseFromFile(const std::string& file_name);
+
+  }; // class Pose
+
+}
+
+#endif

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

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

Controllers/Packages/amr_comunication/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>amr_comunication</name>
+  <version>0.0.0</version>
+  <description>The amr_comunication package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/amr_comunication</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

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;
+}
+

Controllers/Packages/amr_comunication/test/vda_5050_connector_node.cpp

@@ -0,0 +1,21 @@
+#include <ros/ros.h>
+#include "amr_comunication/vda_5050/vda_5050_connector.h"
+
+std::shared_ptr<amr_comunication::VDA5050Connector> client;
+
+void TestOrderWorker()
+{
+
+}
+
+int main(int argc, char** argv) {
+  // Initialize the ROS node
+  ros::init(argc, argv, "test_imr_nova_control");
+  ros::start();
+
+  client = std::make_shared<amr_comunication::VDA5050Connector>();
+  client->execute_order_ = TestOrderWorker;
+  ros::spin();
+  client.reset();
+  return 0;
+}

Controllers/Packages/amr_control/CMakeLists.txt

@@ -0,0 +1,243 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(amr_control)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_2d_utils
+  roscpp
+  rospy
+  std_msgs
+  angles
+  delta_modbus
+  nova5_control
+  loc_core
+  move_base_core
+  amr_comunication
+  vda5050_msgs
+)
+
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(PkgConfig)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES amr_control 
+  CATKIN_DEPENDS geometry_msgs move_base_core loc_core nav_2d_utils roscpp rospy std_msgs
+  DEPENDS Boost 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/amr_control.cpp
+  src/amr_monitor.cpp
+  src/amr_safety.cpp
+  src/amr_opc_ua_server_api.cpp
+  src/amr_vda_5050_client_api.cpp
+  src/amr_make_plan_with_order.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp)
+add_executable(vda_5050_api_test test/vda_5050_api.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+#set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME})
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node 
+  ${PROJECT_NAME}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS} 
+  ${catkin_EXPORTED_TARGETS}
+)
+
+add_dependencies(vda_5050_api_test
+  ${PROJECT_NAME}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS} 
+  ${catkin_EXPORTED_TARGETS}
+)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${FreeOpcUa_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+target_link_libraries(vda_5050_api_test
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME} 
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_control.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Controllers/Packages/amr_control/include/amr_control/amr_control.h

@@ -0,0 +1,122 @@
+#ifndef __AMR_COTROLLER_H_INCLUDED_
+#define __AMR_COTROLLER_H_INCLUDED_
+#include <ros/ros.h>
+#include <memory>
+#include <string>
+#include <condition_variable>
+#include <pluginlib/class_loader.h>
+#include <tf2/convert.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <std_msgs/Bool.h>
+
+#include "delta_modbus/delta_modbus_tcp.h"
+
+// API
+#include "amr_control/amr_opc_ua_server_api.h"
+#include "amr_control/amr_vda_5050_client_api.h"
+
+// Objects
+#include "move_base_core/navigation.h"
+#include "loc_core/localization.h"
+#include "amr_control/amr_monitor.h"
+#include "nova5_control/imr_nova_control.h"
+#include "amr_control/amr_safety.h"
+
+namespace amr_control
+{
+  class AmrController
+  {
+  public:
+    AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
+    AmrController();
+    virtual ~AmrController();
+
+    void init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
+
+    /**
+     * @brief Stop the ServerHandle
+     */
+    virtual void stop(void);
+
+  protected:
+    virtual void initalizingComunicationHandle(ros::NodeHandle &nh);
+    virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh);
+    virtual void initalizingLocalizationHandle(ros::NodeHandle &nh);
+    virtual void initalizingMonitorHandle(ros::NodeHandle &nh);
+
+  private:
+    void ArmCallBack();
+
+    void ArmDotuff();
+
+    void unLoadCallBack();
+
+    void conveyorBeltsShipping(amr_control::State &state);
+
+    void loadCallBack();
+
+    void conveyorBeltsReceiving(amr_control::State &state);
+
+    void controllerDotuff();
+
+    void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg);
+
+    void threadHandle();
+
+    bool initalized_;
+    ros::NodeHandle nh_;
+    std::shared_ptr<tf2_ros::Buffer> tf_;
+    ros::Subscriber is_detected_maker_sub_;
+
+    std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
+    std::shared_ptr<amr_control::VDA5050ClientAPI> vda_5050_client_api_ptr_;
+    std::shared_ptr<std::thread> server_thr_;
+
+    std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
+    std::shared_ptr<std::thread> monitor_thr_;
+
+    std::shared_ptr<move_base_core::BaseNavigation> move_base_ptr_;
+    pluginlib::ClassLoader<move_base_core::BaseNavigation> move_base_loader_;
+    // Synchronous processing thread
+    std::shared_ptr<std::thread> move_base_thr_;
+    std::mutex init_move_base_mutex_;
+    std::condition_variable init_move_base_cv_;
+    bool move_base_initialized_;
+
+    // Safety Speed
+    std::shared_ptr<AmrSafety> amr_safety_ptr_;
+    bool muted_;
+    std::mutex cmd_vel_mtx;
+    nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_recommended_;
+
+    std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
+    pluginlib::ClassLoader<loc_core::BaseLocalization> loc_base_loader_;
+    // Synchronous processing thread
+    std::shared_ptr<std::thread> loc_base_thr_;
+    std::mutex init_loc_base_mutex_;
+    std::condition_variable init_loc_base_cv_;
+    bool loc_base_initialized_;
+
+    bool cancel_, enable_ ,pause_;
+    // Belt properties
+    std::thread belt_thread_;
+    amr_control::State cur_belt_state_en_;
+    bool belt_joined_;
+    std::mutex belt_mutex_;
+
+    // amr propertise
+    std::thread arm_thread_; 
+    bool arm_joined_;
+    std::mutex arm_mutex_;
+    bool arm_continue_;
+    bool arm_go_home_;
+    bool arm_power_on_;
+    unsigned int count_ng_max_, count_ok_max_;
+    unsigned int *status_code_ptr_ = new unsigned int;
+
+  }; // class AMRController
+} // namespace amr_control
+
+#endif // __AMR_COTROLLER_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/amr_make_plan_with_order.h

@@ -0,0 +1,67 @@
+#ifndef _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
+#define _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
+#include <ros/ros.h>
+#include "amr_comunication/vda_5050/utils/curve_common.h"
+#include "amr_comunication/vda_5050/utils/pose.h"
+#include <amr_comunication/vda_5050/utils/common.h>
+
+
+namespace amr_control
+{
+  inline double getYawDegree(double x, double y, double z, double w) {
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (w * z + x * y);
+    double cosy_cosp = 1 - 2 * (y * y + z * z);
+    double yaw = std::atan2(siny_cosp, cosy_cosp);
+    yaw = (180 * yaw) / M_PI;return yaw;
+  }
+
+  inline double getYawRad(double x, double y, double z, double w) {
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (w * z + x * y);
+    double cosy_cosp = 1 - 2 * (y * y + z * z);
+    double yaw = std::atan2(siny_cosp, cosy_cosp);
+    return yaw;
+  }
+
+  inline double calculateAngle(double xA, double yA, double xB, double yB) {
+    double deltaX = xB - xA;
+    double deltaY = yB - yA;
+    double angleRad = atan2(deltaY, deltaX);
+    // double angleDeg = angleRad * 180.0 / M_PI;
+    return angleRad;
+  }
+
+  inline void modifyYaw(double& yaw) {
+    while (yaw < -M_PI)
+      yaw += 2.0 * M_PI;
+    while (yaw > M_PI)
+      yaw -= 2.0 * M_PI;
+  };
+
+  double calculateDistance(vda_5050::Pose& pose1, vda_5050::Pose& pose2);
+
+  double computeDeltaAngle(vda_5050::Pose& Pose1, vda_5050::Pose& Pose2);
+
+  bool isThetaValid(double theta);
+
+  bool curveIsValid(int degree, const std::vector<double>& knot_vector, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
+
+  double computeDeltaAngleStartNode(double theta, vda_5050::Pose& startPose, vda_5050::Pose& next_Pose);
+
+  double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose& Pose);
+
+  double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose);
+
+  double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose& Pose);
+
+  double computeDeltaAngleEndNode(double theta, vda_5050::Pose& endPose, vda_5050::Pose& prev_Pose);
+
+  double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose);
+
+  void setYawAllPosesOnEdge(std::vector<vda_5050::Pose>& posesOnEdge, bool reverse);
+
+  bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t& status, std::string& message, std::vector<vda_5050::Pose>& posesOnPathWay);
+}
+
+#endif // _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_

Controllers/Packages/amr_control/include/amr_control/amr_monitor.h

@@ -0,0 +1,49 @@
+#ifndef __AMR_MONITOR_H_INCLUDED_
+#define __AMR_MONITOR_H_INCLUDED_
+#include <ros/ros.h>
+#include <memory>
+#include <string>
+#include <nav_2d_msgs/Twist2D.h>
+#include <nav_2d_utils/odom_subscriber.h>
+
+namespace amr_control
+{
+  class AmrMonitor
+  {
+  public:
+    /**
+     * @brief Default constructor for AmrMonitor
+     */
+    AmrMonitor();
+
+    /**
+     * @brief Constructor for AmrMonitor with ROS node handle
+     * @param nh ROS NodeHandle for initialization
+     */
+    AmrMonitor(const ros::NodeHandle& nh);
+
+    /**
+     * @brief Virtual destructor for AmrMonitor
+     */
+    virtual ~AmrMonitor();
+
+    /**
+     * @brief Initialize the ServerHandle with a ROS node handle
+     * @param nh ROS NodeHandle for initialization
+     */
+    virtual void init(const ros::NodeHandle& nh);
+
+    /**
+     * @brief Get velocity 
+     * @return True if get done
+     */
+    bool getVelocity(nav_2d_msgs::Twist2D &velocity);
+
+  private:
+
+    std::shared_ptr <nav_2d_utils::OdomSubscriber> odom_sub_;
+    bool initalized_;
+  };
+} // namespace amr_control
+
+#endif // __AMR_MONITOR_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h

@@ -0,0 +1,912 @@
+#ifndef __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_
+#define __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_
+
+#include <ros/ros.h>
+#include "move_base_core/navigation.h"
+#include "loc_core/localization.h"
+#include "amr_comunication/opc_ua/ua_server.h"
+#include "amr_control/amr_monitor.h"
+#include "amr_control/common.h"
+#include <pthread.h>
+
+namespace amr_control
+{
+  class OpcUAServerAPI
+  {
+  public:
+    /**
+     * @brief Contructor
+     * @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện.
+     * @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot.
+     * @param loc_base Con trỏ đến đối tượng LocBase.
+     */
+    OpcUAServerAPI(const ros::NodeHandle &nh, std::shared_ptr<move_base_core::BaseNavigation> move_base,
+                   std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<amr_control::AmrMonitor> monitor);
+
+    /**
+     * @brief Detructor
+     */
+    virtual ~OpcUAServerAPI();
+
+    /**
+     * @brief Start 
+     */
+    void start();
+
+  private:
+    /**
+     * @brief Định nghĩa các đối tượng cần thiết cho API người dùng.
+     *
+     * Hàm này cho phép người dùng định nghĩa và cấu hình các đối tượng cần thiết
+     * cho việc tương tác với server, monitor và hệ thống điều hướng.
+     * Người dùng có thể sử dụng các đối tượng này để thực hiện các hành động
+     * như di chuyển, nhặt đồ, thả đồ, và các hành động khác liên quan đến robot.
+     */
+    virtual void defineObjects();
+
+    /** SLAM**/
+    /**
+     * @brief Thêm phương thức bắt đầu quét map.
+     *        Bắt đầu quá trình tạo bản đồ. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này.
+     *        Khi gọi thành công thì trạng thái biến "SlamState" sẽ chuyển thành "Mapping".
+     *
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addStartMappingMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức bắt đầu quét map.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức dừng quét map.
+     *        Kết thúc quá trình tạo bản đồ, truyền vào tên map để lưu trữ.
+     *        Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Ready".
+     *
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addStopMappingMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức dừng quét map.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức bắt đầu định vị bản đồ.
+     *        Bắt đầu chế độ Localization. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này.
+     *        Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Localization".
+     *
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức bắt đầu định vị bản đồ.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức kết thúc quá trình định vị.
+     *        Tắt chế độ Localization.Lúc này thông tin toạ độ robot sẽ không được cập nhật.
+     *        Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Ready".
+     *
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức kết thúc quá trình định vị.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức liệt kê các file bản đồ.
+     *
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addListMapFilesMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức liệt kê các file bản đồ.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức chọn map file.
+     *        Chọn một map để Localization. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này.
+     *
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addActivateMapMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức chọn map file.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức khai báo vị trí vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức khai báo vị trí
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm thuộc tính tọa độ của robot vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addRobotPoseProperty(UA_Server *server, UA_NodeId parentID);
+
+    static void slamHandle();
+
+    /** Navigation**/
+
+    /**
+     * @brief Thêm phương thức di chuyển đến một nút cụ thể vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức di chuyển đến nút.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức di chuyển đến vị trí nhặt đồ vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addDockToNodeMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức nhặt đồ.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+
+    /**
+     * @brief Thêm phương thức quay vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addRotateToMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức quay.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức di chuyển thẳng vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addMoveStraightMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức di chuyển thẳng.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức hủy bỏ vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addCancelMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức hủy bỏ.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức tạm dừng vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addPauseMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức tạm dừng.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức tiếp tục vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addResumeMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức tiếp tục.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức reset vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addResetMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức reset.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức bật tắt vùng Muted vào server.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addWriteMutedMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức bật tắt vùng Muted.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm thuộc tính trạng thái của Robot khi di chuyển.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addNavigationState(UA_Server *server, UA_NodeId parentID);
+
+    static void navigationHandle();
+
+    /** Moritoring **/
+    /**
+     * @brief Thêm thuộc tính vận tốc của Robot khi di chuyển.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addVelocityCommand(UA_Server *server, UA_NodeId parentID);
+
+    static void monitorHandle();
+
+    /** IMR Plus **/
+    /**
+     * @brief Thêm phương thức khởi động và chạy tay máy Dobot.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức khởi động và chạy tay máy Dobot.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức về gốc tay máy Dobot.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức về gốc tay máy Dobot.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thay đổi số lượng hàng cần lấy
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addCountMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức thay đổi số lượng hàng cần lấy
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức dừng tay máy Dobot.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addCancelDobotMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức dừng tay máy Dobot.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức tiếp tục chạy tay máy Dobot.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addContinueDobotMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức tiếp tục chạy tay máy Dobot.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức khởi động nguồn tay máy Dobot.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức khởi động nguồn tay máy Dobot.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm thuộc tính tay máy Dobot.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addDobotProperties(UA_Server *server, UA_NodeId parentID);
+
+    static void dobotPropertiesHandle();
+    /**
+     * @brief Thêm phương thức trả hàng.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addunLoadMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức trả hàng.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức nhận hàng.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addLoadMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức nhận hàng.
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+
+    /**
+     * @brief Thêm phương thức Reset Băng tải
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID);
+
+    /**
+     * @brief Callback cho phương thức Reset Băng tải
+     * @param server Con trỏ đến server.
+     * @param sessionId ID của phiên làm việc.
+     * @param sessionContext Ngữ cảnh của phiên làm việc.
+     * @param methodId ID của phương thức.
+     * @param methodContext Ngữ cảnh của phương thức.
+     * @param objectId ID của đối tượng.
+     * @param objectContext Ngữ cảnh của đối tượng.
+     * @param inputSize Kích thước của dữ liệu đầu vào.
+     * @param input Dữ liệu đầu vào.
+     * @param outputSize Kích thước của dữ liệu đầu ra.
+     * @param output Dữ liệu đầu ra.
+     * @return UA_StatusCode Trạng thái của callback.
+     */
+    static UA_StatusCode 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);
+    /**
+     * @brief Thêm thuộc tính trạng thái băng tải.
+     * @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
+     * @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
+     */
+    static void addConveyorBeltState(UA_Server *server, UA_NodeId parentID);
+
+    static void ConveyorBeltHandle();
+
+  private:
+    ros::NodeHandle nh_;
+    static ros::Publisher init_pub_;
+    static pthread_t hThread_;
+    static void *ThreadWorker(void *_);
+
+    static std::shared_ptr<amr_comunication::AmrOpcUAServer> server_ptr_;
+    static std::shared_ptr<move_base_core::BaseNavigation> move_base_ptr_;
+    static std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
+    static std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
+
+    static UA_NodeId *robotPoseX_Id_;
+    static UA_NodeId *robotPoseY_Id_;
+    static UA_NodeId *robotPoseYaw_Id_;
+    static UA_NodeId *currentActiveMap_Id_;
+    static UA_NodeId *slamState_Id_;
+    static UA_NodeId *workingDirectory_Id_;
+
+    static UA_String amr_feedback_str_;
+    static UA_String amr_status_str_;
+    static UA_UInt32 amr_status_en_;
+    static UA_NodeId *amr_status_str_Id_;
+    static UA_NodeId *amr_status_en_Id_;
+    static UA_NodeId *amr_robotFeedBack_Id_;
+
+    static UA_NodeId *vx_Id_;
+    static UA_NodeId *vy_Id_;
+    static UA_NodeId *omega_Id_;
+
+    static UA_NodeId *arm_dobot_state_str_Id_;
+    static UA_NodeId *arm_dobot_state_en_Id_;
+    static UA_NodeId *arm_dobot_count_ok_max_Id_;
+    static UA_NodeId *arm_dobot_count_ng_max_Id_;
+    static UA_NodeId *arm_dobot_mode_Id_;
+    static UA_NodeId *arm_dobot_status_code_Id_;
+
+    static UA_NodeId *belt_status_str_Id_;
+    static UA_NodeId *belt_status_en_Id_;
+    static UA_NodeId *have_goods_Id_;
+
+  public:
+    static void resetState();
+
+    /**
+     * @brief helper class for subscribing to odometry
+     */
+    static nav_2d_msgs::Twist2D cmd_vel_max_;
+
+    /**
+     * @brief Set muted value
+     */
+    static bool *muted_value_;
+
+    static std::thread *arm_thread_ptr_;
+    static std::function<void()> arm_function_ptr_;
+    static amr_control::State arm_dobot_state_en_;
+    static UA_String arm_dobot_state_str_;
+    static unsigned int *count_ng_max_, *count_ok_max_;
+    static bool *arm_cancel_;
+    static bool *arm_continue_;
+    static bool *arm_go_home_;
+    static bool *arm_power_on_;
+    static double *mode_ptr_;
+    static unsigned int *status_code_ptr_;
+    static unsigned int old_status_code_;
+
+    static amr_control::State *cur_belt_state_en_;
+    static UA_String belt_state_str_;
+    static UA_Boolean have_goods_;
+    static bool *belt_cancel_;
+    static std::function<void()> unLoad_excuted_;
+    static std::function<void()> load_excuted_;
+    static std::thread *belt_thread_ptr_;
+  };
+} // namespace amr_control
+
+#endif // __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/amr_safety.h

@@ -0,0 +1,55 @@
+#ifndef __AMR_SAFETY_H_INCLUDED_
+#define __AMR_SAFETY_H_INCLUDED_
+#include <ros/ros.h>
+#include <memory>
+#include <string>
+#include <nav_2d_msgs/Twist2D.h>
+#include "delta_modbus/delta_modbus_tcp.h"
+
+namespace amr_control
+{
+  class AmrSafety
+  {
+  public:
+    /**
+     * @brief Default constructor for AmrSafety
+     */
+    AmrSafety();
+
+    /**
+     * @brief Constructor for AmrSafety with ROS node handle
+     * @param nh ROS NodeHandle for initialization
+     */
+    AmrSafety(const ros::NodeHandle &nh);
+
+    /**
+     * @brief Virtual destructor for AmrSafety
+     */
+    virtual ~AmrSafety();
+
+    /**
+     * @brief Initialize the ServerHandle with a ROS node handle
+     * @param nh ROS NodeHandle for initialization
+     */
+    virtual void init(const ros::NodeHandle &nh);
+
+    /**
+     * @brief Get object PLC controller
+     * @param plc_controller_ptr A smart pointer of the PLC object
+     */
+    virtual bool getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr);
+
+    /**
+     * @brief Safety Hanlde
+     */
+    virtual void safetyHandle(nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel);
+
+    virtual void writeMutesSafety(bool value);
+
+  private:
+    bool initalized_;
+    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+  };
+} // namespace amr_control
+
+#endif // __AMR_SAFETY_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/amr_vda_5050_client_api.h

@@ -0,0 +1,122 @@
+#ifndef __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_
+#define __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_
+
+#include <ros/ros.h>
+#include "amr_comunication/vda_5050/vda_5050_connector.h"
+#include "move_base_core/navigation.h"
+#include "loc_core/localization.h"
+#include "amr_control/amr_monitor.h"
+#include "amr_control/common.h"
+#include <nav_2d_utils/odom_subscriber.h>
+#include <nav_2d_msgs/Twist2D.h>
+
+namespace amr_control
+{
+  class VDA5050ClientAPI
+  {
+  public:
+    /**
+     * @brief Contructor
+     */
+    VDA5050ClientAPI();
+
+    /**
+     * @brief Contructor
+     * @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện.
+     * @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot.
+     * @param loc_base Con trỏ đến đối tượng LocBase.
+     */
+    VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<move_base_core::BaseNavigation> move_base,
+                     std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<amr_control::AmrMonitor> monitor);
+
+    /**
+     * @brief Detructor
+     */
+    virtual ~VDA5050ClientAPI();
+
+  protected:
+    virtual void orderWorker();
+
+    virtual void moveTo(vda_5050::Order order, uint8_t &status, std::string &message);
+
+    virtual void moveToDock(vda_5050::Order order, uint8_t &status, std::string &message);
+
+    virtual void rotateTo(vda_5050::Order order, uint8_t &status, std::string &message);
+
+    virtual void instantActionWorker();
+
+    virtual void excuteAction();
+
+    virtual void updateVelocity(double velocity);
+
+    virtual void updateAngular(double angular);
+
+    virtual void cancelOrderCallBack(std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void cancelOrder(std::shared_ptr<vda_5050::ActionState> action_state);
+
+    template <typename T>
+    void excuteNoneAction(
+        void (T::*excute)(std::shared_ptr<vda_5050::Action>, std::shared_ptr<vda_5050::ActionState>), T *obj,
+        std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void mutedSafetyON(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    virtual void mutedSafetyOFF(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
+
+    void updating();
+
+    void updatingvda5050Visualization();
+
+    void updatingvda5050State();
+
+    void resetState();
+
+    // properties
+    std::shared_ptr<amr_comunication::VDA5050Connector> client_ptr_;
+    static std::shared_ptr<move_base_core::BaseNavigation> move_base_ptr_;
+    static std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
+    static std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
+
+    ros::Publisher plan_pub_;
+    std::string global_plan_msg_type_;
+    std::thread update_thread_;
+
+    std::map<std::string, std::thread> threads_map_;
+    std::mutex threads_mutex;
+
+  public:
+    // Belt Convey
+    amr_control::State *cur_belt_state_en_;
+    std::function<void()> unLoad_excuted_;
+    std::function<void()> load_excuted_;
+    std::thread *belt_thread_ptr_;
+    std::mutex belt_mutex_;
+    bool belt_joined_;
+
+    // arm control
+    std::thread *arm_thread_ptr_;
+    std::function<void()> arm_function_ptr_;
+    unsigned int *count_ng_max_, *count_ok_max_;
+
+    bool *muted_value_;
+    bool *pause_action_;
+    bool *cancel_action_;
+    bool *enable_action_;
+    vda_5050::State::OperatingMode mode_;
+    nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_max_saved_;
+  };
+
+} // namespace amr_control
+
+#endif // __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_

Controllers/Packages/amr_control/include/amr_control/common.h

@@ -0,0 +1,25 @@
+#ifndef _AMR_CONTROL_COMMON_H_INCLUDED_
+#define _AMR_CONTROL_COMMON_H_INCLUDED_
+
+namespace amr_control
+{
+  enum State
+  {
+    WAITING,
+    INITIALIZING,
+    RUNNING,
+    PAUSED,
+    FINISHED,
+    FAILED
+  };
+
+  enum OperatingMode
+  {
+    AUTOMATIC,
+    SEMIAUTOMATIC,
+    MANUAL,
+    SERVICE,
+    TEACHING
+  };
+}
+#endif

Controllers/Packages/amr_control/package.xml

@@ -0,0 +1,96 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>amr_control</name>
+  <version>0.0.0</version>
+  <description>The amr_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/amr_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>move_base_core</build_depend>
+  <build_depend>nav_2d_utils</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>angles</build_depend>
+  <build_depend>delta_modbus</build_depend>
+  <build_depend>nova5_control</build_depend>
+  <build_depend>loc_core</build_depend>
+  <build_depend>amr_comunication</build_depend>
+  <build_depend>vda5050_msgs</build_depend>
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>move_base_core</build_export_depend>
+  <build_export_depend>nav_2d_utils</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>angles</build_export_depend>
+  <build_export_depend>delta_modbus</build_export_depend>
+  <build_export_depend>nova5_control</build_export_depend>
+  <build_export_depend>loc_core</build_export_depend>
+  <build_export_depend>amr_comunication</build_export_depend>
+  <build_export_depend>vda5050_msgs</build_export_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>move_base_core</exec_depend>
+  <exec_depend>nav_2d_utils</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>angles</exec_depend>
+  <exec_depend>delta_modbus</exec_depend>
+  <exec_depend>nova5_control</exec_depend>
+  <exec_depend>loc_core</exec_depend>
+  <exec_depend>amr_comunication</exec_depend>
+  <exec_depend>vda5050_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Controllers/Packages/amr_control/src/amr_control.cpp

@@ -0,0 +1,578 @@
+#include "amr_control/amr_control.h"
+#include <geometry_msgs/Vector3.h>
+
+namespace amr_control
+{
+  AmrController::AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
+      : initalized_(false),
+        vda_5050_client_api_ptr_(nullptr),
+        opc_ua_server_api_ptr_(nullptr),
+        move_base_initialized_(false),
+        loc_base_initialized_(false),
+        monitor_ptr_(nullptr),
+        loc_base_ptr_(nullptr),
+        move_base_ptr_(nullptr),
+        amr_safety_ptr_(nullptr),
+        move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
+        loc_base_loader_("loc_core", "loc_core::BaseLocalization")
+  {
+    this->init(nh, buffer);
+  }
+
+  AmrController::AmrController()
+      : initalized_(false),
+        move_base_initialized_(false),
+        loc_base_initialized_(false),
+        vda_5050_client_api_ptr_(nullptr),
+        opc_ua_server_api_ptr_(nullptr),
+        monitor_ptr_(nullptr),
+        loc_base_ptr_(nullptr),
+        move_base_ptr_(nullptr),
+        amr_safety_ptr_(nullptr),
+        move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
+        loc_base_loader_("loc_core", "loc_core::BaseLocalization")
+  {
+  }
+
+  AmrController::~AmrController()
+  {
+    // Wait for both threads to complete
+    if (move_base_thr_)
+    {
+      move_base_thr_->join();
+      move_base_thr_.reset();
+    }
+
+    if (loc_base_thr_)
+    {
+      loc_base_thr_->join();
+      loc_base_thr_.reset();
+    }
+
+    if (server_thr_)
+    {
+      server_thr_->join();
+      server_thr_.reset();
+    }
+
+    if (monitor_thr_)
+    {
+      monitor_thr_->join();
+      monitor_thr_.reset();
+    }
+
+    if (opc_ua_server_api_ptr_)
+    {
+      opc_ua_server_api_ptr_.reset();
+    }
+
+    if (vda_5050_client_api_ptr_)
+    {
+      vda_5050_client_api_ptr_.reset();
+    }
+  }
+
+  void AmrController::init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
+  {
+    if (!initalized_)
+    {
+      nh_ = nh;
+      tf_ = buffer;
+
+      monitor_thr_ = std::make_shared<std::thread>(
+          [this]()
+          {
+            this->initalizingMonitorHandle(nh_);
+            this->threadHandle();
+          });
+
+      move_base_initialized_ = false;
+      // Setup base localization
+      loc_base_thr_ = std::make_shared<std::thread>(
+          [this]()
+          {
+            this->initalizingLocalizationHandle(nh_);
+            {
+              std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
+              loc_base_initialized_ = true;
+            }
+            init_loc_base_cv_.notify_one();
+          });
+
+      // Setup base navigation
+      move_base_thr_ = std::make_shared<std::thread>(
+          [this]()
+          {
+            std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
+            init_loc_base_cv_.wait(lock, [this]()
+                                   { return loc_base_initialized_; });
+
+            this->initalizingMoveBaseHandle(nh_);
+            {
+              std::lock_guard<std::mutex> lock(init_move_base_mutex_);
+              move_base_initialized_ = true;
+            }
+            init_move_base_cv_.notify_one();
+
+            // this->controllerDotuff();
+          });
+
+      // Setup server
+      server_thr_ = std::make_shared<std::thread>(
+          [this]()
+          {
+            std::unique_lock<std::mutex> lock(init_move_base_mutex_);
+            init_move_base_cv_.wait(lock, [this]()
+                                    { return move_base_initialized_; });
+            this->initalizingComunicationHandle(nh_);
+          });
+
+      ros::NodeHandle nh_core;
+      is_detected_maker_sub_ = nh_core.subscribe("/is_detected_maker", 1, &AmrController::isDetectedMakerCallback, this);
+      initalized_ = true;
+    }
+  }
+
+  void AmrController::stop()
+  {
+  }
+
+  void AmrController::initalizingComunicationHandle(ros::NodeHandle &nh)
+  {
+
+    this->count_ng_max_ = this->count_ok_max_ = 1;
+    std::shared_ptr opc_ua_server_thr = std::make_shared<std::thread>(
+        [this, nh]()
+        {
+          opc_ua_server_api_ptr_ =
+              std::make_shared<OpcUAServerAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);
+
+          OpcUAServerAPI::muted_value_ = &this->muted_;
+          OpcUAServerAPI::unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
+          OpcUAServerAPI::load_excuted_ = std::bind(&AmrController::loadCallBack, this);
+          OpcUAServerAPI::belt_thread_ptr_ = &this->belt_thread_;
+          OpcUAServerAPI::cur_belt_state_en_ = &this->cur_belt_state_en_;
+          OpcUAServerAPI::belt_cancel_ = &this->cancel_;
+
+          OpcUAServerAPI::arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
+          OpcUAServerAPI::arm_thread_ptr_ = &this->arm_thread_;
+          OpcUAServerAPI::arm_cancel_ = &this->enable_;
+          OpcUAServerAPI::arm_go_home_ = &this->arm_go_home_;
+          OpcUAServerAPI::arm_continue_ = &this->arm_continue_;
+          OpcUAServerAPI::arm_power_on_ = &this->arm_power_on_;
+          OpcUAServerAPI::count_ng_max_ = &this->count_ng_max_;
+          OpcUAServerAPI::count_ok_max_ = &this->count_ok_max_;
+          OpcUAServerAPI::status_code_ptr_ = this->status_code_ptr_;
+          opc_ua_server_api_ptr_->start();
+        });
+
+    std::shared_ptr vda_5050_client_thr = std::make_shared<std::thread>(
+        [this, nh]()
+        {
+          vda_5050_client_api_ptr_ =
+              std::make_shared<VDA5050ClientAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);
+
+          vda_5050_client_api_ptr_->muted_value_ = &this->muted_;
+          vda_5050_client_api_ptr_->unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
+          vda_5050_client_api_ptr_->load_excuted_ = std::bind(&AmrController::loadCallBack, this);
+          vda_5050_client_api_ptr_->cur_belt_state_en_ = &this->cur_belt_state_en_;
+          vda_5050_client_api_ptr_->belt_thread_ptr_ = &this->belt_thread_;
+
+          vda_5050_client_api_ptr_->arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
+          vda_5050_client_api_ptr_->arm_thread_ptr_ = &this->arm_thread_;
+          vda_5050_client_api_ptr_->enable_action_ = &this->enable_;
+          vda_5050_client_api_ptr_->cancel_action_ = &this->cancel_;
+          vda_5050_client_api_ptr_->pause_action_ = &this->pause_;
+          vda_5050_client_api_ptr_->count_ng_max_ = &this->count_ng_max_;
+          vda_5050_client_api_ptr_->count_ok_max_ = &this->count_ok_max_;
+        });
+    ROS_INFO("Initalizing comunication is done");
+    opc_ua_server_thr->join();
+    vda_5050_client_thr->join();
+  }
+
+  void AmrController::initalizingLocalizationHandle(ros::NodeHandle &nh)
+  {
+    std::string obj_name("loc_base::LocBase");
+    if (tf_ == nullptr)
+      throw std::runtime_error("tf2_ros::Buffer object is null");
+    try
+    {
+      loc_base_ptr_ = loc_base_loader_.createUniqueInstance(obj_name);
+      ros::NodeHandle node = ros::NodeHandle(nh, "global_costmap");
+      loc_base_ptr_->initialize(node, tf_);
+    }
+    catch (pluginlib::PluginlibException &ex)
+    {
+      ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
+      exit(1);
+    }
+    catch (std::exception &e)
+    {
+      ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
+      exit(1);
+    }
+  }
+
+  void AmrController::initalizingMoveBaseHandle(ros::NodeHandle &nh)
+  {
+    std::string obj_name("move_base::MoveBase");
+    if (tf_ == nullptr)
+      throw std::runtime_error("tf2_ros::Buffer object is null");
+    try
+    {
+      move_base_ptr_ = move_base_loader_.createUniqueInstance(obj_name);
+      ROS_INFO("Created object %s susseced", obj_name.c_str());
+      move_base_ptr_->initialize(tf_);
+      ros::Rate r(3);
+      do
+      {
+        r.sleep();
+        ros::spinOnce();
+      } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
+
+      if (move_base_ptr_ != nullptr &&
+          move_base_ptr_->nav_feedback_ != nullptr &&
+          move_base_ptr_->nav_feedback_->is_ready)
+      {
+        geometry_msgs::Vector3 linear;
+        linear.x = 0.3;
+        move_base_ptr_->setTwistLinear(linear);
+        linear.x = -0.3;
+        move_base_ptr_->setTwistLinear(linear);
+      }
+    }
+    catch (pluginlib::PluginlibException &ex)
+    {
+      ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
+      exit(1);
+    }
+    catch (std::exception &e)
+    {
+      ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
+      exit(1);
+    }
+  }
+
+  void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
+  {
+    this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
+  }
+
+  void AmrController::ArmCallBack()
+  {
+    ROS_INFO("Arm Calling");
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    {
+      arm_joined_ = true;
+      this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
+    }
+  }
+
+  void AmrController::ArmDotuff()
+  {
+    std::shared_ptr<imr_nova_control> arm_control_ptr;
+    arm_control_ptr = std::make_shared<imr_nova_control>();
+    arm_control_ptr->enable_ = &this->enable_;
+    arm_control_ptr->go_home_flag_ = &this->arm_go_home_;
+    arm_control_ptr->continue_flag_ = &this->arm_continue_;
+    arm_control_ptr->power_on_flag_ = &this->arm_power_on_;
+    OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_;
+    this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_);
+
+    if (!this->arm_go_home_)
+    {
+      arm_control_ptr->ok_count_max_ = &this->count_ok_max_;
+      arm_control_ptr->ng_count_max_ = &this->count_ng_max_;
+      arm_control_ptr->startModeThread();
+    }
+    else
+    {
+      arm_control_ptr->startHomeThread();
+    }
+
+    arm_control_ptr.reset();
+    ROS_INFO("Arm Finished");
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    this->arm_joined_ = false;
+  }
+
+  void AmrController::unLoadCallBack()
+  {
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    {
+      ROS_INFO("Shiping call");
+      this->belt_joined_ = true;
+      this->cancel_ = false;
+      this->cur_belt_state_en_ = amr_control::State::WAITING;
+      this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_));
+    }
+  }
+
+  void AmrController::conveyorBeltsShipping(amr_control::State &state)
+  {
+    state = amr_control::State::INITIALIZING;
+    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+    plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
+    plc_controller_ptr_->connect();
+
+    if (!plc_controller_ptr_->checkConnected())
+    {
+      state = amr_control::State::FAILED;
+      return;
+    }
+
+    if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
+    {
+      int shipping_regsister = 119, receiving_regsister = 116;
+      plc_controller_ptr_->setM(shipping_regsister);
+      plc_controller_ptr_->resetM(receiving_regsister);
+      ros::Duration(0.1).sleep();
+      plc_controller_ptr_->resetM(shipping_regsister);
+      plc_controller_ptr_->resetM(receiving_regsister);
+
+      ros::Rate r(5);
+      while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
+      {
+        state = amr_control::State::RUNNING;
+        bool output_belt[2];
+        plc_controller_ptr_->mulGetM(124, 125, output_belt);
+
+        bool enable_shipping = output_belt[0];
+        bool enable_receiving = output_belt[1];
+        if (!enable_shipping && !enable_receiving)
+        {
+          state = amr_control::State::FINISHED;
+          break;
+        }
+        r.sleep();
+      }
+
+      if (cancel_ || !plc_controller_ptr_->checkConnected())
+        state = amr_control::State::FAILED;
+
+      plc_controller_ptr_->close();
+    }
+    if (plc_controller_ptr_)
+      plc_controller_ptr_.reset();
+
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    this->belt_joined_ = false;
+  }
+
+  void AmrController::loadCallBack()
+  {
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    {
+      ROS_INFO("Receiving call");
+      this->belt_joined_ = true;
+      this->cancel_ = false;
+      this->cur_belt_state_en_ = amr_control::State::WAITING;
+      this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_));
+    }
+  }
+
+  void AmrController::conveyorBeltsReceiving(amr_control::State &state)
+  {
+    state = amr_control::State::INITIALIZING;
+    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+    plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
+    plc_controller_ptr_->connect();
+
+    if (!plc_controller_ptr_->checkConnected())
+    {
+      state = amr_control::State::FAILED;
+      return;
+    }
+
+    if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
+    {
+      int shipping_regsister = 119, receiving_regsister = 116;
+      plc_controller_ptr_->resetM(shipping_regsister);
+      plc_controller_ptr_->setM(receiving_regsister);
+      ros::Duration(0.1).sleep();
+      plc_controller_ptr_->resetM(shipping_regsister);
+      plc_controller_ptr_->resetM(receiving_regsister);
+
+      ros::Rate r(5);
+      while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
+      {
+        state = amr_control::State::RUNNING;
+        bool output_belt[2];
+        plc_controller_ptr_->mulGetM(124, 125, output_belt);
+
+        bool enable_shipping = output_belt[0];
+        bool enable_receiving = output_belt[1];
+        if (!enable_shipping && !enable_receiving)
+        {
+          state = amr_control::State::FINISHED;
+          break;
+        }
+        r.sleep();
+      }
+      if (this->cancel_ || !plc_controller_ptr_->checkConnected())
+        state = amr_control::State::FAILED;
+
+      plc_controller_ptr_->close();
+    }
+    if (plc_controller_ptr_)
+      plc_controller_ptr_.reset();
+
+    std::lock_guard<std::mutex> lock(this->arm_mutex_);
+    this->belt_joined_ = false;
+  }
+
+  void AmrController::controllerDotuff()
+  {
+    ros::Rate r(10);
+    while (ros::ok())
+    {
+      std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
+      plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
+      plc_controller_ptr_->connect();
+      r.sleep();
+      ros::spinOnce();
+
+      if (plc_controller_ptr_ == nullptr)
+        continue;
+      if (!plc_controller_ptr_->checkConnected())
+        continue;
+
+      this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
+      this->amr_safety_ptr_->getController(plc_controller_ptr_);
+
+      while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
+      {
+        if (!this->monitor_ptr_)
+          continue;
+        nav_2d_msgs::Twist2D velocity;
+        if (this->monitor_ptr_->getVelocity(velocity))
+        {
+          cmd_vel_mtx.lock();
+          this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
+          cmd_vel_mtx.unlock();
+        }
+
+        if (move_base_ptr_ != nullptr &&
+            move_base_ptr_->nav_feedback_ != nullptr &&
+            move_base_ptr_->nav_feedback_->is_ready)
+        {
+          if (velocity.x <= -0.01)
+            this->amr_safety_ptr_->writeMutesSafety(true);
+          else
+          {
+            this->amr_safety_ptr_->writeMutesSafety(this->muted_);
+          }
+        }
+
+        bool have_goods;
+        int have_goods_regsister = 142;
+        if (!OpcUAServerAPI::belt_cancel_)
+        {
+          plc_controller_ptr_->getM(have_goods_regsister, have_goods);
+          OpcUAServerAPI::have_goods_ = have_goods;
+        }
+        else
+        {
+          plc_controller_ptr_->resetM(have_goods_regsister);
+        }
+
+        if (vda_5050_client_api_ptr_)
+        {
+          amr_control::OperatingMode mode_;
+          bool operating_mode[4];
+          plc_controller_ptr_->mulGetM(14, 17, operating_mode);
+          if (operating_mode[0])
+            mode_ = amr_control::OperatingMode::AUTOMATIC;
+          else if (operating_mode[2])
+            mode_ = amr_control::OperatingMode::SERVICE;
+          else if (operating_mode[3])
+            mode_ = amr_control::OperatingMode::MANUAL;
+
+          switch (mode_)
+          {
+          case amr_control::OperatingMode::AUTOMATIC:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
+            break;
+          case amr_control::OperatingMode::MANUAL:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
+            break;
+          case amr_control::OperatingMode::SERVICE:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
+            break;
+          default:
+            vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
+            break;
+          }
+        }
+        r.sleep();
+        ros::spinOnce();
+      }
+      if (plc_controller_ptr_)
+        plc_controller_ptr_.reset();
+      if (amr_safety_ptr_)
+        amr_safety_ptr_.reset();
+    }
+  }
+
+  void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
+  {
+    this->muted_ = msg->data;
+  }
+
+  void AmrController::threadHandle()
+  {
+    ros::Rate r(5);
+    while (ros::ok())
+    {
+      if (this->arm_thread_.joinable())
+      {
+        std::lock_guard<std::mutex> lock(this->arm_mutex_);
+        {
+          if (!this->arm_joined_)
+          {
+            this->arm_thread_.join();
+          }
+        }
+      }
+
+      if (this->belt_thread_.joinable())
+      {
+        std::lock_guard<std::mutex> lock(this->belt_mutex_);
+        {
+          if (!this->belt_joined_)
+          {
+            this->belt_thread_.join();
+          }
+        }
+      }
+
+      if (move_base_ptr_ != nullptr &&
+          move_base_ptr_->nav_feedback_ != nullptr &&
+          move_base_ptr_->nav_feedback_->is_ready)
+      {
+        nav_2d_msgs::Twist2D velocity;
+        if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
+        {
+          this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
+          this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
+
+          geometry_msgs::Vector3 linear;
+          geometry_msgs::Vector3 angular;
+
+          cmd_vel_mtx.lock();
+          linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
+          angular.z = this->cmd_vel_max_.theta;
+          cmd_vel_mtx.unlock();
+
+          this->move_base_ptr_->setTwistLinear(linear);
+          linear.x *= -1.0;
+          this->move_base_ptr_->setTwistLinear(linear);
+          this->move_base_ptr_->setTwistAngular(angular);
+        }
+      }
+      r.sleep();
+      ros::spinOnce();
+    }
+  }
+}

Controllers/Packages/amr_control/src/amr_control_node.cpp

@@ -0,0 +1,27 @@
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include "amr_control/amr_control.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "move_base_node");
+  ros::start();
+  ROS_INFO("ros::this_node::getName() : %s", ros::this_node::getName().c_str());
+  ros::NodeHandle nh(ros::this_node::getName());
+  std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
+  tf2_ros::TransformListener tf2(*buffer);
+  try
+  {
+    std::shared_ptr<amr_control::AmrController> amr_ctr_;
+    amr_ctr_ = std::make_shared<amr_control::AmrController>(nh, buffer);
+    ros::spin();
+    if(amr_ctr_ != nullptr) 
+      amr_ctr_->stop();
+    amr_ctr_.reset();
+  }
+  catch (const std::exception& exc)
+  {
+    std::cout << exc.what() << std::endl;
+  }
+  return 0;
+}

Controllers/Packages/amr_control/src/amr_make_plan_with_order.cpp

@@ -0,0 +1,712 @@
+#include "amr_control/amr_make_plan_with_order.h"
+
+namespace amr_control
+{
+  bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t &status, std::string &message, std::vector<vda_5050::Pose> &posesOnPathWay)
+  {
+    std::map<std::string, vda_5050::Node> orderNodes;
+    Spline_Inf *input_spline_inf = new Spline_Inf();
+    CurveCommon *curve_design = new CurveCommon();
+    posesOnPathWay.clear();
+    std::map<std::string, vda_5050::Node> map_str;
+    if ((int)order.nodes.size() == 0 || (int)order.edges.size() == 0)
+    {
+      status = 1;
+      message = "Nodes or Edges in Order is empty";
+      return false;
+    }
+    for (auto &node : order.nodes)
+    {
+      orderNodes.insert({node.nodeId, node});
+    }
+    for (int i = 0; i < (int)order.edges.size(); i++)
+    {
+      auto start_nodeId_it = orderNodes.find(order.edges[i].startNodeId);
+      auto end_nodeId_it = orderNodes.find(order.edges[i].endNodeId);
+
+      if (start_nodeId_it != orderNodes.end() && end_nodeId_it != orderNodes.end())
+      {
+        std::vector<vda_5050::Pose> posesOnEdge;
+        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> control_points;
+        std::vector<double> knot_vector;
+        std::vector<double> weight_vector;
+        int degree = 0;
+        int order_ = 0;
+        control_points.reserve(order.edges[i].trajectory.controlPoints.size());
+        knot_vector.reserve(order.edges[i].trajectory.knotVector.size());
+        weight_vector.reserve(order.edges[i].trajectory.controlPoints.size());
+        for (int j = 0; j < (int)order.edges[i].trajectory.controlPoints.size(); j++)
+        {
+          control_points.push_back(Eigen::Vector3d(order.edges[i].trajectory.controlPoints[j].x, order.edges[i].trajectory.controlPoints[j].y, 0));
+          weight_vector.push_back(order.edges[i].trajectory.controlPoints[j].weight);
+        }
+        for (int k = 0; k < (int)order.edges[i].trajectory.knotVector.size(); k++)
+        {
+          knot_vector.push_back(order.edges[i].trajectory.knotVector[k]);
+        }
+        degree = (int)order.edges[i].trajectory.degree;
+        if (curveIsValid(degree, knot_vector, control_points))
+        {
+          double step = 0.01;
+          order_ = degree + 1;
+          input_spline_inf->control_point.clear();
+          input_spline_inf->knot_vector.clear();
+          input_spline_inf->weight.clear();
+          curve_design->ReadSplineInf(input_spline_inf, order_, control_points, knot_vector);
+          curve_design->ReadSplineInf(input_spline_inf, weight_vector, false);
+          for (double u_test = 0; u_test <= 1; u_test += step)
+          {
+            geometry_msgs::Point curve_point;
+            curve_point = curve_design->CalculateCurvePoint(input_spline_inf, u_test, true);
+            if (!std::isnan(curve_point.x) && !std::isnan(curve_point.y))
+              posesOnEdge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0));
+            // ROS_INFO("curve_point: %f, %f   at u: %f",curve_point.x, curve_point.y, u_test);
+          }
+          if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
+          {
+            // if startNode of this edge and start element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
+                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
+            }
+            // if endNode of this edge and end element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
+                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
+            }
+            if (!posesOnPathWay.empty())
+            {
+              if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626)
+              {
+                setYawAllPosesOnEdge(posesOnEdge, false);
+              }
+              else if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276)
+              {
+                setYawAllPosesOnEdge(posesOnEdge, true);
+              }
+              else
+              {
+                ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+                status = 3;
+                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                return false;
+                break;
+              }
+            }
+            else
+            {
+              if (is_move_backward == false)
+              {
+                setYawAllPosesOnEdge(posesOnEdge, false);
+              }
+              else
+              {
+                setYawAllPosesOnEdge(posesOnEdge, true);
+              }
+            }
+            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
+            {
+              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
+                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
+              {
+                if (i != 1)
+                {                                                                                                                                   // don't check angle of edge 1
+                  if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
+                  {
+                    // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
+                    posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
+                    posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
+                  }
+                  else
+                  {
+                    ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                             order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+                    status = 3;
+                    message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                              ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                    return false;
+                    break;
+                  }
+                }
+                else
+                {
+                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
+                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
+                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
+                }
+              }
+              else
+              {
+                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
+                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
+                status = 3;
+                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                return false;
+                break;
+              }
+            }
+            else // posesOnPathWay is empty
+            {
+              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
+            }
+          }
+          else if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
+          {
+            // if startNode of this edge and start element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
+                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
+            }
+            // if endNode of this edge and end element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
+                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
+            }
+            if (i == ((int)order.edges.size() - 1))
+            {
+              if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 1.5707963268) // <= 90 degree
+              {
+                setYawAllPosesOnEdge(posesOnEdge, false);
+                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
+              }
+              else
+              {
+                setYawAllPosesOnEdge(posesOnEdge, true);
+                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
+              }
+            }
+            else
+            {
+              if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626) // <= 50 degree
+              {
+                setYawAllPosesOnEdge(posesOnEdge, false);
+                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
+              }
+              else if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276) // >= 130 degree
+              {
+                setYawAllPosesOnEdge(posesOnEdge, true);
+                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
+              }
+              else
+              {
+                ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+                status = 3;
+                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                return false;
+                break;
+              }
+            }
+            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
+            {
+              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
+                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
+              {
+                if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
+                {
+                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
+                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
+                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
+                }
+                else
+                {
+                  ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                           order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+                  status = 3;
+                  message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                            ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                  return false;
+                  break;
+                }
+              }
+              else
+              {
+                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
+                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
+                status = 3;
+                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                return false;
+                break;
+              }
+            }
+            else // posesOnPathWay is empty
+            {
+              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
+            }
+          }
+          else if (isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
+          {
+            // if startNode of this edge and start element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
+                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
+            }
+            // if endNode of this edge and end element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
+                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
+            }
+            if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626) // <= 50 degree)
+            {
+              setYawAllPosesOnEdge(posesOnEdge, false);
+              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
+            }
+            else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276) // >= 130 degree
+            {
+              setYawAllPosesOnEdge(posesOnEdge, true);
+              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
+            }
+            else
+            {
+              ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                       order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+              status = 3;
+              message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                        ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+              return false;
+              break;
+            }
+            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
+            {
+              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
+                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
+              {
+                if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
+                {
+                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
+                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
+                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
+                }
+                else
+                {
+                  ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                           order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+                  status = 3;
+                  message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                            ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                  return false;
+                  break;
+                }
+              }
+              else
+              {
+                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
+                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
+                status = 3;
+                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                return false;
+                break;
+              }
+            }
+            else // posesOnPathWay is empty
+            {
+              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
+            }
+          }
+          else
+          {
+            // if startNode of this edge and start element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
+                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
+            }
+            // if endNode of this edge and end element of posesOnEdge are the different point
+            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
+                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
+            {
+              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
+            }
+            // DeltaAngleStart <= 50 degree and DeltaAngleEnd <= 50 degree
+            if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626 &&
+                computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626)
+            {
+              setYawAllPosesOnEdge(posesOnEdge, false);
+              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
+              posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta);    // set yaw angle of the end pose to endNode theta
+            }
+            // DeltaAngleStart >= 130 degree and DeltaAngleEnd >= 130 degree
+            else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276 &&
+                     computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276)
+            {
+              setYawAllPosesOnEdge(posesOnEdge, true);
+              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
+              posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta);    // set yaw angle of the end pose to endNode theta
+            }
+            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
+            {
+              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
+                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
+              {
+                if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
+                {
+                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
+                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
+                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
+                }
+                else
+                {
+                  ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
+                           order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+                  status = 3;
+                  message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                            ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                  return false;
+                  break;
+                }
+              }
+              else
+              {
+                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
+                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
+                status = 3;
+                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
+                return false;
+                break;
+              }
+            }
+            else // posesOnPathWay is empty
+            {
+              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
+            }
+          }
+        }
+        else
+        {
+          ROS_WARN("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", order.edges[i].edgeId.c_str(),
+                   order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+          status = 2;
+          message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
+                    ", endNode: " + order.edges[i].endNodeId.c_str() + " is invalid NURBS-curve";
+          return false;
+          break;
+        }
+      }
+      else
+      {
+        ROS_WARN("Edge: %s not found startNodeId: %s or endNodeId: %s", order.edges[i].edgeId.c_str(),
+                 order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
+        status = 1;
+        message = "Edge: " + order.edges[i].edgeId + " not found startNodeId: " + order.edges[i].startNodeId.c_str() +
+                  " or endNodeId: " + order.edges[i].endNodeId.c_str();
+        return false;
+        break;
+      }
+      // ROS_INFO("Finish to compute at Edge[%d]: %s", i, order.edges[i].edgeDescription.c_str());
+    }
+
+    status = 0;
+    message = "Success to make plan: StartNode: " + order.edges[0].startNodeId + ", EndNode: " + order.edges[order.edges.size() - 1].endNodeId;
+
+    if (input_spline_inf)
+      delete (input_spline_inf);
+    if (curve_design)
+      delete (curve_design);
+    return true;
+  }
+
+  bool isThetaValid(double theta)
+  {
+    bool result = false;
+    if (theta < -M_PI || theta > M_PI)
+      result = false;
+    else
+      result = true;
+    return result;
+  }
+
+  bool curveIsValid(int degree, const std::vector<double> &knot_vector,
+                    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &control_points)
+  {
+    if (degree < 1 || degree > 9)
+    {
+      ROS_WARN("degree is invalid value");
+      return false;
+    }
+    if (!((knot_vector.size() - degree - 1) == control_points.size()))
+    {
+      ROS_WARN("relation between degree, number of knots, and number of control points is invalid");
+      return false;
+    }
+    // if(std::is_sorted(knot_vector.begin(), knot_vector.end()))
+    // {
+    //   ROS_WARN("knot vector is not monotonic");
+    //   return false;
+    // }
+    return true;
+  }
+
+  double computeDeltaAngleStartNode(double theta, vda_5050::Pose &startPose, vda_5050::Pose &next_Pose)
+  {
+    double delta_angle = 0;
+    if (isThetaValid(theta))
+    {
+      double xAB = next_Pose.getX() - startPose.getX();
+      double yAB = next_Pose.getY() - startPose.getY();
+      double d = sqrt(xAB * xAB + yAB * yAB);
+      double xC = startPose.getX() + d * cos(theta);
+      double yC = startPose.getY() + d * sin(theta);
+      double xAC = xC - startPose.getX();
+      double yAC = yC - startPose.getY();
+      double dAB = sqrt(xAB * xAB + yAB * yAB);
+      double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
+      if (cos_a > 1)
+        cos_a = 1;
+      else if (cos_a < (-1))
+        cos_a = -1;
+      delta_angle = acos(cos_a);
+      // delta_angle = delta_angle*180/M_PI;
+      // ROS_WARN("xC: %f, yC: %f", xC, yC);
+      // ROS_WARN("dAB: %f", dAB);
+      // ROS_WARN("delta_angle: %f", delta_angle);
+    }
+    return delta_angle;
+  }
+
+  double computeDeltaAngleEndNode(double theta, vda_5050::Pose &endPose, vda_5050::Pose &prev_Pose)
+  {
+    double delta_angle = 0;
+    if (isThetaValid(theta))
+    {
+      double xAB = endPose.getX() - prev_Pose.getX();
+      double yAB = endPose.getY() - prev_Pose.getY();
+      double d = sqrt(xAB * xAB + yAB * yAB);
+      double xC = endPose.getX() + d * cos(theta);
+      double yC = endPose.getY() + d * sin(theta);
+      double xBC = xC - endPose.getX();
+      double yBC = yC - endPose.getY();
+      double dAB = sqrt(xAB * xAB + yAB * yAB);
+      double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
+      if (cos_a > 1)
+        cos_a = 1;
+      else if (cos_a < (-1))
+        cos_a = -1;
+      delta_angle = acos(cos_a);
+      // delta_angle = delta_angle*180/M_PI;
+      // ROS_WARN("xC: %f, yC: %f", xC, yC);
+      // ROS_WARN("dAB: %f", dAB);
+      // ROS_WARN("delta_angle: %f", delta_angle);
+    }
+    return delta_angle;
+  }
+
+  double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose &endPose, geometry_msgs::Pose &prev_Pose)
+  {
+    double delta_angle = 0;
+    if (isThetaValid(theta))
+    {
+      double xAB = endPose.position.x - prev_Pose.position.x;
+      double yAB = endPose.position.y - prev_Pose.position.y;
+      double d = sqrt(xAB * xAB + yAB * yAB);
+      double xC = endPose.position.x + d * cos(theta);
+      double yC = endPose.position.y + d * sin(theta);
+      double xBC = xC - endPose.position.x;
+      double yBC = yC - endPose.position.y;
+      double dAB = sqrt(xAB * xAB + yAB * yAB);
+      double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
+      if (cos_a > 1)
+        cos_a = 1;
+      else if (cos_a < (-1))
+        cos_a = -1;
+      delta_angle = acos(cos_a);
+      // delta_angle = delta_angle*180/M_PI;
+      // ROS_WARN("xC: %f, yC: %f", xC, yC);
+      // ROS_WARN("dAB: %f", dAB);
+      // ROS_WARN("delta_angle: %f", delta_angle);
+    }
+    return delta_angle;
+  }
+
+  void setYawAllPosesOnEdge(std::vector<vda_5050::Pose> &posesOnEdge, bool reverse)
+  {
+    if (!reverse)
+    {
+      if (!posesOnEdge.empty())
+      {
+        if (posesOnEdge.size() > 2)
+        {
+          for (int i = 0; i < ((int)posesOnEdge.size() - 1); i++)
+          {
+            double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
+                                          posesOnEdge[i + 1].getX(), posesOnEdge[i + 1].getY());
+            posesOnEdge[i].setYaw(theta);
+          }
+          posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size() - 2].getYaw());
+        }
+        else if (posesOnEdge.size() == 2)
+        {
+          if (posesOnEdge[0].getX() != posesOnEdge[1].getX())
+          {
+            double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
+                                          posesOnEdge[1].getX(), posesOnEdge[1].getY());
+            posesOnEdge[0].setYaw(theta);
+            posesOnEdge[1].setYaw(theta);
+          }
+        }
+      }
+    }
+    else
+    {
+      if (!posesOnEdge.empty())
+      {
+        if (posesOnEdge.size() > 2)
+        {
+          for (int i = (int)posesOnEdge.size() - 1; i > 0; i--)
+          {
+            double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
+                                          posesOnEdge[i - 1].getX(), posesOnEdge[i - 1].getY());
+            posesOnEdge[i].setYaw(theta);
+          }
+          posesOnEdge.front().setYaw(posesOnEdge[1].getYaw());
+        }
+        else if (posesOnEdge.size() == 2)
+        {
+          if (posesOnEdge[1].getX() != posesOnEdge[0].getX())
+          {
+            double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
+                                          posesOnEdge[0].getX(), posesOnEdge[0].getY());
+            posesOnEdge[1].setYaw(theta);
+            posesOnEdge[0].setYaw(theta);
+          }
+        }
+      }
+    }
+  }
+
+  double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose &Pose)
+  {
+    double delta_angle = 0;
+    if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
+    {
+      double d = 1;
+      double xA = Pose.getX();
+      double yA = Pose.getY();
+      double xB = xA + d * cos(thetaEnd);
+      double yB = yA + d * sin(thetaEnd);
+      double xAB = xB - xA;
+      double yAB = yB - yA;
+      double xC = xA + d * cos(thetaStart);
+      double yC = yA + d * sin(thetaStart);
+      double xAC = xC - xA;
+      double yAC = yC - yA;
+      double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
+      if (cos_a > 1)
+        cos_a = 1;
+      else if (cos_a < (-1))
+        cos_a = -1;
+      delta_angle = acos(cos_a);
+      // delta_angle = delta_angle*180/M_PI;
+      // ROS_WARN("delta_angle: %f", delta_angle);
+    }
+    return delta_angle;
+  }
+
+  double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose &startPose, geometry_msgs::Pose &next_Pose)
+  {
+    double delta_angle = 0;
+    if (isThetaValid(theta))
+    {
+      double xAB = next_Pose.position.x - startPose.position.x;
+      double yAB = next_Pose.position.y - startPose.position.y;
+      double d = sqrt(xAB * xAB + yAB * yAB);
+      double xC = startPose.position.x + d * cos(theta);
+      double yC = startPose.position.y + d * sin(theta);
+      double xAC = xC - startPose.position.x;
+      double yAC = yC - startPose.position.y;
+      double dAB = sqrt(xAB * xAB + yAB * yAB);
+      double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
+      if (cos_a > 1)
+        cos_a = 1;
+      else if (cos_a < (-1))
+        cos_a = -1;
+      delta_angle = acos(cos_a);
+      // delta_angle = delta_angle*180/M_PI;
+      // ROS_WARN("xC: %f, yC: %f", xC, yC);
+      // ROS_WARN("dAB: %f", dAB);
+      // ROS_WARN("delta_angle: %f", delta_angle);
+    }
+    return delta_angle;
+  }
+
+  double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose &Pose)
+  {
+    double delta_angle = 0;
+    if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
+    {
+      double d = 1;
+      double xA = Pose.position.x;
+      double yA = Pose.position.y;
+      double xB = xA + d * cos(thetaEnd);
+      double yB = yA + d * sin(thetaEnd);
+      double xAB = xB - xA;
+      double yAB = yB - yA;
+      double xC = xA + d * cos(thetaStart);
+      double yC = yA + d * sin(thetaStart);
+      double xAC = xC - xA;
+      double yAC = yC - yA;
+      double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
+      if (cos_a > 1)
+        cos_a = 1;
+      else if (cos_a < (-1))
+        cos_a = -1;
+      delta_angle = acos(cos_a);
+      // delta_angle = delta_angle*180/M_PI;
+      // ROS_WARN("delta_angle: %f", delta_angle);
+    }
+    return delta_angle;
+  }
+
+  double calculateDistance(vda_5050::Pose &pose1, vda_5050::Pose &pose2)
+  {
+    double dx = pose1.getX() - pose2.getX();
+    double dy = pose1.getY() - pose2.getY();
+    return std::sqrt(dx * dx + dy * dy);
+  }
+
+  double computeDeltaAngle(vda_5050::Pose &Pose1, vda_5050::Pose &Pose2)
+  {
+    double delta_angle = 0;
+
+    double xA = Pose1.getX();
+    double yA = Pose1.getY();
+    double xB = Pose2.getX();
+    double yB = Pose2.getY();
+    double xAB = xB - xA;
+    double yAB = yB - yA;
+    double d = sqrt(xAB * xAB + yAB * yAB);
+    double xC = xB + d * cos(Pose2.getYaw());
+    double yC = yB + d * sin(Pose2.getYaw());
+    double xBC = xC - xB;
+    double yBC = yC - yB;
+    double cos_a = (xAB * xBC + yAB * yBC) / (d * d);
+    if (cos_a > 1)
+      cos_a = 1;
+    else if (cos_a < (-1))
+      cos_a = -1;
+    double delta_angle_tmp = acos(cos_a);
+    if (delta_angle_tmp >= 1.5707963268)
+    {
+      delta_angle = M_PI - delta_angle_tmp;
+    }
+    else
+    {
+      delta_angle = delta_angle_tmp;
+    }
+    // delta_angle = delta_angle*180/M_PI;
+    // ROS_WARN("delta_angle: %f", delta_angle);
+    return delta_angle;
+  }
+}

Controllers/Packages/amr_control/src/amr_monitor.cpp

@@ -0,0 +1,37 @@
+#include "amr_control/amr_monitor.h"
+
+namespace amr_control
+{
+  AmrMonitor::AmrMonitor() {}
+
+  AmrMonitor::AmrMonitor(const ros::NodeHandle &nh)
+  {
+    if (!initalized_)
+      this->init(nh);
+  }
+
+  AmrMonitor::~AmrMonitor()
+  {
+    if (odom_sub_)
+      odom_sub_.reset();
+  }
+
+  void AmrMonitor::init(const ros::NodeHandle &nh)
+  {
+    if (!initalized_)
+    {
+      ros::NodeHandle nh_core;
+      odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh_core);
+      initalized_ = true;
+    }
+  }
+
+  bool AmrMonitor::getVelocity(nav_2d_msgs::Twist2D &velocity)
+  {
+    if (!odom_sub_)
+      return false;
+    else
+      velocity = odom_sub_->getTwist();
+    return true;
+  }
+}

Controllers/Packages/amr_control/src/amr_safety.cpp

@@ -0,0 +1,85 @@
+#include "amr_control/amr_safety.h"
+
+namespace amr_control
+{
+  AmrSafety::AmrSafety() {}
+
+  AmrSafety::AmrSafety(const ros::NodeHandle &nh)
+  {
+    if (!initalized_)
+      this->init(nh);
+  }
+
+  AmrSafety::~AmrSafety() {}
+
+  void AmrSafety::init(const ros::NodeHandle &nh)
+  {
+    if (!initalized_)
+    {
+
+      initalized_ = true;
+    }
+  }
+
+  bool AmrSafety::getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr)
+  {
+    if (plc_controller_ptr == nullptr)
+      return false;
+    else
+    {
+      plc_controller_ptr_ = plc_controller_ptr;
+      return true;
+    }
+  }
+
+  void AmrSafety::safetyHandle(
+      nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel)
+  {
+    if (plc_controller_ptr_ == nullptr)
+      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
+    else if (!plc_controller_ptr_->checkConnected())
+      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
+    else
+    {
+      nav_2d_msgs::Twist2D velocity_warn;
+      velocity_warn.x = 0.15;
+      velocity_warn.theta = 0.5;
+      nav_2d_msgs::Twist2D velocity_stop;
+
+      // Dieu khien chuyen vung giam toc va mute
+      bool zone_warn_cases;
+      if (velocity.x >= velocity_warn.x)
+        zone_warn_cases = false;
+      else if (velocity.x < velocity_warn.x && velocity.x > 0.05)
+        zone_warn_cases = true;
+      plc_controller_ptr_->writeM(52, zone_warn_cases);
+
+      // Nhan tin hieu 3 vung tu lidar
+      bool output_lidar[4];
+      plc_controller_ptr_->mulGetM(30, 33, output_lidar);
+      if (output_lidar[0] && output_lidar[1] && output_lidar[2])
+        cmd_vel = velocity_stop;
+      else if (output_lidar[0] && !output_lidar[1] && output_lidar[2])
+        cmd_vel = velocity_warn;
+      else if (output_lidar[0] && !output_lidar[1] && !output_lidar[2])
+        cmd_vel = cmd_vel_max;
+      else
+        cmd_vel = velocity_stop;
+    }
+  }
+
+  void AmrSafety::writeMutesSafety(bool value)
+  {
+    if (plc_controller_ptr_ == nullptr)
+      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
+    else if (!plc_controller_ptr_->checkConnected())
+      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
+    else
+    {
+      bool f_data;
+      plc_controller_ptr_->getM(53, f_data);
+      if (f_data != value)
+        plc_controller_ptr_->writeM(53, value);
+    }
+  }
+}

Controllers/Packages/amr_control/test/test_move_base.cpp

@@ -0,0 +1,279 @@
+
+#include <ros/ros.h>
+#include <pluginlib/class_loader.h>
+#include <memory>
+#include <string>
+#include <move_base_core/navigation.h>
+#include <amr_control/moveTo.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Int16.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/Pose2D.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+#include <vda5050_msgs/Order.h>
+#include <vda5050_msgs/InstantActions.h>
+
+#include <tf2/convert.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/Pose2D.h>
+
+move_base_core::BaseNavigation::Ptr move_base = nullptr;
+geometry_msgs::PoseStamped amcl_pose;
+boost::mutex ctr_mutex;
+bool pub_order = false;
+amr_control::moveTo order;
+bool pub_cancel = false;
+
+void moveToCallback(const amr_control::moveTo::ConstPtr& msg)
+{
+  // boost::unique_lock<boost::mutex> lock(ctr_mutex);
+  if(move_base)
+  {
+    try
+    {
+      geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(msg->goal, "map", ros::Time::now());
+      geometry_msgs::PoseStamped of_goal =  move_base_core::offset_goal(amcl_pose, msg->goal.x);
+      double xy_goal_tolerance = 0.04;
+      double yaw_goal_tolerance = 0.06;
+      // ROS_INFO_STREAM(of_goal);
+      switch ((int)msg->maker.data)
+      {
+      case 1:
+        move_base->moveTo(goal, move_base_core::PICK_UP, xy_goal_tolerance, yaw_goal_tolerance);
+        break;
+      case 2:
+        move_base->moveTo(goal, move_base_core::POP_DOWN, xy_goal_tolerance, yaw_goal_tolerance);
+        break; 
+      case 3:
+        move_base->moveTo(goal, move_base_core::POSITION, xy_goal_tolerance, yaw_goal_tolerance);
+        break; 
+      case 4:
+        move_base->moveTo(goal, move_base_core::CHARGER, xy_goal_tolerance, yaw_goal_tolerance);
+        break; 
+      case 5:
+        move_base->moveStraightTo(of_goal, xy_goal_tolerance);
+        break; 
+      case 6:
+        move_base->rotateTo(goal, yaw_goal_tolerance);
+        break; 
+      default:
+          throw std::bad_function_call();
+        break;
+      }
+
+      geometry_msgs::Vector3 linear;
+      linear.x = 1.1;
+      move_base->setTwistLinear(linear);
+      linear.x = -0.5;
+      move_base->setTwistLinear(linear);
+    }
+    catch(const std::exception& e)
+    {
+      ROS_ERROR_STREAM(e.what() << "\n");
+    }
+  }
+}
+
+void getposeCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
+{
+  amcl_pose.header = msg->header;
+  amcl_pose.pose.position = msg->pose.pose.position;
+  amcl_pose.pose.orientation = msg->pose.pose.orientation;
+}
+
+void setTiwstCallback(const geometry_msgs::Vector3::ConstPtr& msg)
+{
+  if(move_base)
+  {
+    move_base->setTwistLinear(*msg);
+  }
+}
+
+void cancelCallback(const std_msgs::Bool::ConstPtr& msg)
+{
+  move_base->cancel();
+}
+
+void pauseCallback(const std_msgs::Bool::ConstPtr& msg)
+{
+  move_base->pause();
+}
+
+void resumeCallback(const std_msgs::Bool::ConstPtr& msg)
+{
+  move_base->resume();
+}
+
+void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg)
+{
+  if(msg->serialNumber == "AMR-150")
+  {
+    ros::Duration(0.5).sleep();
+    if(!msg->nodes.empty())
+    {
+      order.header.frame_id = "map";
+      order.header.stamp = ros::Time::now();
+      order.goal.x = msg->nodes.back().nodePosition.x;
+      order.goal.y = msg->nodes.back().nodePosition.y;
+      order.goal.theta = msg->nodes.back().nodePosition.theta;
+      if(!msg->nodes.back().actions.empty())
+      {
+        if(msg->nodes.back().actions.front().actionType == "startInPallet")
+          order.maker.data = 1;
+        else
+          order.maker.data = 3;
+      }
+      else 
+        order.maker.data = 3;
+      pub_order = true;
+    }
+  }
+}
+
+void instantActions_msg_handle(const vda5050_msgs::InstantActions::ConstPtr& msg)
+{
+  if(msg->serialNumber == "AMR-150")
+  {
+    for(int i=0; i< msg->actions.size(); i++)
+    {
+      if(msg->actions[i].actionType == "cancelOrder") pub_cancel = true;
+    }
+  }
+}
+
+bool getRobotPose(geometry_msgs::PoseStamped& global_pose, std::shared_ptr<tf2_ros::Buffer> tf_)
+{
+	std::string global_frame_ = "map";
+	std::string robot_base_frame_ = "base_link";
+  // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose");
+  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
+  geometry_msgs::PoseStamped robot_pose;
+  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
+  robot_pose.header.frame_id = robot_base_frame_;
+  robot_pose.header.stamp = ros::Time();
+  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
+
+  // get the global pose of the robot
+  try
+  {
+    // use current time if possible (makes sure it's not in the future)
+    if (tf_->canTransform(global_frame_, robot_base_frame_, current_time))
+    {
+      geometry_msgs::TransformStamped transform = tf_->lookupTransform(global_frame_, robot_base_frame_, current_time);
+      tf2::doTransform(robot_pose, global_pose, transform);
+    }
+    // use the latest otherwise
+    else
+    {
+      tf_->transform(robot_pose, global_pose, global_frame_);
+    }
+  }
+  catch (tf2::LookupException& ex)
+  {
+    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
+    return false;
+  }
+  catch (tf2::ConnectivityException& ex)
+  {
+    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
+    return false;
+  }
+  catch (tf2::ExtrapolationException& ex)
+  {
+    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
+    return false;
+  }
+  // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
+  // check global_pose timeout
+  if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > 1.0)
+  {
+    ROS_WARN_THROTTLE(1.0,
+    // ROS_WARN(
+                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
+                      current_time.toSec(), global_pose.header.stamp.toSec(), 1.0);
+    return false;
+  }
+
+  return true;
+}
+
+
+int main(int argc, char** argv){
+  ros::init(argc, argv, "move_base_node");
+  ros::NodeHandle nh = ros::NodeHandle("move_base_node");
+  ros::NodeHandle nh_;
+  std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
+  tf2_ros::TransformListener tf2(*buffer);
+
+  pluginlib::ClassLoader<move_base_core::BaseNavigation> mb_loader("move_base_core", "move_base_core::BaseNavigation");
+  
+  std::string obj_name("move_base::MoveBase");
+  try
+  {
+    move_base = mb_loader.createUniqueInstance(obj_name);
+    ROS_INFO("Created object %s susseced", obj_name.c_str());
+    move_base->initialize(buffer);
+  }
+  catch (pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
+    return 1;
+  }
+  catch(std::exception &e)
+  {
+    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
+    return 1;
+  }
+  ros::Subscriber moveto_sub = nh.subscribe("moveTo", 1000, moveToCallback);
+  ros::Subscriber set_twist_sub = nh.subscribe("setTiwst", 1000, setTiwstCallback);
+  ros::Subscriber cancel_sub = nh.subscribe("cancel", 1000, cancelCallback);
+  ros::Subscriber pause_sub = nh.subscribe("pause", 1000, pauseCallback);
+  ros::Subscriber resume_sub = nh.subscribe("resume", 1000, resumeCallback);
+  ros::Subscriber amcl_pose_sub = nh.subscribe("amcl_pose", 1000, getposeCallback);
+  ros::Subscriber order_msg_sub_ = nh_.subscribe("/order",1000, order_msg_handle);
+  ros::Subscriber instantAc_msg_sub_ = nh_.subscribe("/instantActions",1000, instantActions_msg_handle);
+  ros::Publisher  moveto_pub = nh.advertise<amr_control::moveTo>("moveTo", 1000);
+  ros::Publisher  cancel_pub = nh.advertise<std_msgs::Bool>("cancel", 1000);
+  ros::Rate r(10);
+  
+  while (ros::ok())
+  {
+    if(pub_order)
+    {
+      moveto_pub.publish(order);
+      pub_order = false;
+    }
+    else if (pub_cancel)
+    {
+      std_msgs::Bool value;
+      value.data = true;
+      cancel_pub.publish(value);
+      pub_cancel = false;
+    }
+    getRobotPose(amcl_pose, buffer);
+    // boost::unique_lock<boost::mutex> lock(ctr_mutex);
+    // {
+      if(move_base && move_base->nav_feedback_)
+      {
+        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::ABORTED)
+        //   ROS_ERROR("ABORTED %s", move_base->nav_feedback_->feed_back_str.c_str());
+        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::PAUSED) 
+        //   ROS_WARN("PAUSED %s", move_base->nav_feedback_->feed_back_str.c_str());
+        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::PREEMPTED) 
+        //   ROS_WARN("PREEMPTED %s", move_base->nav_feedback_->feed_back_str.c_str());
+        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::SUCCEEDED) 
+        //   ROS_INFO_STREAM("SUCCEEDED \n" << move_base->nav_feedback_->current_pose);
+      }
+    // }
+    ros::spinOnce();
+    r.sleep();
+  }
+  
+  ros::spin();
+  return(0);
+}

Controllers/Packages/amr_control/test/vda_5050_api.cpp

@@ -0,0 +1,15 @@
+#include <ros/ros.h>
+#include "amr_control/amr_vda_5050_client_api.h"
+
+std::shared_ptr<amr_control::VDA5050ClientAPI> client;
+
+int main(int argc, char** argv) {
+  // Initialize the ROS node
+  ros::init(argc, argv, "test_imr_nova_control");
+  ros::start();
+
+  client = std::make_shared<amr_control::VDA5050ClientAPI>();
+  ros::spin();
+  client.reset();
+  return 0;
+}

Controllers/Packages/amr_startup/CMakeLists.txt

@@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(amr_startup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES amr_startup
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/amr_startup.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/amr_startup_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY
+    config
+    launch
+    rviz
+    sdf
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )
+  
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_startup.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Controllers/Packages/amr_startup/config/base_local_planner_params.yaml

@@ -0,0 +1,48 @@
+#For full documentation of the parameters in this file, and a list of all the
+#parameters available for TrajectoryPlannerROS, please see
+#http://www.ros.org/wiki/base_local_planner
+TrajectoryPlannerROS:
+  #Set the acceleration limits of the robot
+  acc_lim_th: 3.2 
+  acc_lim_x: 0.5
+  acc_lim_y: 0.5
+
+  #Set the velocity limits of the robot
+  max_vel_x: 0.4
+  min_vel_x: 0.1
+  max_rotational_vel: 1.0
+  min_in_place_rotational_vel: 0.4
+
+  #The velocity the robot will command when trying to escape from a stuck situation
+  escape_vel: -0.2
+  
+  #For this example, we'll use a holonomic robot
+  holonomic_robot: false
+
+  #Since we're using a holonomic robot, we'll set the set of y velocities it will sample
+  y_vels: [-0.3, -0.1, 0.1, 0.3]
+
+  #Set the tolerance on achieving a goal
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.05
+
+  #We'll configure how long and with what granularity we'll forward simulate trajectories
+  sim_time: 3.0
+  sim_granularity: 0.025
+  vx_samples: 5
+  vtheta_samples: 20
+
+  #Parameters for scoring trajectories
+  goal_distance_bias: 0.4
+  path_distance_bias: 0.7
+  occdist_scale: 0.3
+  heading_lookahead: 0.325
+
+  #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example
+  dwa: false
+
+  #How far the robot must travel before oscillation flags are reset
+  oscillation_reset_dist: 0.01
+
+  #Eat up the plan as the robot moves along it
+  prune_plan: false

Controllers/Packages/amr_startup/config/costmap_common_params.yaml

@@ -0,0 +1,54 @@
+robot_base_frame: base_footprint
+transform_tolerance: 1.0
+obstacle_range: 3.0
+#mark_threshold: 1
+publish_voxel_map: true
+footprint_padding: 0.0
+
+navigation_map:
+  map_topic: /map
+  map_pkg: managerments
+  map_file: maze
+
+virtual_walls_map:
+  map_topic: /virtual_walls/map
+  namespace: /virtual_walls
+  map_pkg: managerments
+  map_file: maze
+  use_maximum: true
+  lethal_cost_threshold: 100
+
+obstacles:
+  observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
+  f_scan_marking:
+    topic: /f_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  f_scan_clearing:
+    topic: /f_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  b_scan_marking:
+    topic: /b_scan
+    data_type: LaserScan
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25
+  b_scan_clearing:
+    topic: /b_scan
+    data_type: LaserScan
+    clearing: true
+    marking: false
+    inf_is_valid: false
+    min_obstacle_height: 0.0
+    max_obstacle_height: 0.25

Controllers/Packages/amr_startup/config/costmap_global_params.yaml

@@ -0,0 +1,13 @@
+global_costmap:
+    global_frame: map
+    update_frequency: 1.0
+    publish_frequency: 1.0
+    raytrace_range: 2.0
+    resolution: 0.05
+    z_resolution: 0.2
+    rolling_window: false
+    z_voxels: 10
+    inflation:
+      cost_scaling_factor:  10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+      inflation_radius:     0.6  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+

Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -0,0 +1,10 @@
+global_costmap:
+    frame_id: map
+    plugins:
+        - {name: navigation_map, type: "costmap_2d::StaticLayer" }
+        - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
+        - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
+        - {name: inflation,  type: "costmap_2d::InflationLayer" }
+    obstacles:
+        enabled: false
+        footprint_clearing_enabled: false

Controllers/Packages/amr_startup/config/costmap_local_params.yaml

@@ -0,0 +1,16 @@
+local_costmap:  
+    global_frame: odom
+    update_frequency: 5.0
+    publish_frequency: 5.0
+    rolling_window: true
+    raytrace_range: 2.0
+    resolution: 0.05
+    z_resolution: 0.15
+    z_voxels: 8
+    inflation:
+      cost_scaling_factor: 10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+      inflation_radius: 0.3  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+    width: 8.0
+    height: 8.0
+    origin_x: 0.0
+    origin_y: 0.0

Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml

@@ -0,0 +1,8 @@
+local_costmap:
+    frame_id: odom
+    plugins:
+        - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
+        - {name: inflation,  type: "costmap_2d::InflationLayer" }
+    obstacles:
+        enabled: true
+        footprint_clearing_enabled: true

Controllers/Packages/amr_startup/config/custom_global_params.yaml

@@ -0,0 +1,17 @@
+base_global_planner: CustomPlanner
+CustomPlanner:
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 1.0
+  force_scratch_limit: 10000
+  forward_search: false
+  nominalvel_mpersecs: 0.8
+  timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
+  allow_unknown: true
+  directory_to_save_paths: "/init/paths"
+  pathway_filename: "pathway.txt"
+  current_pose_topic_name: "/amcl_pose"
+  map_frame_id: "map"
+  base_frame_id: "base_link"
+  

Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml

@@ -0,0 +1,55 @@
+base_local_planner: dwa_local_planner/DWAPlannerROS
+DWAPlannerROS:
+  # Robot configuration
+  max_vel_x:  0.8
+  min_vel_x: -0.2
+
+  max_vel_y: 0.0  # diff drive robot
+  min_vel_y: 0.0  # diff drive robot
+
+  max_trans_vel: 0.8  # choose slightly less than the base's capability
+  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  trans_stopped_vel: 0.03
+
+  # Warning!
+  #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
+  #   are non-negligible and small in place rotational velocities will be created.
+
+  max_rot_vel: 1.0  # choose slightly less than the base's capability
+  min_rot_vel: 0.1  # this is the min angular velocity when there is negligible translational velocity
+  rot_stopped_vel: 0.1
+
+  acc_lim_x: 1.5
+  acc_lim_y: 0.0      # diff drive robot
+  acc_limit_trans: 1.5
+  acc_lim_theta: 2.0
+
+  # Goal tolerance
+  yaw_goal_tolerance: 0.03  # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
+  xy_goal_tolerance: 0.08   # xy_goal_tolerance  > (sim_time * min_vel_x)   / 2
+  latch_xy_goal_tolerance: true
+
+  # Forward simulation
+  sim_time: 1.2
+  vx_samples: 15
+  vy_samples: 1       # diff drive robot, there is only one sample
+  vtheta_samples: 20
+
+  # Trajectory scoring
+  path_distance_bias: 64.0      # default: 32.0  mir: 32.0  - weighting for how much it should stick to the global path plan
+  goal_distance_bias: 12.0      # default: 24.0  mir: 48.0  - weighting for how much it should attempt to reach its goal
+  occdist_scale: 0.5            # default: 0.01  mir: 0.01  - weighting for how much the controller should avoid obstacles
+  forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
+  stop_time_buffer: 0.2         # default: 0.2   mir: 0.2   - amount of time a robot must stop before colliding for a valid traj.
+  scaling_speed: 0.25           # default: 0.25  mir: 0.25  - absolute velocity at which to start scaling the robot's footprint
+  max_scaling_factor: 0.2       # default: 0.2   mir: 0.2   - how much to scale the robot's footprint when at speed.
+  prune_plan: true
+
+  # Oscillation prevention
+  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags, in m
+  oscillation_reset_angle: 0.2  # 0.2    - the angle the robot must turn before resetting Oscillation flags, in rad
+
+  # Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
+  global_frame_id: /odom  # or <robot namespace>/odom

Controllers/Packages/amr_startup/config/ekf.yaml

@@ -0,0 +1,217 @@
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+frequency: 50
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+sensor_timeout: 0.1
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+transform_time_offset: 0.0
+
+# Use this parameter to specify how long the tf listener should wait for a transform to become available.
+# Defaults to 0.0 if unspecified.
+transform_timeout: 0.0
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+print_diagnostics: true
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+map_frame: map                                   # Defaults to "map" if unspecified
+odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
+base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+odom0: odom
+
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+odom0_config: [false, false, false,   # x y z
+               false, false, false,   # roll pitch yaw
+               true,  true,  false,   # vx vy vz
+               false, false, true,    # vroll vpitch vyaw
+               false, false, false]   # ax ay az
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+odom0_queue_size: 10
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+odom0_differential: false
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+#odom0_pose_rejection_threshold: 5
+#odom0_twist_rejection_threshold: 1
+
+# Further input parameter examples
+# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
+imu0: imu_data
+imu0_config: [false, false, false,   # x y z
+              false, false, true,    # roll pitch yaw
+              false, false, false,   # vx vy vz
+              false, false, true,    # vroll vpitch vyaw
+              true,  false, false]    # ax ay az
+imu0_nodelay: false
+imu0_differential: false
+imu0_relative: true
+imu0_queue_size: 10
+#imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
+#imu0_twist_rejection_threshold: 0.8                #
+#imu0_linear_acceleration_rejection_threshold: 0.8  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+imu0_remove_gravitational_acceleration: false
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
+                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]
+
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+initial_estimate_covariance: [100.0,0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    100.0,0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    1.0,  0,    0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     10.0, 0,    0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    10.0, 0,
+                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

Controllers/Packages/amr_startup/config/localization.yaml

@@ -0,0 +1,36 @@
+Amcl:
+  use_map_topic: true
+  odom_model_type: "diff-corrected"
+  gui_publish_rate: 5.0
+  save_pose_rate: 0.5
+  laser_max_beams: 300
+  laser_min_range: -1.0
+  laser_max_range: -1.0
+  min_particles: 1000
+  max_particles: 3000
+  kld_err: 0.05
+  kld_z: 0.99
+  odom_alpha1: 0.02
+  odom_alpha2: 0.01
+  odom_alpha3: 0.01
+  odom_alpha4: 0.02
+  laser_z_hit: 0.5
+  laser_z_short:       0.05
+  laser_z_max: 0.05
+  laser_z_rand: 0.5
+  laser_sigma_hit:     0.2
+  laser_lambda_short:  0.1
+  laser_model_type:    "likelihood_field"
+  laser_likelihood_max_dist: 1.0
+  update_min_d: 0.05
+  update_min_a: 0.05
+  odom_frame_id: odom
+  base_frame_id: base_footprint
+  global_frame_id: map
+  resample_interval: 1
+  transform_tolerance: 0.2
+  recovery_alpha_slow: 0.001
+  recovery_alpha_fast: 0.001
+  initial_pose_x: 0.0
+  initial_pose_y: 0.0
+  initial_pose_a: 0.0

Controllers/Packages/amr_startup/config/maker_sources.yaml

@@ -0,0 +1,120 @@
+maker_sources: trolley charger dock_station  undock_station
+trolley:
+  plugins:
+    - {name: 4legs,  docking_planner: "DockPlanner", docking_nav: ""}
+    - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
+
+  4legs:
+    maker_goal_frame: trolley_goal
+    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.1
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+  qrcode:
+    maker_goal_frame: qr_trolley
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.05
+    vel_theta: 0.2
+    allow_rotate: true
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+charger:
+  plugins:
+    - {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
+
+  charger:
+    maker_goal_frame: charger_goal
+    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    delay: 2
+    timeout: 60
+    vel_x: 0.1
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+dock_station:
+  plugins:
+  - {name: station, docking_planner: "DockPlanner", docking_nav: ""}
+
+  station:
+    maker_goal_frame: dock_station_goal
+    footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
+    delay: 2
+    timeout: 60
+    vel_x: 0.15
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.01  
+    xy_goal_tolerance: 0.01 
+     min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+
+dock_station_2:
+  plugins:
+  - {name: station, docking_planner: "DockPlanner", docking_nav: ""}
+
+  station:
+    maker_goal_frame: dock_station_goal_2
+    footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
+    delay: 2
+    timeout: 60
+    vel_x: 0.15
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.01  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+undock_station:
+  plugins:
+    - {name: station,  docking_planner: "DockPlanner", docking_nav: ""}
+    - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
+
+  station:
+    maker_goal_frame: undock_station_goal
+    footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.15
+    vel_theta: 0.3
+    yaw_goal_tolerance: 0.017  
+    xy_goal_tolerance: 0.01 
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36
+
+  qrcode:
+    maker_goal_frame: qr_station
+    delay: 2.0
+    timeout: 60.0
+    vel_x: 0.05
+    vel_theta: 0.2
+    allow_rotate: true
+    yaw_goal_tolerance: 0.01 
+    xy_goal_tolerance: 0.01
+    min_lookahead_dist: 0.4
+    max_lookahead_dist: 1.0
+    lookahead_time: 1.5
+    angle_threshold: 0.36

Controllers/Packages/amr_startup/config/mapping.yaml

@@ -0,0 +1,63 @@
+SlamToolBox:
+  # Plugin params
+  solver_plugin: solver_plugins::CeresSolver
+  ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+  ceres_preconditioner: SCHUR_JACOBI
+  ceres_trust_strategy: LEVENBERG_MARQUARDT
+  ceres_dogleg_type: TRADITIONAL_DOGLEG
+  ceres_loss_function: None
+
+  # ROS Parameters
+  odom_frame: odom
+  map_frame: map
+  base_frame: base_link
+  scan_topic: /scan
+  mode: mapping #localization
+  debug_logging: false
+  throttle_scans: 1
+  transform_publish_period: 0.02 #if 0 never publishes odometry
+  map_update_interval: 10.0
+  resolution: 0.05
+  max_laser_range: 20.0 #for rastering images
+  minimum_time_interval: 0.5
+  transform_timeout: 0.2
+  tf_buffer_duration: 14400.
+  stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+  enable_interactive_mode: true
+
+  # General Parameters
+  use_scan_matching: true
+  use_scan_barycenter: true
+  minimum_travel_distance: 0.5
+  minimum_travel_heading: 0.5
+  scan_buffer_size: 10
+  scan_buffer_maximum_scan_distance: 10
+  link_match_minimum_response_fine: 0.1  
+  link_scan_maximum_distance: 1.5
+  loop_search_maximum_distance: 3.0
+  do_loop_closing: true 
+  loop_match_minimum_chain_size: 10           
+  loop_match_maximum_variance_coarse: 3.0  
+  loop_match_minimum_response_coarse: 0.35    
+  loop_match_minimum_response_fine: 0.45
+
+  # Correlation Parameters - Correlation Parameters
+  correlation_search_space_dimension: 0.5
+  correlation_search_space_resolution: 0.01
+  correlation_search_space_smear_deviation: 0.1 
+
+  # Correlation Parameters - Loop Closure Parameters
+  loop_search_space_dimension: 8.0
+  loop_search_space_resolution: 0.05
+  loop_search_space_smear_deviation: 0.03
+
+  # Scan Matcher Parameters
+  distance_variance_penalty: 0.5      
+  angle_variance_penalty: 1.0    
+
+  fine_search_angle_offset: 0.00349     
+  coarse_search_angle_offset: 0.349   
+  coarse_angle_resolution: 0.0349        
+  minimum_angle_penalty: 0.9
+  minimum_distance_penalty: 0.5
+  use_response_expansion: true

Controllers/Packages/amr_startup/config/move_base_common_params.yaml

@@ -0,0 +1,24 @@
+
+### replanning
+controller_frequency: 20.0   # run controller at 15.0 Hz
+controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
+planner_frequency: 0.0       # don't continually replan (only when controller failed)
+planner_patience: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
+max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
+oscillation_timeout: -1.0    # abort controller and trigger recovery behaviors after 30.0 s
+oscillation_distance: 1.5
+### recovery behaviors
+recovery_behavior_enabled: false
+recovery_behaviors: [       
+                      {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
+                      {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},                      
+                    ]
+
+conservative_reset:
+  reset_distance: 3.0        # clear obstacles farther away than 3.0 m
+  invert_area_to_clear: true
+
+aggressive_reset:
+  reset_distance: 3.0
+
+

Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml

@@ -0,0 +1,106 @@
+base_local_planner: mpc_local_planner/MpcLocalPlannerROS
+MpcLocalPlannerROS:
+
+  odom_topic: odom
+    
+   ## Robot settings
+  robot:
+    type: "unicycle"
+    unicycle:
+      max_vel_x: 0.5
+      max_vel_x_backwards: 0.3
+      max_vel_theta: 0.3
+      acc_lim_x: 0.4 # deactive bounds with zero
+      dec_lim_x: 0.4 # deactive bounds with zero
+      acc_lim_theta: 0.6 # deactivate bounds with zero
+
+  ## Footprint model for collision avoidance
+  footprint_model:
+    type: "point"
+    is_footprint_dynamic: False
+
+  ## Collision avoidance
+  collision_avoidance:
+    min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
+    enable_dynamic_obstacles: False
+    force_inclusion_dist: 0.5
+    cutoff_dist: 2.0  
+    include_costmap_obstacles: True
+    costmap_obstacles_behind_robot_dist: 1.5
+    collision_check_no_poses: 5
+
+  ## Planning grid 
+  grid:
+    type: "fd_grid"
+    grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist
+    dt_ref: 0.3 # and here the corresponding temporal resolution  
+    xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below
+    warm_start: True
+    collocation_method: "forward_differences"
+    cost_integration_method: "left_sum"
+    variable_grid:
+      enable: False # We want a fixed grid  
+      min_dt: 0.0;
+      max_dt: 10.0;
+      grid_adaptation:
+        enable: True
+        dt_hyst_ratio: 0.1
+        min_grid_size: 2
+        max_grid_size: 50
+
+  ## Planning options
+  planning:
+    objective:
+      type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
+      quadratic_form:
+        state_weights: [2.0, 2.0, 0.25]
+        control_weights: [0.1, 0.05]
+        integral_form: False
+    terminal_cost:
+      type: "quadratic" # can be "none"
+      quadratic:
+        final_state_weights: [10.0, 10.0, 0.5]
+    terminal_constraint:
+      type: "none" # can be "none"
+      l2_ball:
+        weight_matrix: [1.0, 1.0, 1.0]
+        radius: 5
+
+  ## Controller options
+  controller:
+    outer_ocp_iterations: 1
+    xy_goal_tolerance: 0.05
+    yaw_goal_tolerance: 0.04
+    global_plan_overwrite_orientation: False
+    global_plan_prune_distance: 1.5
+    allow_init_with_backward_motion: True
+    max_global_plan_lookahead_dist: 1.0 # Check horizon length
+    force_reinit_new_goal_dist: 1.0 
+    force_reinit_new_goal_angular: 1.57
+    force_reinit_num_steps: 0
+    prefer_x_feedback: False
+    publish_ocp_results: True
+    
+  ## Solver settings
+  solver:
+    type: "ipopt"
+    ipopt:
+      iterations: 100
+      max_cpu_time: -1.0
+      ipopt_numeric_options:
+        tol: 1e-3
+      ipopt_string_options:
+        linear_solver: "mumps" 
+        hessian_approximation: "exact" # exact or limited-memory
+    lsq_lm:
+      iterations: 10
+      weight_init_eq: 2
+      weight_init_ineq: 2
+      weight_init_bounds: 2
+      weight_adapt_factor_eq: 1.5
+      weight_adapt_factor_ineq: 1.5
+      weight_adapt_factor_bounds: 1.5
+      weight_adapt_max_eq: 500
+      weight_adapt_max_ineq: 500
+      weight_adapt_max_bounds: 500
+

Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m

@@ -0,0 +1,280 @@
+% /*
+%  * Copyright (c) 2008, Maxim Likhachev
+%  * All rights reserved.
+%  *
+%  * Redistribution and use in source and binary forms, with or without
+%  * modification, are permitted provided that the following conditions are met:
+%  *
+%  *     * Redistributions of source code must retain the above copyright
+%  *       notice, this list of conditions and the following disclaimer.
+%  *     * Redistributions in binary form must reproduce the above copyright
+%  *       notice, this list of conditions and the following disclaimer in the
+%  *       documentation and/or other materials provided with the distribution.
+%  *     * Neither the name of the Carnegie Mellon University nor the names of its
+%  *       contributors may be used to endorse or promote products derived from
+%  *       this software without specific prior written permission.
+%  *
+%  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+%  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+%  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+%  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+%  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+%  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+%  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+%  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+%  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+%  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+%  * POSSIBILITY OF SUCH DAMAGE.
+%  */
+
+function[] = genmprim_unicycle_highcost_5cm(outfilename)
+
+%
+%generates motion primitives and saves them into file
+%
+%written by Maxim Likhachev
+%---------------------------------------------------
+%
+
+%defines
+
+UNICYCLE_MPRIM_16DEGS = 1;
+
+
+if UNICYCLE_MPRIM_16DEGS == 1
+    resolution = 0.05;
+    numberofangles = 16; %preferably a power of 2, definitely multiple of 8
+    numberofprimsperangle = 7;
+
+    %multipliers (multiplier is used as costmult*cost)
+    forwardcostmult = 1;
+    backwardcostmult = 40;
+    forwardandturncostmult = 2;
+    sidestepcostmult = 10;
+    turninplacecostmult = 20;
+
+    %note, what is shown x,y,theta changes (not absolute numbers)
+
+    %0 degreees
+    basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
+    basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
+    basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
+    basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
+
+    %45 degrees
+    basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
+    basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
+    basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
+    basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
+
+    %22.5 degrees
+    basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
+    %x aligned with the heading of the robot, angles are positive
+    %counterclockwise
+    %0 theta change
+    basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
+    basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
+    basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
+    %1/16 theta change
+    basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
+    basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
+    %turn in place
+    basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
+    basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
+
+else
+    fprintf(1, 'ERROR: undefined mprims type\n');
+    return;
+end;
+
+
+fout = fopen(outfilename, 'w');
+
+
+%write the header
+fprintf(fout, 'resolution_m: %f\n', resolution);
+fprintf(fout, 'numberofangles: %d\n', numberofangles);
+fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
+
+%iterate over angles
+for angleind = 1:numberofangles
+
+    figure(1);
+    hold off;
+
+    text(0, 0, int2str(angleind));
+
+    %iterate over primitives
+    for primind = 1:numberofprimsperangle
+        fprintf(fout, 'primID: %d\n', primind-1);
+        fprintf(fout, 'startangle_c: %d\n', angleind-1);
+
+        %current angle
+        currentangle = (angleind-1)*2*pi/numberofangles;
+        currentangle_36000int = round((angleind-1)*36000/numberofangles);
+
+        %compute which template to use
+        if (rem(currentangle_36000int, 9000) == 0)
+            basemprimendpts_c = basemprimendpts0_c(primind,:);
+            angle = currentangle;
+        elseif (rem(currentangle_36000int, 4500) == 0)
+            basemprimendpts_c = basemprimendpts45_c(primind,:);
+            angle = currentangle - 45*pi/180;
+        elseif (rem(currentangle_36000int-7875, 9000) == 0)
+            basemprimendpts_c = basemprimendpts33p75_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
+            angle = currentangle - 78.75*pi/180;
+            fprintf(1, '78p75\n');
+        elseif (rem(currentangle_36000int-6750, 9000) == 0)
+            basemprimendpts_c = basemprimendpts22p5_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
+            %fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
+            %    basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
+            angle = currentangle - 67.5*pi/180;
+            fprintf(1, '67p5\n');
+        elseif (rem(currentangle_36000int-5625, 9000) == 0)
+            basemprimendpts_c = basemprimendpts11p25_c(primind,:);
+            basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
+            basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
+            basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
+            angle = currentangle - 56.25*pi/180;
+            fprintf(1, '56p25\n');
+        elseif (rem(currentangle_36000int-3375, 9000) == 0)
+            basemprimendpts_c = basemprimendpts33p75_c(primind,:);
+            angle = currentangle - 33.75*pi/180;
+            fprintf(1, '33p75\n');
+        elseif (rem(currentangle_36000int-2250, 9000) == 0)
+            basemprimendpts_c = basemprimendpts22p5_c(primind,:);
+            angle = currentangle - 22.5*pi/180;
+            fprintf(1, '22p5\n');
+        elseif (rem(currentangle_36000int-1125, 9000) == 0)
+            basemprimendpts_c = basemprimendpts11p25_c(primind,:);
+            angle = currentangle - 11.25*pi/180;
+            fprintf(1, '11p25\n');
+        else
+            fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
+            return;
+        end;
+
+        %now figure out what action will be
+        baseendpose_c = basemprimendpts_c(1:3);
+        additionalactioncostmult = basemprimendpts_c(4);
+        endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
+        endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
+        endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
+        endpose_c = [endx_c endy_c endtheta_c];
+
+        fprintf(1, 'rotation angle=%f\n', angle*180/pi);
+
+        if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
+            %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+        end;
+
+        %generate intermediate poses (remember they are w.r.t 0,0 (and not
+        %centers of the cells)
+        numofsamples = 10;
+        intermcells_m = zeros(numofsamples,3);
+        if UNICYCLE_MPRIM_16DEGS == 1
+            startpt = [0 0 currentangle];
+            endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
+                rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
+            intermcells_m = zeros(numofsamples,3);
+            if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
+                for iind = 1:numofsamples
+                    intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
+                                            startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
+                                            0];
+                    rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
+                    intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
+
+                end;
+            else %unicycle-based move forward or backward
+                R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
+                    sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
+                S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
+                l = S(1);
+                tvoverrv = S(2);
+                rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
+                tv = tvoverrv*rv;
+
+                if l < 0
+                    fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
+                    l = 0;
+                end;
+                %compute rv
+                %rv = baseendpose_c(3)*2*pi/numberofangles;
+                %compute tv
+                %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
+                %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
+                %tv = (tvx + tvy)/2.0;
+                %generate samples
+                for iind = 1:numofsamples
+                    dt = (iind-1)/(numofsamples-1);
+
+                    %dtheta = rv*dt + startpt(3);
+                    %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
+                    %                        startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
+                    %                        dtheta];
+
+                    if(dt*tv < l)
+                        intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
+                                                 startpt(2) + dt*tv*sin(startpt(3)) ...
+                                                 startpt(3)];
+                    else
+                        dtheta = rv*(dt - l/tv) + startpt(3);
+                        intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
+                                                 startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
+                                                 dtheta];
+                    end;
+                end;
+                %correct
+                errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
+                           endpt(2) - intermcells_m(numofsamples,2)];
+                fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
+                interpfactor = [0:1/(numofsamples-1):1];
+                intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
+                intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
+            end;
+        end;
+
+        %write out
+        fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+        fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
+        fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
+        for interind = 1:size(intermcells_m, 1)
+            fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
+        end;
+
+        plot(intermcells_m(:,1), intermcells_m(:,2));
+        axis([-0.3 0.3 -0.3 0.3]);
+        text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
+        hold on;
+
+    end;
+    grid;
+    pause;
+end;
+
+fclose('all');

Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py

@@ -0,0 +1,416 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2016, David Conner (Christopher Newport University)
+# Based on genmprim_unicycle.m
+# Copyright (c) 2008, Maxim Likhachev
+# All rights reserved.
+# converted by libermate utility (https://github.com/awesomebytes/libermate)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Carnegie Mellon University nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import numpy as np
+import rospkg
+
+# if available import pylab (from matlibplot)
+matplotlib_found = False
+try:
+    import matplotlib.pylab as plt
+
+    matplotlib_found = True
+except ImportError:
+    pass
+
+
+def matrix_size(mat, elem=None):
+    if not elem:
+        return mat.shape
+    else:
+        return mat.shape[int(elem) - 1]
+
+
+def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
+    visualize = matplotlib_found and visualize  # Plot the primitives
+
+    # Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
+    #   baseendpose_c, additionalactioncostmult, fout, numofsamples,
+    #   basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
+    #   numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
+    #   rotation_angle, basemprimendpts_c, forwardandturncostmult,
+    #   forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
+    #   interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
+    #   currentangle, numberofprimsperangle, resolution, currentangle_36000int,
+    #   l, iind, errorxy, interind, endy_c, angleind, endpt
+    # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
+    #   int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
+    #   round, size
+    # %
+    # %generates motion primitives and saves them into file
+    # %
+    # %written by Maxim Likhachev
+    # %---------------------------------------------------
+    # %
+    # %defines
+    UNICYCLE_MPRIM_16DEGS = 1.0
+    if UNICYCLE_MPRIM_16DEGS == 1.0:
+        resolution = 0.05
+        numberofangles = 16
+        # %preferably a power of 2, definitely multiple of 8
+        numberofprimsperangle = 7
+        # %multipliers (multiplier is used as costmult*cost)
+        forwardcostmult = 1.0
+        backwardcostmult = 40.0
+        forwardandturncostmult = 2.0
+        # sidestepcostmult = 10.0
+        turninplacecostmult = 20.0
+        # %note, what is shown x,y,theta changes (not absolute numbers)
+        # %0 degreees
+        basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
+        basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
+        basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
+        basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+        # %45 degrees
+        basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult (multiplier is used as costmult*cost)
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
+        basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
+        basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
+        basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+        # %22.5 degrees
+        basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
+        # %x,y,theta,costmult (multiplier is used as costmult*cost)
+        # %x aligned with the heading of the robot, angles are positive
+        # %counterclockwise
+        # %0 theta change
+        basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
+        basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
+        basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
+        # %1/16 theta change
+        basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
+        basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
+        # %turn in place
+        basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
+        basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
+    else:
+        print('ERROR: undefined mprims type\n')
+        return []
+
+    fout = open(outfilename, 'w')
+    # %write the header
+    fout.write('resolution_m: %f\n' % (resolution))
+    fout.write('numberofangles: %d\n' % (numberofangles))
+    fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
+    # %iterate over angles
+    for angleind in np.arange(1.0, (numberofangles) + 1):
+        currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
+        currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
+        if visualize:
+            if separate_plots:
+                fig = plt.figure(angleind)
+                plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
+            else:
+                fig = plt.figure(1)
+
+            plt.axis('equal')
+            plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
+            ax = fig.add_subplot(1, 1, 1)
+            major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
+            minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
+            ax.set_xticks(major_ticks)
+            ax.set_xticks(minor_ticks, minor=True)
+            ax.set_yticks(major_ticks)
+            ax.set_yticks(minor_ticks, minor=True)
+            ax.grid(which='minor', alpha=0.5)
+            ax.grid(which='major', alpha=0.9)
+
+        # %iterate over primitives
+        for primind in np.arange(1.0, (numberofprimsperangle) + 1):
+            fout.write('primID: %d\n' % (primind - 1))
+            fout.write('startangle_c: %d\n' % (angleind - 1))
+            # %current angle
+            # %compute which template to use
+            if (currentangle_36000int % 9000) == 0:
+                basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
+                angle = currentangle
+            elif (currentangle_36000int % 4500) == 0:
+                basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
+                angle = currentangle - 45.0 * np.pi / 180.0
+
+            # commented out because basemprimendpts33p75_c is undefined
+            # elif ((currentangle_36000int - 7875) % 9000) == 0:
+            #     basemprimendpts_c = (
+            #         1 * basemprimendpts33p75_c[primind, :]
+            #     )  # 1* to force deep copy to avoid reference update below
+            #     basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
+            #     # %reverse x and y
+            #     basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
+            #     basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
+            #     # %reverse the angle as well
+            #     angle = currentangle - (78.75 * np.pi) / 180.0
+            #     print('78p75\n')
+
+            elif ((currentangle_36000int - 6750) % 9000) == 0:
+                basemprimendpts_c = (
+                    1 * basemprimendpts22p5_c[int(primind) - 1, :]
+                )  # 1* to force deep copy to avoid reference update below
+                basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
+                # %reverse x and y
+                basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
+                basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
+                # %reverse the angle as well
+                # print(
+                #     '%d : %d %d %d onto %d %d %d\n'
+                #     % (
+                #         primind - 1,
+                #         basemprimendpts22p5_c[int(primind) - 1, 0],
+                #         basemprimendpts22p5_c[int(primind) - 1, 1],
+                #         basemprimendpts22p5_c[int(primind) - 1, 2],
+                #         basemprimendpts_c[0],
+                #         basemprimendpts_c[1],
+                #         basemprimendpts_c[2],
+                #     )
+                # )
+                angle = currentangle - (67.5 * np.pi) / 180.0
+                print('67p5\n')
+
+            # commented out because basemprimendpts11p25_c is undefined
+            # elif ((currentangle_36000int - 5625) % 9000) == 0:
+            #     basemprimendpts_c = (
+            #         1 * basemprimendpts11p25_c[primind, :]
+            #     )  # 1* to force deep copy to avoid reference update below
+            #     basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
+            #     # %reverse x and y
+            #     basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
+            #     basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
+            #     # %reverse the angle as well
+            #     angle = currentangle - (56.25 * np.pi) / 180.0
+            #     print('56p25\n')
+
+            # commented out because basemprimendpts33p75_c is undefined
+            # elif ((currentangle_36000int - 3375) % 9000) == 0:
+            #     basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
+            #     angle = currentangle - (33.75 * np.pi) / 180.0
+            #     print('33p75\n')
+
+            elif ((currentangle_36000int - 2250) % 9000) == 0:
+                basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
+                angle = currentangle - (22.5 * np.pi) / 180.0
+                print('22p5\n')
+
+            # commented out because basemprimendpts11p25_c is undefined
+            # elif ((currentangle_36000int - 1125) % 9000) == 0:
+            #     basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
+            #     angle = currentangle - (11.25 * np.pi) / 180.0
+            #     print('11p25\n')
+
+            else:
+                print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
+                return []
+
+            # %now figure out what action will be
+            baseendpose_c = basemprimendpts_c[0:3]
+            additionalactioncostmult = basemprimendpts_c[3]
+            endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
+            endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
+            endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
+            endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
+            print("endpose_c=", endpose_c)
+            print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
+            # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
+            # %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
+
+            # %generate intermediate poses (remember they are w.r.t 0,0 (and not
+            # %centers of the cells)
+            numofsamples = 10
+            intermcells_m = np.zeros((numofsamples, 3))
+            if UNICYCLE_MPRIM_16DEGS == 1.0:
+                startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
+                endpt = np.array(
+                    np.hstack(
+                        (
+                            (endpose_c[0] * resolution),
+                            (endpose_c[1] * resolution),
+                            (
+                                ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
+                                / numberofangles
+                            ),
+                        )
+                    )
+                )
+
+                print("startpt =", startpt)
+                print("endpt   =", endpt)
+                intermcells_m = np.zeros((numofsamples, 3))
+                if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
+                    # %turn in place or move forward
+                    for iind in np.arange(1.0, (numofsamples) + 1):
+                        fraction = float(iind - 1) / (numofsamples - 1)
+                        intermcells_m[int(iind) - 1, :] = np.array(
+                            (
+                                startpt[0] + (endpt[0] - startpt[0]) * fraction,
+                                startpt[1] + (endpt[1] - startpt[1]) * fraction,
+                                0,
+                            )
+                        )
+                        rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
+                        intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
+                        # print " ",iind,"  of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
+
+                else:
+                    # %unicycle-based move forward or backward  (http://sbpl.net/node/53)
+                    R = np.array(
+                        np.vstack(
+                            (
+                                np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
+                                np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
+                            )
+                        )
+                    )
+
+                    S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
+                    l = S[0]
+                    tvoverrv = S[1]
+                    rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
+                    tv = tvoverrv * rv
+
+                    # print "R=\n",R
+                    # print "Rpi=\n",np.linalg.pinv(R)
+                    # print "S=\n",S
+                    # print "l=",l
+                    # print "tvoverrv=",tvoverrv
+                    # print "rv=",rv
+                    # print "tv=",tv
+
+                    if l < 0.0:
+                        print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
+                        l = 0.0
+
+                    # %compute rv
+                    # %rv = baseendpose_c(3)*2*pi/numberofangles;
+                    # %compute tv
+                    # %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
+                    # %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
+                    # %tv = (tvx + tvy)/2.0;
+                    # %generate samples
+                    for iind in np.arange(1, numofsamples + 1):
+                        dt = (iind - 1) / (numofsamples - 1)
+                        # %dtheta = rv*dt + startpt(3);
+                        # %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
+                        # %                        startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
+                        # %                        dtheta];
+                        if (dt * tv) < l:
+                            intermcells_m[int(iind) - 1, :] = np.array(
+                                np.hstack(
+                                    (
+                                        startpt[0] + dt * tv * np.cos(startpt[2]),
+                                        startpt[1] + dt * tv * np.sin(startpt[2]),
+                                        startpt[2],
+                                    )
+                                )
+                            )
+                        else:
+                            dtheta = rv * (dt - l / tv) + startpt[2]
+                            intermcells_m[int(iind) - 1, :] = np.array(
+                                np.hstack(
+                                    (
+                                        startpt[0]
+                                        + l * np.cos(startpt[2])
+                                        + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
+                                        startpt[1]
+                                        + l * np.sin(startpt[2])
+                                        - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
+                                        dtheta,
+                                    )
+                                )
+                            )
+
+                    # %correct
+                    errorxy = np.array(
+                        np.hstack(
+                            (
+                                endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
+                                endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
+                            )
+                        )
+                    )
+                    # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
+                    interpfactor = np.array(
+                        np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
+                    )
+
+                    # print "intermcells_m=",intermcells_m
+                    # print "interp'=",interpfactor.conj().T
+
+                    intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
+                    intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
+
+            # %write out
+            fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
+            fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
+            fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
+            for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
+                fout.write(
+                    '%.4f %.4f %.4f\n'
+                    % (
+                        intermcells_m[int(interind) - 1, 0],
+                        intermcells_m[int(interind) - 1, 1],
+                        intermcells_m[int(interind) - 1, 2],
+                    )
+                )
+
+            if visualize:
+                plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
+                plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
+        # if (visualize):
+        #    plt.waitforbuttonpress()  # uncomment to plot each primitive set one at a time
+
+    fout.close()
+    if visualize:
+        #    plt.waitforbuttonpress()  # hold until buttom pressed
+        plt.show()  # Keep windows open until the program is terminated
+    return []
+
+
+if __name__ == "__main__":
+    rospack = rospkg.RosPack()
+    outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
+    genmprim_unicycle(outfilename, visualize=True)

Controllers/Packages/amr_startup/config/mqtt_general.yaml

@@ -0,0 +1,8 @@
+MQTT:
+  Name: T801
+  Host: 172.20.235.170
+  Port: 1885
+  Client_ID: T801
+  Username: robotics
+  Password: robotics
+  Keep_Alive: 60

Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml

@@ -0,0 +1,188 @@
+
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+
+base_local_planner: nav_core_adapter::LocalPlannerAdapter
+yaw_goal_tolerance: 0.017  
+xy_goal_tolerance: 0.015 
+stateful: true
+publish_topic: true  
+
+LocalPlannerAdapter:
+  PNKXLocalPlanner:
+    # Algorithm
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
+    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    # Goal checking
+    goal_checker_name: mkt_plugins::GoalChecker 
+
+  PNKXDockingLocalPlanner: 
+    # Algorithm
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
+    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    # Goal checking
+    goal_checker_name: mkt_plugins::GoalChecker 
+
+  PNKXGoStraightLocalPlanner:
+    # Algorithm
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    algorithm_nav_name: mkt_algorithm::diff::GoStraight
+    # Goal checking
+    goal_checker_name: mkt_plugins::GoalChecker 
+
+  PNKXRotateLocalPlanner:
+    # Algorithm
+    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
+    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
+    # Goal checking
+    goal_checker_name: mkt_plugins::SimpleGoalChecker
+    stateful: true
+    
+LimitedAccelGenerator:
+  max_vel_x: 1.2
+  min_vel_x: -1.2
+
+  max_vel_y: 0.0  # diff drive robot
+  min_vel_y: 0.0  # diff drive robot
+
+  max_speed_xy: 2.0 # max_trans_vel: 0.8  # choose slightly less than the base's capability
+  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+
+  max_vel_theta: 0.3    # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  min_vel_theta: 0.05  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
+
+  acc_lim_x: 1.0
+  acc_lim_y: 0.0      # diff drive robot
+  acc_lim_theta: 0.5
+  decel_lim_x: -2.0
+  decel_lim_y: -0.0
+  decel_lim_theta: -1.0
+
+  # Whether to split the path into segments or not
+  split_path: true
+  sim_time: 1.5
+  vx_samples: 15
+  vy_samples: 1       
+  vtheta_samples: 10
+  discretize_by_time: true
+  angular_granularity: 0.05
+  linear_granularity: 0.1
+  # sim_period
+  include_last_point: true
+
+PredictiveTrajectory:
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.5
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.5                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  # Regulated linear velocity scaling
+  use_regulated_linear_velocity_scaling: true   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  # only when true:
+  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
+  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
+
+  # Inflation cost scaling (Limit velocity by proximity to obstacles)
+  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
+  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
+  cost_scaling_dist: 0.2                             # (default: 0.6)
+  cost_scaling_gain: 2.0                             # (default: 1.0)
+
+GoStraight:
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.6
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true   # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.325                        # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.5                    # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.6                    # Minimum squared journey to consider for goal (default: 0.2) 
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.35   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  # Speed
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  use_regulated_linear_velocity_scaling: false
+  use_cost_regulated_linear_velocity_scaling: false
+
+RotateToGoal:
+  avoid_obstacles: false
+  xy_local_goal_tolerance: 0.01
+  angle_threshold: 0.6
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.5                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.6                    # Maximum squared journey to consider for goal (default: 0.2) 
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+
+  # stoped
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.06
+  min_approach_linear_velocity: 0.05   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
+  
+  # Regulated linear velocity scaling
+  use_regulated_linear_velocity_scaling: true   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
+  # only when true:
+  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
+  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
+
+  # Inflation cost scaling (Limit velocity by proximity to obstacles)
+  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
+  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
+  cost_scaling_dist: 0.2                             # (default: 0.6)
+  cost_scaling_gain: 2.0                             # (default: 1.0)

Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml

@@ -0,0 +1,71 @@
+# -----------------------------------
+left_wheel  : 'left_wheel_joint' 
+right_wheel : 'right_wheel_joint'
+publish_rate: 50              # this is what the real MiR platform publishes (default: 50)
+# These covariances are exactly what the real MiR platform publishes
+pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+enable_odom_tf: true
+enable_wheel_tf: true
+allow_multiple_cmd_vel_publishers: true
+# open_loop: false
+# Wheel separation and diameter. These are both optional.
+# diff_drive_controller will attempt to read either one or both from the
+# URDF if not specified as a parameter.
+# We don't set the value here because it's different for each MiR type (100, 250, ...), and
+# the plugin figures out the correct values.
+wheel_separation : 0.5106
+wheel_radius : 0.1
+
+# Wheel separation and radius multipliers
+wheel_separation_multiplier: 1.0 # default: 1.0
+wheel_radius_multiplier    : 1.0 # default: 1.0
+
+# Velocity commands timeout [s], default 0.5
+cmd_vel_timeout: 1.0
+
+# frame_ids (same as real MiR platform)
+base_frame_id: base_footprint # default: base_link base_footprint
+odom_frame_id: odom           # default: odom 
+
+# Velocity and acceleration limits
+# Whenever a min_* is unspecified, default to -max_*
+linear:
+  x:
+    has_velocity_limits    : true
+    max_velocity           : 1.5  # m/s; move_base max_vel_x: 0.8
+    min_velocity           : -1.5 # m/s
+    has_acceleration_limits: true
+    max_acceleration       : 3.0  # m/s^2; move_base acc_lim_x: 1.5
+    min_acceleration       : -3.0 # m/s^2
+    has_jerk_limits        : true
+    max_jerk               : 5.0  # m/s^3
+angular:
+  z:
+    has_velocity_limits    : true
+    max_velocity           : 0.6  # rad/s; move_base max_rot_vel: 1.0
+    min_velocity           : -0.6 
+    has_acceleration_limits: true
+    max_acceleration       : 3.0 # rad/s^2; move_base acc_lim_th: 2.0
+    has_jerk_limits        : true
+    max_jerk               : 2.5  # rad/s^3
+
+left_wheel_joint: 
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: left_encoder
+  frame_id: left_wheel_link
+  wheel_topic: /left_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0
+  origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0]  # origin from base_frame_id
+
+right_wheel_joint:  
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: right_encoder
+  frame_id: right_wheel_link
+  wheel_topic: /right_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0
+  origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0]  # origin from base_frame_id

Controllers/Packages/amr_startup/config/sbpl_global_params.yaml

@@ -0,0 +1,10 @@
+base_global_planner: SBPLLatticePlanner
+SBPLLatticePlanner:
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 1.0
+  force_scratch_limit: 10000
+  forward_search: true
+  nominalvel_mpersecs: 0.2
+  timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s

Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml

@@ -0,0 +1,61 @@
+bus:
+  device: can0
+  driver_plugin: can::SocketCANInterface
+  master_allocator: canopen::SimpleMaster::Allocator
+sync:
+  interval_ms: 10
+  overflow: 0
+#
+# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
+#   rate: 100 # heartbeat rate (1/rate in seconds)
+#   msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
+nodes:
+  f_mlse:
+    id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: mls/SICK-MLS.eds      # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
+    # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "MLS"              # can devices of OLS10, OLS20 or MLS family currently supported
+    sick_topic: "f_mlse"                      # MLS_Measurement messages are published in topic "/mls"
+    subscribe_queue_size: 1
+    sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2025":     "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
+    #   "2026":        "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
+    #   "2027":     "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2029":     "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
+    #
+
+  b_mlse:
+    id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
+    eds_pkg: sick_line_guidance # package name for relative path
+    eds_file: mls/SICK-MLS.eds      # path to EDS/DCF file
+    publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
+    # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
+    
+    # sick_line_guidance configuration of this node:
+    sick_device_family: "MLS"              # can devices of OLS10, OLS20 or MLS family currently supported
+    sick_topic: "b_mlse"                      # MLS_Measurement messages are published in topic "/mls"
+    subscribe_queue_size: 1
+    sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
+
+    # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
+    # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
+    # dcf_overlay:
+    #   "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
+    #   "2025":     "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
+    #   "2026":        "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
+    #   "2027":     "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
+    #   "2029":     "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
+    #

Controllers/Packages/amr_startup/config/two_points_global_params.yaml

@@ -0,0 +1,3 @@
+base_global_planner: TwoPointsPlanner
+TwoPointsPlanner:
+  lethal_obstacle: 20

Controllers/Packages/amr_startup/launch/amr_control.launch

@@ -0,0 +1,50 @@
+<launch>
+  <arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
+  <arg name="local_planner"   default="pnkx"  doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
+  <arg name="global_planner"  default="custom" doc="Global planner can be either sbpl two_points custom"/>
+  <arg name="global_plan_msg_type" default="vda5050_msgs::Order" doc="nav_msgs::Path or vda5050_msgs::Order"/> 
+  <arg name="primitive_filename" default="$(find amr_startup)/config/mprim/unicycle_highcost_5cm.mprim"/>
+  <arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
+  <arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
+
+  <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
+  <rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
+  <node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
+    <!-- robot type -->
+    <rosparam param="footprint" if="$(eval robot_type == 'imr')">
+      [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]]
+    </rosparam>
+    <!-- Footprint cho imr -->
+    <rosparam param="footprint" if="$(eval robot_type == 'hook_150')">
+      [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]]
+    </rosparam>
+
+    <rosparam param="footprint" if="$(eval robot_type == 'T800')">
+      [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
+    </rosparam>
+
+    <param name="primitive_filename" value="$(arg primitive_filename)" />
+    <param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
+    
+    <rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
+
+    <!-- global costmap params -->
+    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_global_params.yaml" command="load" />
+    <rosparam file="$(find amr_startup)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" />
+
+    <!-- local costmap params -->
+    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_local_params.yaml" command="load" subst_value="true" />
+    <rosparam file="$(find amr_startup)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" />
+
+    <rosparam file="$(find amr_startup)/config/localization.yaml" command="load" />
+    
+    <remap from="map" to="/map" />
+    <remap from="marker" to="move_base_node/markers" />
+    <!-- <remap from="cmd_vel" to="/cmd_vel_1" /> -->
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/amr_startup.launch

@@ -0,0 +1,102 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_prefix" default="" />
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="imu_link_to_base_link" args="0 0 0.45 0 0 0 base_link imu_link" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="surface_to_base_link" args="0 0 0.9 0 0 0 base_link surface" />
+
+
+  <include file="$(find sick_line_guidance)/launch/sick_line_guidance.launch">
+    <arg name="yaml"     value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
+  </include>
+  <include file="$(find mlse_tf_base_link)/launch/msle_tf_base_link.launch">
+    <arg name="yaml"     value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
+  </include>
+
+  <node pkg="diff_wheel_plugin"   type="diff_wheel_feedback" name="diff_wheel_feedback" output="screen"> 
+    <rosparam file="$(find amr_startup)/config/plc_config.yaml" command="load" /> 
+    <param name="start_bit"     value="111"/>
+    <param name="end_bit"       value="114"/>
+  </node>
+
+  <node pkg="diff_wheel_plugin" type="diff_wheel_controller" name="diff_wheel_controller" output="screen">
+    <rosparam file="$(find amr_startup)/config/motorInfomation.yaml" command="load" /> 
+    <param name="port_name" type="str" value="/dev/ttyTHS0"/> 
+  </node>
+
+  <arg name="frame_id" default="imu_frame"/>
+  <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg frame_id)_to_imu_link" args="0 0 0 0 0 0 imu_link $(arg frame_id)" />
+  <node pkg="wit_wt901ble_reader" name="wit_wt901ble_reader_node" type="wit_wt901ble_reader_node" output="screen">
+    <param name="baudrate" type="int" value="115200"/>
+    <param name="portname" type="string" value="/dev/USB_IMU"/>
+    <param name="topic_name" type="string" value="/imu_data"/>
+    <param name="frame_id" type="string" value="$(arg frame_id)"/>
+    <param name="imu_diagnostics_topic_name" type="string" value="/imu_diagnostics"/>
+  </node>
+  
+  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics" output="screen"> 
+    <rosparam file="$(find amr_startup)/config/ros_diff_drive_controller.yaml" command="load" />
+    <param name="use_encoder" type="bool" value="false"/>
+    <param name="type" type="int" value="2"/>
+    <remap from="/ros_kinematics/odom" to="/odom" />
+  </node>
+
+  <!-- <include file="$(find manual_psx)/launch/ps4.launch"></include> -->
+
+  <!-- <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
+  </node> -->
+
+  <!-- Load URDF -->
+  <include file="$(find robot_description)/launch/upload_urdf.launch">
+      <arg name="robot_type" value="imr" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
+
+  <!-- driver -->
+  <arg name="pkg" value="olelidar"/>
+  
+  <arg name="driver" default="true"/>
+  <arg name="f_device_ip" default="192.168.2.101"/>
+  <arg name="f_device_port" default="2368"/>
+  <arg name="local_ip" default="192.168.2.100"/>
+  <arg name="multiaddr" default=""/>
+  
+  <!-- decoder -->
+  <arg name="f_frame_id" default="front_laser_link"/>
+  <arg name="f_topic_name" default="/f_scan"/>
+  <arg name="f_r_max" default="25"/>
+  <arg name="f_ang_start" default="-90"/>
+  <arg name="f_ang_end" default="90"/>
+  <arg name="decoder" default="true"/>
+  <arg name="inverted" default="false"/>
+  <arg name="f_debug" default="false"/>
+  <env if="$(arg f_debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find olelidar)/launch/debug.conf"/>
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg f_frame_id)_to_base_link" args="0.432 0 0.135 0 0 0 base_link $(arg f_frame_id)" />
+
+  <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen">
+    <param name="frame_id" type="string" value="$(arg f_frame_id)"/>
+    <param name="r_max" type="int" value="$(arg f_r_max)"/>
+    <param name="ang_start" type="int" value="$(arg f_ang_start)"/>
+    <param name="ang_end" type="int" value="$(arg f_ang_end)"/>
+    <param name="inverted" type="bool" value="$(arg inverted)"/>
+    <param name="device_ip" type="string" value="$(arg f_device_ip)"/>
+    <param name="device_port" type="int" value="$(arg f_device_port)"/>
+    <param name="local_ip" type="string" value="$(arg local_ip)"/>
+    <param name="multiaddr" type="string" value="$(arg multiaddr)"/>
+
+    <remap from="~packet" to="packet"/>
+    <remap from="~scan" to="$(arg f_topic_name)"/>
+  </node>
+
+  <node pkg="topic_tools" type="relay" name="f_scan_relay" args="$(arg f_topic_name) scan"/>
+
+  <include file="$(find amr_startup)/launch/amr_control.launch"></include>
+
+  <!-- <include file="$(find amr_startup)/launch/includes/rviz.launch"></include> -->
+
+  <!-- <include file="$(find slam_toolbox)/launch/offline.launch"></include> -->
+
+</launch>  

Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch

@@ -0,0 +1,5 @@
+
+<launch>
+    <!-- <node pkg="tf" type="static_transform_publisher" name="goal_charger_broadcaster" args="1.9 0.07 0 0.0 0 0  base_link shelf_target 100" /> -->
+    <node pkg="tf" type="static_transform_publisher" name="base_link_surface_broadcaster" args="0 0 0 0 0 0  base_link surface 1000" />
+</launch>

Controllers/Packages/amr_startup/launch/hector_mapping.launch

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
+  <arg name="base_frame" default="base_footprint"/>
+  <arg name="odom_frame" default="odom"/>
+  <arg name="pub_map_odom_transform" default="true"/>
+  <arg name="scan_subscriber_queue_size" default="5"/>
+  <arg name="scan_topic" default="scan"/>
+  <arg name="map_size" default="2048"/>
+    <!-- set use_tf_pose_start_estimate and map_with_known_poses to `true` when
+         the map-base_footprint transform is provided by a different node, such
+         as fake_localization -->
+  <arg name="disable_localization" default="false"/>
+
+  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
+
+    <!-- Frame names -->
+    <param name="map_frame" value="map" />
+    <param name="base_frame" value="$(arg base_frame)" />
+    <param name="odom_frame" value="$(arg odom_frame)" />
+
+    <!-- Tf use -->
+    <param name="use_tf_scan_transformation" value="true"/>
+    <param name="use_tf_pose_start_estimate" value="$(arg disable_localization)"/>
+    <param name="map_with_known_poses" value="$(arg disable_localization)" />
+    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
+    <param name="pub_map_scanmatch_transform" value="true" />
+
+    <!-- Map size / start point -->
+    <param name="map_resolution" value="0.050"/>
+    <param name="map_size" value="$(arg map_size)"/>
+    <param name="map_start_x" value="0.5"/>
+    <param name="map_start_y" value="0.5" />
+    <param name="map_multi_res_levels" value="3" />
+    <param name="map_pub_period" value ="2.0" />
+
+    <!-- Map update parameters -->
+    <param name="update_factor_free" value="0.4"/>
+    <param name="update_factor_occupied" value="0.9" />
+    <param name="map_update_distance_thresh" value="0.4"/>
+    <param name="map_update_angle_thresh" value="0.06" />
+    <param name="laser_min_dist" value="0.1" />
+    <param name="laser_max_dist" value="10.0" />
+    <param name="laser_z_min_value" value="-1.0" />
+    <param name="laser_z_max_value" value="1.0" />
+
+    <!-- Advertising config -->
+    <param name="advertise_map_service" value="true"/>
+
+    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
+    <param name="scan_topic" value="$(arg scan_topic)"/>
+    <param name="pose_update_topic" value="poseupdate" />
+    <param name="sys_msg_topic" value="syscommand" />
+    <param name="pub_odometry" value="false" />
+
+    <!-- Debug parameters -->
+    <!--
+      <param name="output_timing" value="false"/>
+      <param name="pub_drawings" value="true"/>
+      <param name="pub_debug_output" value="true"/>
+    -->
+    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
+    <remap from="map" to="/map" />
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/includes/amcl.launch

@@ -0,0 +1,105 @@
+<?xml version="1.0" ?>
+
+<launch>
+  <arg name="tf_prefix" default="" />
+  <arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
+
+  <arg name="use_map_topic"   default="true"/>
+  <arg name="scan_topic"      default="scan"/>
+  <arg name="map_topic"       default="/map"/>         <!-- if use_map_topic = true  -->
+  <arg name="map_service"     default="/static_map"/>  <!-- if use_map_topic = false -->
+  <arg name="initial_pose_x"  default="0.0"/>
+  <arg name="initial_pose_y"  default="0.0"/>
+  <arg name="initial_pose_a"  default="0.0"/>
+  <arg name="odom_frame_id"   default="$(arg tf_prefix)/odom"/>
+  <arg name="base_frame_id"   default="$(arg tf_prefix)/base_footprint"/>
+  <arg name="global_frame_id" default="/map"/>
+
+  <group if="$(eval namespace != '')" ns="$(arg namespace)">
+    <node pkg="amcl" type="amcl" name="amcl" output="screen" >
+      <param name="use_map_topic"             value="$(arg use_map_topic)"/>
+      <param name="odom_model_type"           value="diff-corrected"/>
+      <param name="gui_publish_rate"          value="10.0"/>
+      <param name="save_pose_rate"            value="0.5"/>
+      <param name="laser_max_beams"           value="200"/>
+      <param name="laser_min_range"           value="-1.0"/>
+      <param name="laser_max_range"           value="-1.0"/>
+      <param name="min_particles"             value="500"/>
+      <param name="max_particles"             value="5000"/>
+      <param name="kld_err"                   value="0.09"/>
+      <param name="kld_z"                     value="0.99"/>
+      <param name="odom_alpha1"               value="0.02"/>
+      <param name="odom_alpha2"               value="0.01"/>
+      <param name="odom_alpha3"               value="0.01"/>
+      <param name="odom_alpha4"               value="0.02"/>
+      <param name="laser_z_hit"               value="0.5"/>
+      <param name="laser_z_short"             value="0.05"/>
+      <param name="laser_z_max"               value="0.05"/>
+      <param name="laser_z_rand"              value="0.5"/>
+      <param name="laser_sigma_hit"           value="0.2"/>
+      <param name="laser_lambda_short"        value="0.1"/>
+      <param name="laser_model_type"          value="likelihood_field"/>
+      <param name="laser_likelihood_max_dist" value="2.0"/>
+      <param name="update_min_d"              value="0.2"/>
+      <param name="update_min_a"              value="0.2"/>
+      <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>
+      <param name="base_frame_id"             value="$(arg base_frame_id)"/>
+      <param name="global_frame_id"           value="$(arg global_frame_id)"/>
+      <param name="resample_interval"         value="1"/>
+      <param name="transform_tolerance"       value="0.2"/>
+      <param name="recovery_alpha_slow"       value="0.0"/>
+      <param name="recovery_alpha_fast"       value="0.0"/>
+      <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+      <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+      <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+      <remap from="scan"                      to="$(arg scan_topic)"/>
+      <remap from="map"                       to="$(arg map_topic)"/>
+      <remap from="static_map"                to="$(arg map_service)"/>
+    </node>
+  </group>
+
+  <!-- Duplicate of the above in case namespace is empty. This is necessary to
+       avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
+  <group unless="$(eval namespace != '')">
+    <node pkg="amcl" type="amcl" name="amcl" output="screen" >
+      <param name="use_map_topic"             value="$(arg use_map_topic)"/>
+      <param name="odom_model_type"           value="diff-corrected"/>
+      <param name="gui_publish_rate"          value="10.0"/>
+      <param name="save_pose_rate"            value="0.5"/>
+      <param name="laser_max_beams"           value="200"/>
+      <param name="laser_min_range"           value="-1.0"/>
+      <param name="laser_max_range"           value="-1.0"/>
+      <param name="min_particles"             value="500"/>
+      <param name="max_particles"             value="5000"/>
+      <param name="kld_err"                   value="0.09"/>
+      <param name="kld_z"                     value="0.99"/>
+      <param name="odom_alpha1"               value="0.02"/>
+      <param name="odom_alpha2"               value="0.01"/>
+      <param name="odom_alpha3"               value="0.01"/>
+      <param name="odom_alpha4"               value="0.02"/>
+      <param name="laser_z_hit"               value="0.5"/>
+      <param name="laser_z_short"             value="0.05"/>
+      <param name="laser_z_max"               value="0.05"/>
+      <param name="laser_z_rand"              value="0.5"/>
+      <param name="laser_sigma_hit"           value="0.2"/>
+      <param name="laser_lambda_short"        value="0.1"/>
+      <param name="laser_model_type"          value="likelihood_field"/>
+      <param name="laser_likelihood_max_dist" value="2.0"/>
+      <param name="update_min_d"              value="0.2"/>
+      <param name="update_min_a"              value="0.2"/>
+      <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>
+      <param name="base_frame_id"             value="$(arg base_frame_id)"/>
+      <param name="global_frame_id"           value="$(arg global_frame_id)"/>
+      <param name="resample_interval"         value="1"/>
+      <param name="transform_tolerance"       value="0.2"/>
+      <param name="recovery_alpha_slow"       value="0.0"/>
+      <param name="recovery_alpha_fast"       value="0.0"/>
+      <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+      <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+      <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+      <remap from="scan"                      to="$(arg scan_topic)"/>
+      <remap from="map"                       to="$(arg map_topic)"/>
+      <remap from="static_map"                to="$(arg map_service)"/>
+    </node>
+  </group>
+</launch>

Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="tf_prefix" default="" />
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
+    <rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/includes/rviz.launch

@@ -0,0 +1,3 @@
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="true" />
+</launch>

Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" args="-sdf
+      -file $(find amr_startup)/sdf/maze/model.sdf -model walls" output="screen" />
+</launch>

Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="robot_x"   default="17.5" />
+    <arg name="robot_y"   default="34.5" />
+    <arg name="robot_yaw" default="1.57079" />
+    <arg name="model" default="trolley" />
+    <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" 
+          args="-sdf -file $(find factory_ss_demo)models/sehc_trolley/model.sdf -model $(arg model) 
+          -x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw)" output="screen" />
+
+</launch>

Controllers/Packages/amr_startup/launch/robot_empty_world.launch

@@ -0,0 +1,72 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable, but can also be an absolute path -->
+
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+
+  <arg name="robot_type" default="hook_150" doc="Can be 'hook_150' or 'imr' for now." />
+
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
+  
+  <group if="$(eval namespace != '')">
+    <group>
+      <remap from="$(arg namespace)/joint_states"                   to="$(arg namespace)/robot/joint_states" />
+      <remap from="$(arg namespace)/mobile_base_controller/cmd_vel" to="$(arg namespace)/cmd_vel" />
+      <remap from="$(arg namespace)/mobile_base_controller/odom"    to="$(arg namespace)/odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" default="false" />
+        <arg name="use_sim_time" default="true" />
+        <arg name="gui" default="true" />
+        <arg name="debug" default="false" />
+        <arg name="verbose" default="true" /> 
+      </include>
+    </group>
+
+    <group ns="$(arg namespace)">
+      <!-- spawn robot and bring up controllers etc. -->
+      <include file="$(find amr_startup)/launch/robot_gazebo_common.launch">
+        <arg name="robot_x"   value="$(arg robot_x)" />
+        <arg name="robot_y"   value="$(arg robot_y)" />
+        <arg name="robot_yaw" value="$(arg robot_yaw)" />
+        <arg name="robot_type"  value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+      </include>
+    </group>
+  </group>
+
+  <!-- Duplicate of the above in case namespace is empty. This is necessary to
+       avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
+  <group unless="$(eval namespace != '')">
+    <group>
+      <remap from="joint_states"                   to="robot/joint_states" />
+      <remap from="mobile_base_controller/cmd_vel" to="cmd_vel" />
+      <remap from="mobile_base_controller/odom"    to="odom" />
+
+      <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)"/>
+        <arg name="paused" default="false" />
+        <arg name="use_sim_time" default="true" />
+        <arg name="gui" default="true" />
+        <arg name="debug" default="false" />
+        <arg name="verbose" default="true" /> 
+      </include>
+    </group>
+
+
+    <include file="$(find amr_startup)/launch/robot_gazebo_common.launch">
+      <arg name="robot_x"   value="$(arg robot_x)" />
+      <arg name="robot_y"   value="$(arg robot_y)" />
+      <arg name="robot_yaw" value="$(arg robot_yaw)" />
+        <arg name="robot_type"  value="$(arg robot_type)" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+  </group>
+
+</launch>

Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch

@@ -0,0 +1,59 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_x"   default="0.0" />
+  <arg name="robot_y"   default="0.0" />
+  <arg name="robot_yaw" default="0.0" />
+  <arg name="robot_type" default="hool_150" doc="Can be 'hool_150' or 'imr' for now." />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+  <arg name="prefix" value="$(arg tf_prefix)/" if="$(eval tf_prefix != '')" /> <!-- $(arg prefix) is used in all the config files! TODO: For multiple robots, create groups when loading the parameters to overwrite the arg? -->
+  <arg name="prefix" value=""                  unless="$(eval tf_prefix != '')" />
+  <arg name="model_name" default="robot" doc="Name of the Gazebo robot model (needs to be different for each robot)" />
+
+
+  <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
+
+  <!-- Load URDF -->
+  <include file="$(find robot_description)/launch/upload_urdf.launch">
+      <arg name="robot_type" value="$(arg robot_type)" />
+      <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
+
+  <!-- Spawn the robot into Gazebo -->
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg model_name) 
+      -x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw)" output="screen" />
+
+  <!-- Load ros_control controller configurations -->
+  <rosparam file="$(find robot_description)/config/joint_state_controller.yaml" command="load" />
+  <rosparam file="$(find robot_description)/config/diffdrive_controller.yaml" command="load" subst_value="true" />
+  <rosparam file="$(find robot_description)/config/pid_gains.yaml" command="load" />
+
+  <!-- Start the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+      args="joint_state_controller mobile_base_controller"/>
+
+  <!-- Add passive + mimic joints to joint_states topic -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <rosparam param="source_list">[robot/joint_states]</rosparam>
+    <param name="rate" value="200.0" />
+  </node>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
+
+  <node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering">
+    <param name="default_topic" value="cmd_vel"/>
+    <param name="default_vx_max" value="1.0" />
+    <param name="default_vx_min" value="-1.0" />
+    <param name="default_vw_max" value="1.5" />
+    <param name="default_vw_min" value="-1.5" />
+  </node>
+
+  <!-- EKF -->
+  <include file="$(find amr_startup)/launch/includes/ekf.launch.xml">
+      <arg name="tf_prefix" value="$(arg prefix)" />
+  </include>
+
+  <!-- create combined scan topic (like on robot real) -->
+  <node pkg="topic_tools" type="relay" name="b_scan_relay" args="b_scan scan"/>
+  <node pkg="topic_tools" type="relay" name="f_scan_relay" args="f_scan scan"/>
+
+</launch>

Controllers/Packages/amr_startup/launch/robot_maze_world.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="true" />
+  <arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <include file="$(find amr_startup)/launch/robot_empty_world.launch">
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="robot_type" value="$(arg robot_type)" />
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
+
+  <include file="$(find amr_startup)/launch/includes/spawn_maze.launch.xml" />
+  <!-- <include file="$(find amr_startup)/launch/includes/rviz.launch" /> -->
+</launch>

Controllers/Packages/amr_startup/launch/sehc_world.launch

@@ -0,0 +1,18 @@
+<launch>
+    <arg name="gui" default="true" />
+    <arg name="robot_type" default="imr" doc="Can be 'hook_150' or 'imr' for now." />
+    <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+    <include file="$(find amr_startup)/launch/robot_empty_world.launch">   
+        <arg name='world_name' value="$(find factory_ss_demo)/worlds/factory_ss_demo.world"/>
+        <arg name="robot_x" value="5.5"/>
+        <arg name="robot_y" value="34.5"/>
+        <arg name="robot_yaw" value="0.0"/>
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="robot_type" value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+
+    <include file="$(find amr_startup)/launch/includes/spawn_trolley.launch"/> 
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="false" />
+</launch>

Controllers/Packages/amr_startup/launch/start_maps.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="map_file" default="$(find managerments)/maps/maze.yaml" doc="Path to a map .yaml file (required)." />
+  <arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
+  <arg name="with_virtual_walls" default="true" />
+
+  <node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+  </node>
+
+  <node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+  </node>
+</launch>

Controllers/Packages/amr_startup/launch/ware_houses_world.launch

@@ -0,0 +1,16 @@
+<launch>
+    <arg name="gui" default="true" />
+    <arg name="robot_type" default="imr" doc="Can be 'hook_150' or 'imr' for now." />
+    <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+    <include file="$(find amr_startup)/launch/robot_empty_world.launch">   
+        <arg name='world_name' value="$(find dynamic_logistics_warehouse)/worlds/warehouse.world"/>
+        <arg name="robot_x" value="12.0"/>
+        <arg name="robot_y" value="22.0"/>
+        <arg name="robot_yaw" value="3.141592654"/>
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="robot_type" value="$(arg robot_type)" />
+        <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    </include>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="true" />
+</launch>

Controllers/Packages/amr_startup/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>amr_startup</name>
+  <version>0.0.0</version>
+  <description>The amr_startup package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/amr_startup</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Controllers/Packages/amr_startup/rosconsole.config

@@ -0,0 +1,2 @@
+log4j.logger.ros.tf=ERROR
+log4j.logger.ros.tf2=ERROR

Controllers/Packages/amr_startup/rviz/navigation.rviz

@@ -0,0 +1,646 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1/Offset1
+        - /TF1/Frames1
+        - /Global Map1/Straight Path1
+        - /Local Map1
+        - /Local Map1/Trajectory1
+        - /Local Map1/Trajectory1/transform plan1
+        - /Pose1
+      Splitter Ratio: 0.37295082211494446
+    Tree Height: 422
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: Back LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 5
+        Y: 5
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: map
+      Value: true
+    - Alpha: 0.4000000059604645
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: true
+      Enabled: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: false
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        back_nanoscan3_base_link:
+          Value: false
+        back_nanoscan3_sensor_link:
+          Value: false
+        base_footprint:
+          Value: true
+        base_link:
+          Value: false
+        bl_caster_rotation_link:
+          Value: false
+        bl_caster_wheel_link:
+          Value: false
+        br_caster_rotation_link:
+          Value: false
+        br_caster_wheel_link:
+          Value: false
+        camera_link:
+          Value: false
+        fl_caster_rotation_link:
+          Value: false
+        fl_caster_wheel_link:
+          Value: false
+        fr_caster_rotation_link:
+          Value: false
+        fr_caster_wheel_link:
+          Value: false
+        front_nanoscan3_base_link:
+          Value: false
+        front_nanoscan3_sensor_link:
+          Value: false
+        imu_frame:
+          Value: false
+        imu_link:
+          Value: false
+        left_wheel_link:
+          Value: false
+        lifting_link:
+          Value: false
+        map:
+          Value: true
+        odom:
+          Value: true
+        right_wheel_link:
+          Value: false
+        surface:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                back_nanoscan3_base_link:
+                  back_nanoscan3_sensor_link:
+                    {}
+                bl_caster_rotation_link:
+                  bl_caster_wheel_link:
+                    {}
+                br_caster_rotation_link:
+                  br_caster_wheel_link:
+                    {}
+                camera_link:
+                  {}
+                fl_caster_rotation_link:
+                  fl_caster_wheel_link:
+                    {}
+                fr_caster_rotation_link:
+                  fr_caster_wheel_link:
+                    {}
+                front_nanoscan3_base_link:
+                  front_nanoscan3_sensor_link:
+                    {}
+                imu_link:
+                  imu_frame:
+                    {}
+                left_wheel_link:
+                  {}
+                lifting_link:
+                  surface:
+                    {}
+                right_wheel_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 204; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: Front LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 6
+      Size (m): 0.09000000357627869
+      Style: Points
+      Topic: f_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: Back LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /b_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.20000000298023224
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz/PoseArray
+      Color: 0; 192; 0
+      Enabled: false
+      Head Length: 0.07000000029802322
+      Head Radius: 0.029999999329447746
+      Name: Amcl Particle Swarm
+      Queue Size: 10
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.009999999776482582
+      Shape: Arrow (Flat)
+      Topic: particlecloud
+      Unreliable: false
+      Value: false
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.5
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: true
+          Name: Costmap
+          Topic: /amr_node/global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: false
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.20000000298023224
+          Line Style: Lines
+          Line Width: 0.05000000074505806
+          Name: Full Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0.8999999761581421
+          Pose Color: 255; 85; 255
+          Pose Style: Axes
+          Queue Size: 10
+          Radius: 0.019999999552965164
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /amr_node/SBPLLatticePlanner/plan
+          Unreliable: false
+          Value: false
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 255; 0; 0
+          Enabled: true
+          Name: Footprint
+          Queue Size: 10
+          Topic: move_base_node/global_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.00800000037997961
+          Line Style: Billboards
+          Line Width: 0.009999999776482582
+          Name: CustomPath
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0.8999999761581421
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Queue Size: 10
+          Radius: 0.004999999888241291
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /amr_node/CustomPlanner/plan
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 0; 0; 0
+          Enabled: false
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Straight Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: Axes
+          Queue Size: 10
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan
+          Unreliable: false
+          Value: false
+      Enabled: true
+      Name: Global Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 252; 175; 62
+          Enabled: false
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.03999999910593033
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Global Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: Axes
+          Queue Size: 10
+          Radius: 0.009999999776482582
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /amr_node/LocalPlannerAdapter/global_plan
+          Unreliable: false
+          Value: false
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Costmap
+          Topic: /amr_node/local_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Class: rviz/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Footprint
+          Queue Size: 10
+          Topic: /amr_node/local_costmap/footprint
+          Unreliable: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 138; 226; 52
+          Enabled: false
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.029999999329447746
+          Line Style: Billboards
+          Line Width: 0.029999999329447746
+          Name: Local Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: Axes
+          Queue Size: 10
+          Radius: 0.009999999776482582
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan
+          Unreliable: false
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: PointCloud2
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic: /amr_node/PredictiveTrajectory/cost_cloud
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Class: rviz/Group
+          Displays:
+            - Alpha: 1
+              Axes Length: 0.10000000149011612
+              Axes Radius: 0.029999999329447746
+              Class: rviz/Pose
+              Color: 0; 0; 0
+              Enabled: true
+              Head Length: 0.05000000074505806
+              Head Radius: 0.05000000074505806
+              Name: SubGoalPose
+              Queue Size: 10
+              Shaft Length: 0.029999999329447746
+              Shaft Radius: 0.029999999329447746
+              Shape: Arrow
+              Topic: /amr_node/sub_goal
+              Unreliable: false
+              Value: true
+            - Alpha: 1
+              Axes Length: 0.30000001192092896
+              Axes Radius: 0.10000000149011612
+              Class: rviz/Pose
+              Color: 252; 233; 79
+              Enabled: false
+              Head Length: 0.05000000074505806
+              Head Radius: 0.05000000074505806
+              Name: ClosetPose
+              Queue Size: 10
+              Shaft Length: 0.029999999329447746
+              Shaft Radius: 0.029999999329447746
+              Shape: Arrow
+              Topic: /amr_node/closet_robot_goal
+              Unreliable: false
+              Value: false
+            - Alpha: 1
+              Axes Length: 0.05000000074505806
+              Axes Radius: 0.019999999552965164
+              Class: rviz/Pose
+              Color: 173; 127; 168
+              Enabled: false
+              Head Length: 0.05000000074505806
+              Head Radius: 0.05000000074505806
+              Name: Look A Head Pose
+              Queue Size: 10
+              Shaft Length: 0.05000000074505806
+              Shaft Radius: 0.029999999329447746
+              Shape: Axes
+              Topic: /amr_node/lookahead_point
+              Unreliable: false
+              Value: false
+            - Alpha: 1
+              Arrow Length: 0.30000001192092896
+              Axes Length: 0.30000001192092896
+              Axes Radius: 0.029999999329447746
+              Class: rviz/PoseArray
+              Color: 0; 0; 0
+              Enabled: true
+              Head Length: 0.07000000029802322
+              Head Radius: 0.05000000074505806
+              Name: reached intermediated goals
+              Queue Size: 10
+              Shaft Length: 0.23000000417232513
+              Shaft Radius: 0.029999999329447746
+              Shape: Axes
+              Topic: /amr_node/reached_intermediate_goals
+              Unreliable: false
+              Value: true
+            - Alpha: 1
+              Buffer Length: 1
+              Class: rviz/Path
+              Color: 245; 121; 0
+              Enabled: true
+              Head Diameter: 0.30000001192092896
+              Head Length: 0.019999999552965164
+              Length: 0.029999999329447746
+              Line Style: Billboards
+              Line Width: 0.009999999776482582
+              Name: transform plan
+              Offset:
+                X: 0
+                Y: 0
+                Z: 0.5
+              Pose Color: 138; 226; 52
+              Pose Style: Axes
+              Queue Size: 10
+              Radius: 0.019999999552965164
+              Shaft Diameter: 0.009999999776482582
+              Shaft Length: 0.009999999776482582
+              Topic: /amr_node/transformed_plan
+              Unreliable: false
+              Value: true
+            - Class: rviz/Marker
+              Enabled: false
+              Marker Topic: /amr_node/PredictiveTrajectory/cost_left_goals
+              Name: L
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+            - Class: rviz/Marker
+              Enabled: false
+              Marker Topic: /amr_node/PredictiveTrajectory/cost_right_goals
+              Name: R
+              Namespaces:
+                {}
+              Queue Size: 100
+              Value: false
+          Enabled: true
+          Name: Trajectory
+      Enabled: true
+      Name: Local Map
+    - Alpha: 1
+      Axes Length: 0.05000000074505806
+      Axes Radius: 0.014999999664723873
+      Class: rviz/Pose
+      Color: 46; 52; 54
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic: /amr_node/current_goal
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/MoveCamera
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: move_base_simple/goal
+    - Class: rviz/Measure
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: -3.135005474090576
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: -359.3964538574219
+      Target Frame: base_link
+      X: -0.49439820647239685
+      Y: 0.196189746260643
+    Saved:
+      - Angle: -34.55989074707031
+        Class: rviz/TopDownOrtho
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: TopDownOrtho
+        Near Clip Distance: 0.009999999776482582
+        Scale: -132.97349548339844
+        Target Frame: base_link
+        X: 34.338645935058594
+        Y: 35.28913879394531
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 573
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1048
+  X: 0
+  Y: 21

Controllers/Packages/amr_startup/sdf/maze/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>maze</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name>Martin Günther</name>
+        <email>martin.guenther@dfki.de</email>
+    </author>
+    <description></description>
+</model>

Controllers/Packages/amr_startup/sdf/maze/model.sdf

@@ -0,0 +1,345 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='maze'>
+    <pose frame=''>-0.078283 0.098984 0 0 -0 0</pose>
+    <link name='Wall_0'>
+      <collision name='Wall_0_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_0_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 9.925 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_1'>
+      <collision name='Wall_1_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_1_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>9.95554 0 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_2'>
+      <collision name='Wall_2_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_2_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.030536 -9.925 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_24'>
+      <collision name='Wall_24_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_24_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>5.35089 3.21906 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_25'>
+      <collision name='Wall_25_Collision'>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_25_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.25 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>4.67589 5.76906 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_27'>
+      <collision name='Wall_27_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_27_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>7.10914 4.73454 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_28'>
+      <collision name='Wall_28_Collision'>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_28_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>3 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>8.53414 2.05954 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_3'>
+      <collision name='Wall_3_Collision'>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_3_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>20 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-9.89446 0 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_30'>
+      <collision name='Wall_30_Collision'>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_30_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-4.35914 -2.82889 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_31'>
+      <collision name='Wall_31_Collision'>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_31_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>5.75 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-7.15914 -5.50389 0 0 -0 3.14159</pose>
+    </link>
+    <link name='Wall_5'>
+      <collision name='Wall_5_Collision'>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_5_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>16 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-1.89911 1.86906 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_6'>
+      <collision name='Wall_6_Collision'>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_6_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>1.5 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 2.54406 0 0 -0 1.5708</pose>
+    </link>
+    <link name='Wall_8'>
+      <collision name='Wall_8_Collision'>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_8_Visual'>
+        <pose frame=''>0 0 1.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.15 2.5</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>6.02589 3.21906 0 0 -0 0</pose>
+    </link>
+    <static>1</static>
+  </model>
+</sdf>

Controllers/Packages/nova5_control/CMakeLists.txt

@@ -0,0 +1,145 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(nova5_control)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  modbus_tcp
+)
+
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES nova5_control
+ CATKIN_DEPENDS roscpp std_msgs modbus_tcp
+ DEPENDS Boost 
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations ${GSTREAMER_LIBRARIES}
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} src/nova5_modbus.cpp src/imr_nova_control.cpp)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES})
+
+
+
+add_executable(test_nova example/simple.cpp)
+add_dependencies(test_nova ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_nova ${catkin_LIBRARIES} nova5_control)
+
+add_executable(test_thread_nova example/test_thread.cpp)
+add_dependencies(test_thread_nova ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_thread_nova ${catkin_LIBRARIES} nova5_control)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/nova5_control.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/nova5_control_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_nova5_control.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Controllers/Packages/nova5_control/README.md

@@ -0,0 +1,42 @@
+# Reference
+https://docs.google.com/spreadsheets/d/1LkLH5IGXDXprNB6_w0-2-jNupRHEH-4PBC6LIrh3G1I/edit?gid=0#gid=0
+https://www.dobot-robots.com/service/download-center?keyword=&products%5B%5D=535
+https://tranduc-my.sharepoint.com/:w:/g/personal/tranduong1692_tranduc_onmicrosoft_com/EVSb3HeS-HZPlmAMflUsuLMBLrq5_Dnm5ku2Kj9Bu8C8Rw?e=FEROOU
+# Require
+Clone:  http://git.pnkx/HiepLM/modbus.git
+        http://git.pnkx/HiepLM/libserial.git
+# Guide
+    // Nên chạy trong một thread//
+    // Khởi tạo cổng kết nối robot thông qua modbus tcp //
+    cnt_nova5 *nova5 = nullptr;
+    nova5 = new cnt_nova5(IP,PORT);
+    //==================================================//
+
+    // Quy trình kết nối robot //
+    if((*nova5).nova_connect())
+    {
+        // Kiểm tra xem robot có đang trong trạng thái EMC không
+        if((*nova5).nova_robotMode() == 3) (*nova5).nova_powerOn();
+
+        // Sau khi kết nối thành công cần enable robot
+        if((*nova5).nova_robotMode() == 4) (*nova5).nova_enable();
+
+        // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
+        if((*nova5).nova_robotMode() == 5) (*nova5).nova_startProgram();
+    } 
+    else (*nova5).nova_close();
+    //==========================//
+
+    // Sử dụng hàm nova_movePoint() cần đưa vào vòng lặp chờ đến khi robot di chuyển xong và trả về phản hồi, sau đó mới được đưa ra lệnh điều khiển tiếp theo
+
+    // Ví dụ hàm nova_movePoint() //
+        if(count == 0)
+        {
+                // Set tốc độ global tay máy
+                nova5->nova_speedGlobal(50.0);
+
+                if(nova5->nova_movePoint(ORIGIN_COOR)==1 && count == 0) count ++;
+        }
+    //============================//
+
+

Controllers/Packages/nova5_control/example/simple.cpp

@@ -0,0 +1,500 @@
+#include"nova5_control/nova5_modbus.h"
+#include <termios.h>
+#include <stdio.h>
+
+
+
+char getKey()
+{
+    struct termios oldt, newt;
+    char ch;
+    tcgetattr(STDIN_FILENO, &oldt);
+    newt = oldt;
+    newt.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+    ch = getchar();
+    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+    return ch;
+}
+
+
+int main(int argc,char **argv)
+{
+    ros::init(argc,argv,"EXAMPLE");
+    ros::NodeHandle nh("~");
+
+    std::string IP = "192.168.2.6";
+    int PORT = 502;
+
+
+    // Coordinates
+    std::vector<double> HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0};
+    std::vector<double> CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00};
+    std::vector<double> ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00};
+    std::vector<double> OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0};
+    std::vector<double> NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0};
+
+    std::vector<double> TEST_COOR = {250.71,-480.9,424.27,-179.4,-0.04,-90.00};
+
+    std::vector<double> STORE_COOR(6);
+    std::vector<double> Point(6);
+    std::vector<double> Point_current(6);
+    std::vector<double> Center_temp(6);
+    std::vector<double> Point_temp1(6);
+    std::vector<double> Point_temp2(6);
+    std::vector<double> Point_temp3(6);
+
+    uint16_t isCheck = 0;
+    int timeStamp_cam = 0;
+    static int count = 0, count_1 = 0;
+    static int center_count = 0;
+    static int get_point_current =0;
+    static int ob = 0;
+    static uint16_t speed = 0;
+    static int count_ok = 0;
+    static int count_ng = 0;
+    bool connect = false;
+    bool drag = false;
+    bool flag_home = false;
+    bool poweroff = false;
+    
+    // Khởi tạo 
+    std::shared_ptr<cnt_nova5> nova5_ptr_ = nullptr;
+    nova5_ptr_ = std::make_shared<cnt_nova5>(IP,PORT);
+
+    // Kết nối đến robot nova
+    connect = nova5_ptr_->nova_connect();
+    if(connect)
+    {
+        if(nova5_ptr_->nova_robotMode() == 3)
+        {
+            if(nova5_ptr_->nova_powerOn() == -1)
+            {
+                ROS_ERROR("Please check robot arm");
+                return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
+            }
+        } 
+        // Sau khi kết nối thành công cần enable robot
+        nova5_ptr_->nova_enable();
+
+        // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
+        nova5_ptr_->nova_startProgram();
+    }
+    else 
+    {
+        // alarm = nova5_ptr_->nova_statusAlarm();
+        ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
+        for(int i=0; i<180U; ++i)
+        {
+            connect = nova5_ptr_->nova_connect();
+            if(connect || !ros::ok())
+                {
+                    while(ros::ok())
+                    {
+                        double mode = nova5_ptr_->nova_robotMode();
+                        if(nova5_ptr_->nova_statusAlarm() == 1 && mode == 9) ROS_ERROR("Kinematic ERROR! Please check robot!");
+                        if(mode == 3)
+                        {
+                            if(nova5_ptr_->nova_powerOn() == -1)
+                            {
+                                ROS_ERROR("Please check robot arm");
+                                return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
+                            }
+                        } 
+                        if(mode == 4) break;
+                        ros::Duration(5).sleep();
+                    }
+                    
+                    // Sau khi kết nối thành công cần enable robot
+                    nova5_ptr_->nova_enable();
+
+                    // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
+                    nova5_ptr_->nova_startProgram();
+
+                    break;
+                }
+            ros::Duration(5).sleep();
+        }
+        
+    }
+
+    ros::Rate loop_rate(100);
+
+
+    //===========================Vòng lặp chính========================//
+    //=================================================================//
+    //=================================================================//
+    while(ros::ok() && connect)
+    {
+        static int temp = 0;
+        static bool checkconnect = false;
+        checkconnect = nova5_ptr_->nova_checkConnected();
+        if(checkconnect)
+        {
+            double robotmode = nova5_ptr_->nova_robotMode();
+
+            //===Kiểm tra trạng thái robot===//
+            //===============================//
+            if( robotmode == 3)
+            {
+                poweroff = true;
+                // Power on robot
+                if(nova5_ptr_->nova_powerOn() == 1)
+                {
+                    // Sau khi power on cần enable robot
+                    nova5_ptr_->nova_enable();
+                    nova5_ptr_->nova_startProgram();
+                }
+            }
+            
+            else if(robotmode == 6)
+            {
+                ROS_WARN("Robot is in drag mode");
+                drag = true;
+            }
+
+            if(drag == true && robotmode == 5)
+            {
+                ROS_WARN("PRESS: n to continute");
+                ROS_WARN("PRESS: h to go home");
+                char key = getKey();
+                ROS_WARN("%d", key);
+                /*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/
+                if(key == 110 || key == 78)
+                {
+                    nova5_ptr_->nova_startProgram();
+                    drag = false;
+                    get_point_current = 0;
+                    count = 0;
+                    center_count = 0;
+                    flag_home = false;
+                }
+                /*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */
+                else if(key == 104)
+                {
+                    nova5_ptr_->nova_startProgram();
+                    drag = false;
+                    get_point_current = 0;
+                    center_count = 0;
+                    ROS_INFO("Robot is comming home...");
+                    flag_home = true;
+                }
+            }
+            //===============================//
+            //===============================//
+            
+            
+            //===Chu trình gắp phôi và lưu trữ phôi===//
+            //========================================//
+            /* Bước 0 ve home */
+            if(center_count == 0)
+            {
+                if(get_point_current == 0)
+                {
+                    //Đọc tọa độ robot hiện tại trong vòng 6s
+
+                    nova5_ptr_->nova_getCoorcurrent(Point_current);
+                    
+                    ROS_ERROR("x current: %f", Point_current[0]);
+                    ROS_ERROR("y current: %f", Point_current[1]);
+                    ROS_ERROR("z current: %f", Point_current[2]);
+
+                    if(CENTER_P[2] > (Point_current[2] + 10))
+                    {
+                        Center_temp[0] = Point_current[0];
+                        Center_temp[1] = Point_current[1];
+                        Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0);
+                        Center_temp[3] = Point_current[3];
+                        Center_temp[4] = Point_current[4];
+                        Center_temp[5] = Point_current[5];
+                    }
+                    else if(CENTER_P[2] < (Point_current[2] - 10))
+                    {
+                        Center_temp[0] = CENTER_P[0];
+                        Center_temp[1] = CENTER_P[1];
+                        Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0);
+                        Center_temp[3] = CENTER_P[3];
+                        Center_temp[4] = CENTER_P[4];
+                        Center_temp[5] = CENTER_P[5];
+                    }
+                    else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1;
+
+                    ROS_WARN("x : %f", Center_temp[0]);
+                    ROS_WARN("y : %f", Center_temp[1]);
+                    ROS_WARN("z : %f", Center_temp[2]);
+                    get_point_current = 1;
+                }
+
+                if(center_count == 0)
+                {
+                    if(nova5_ptr_->nova_movePoint(Center_temp,100,1)==1 && center_count == 0) center_count++;
+                }
+            }
+
+            /* Bước 1 */
+            else if(center_count == 1)
+            {
+                if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1)
+                {
+                    center_count = 2;
+                }
+            }
+
+            if(flag_home && center_count == 2)
+            {
+                if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2)
+                {
+                    ROS_INFO("Complete");
+                    // nova5_ptr_->nova_stopProgram();
+                    // nova5_ptr_->nova_close();
+                    return 0;
+                }
+            }
+
+            /* Bước 2 di chuyển cánh tay đến vị trí chụp ảnh */
+            if(!flag_home && center_count ==  2 && count == 0)
+            {
+                if(nova5_ptr_->nova_movePoint(ORIGIN_COOR,100,1)==1 && count == 0)
+                {
+                    count++ ;
+                }
+            }
+
+            /* Bước 3 xác định vị trí vật thể và di chuyển tới vị trí trên vật thể */
+            else if(!flag_home && center_count ==  2 && count == 1)
+            {
+                static bool continue_flag = false;
+                if(temp == 0)
+                {
+                    
+                    // Đọc tọa độ vật trong thời gian max là 10s
+                    while( ros::ok() && !nova5_ptr_->nova_statusAlarm())
+                    {
+                        if(!nova5_ptr_->nova_checkConnected() || !ros::ok() || nova5_ptr_->nova_statusAlarm() || nova5_ptr_->nova_robotMode() != 7)
+                        {
+                            continue_flag = true;
+                            break;
+                        } 
+                        ros::Duration(1).sleep();
+                        ROS_INFO("Read camera");
+
+                        if(nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam) == 1)
+                        {
+                            // Lấy thời gian của máy tính hiện tại
+                            ros::Time current_time = ros::Time::now();
+                            // Chỉ lấy phần giây và ép kiểu sang int
+                            int seconds = static_cast<int>(current_time.sec);
+
+                            // Dữ liệu cũ không được cập nhật, chờ cho đến khi nhận được dữ liệu mới nhất
+                            while ((seconds - timeStamp_cam) >= TIME_ERR && ros::ok())
+                            {
+                                ROS_WARN("Data is out of date!");
+                                ROS_WARN("Wait for data update or press ctrl + c to cancel!");
+
+                                // Lấy thời gian của máy tính hiện tại
+                                ros::Time current_time = ros::Time::now();
+                                // Chỉ lấy phần giây và ép kiểu sang int
+                                int seconds = static_cast<int>(current_time.sec);
+
+                                nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
+                                // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ
+                                if((seconds - timeStamp_cam) < TIME_ERR){
+                                    nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
+                                    break;
+                                }
+                                if(!nova5_ptr_->nova_checkConnected() || nova5_ptr_->nova_robotMode() == 6)
+                                {
+                                    ROS_WARN("Robot is in drag mode");
+                                    continue_flag = true;
+                                    break;
+                                } 
+                                ros::Duration(1).sleep();
+                            }
+                            
+                            // Set tọa độ cho điểm trung gian 1
+                            Point_temp1[0] = Point[0];
+                            Point_temp1[1] = Point[1];
+                            Point_temp1[2] = nova5_ptr_->nova_check_z(Point[0],Point[1],ORIGIN_COOR[2], 0);
+                            Point_temp1[3] = Point[3];
+                            Point_temp1[4] = Point[4];
+                            Point_temp1[5] = Point[5];
+                            temp = 1;
+                            break;
+                        }
+                    
+                    }
+
+                    if(continue_flag)
+                    {
+                        continue_flag = false;
+                        continue;
+                    } 
+
+                    // Point_temp1[0] = TEST_COOR[0];
+                    // Point_temp1[1] = TEST_COOR[1];
+                    // Point_temp1[2] = nova5_ptr_->nova_check_z(TEST_COOR[0],TEST_COOR[1],ORIGIN_COOR[2], 0);
+                    // Point_temp1[3] = TEST_COOR[3];
+                    // Point_temp1[4] = TEST_COOR[4];
+                    // Point_temp1[5] = TEST_COOR[5];
+                    temp =1;
+                }
+
+                if(count == 1 && continue_flag == false)
+                {
+                    if(nova5_ptr_->nova_movePoint(Point_temp1,80,1) == 1) count++;
+                }
+            }
+
+            /* Bước 4 di chuyển tới vị trí vật thể */
+            else if(!flag_home && center_count ==  2 && count == 2)
+            {
+                if(nova5_ptr_->nova_movePoint(Point,20,0)== 1 && count == 2)
+                {
+                    nova5_ptr_->nova_onTool();
+                    count++;
+                }
+            }
+
+            /* Bước 5 di chuyển tới vị trí trên vật thể */
+            else if(!flag_home && center_count ==  2 && count == 3)
+            {
+                if(nova5_ptr_->nova_movePoint(Point_temp1,40,0)== 1 && count == 3)
+                {
+                    count++;
+                }
+            }
+
+            /* Bước 6 di chuyển tới vị trí điểm home */
+            else if(!flag_home && center_count ==  2 &&count == 4)
+            {
+                if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && count == 4)
+                {
+                    
+                    // Nếu vật thể là NG thì set điểm lưu trữ NG_COOR
+                    if(isCheck == 0)
+                    {
+                        NG_COOR[0]+=55;
+
+                        STORE_COOR[0] = NG_COOR[0];
+                        STORE_COOR[1] = NG_COOR[1];
+                        STORE_COOR[2] = NG_COOR[2];
+                        STORE_COOR[3] = NG_COOR[3];
+                        STORE_COOR[4] = NG_COOR[4];
+                        STORE_COOR[5] = NG_COOR[5];
+
+                        Point_temp3[0] = NG_COOR[0];
+                        Point_temp3[1] = NG_COOR[1];
+                        Point_temp3[2] = nova5_ptr_->nova_check_z(NG_COOR[0],NG_COOR[1],CENTER_P[2], 0);
+                        Point_temp3[3] = NG_COOR[3];
+                        Point_temp3[4] = NG_COOR[4];
+                        Point_temp3[5] = NG_COOR[5];
+                        
+                        speed = 30;
+                        count_ng++;
+                    }
+                    // Nếu vật thể là OK thì set điểm lưu trữ OK_COOR
+                    else if(isCheck == 1)
+                    {
+                        OK_COOR[0]+=55;
+
+                        STORE_COOR[0] = OK_COOR[0];
+                        STORE_COOR[1] = OK_COOR[1];
+                        STORE_COOR[2] = OK_COOR[2];
+                        STORE_COOR[3] = OK_COOR[3];
+                        STORE_COOR[4] = OK_COOR[4];
+                        STORE_COOR[5] = OK_COOR[5];
+
+                        Point_temp3[0] = OK_COOR[0];
+                        Point_temp3[1] = OK_COOR[1];
+                        Point_temp3[2] = nova5_ptr_->nova_check_z(OK_COOR[0],OK_COOR[1],CENTER_P[2], 0);
+                        Point_temp3[3] = OK_COOR[3];
+                        Point_temp3[4] = OK_COOR[4];
+                        Point_temp3[5] = OK_COOR[5];
+
+                        speed = 20;
+                        count_ok++;
+                    }
+                    count++;
+                }
+            }
+
+            /* Bước 7 di chuyển đến điểm trung gian trên điểm lưu trữ */
+            else if(!flag_home && center_count ==  2 &&count == 5)
+            {
+                if(nova5_ptr_->nova_movePoint(Point_temp3,100,1) == 1 && count == 5)
+                {
+                    count++ ;
+                }
+            }
+
+            /* Bước 8 di chuyển đến điểm lưu trữ */
+            else if(!flag_home && center_count ==  2 &&count == 6)
+            {
+                if(nova5_ptr_->nova_movePoint(STORE_COOR,speed,0) == 1 && count == 6)
+                {
+                    nova5_ptr_->nova_offTool(0.5);
+                    count++ ;
+                }
+            }
+
+            /* Bước 9 di chuyển đến điểm trung gian trên điểm lưu trữ */
+            else if(!flag_home && center_count ==  2 &&count == 7)
+            {
+                if(nova5_ptr_->nova_movePoint(Point_temp3,speed,0)==1 && count == 7)
+                {
+                    count++;
+                }
+            }
+
+            /* Bước 10 reset các biến */
+            else if(!flag_home && center_count == 2 && count >= 8)
+            {
+                // timeStamp_cam_1 = timeStamp_cam;
+                temp = 0;
+                count = 0;
+                center_count = 1;    
+                // if(isCheck > 1) isCheck =0;
+
+                if(count_ok > 4 && count_ng > 4) break;
+                // break;
+            }
+            //========================================//
+            //========================================//
+
+            // Hiển thị bước robot đang thực hiện
+            if((count + center_count) != count_1) ROS_INFO("STEP: %d", count + center_count);
+            count_1 = count + center_count;
+        }
+        else
+        { 
+        // alarm = nova5_ptr_->nova_statusAlarm();
+        ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
+        for(int i=0; i<180U; ++i)
+        {
+            
+            if(!ros::ok()) break;
+            if(nova5_ptr_->nova_connect() )
+                {
+                    nova5_ptr_->nova_stopProgram();
+                    // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
+                    nova5_ptr_->nova_startProgram();
+                    temp = 0;
+                    count = 0;
+                    center_count = 0;    
+                    get_point_current = 0;
+                    flag_home = false;
+
+                    break;
+                }
+            ros::Duration(5).sleep();
+        }
+        }
+        loop_rate.sleep();
+        ros::spinOnce();
+    }
+    
+    // nova5_ptr_->nova_stopProgram();
+    // nova5_ptr_->nova_close();
+}
+
+

Controllers/Packages/nova5_control/example/test_thread.cpp

@@ -0,0 +1,101 @@
+#include "nova5_control/imr_nova_control.h"
+#include <iostream>
+#include <memory>
+#include <ros/ros.h>
+#include <std_msgs/UInt16.h>
+
+std::shared_ptr<imr_nova_control> controller = nullptr;
+
+unsigned int ok_count_max = 0;
+unsigned int ng_count_max = 1;
+bool enable = true;
+bool continue_flag = false;
+bool go_home_flag = false;
+bool power_on_flag = false;
+
+void callBack(const std_msgs::UInt16::ConstPtr& msg)
+{
+    if(controller != nullptr)
+    {
+        switch (msg->data)
+        {
+        case 1:
+        controller->startHomeThread();
+            // continue_flag = true;
+            // go_home_flag = false;
+            // power_on_flag = false;
+            break;
+        case 2:
+            // continue_flag = false;
+            // go_home_flag = true;
+            // power_on_flag = false;
+            break;
+        case 3:
+            // continue_flag = false;
+            // go_home_flag = false;
+            // power_on_flag = true;
+            break;        
+        default:
+            // continue_flag = false;
+            // go_home_flag = false;
+            // power_on_flag = false;
+            break;
+        }
+    }
+}
+
+int main(int argc, char** argv) {
+    // Initialize the ROS node
+    ros::init(argc, argv, "test_imr_nova_control");
+    ros::NodeHandle nh = ros::NodeHandle("~");
+    ros::Subscriber sub = nh.subscribe("mode", 1, callBack);
+    ros::Publisher pub = nh.advertise<std_msgs::UInt16>("status", 1);
+
+    try {
+        // Initialize ROS time
+        // ros::Time::init();
+
+        // Create an instance of imr_nova_control
+        controller = std::make_shared<imr_nova_control>();
+
+        
+
+        controller->ok_count_max_ = &ok_count_max;
+        controller->ng_count_max_ = &ng_count_max;
+        controller->enable_ = &enable;
+
+        controller->continue_flag_ = &continue_flag;
+        controller->go_home_flag_ = &go_home_flag;
+        controller->power_on_flag_ = &power_on_flag;
+
+        
+        #if 0
+        controller->startHomeThread();
+        #endif
+
+        #if 0
+        controller->startModeThread();
+        #endif
+
+        std::cout << "imr_nova_control instance created successfully." << std::endl;
+
+        // Run ROS spin to process callbacks (if applicable)
+        // ros::AsyncSpinner spinner(2); // Adjust the number of threads as needed
+        // spinner.start();
+        ros::Rate rate(5);
+
+        while (ros::ok())
+        {
+            std_msgs::UInt16 msg;
+            msg.data = controller->statusCode_;
+            pub.publish(msg);
+            ros::spinOnce();
+            rate.sleep();
+        }
+
+    } catch (const std::exception& e) {
+        std::cerr << "An error occurred: " << e.what() << std::endl;
+    }
+    ros::spin();
+    return 0;
+}

Controllers/Packages/nova5_control/include/nova5_control/imr_nova_control.h

@@ -0,0 +1,95 @@
+#ifndef __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__
+#define __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__
+
+// #include <ros/ros.h>
+#include <termios.h>
+#include <stdio.h>
+#include <thread>
+#include"nova5_control/nova5_modbus.h"
+
+class imr_nova_control
+{
+public:
+    imr_nova_control();
+    virtual ~imr_nova_control(); 
+
+    void startModeThread();
+    void stopModeThread(); 
+
+    void startHomeThread();
+    void stopHomeThread(); 
+
+    unsigned int *ok_count_max_;
+    unsigned int *ng_count_max_;
+
+    bool *enable_;
+    double mode_;
+
+    bool *continue_flag_;
+    bool *go_home_flag_;
+    bool *power_on_flag_;
+
+    enum statusCode
+    {
+        ROBOT_WAITING = 98,
+        ROBOT_RECONNECTING = 99,
+        ROBOT_RUNNING = 100,
+        ROBOT_DISCONNECTED = 101,
+        ROBOT_READ_DATA_ZERO = 102,
+        ROBOT_DATA_NOT_UPDATED = 103,
+        ROBOT_TARGET_COORDINATES_OUT_OF_RANGE = 104,
+        ROBOT_COMING_HOME = 105,
+        ROBOT_CHECK_EMERGENCY_STOP_BUTTON = 106,
+        ROBOT_IN_ALARM_STATE = 107,
+        ROBOT_POWER_ON_WAITTING = 108,
+        ROBOT_POWER_ON = 109,
+        ROBOT_DRAG = 110,
+        ROBOT_WAITING_SELECT_CONTINUE_OR_GO_HOME = 111,
+        ROBOT_OK_FULL = 112,
+        ROBOT_NG_FULL = 113,
+        ROBOT_ERROR = 999
+    };
+
+    /**
+     * @var statusCode_
+     * 98: Robot is waiting
+     * 99: Robot is reconnecting
+     * 100: Robot is running
+     * 101: Robot is disconnected
+     * 102: Read data is 0 (Camera can not detect object or lost connection)
+     * 103: Data is not updated (camera maybe disconnected)
+     * 104: Target coordinates are not within the working range
+     * 105: Robot is comming home
+     * 106: Check the robot's emergency stop button
+     * 107: Robot is in alarm state
+     * 108: Robot is power on
+     * 109: Robot is in drag mode
+     * 110: OK object is over the limit
+     * 111: NG object is over the limit
+     * 999: Robot is error
+     */
+    statusCode statusCode_;
+    
+private:
+    char getKey();
+    void imr_dobot_control_thread();
+    void imr_dobot_go_home_thread();
+
+    // Coordinates
+    std::vector<double> HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0};
+    std::vector<double> CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00};
+    std::vector<double> ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00};
+    std::vector<double> OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0};
+    std::vector<double> NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0};
+
+    std::string IP = "192.168.2.6";
+    int PORT = 502;
+
+    bool modeThreadRunning_; // Variable to check if the thread is running
+    bool homeThreadRunning_; // Variable to check if the thread is running
+    std::shared_ptr<std::thread> thr_main_;
+    std::shared_ptr<std::thread> thr_go_home_;
+
+};
+
+#endif // __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__

Controllers/Packages/nova5_control/include/nova5_control/nova5_modbus.h

@@ -0,0 +1,300 @@
+// nova5_modbus.h
+#ifndef NOVA5_CONTROL_H
+#define NOVA5_CONTROL_H
+// #pragma once
+
+#include<ros/ros.h>
+#include <boost/shared_ptr.hpp>
+#include<iostream>
+#include<math.h>
+#include<cstring>
+#include<array>
+#include<stdexcept>
+
+#include"modbus_tcp/modbus.h"
+
+
+#define TIME_ERR  1
+#define ERR  -1
+#define TIME_OUT  1.5
+
+// Coil Registers
+#define COIL_START                0
+#define COIL_PAUSE                1
+#define COIL_CONTINUE             2
+#define COIL_STOP                 3
+#define COIL_EMERGENCY_STOP       4
+#define COIL_CLEAR_ALARM          5
+#define COIL_POWER_ON             6
+#define COIL_ENABLE               100
+#define COIL_DISABLE              101
+// #define USER_DEFINED_COIL_START   3095
+#define COILD_MOVE                3095
+#define COILD_MOVING_DONE         3096
+#define COILD_TOOL                3097
+#define COILD_MOVE_MODE           3098 // 0 moveJ - 1 moveL
+// #define USER_DEFINED_COIL_END     9999
+
+// Discrete Input Registers
+#define DISCRETE_STOP_STATE         1
+#define DISCRETE_PAUSE_STATE        2
+#define DISCRETE_RUNNING_STATE      3
+#define DISCRETE_ALARM_STATE        4
+#define DISCRETE_RESERVED_1         5
+#define DISCRETE_REMOTE_CONTROL     7
+
+
+// Input Registers
+#define INPUT_ROBOT_MODE_START                          1012
+#define INPUT_ROBOT_MODE_END                            1015
+#define INPUT_TIMESTAMP_START                           1016
+#define INPUT_TIMESTAMP_END                             1019
+#define INPUT_TARGET_JOINT_POSITION_START               1096
+#define INPUT_TARGET_JOINT_POSITION_END                 1119
+#define INPUT_TARGET_JOINT_CURRENT_START                1168
+#define INPUT_TARGET_JOINT_CURRENT_END                  1191
+#define INPUT_ACTUAL_JOINT_POSITION_START               1216
+#define INPUT_ACTUAL_JOINT_POSITION_END                 1239
+#define INPUT_ACTUAL_JOINT_CURRENT_START                1264
+#define INPUT_ACTUAL_JOINT_CURRENT_END                  1287
+#define INPUT_ROBOT_BRAKE_STATUS_H                      1512 //HIGH BYTE
+#define INPUT_ROBOT_ENABLING_STATUS_L                   1513 //LOW BYTE
+#define INPUT_ROBOT_DRAG_STATUS_H                       1513 //HIGH BYTE
+#define INPUT_ROBOT_DRAG_STATUS_L                       1514 //HIGH BYTE
+#define INPUT_ROBOT_ALARM_STATUS_H                      1514 //HIGH BYTE
+#define INPUT_LOAD_WEIGHT_START                         1584
+#define INPUT_LOAD_WEIGHT_END                           1587
+#define INPUT_ECCENTRIC_DISTANCE_X_START                1588
+#define INPUT_ECCENTRIC_DISTANCE_X_END                  1591
+#define INPUT_ECCENTRIC_DISTANCE_Y_START                1592
+#define INPUT_ECCENTRIC_DISTANCE_Y_END                  1595
+#define INPUT_ECCENTRIC_DISTANCE_Z_START                1596
+#define INPUT_ECCENTRIC_DISTANCE_Z_END                  1599
+#define INPUT_USER_COORDINATES_START                    1600
+#define INPUT_USER_COORDINATES_END                      1623
+#define INPUT_TOOL_COORDINATES_START                    1624
+#define INPUT_TOOL_COORDINATES_END                      1647
+
+// Holding Registers
+#define HOLDING_GLOBAL_SPEED                100
+
+// #define HOLDING_USER_DEFINED_START          3095
+#define HOLDING_TARGET_POINT_START          3095
+#define HOLDING_TARGET_POINT_END            3118
+#define HOLDING_CAMERA_POINT_START          3119
+#define HOLDING_CAMERA_POINT_END            3134
+#define HOLDING_CAMERA_OK                   3135
+#define HOLDING_CAMERA_TIMESTAMP_START      3136
+#define HOLDING_CAMERA_TIMESTAMP_END        3139
+#define HOLDING_POINT_CURRENT_START         3140
+#define HOLDING_POINT_CURRENT_END           3163
+#define HOLDING_Z_MIN_USER_SET_START        3164
+#define HOLDING_Z_MIN_USER_SET_END          3167
+// #define HOLDING_USER_DEFINED_END            8999
+
+// #define HOLDING_CURRENT_SCREW_INDEX         9000
+#define HOLDING_ALARM_CODE                  9054
+
+
+
+
+class cnt_nova5
+{
+public:
+
+    cnt_nova5(std::string ip_address, int port);
+    virtual ~cnt_nova5();
+    /**
+     * @brief Connect to nova
+     */
+    int nova_connect(void);
+    /**
+     * @brief Check connection
+     */
+    bool nova_checkConnected(void);
+    
+    void nova_disable(void);
+    /**
+     * @brief Close to nova
+     */
+    void nova_close(void);
+    /**
+     * @brief Alarm notification
+     */
+    bool nova_statusAlarm(void);
+    /**
+     * @brief Clear alarm
+     */
+    void nova_clearAlarm(void);
+    /**
+     * @brief Power on robot
+     */
+    int nova_powerOn(void);
+    /**
+     * @brief enable robot
+     */
+    bool nova_enable(void);
+    /**
+     * @brief start program
+     */
+    bool nova_startProgram(void);
+    /**
+     * @brief stop program
+     */
+    void nova_stopProgram(void);
+    /**
+     * @brief   Read robot mode and return robot mode
+    3: Emergency stop mode, not powered on
+    4: Powered on mode, not enabled
+    5: Enabled mode, program not started
+    6: Drag mode
+    7: Program started mode
+    9: Kinematic error of robot nova
+     */
+    double nova_robotMode(void);
+    /**
+     * @brief     Select moveJ or moveL 
+     */
+    void nova_robotMove(void);
+    /**
+     * @brief      Set speed global for nova
+     * @param speed Speed setting for nova
+     */
+    void nova_speedGlobal(uint16_t speed);
+    /**
+     * @brief       Check nova status (nova has 7 status registers form 1-7)
+     * @param add   Address of status register
+     * @param value Register status
+     */
+    int nova_status(int add, bool &value);
+    /**
+     * @brief           Check nova multistate (nova has 7 status registers form 1-7)
+     * @param add       First status register address
+     * @param length    Number of rgisters to read
+     * @param buff      Register states
+     */
+    int nova_multistate(int add, int length, bool *buff);
+    /**
+     * @brief               Get 6 values ​​x, y, z, Rx, Ry, Rz
+     * @param Point         Condinates after conversion
+     * @param Point_initial Scan coordinates
+     * @param isCheck       True or false check variable
+     * @param Timestamp     Time stamp
+     */
+
+    int nova_readCoordinates(std::vector<double>& Point,std::vector<double>& Point_initial, uint16_t& isCheck, int& Timestamp);
+    /**
+     * @brief      Get coordinates from camera
+     * @param Point Variable store coordinates current
+     */
+    int nova_getCoorcurrent(std::vector<double>& Point);
+    /**
+     * @brief      Read time stamp nova
+     * @param time Time stamp of nova
+     */
+    int nova_timeStamp(uint64_t& time);
+    /**
+     * @brief      
+     */
+    int nova_createArea(void);
+    /**
+     * @brief      Turn on tool
+     */
+    void nova_onTool(void);
+    /**
+     * @brief      Turn off tool
+     */
+    void nova_offTool(const double delay);
+    /**
+     * @brief      Move robot nova
+     * @param Point Target coordinates
+     * @param speed cai toc do cho tay may
+     * @param mode 0 - moveL, const int mode = 1
+     *             1 - moveJ 
+     */
+    int nova_movePoint(std::vector<double>& Point,const int speed, const int mode = 1);
+    /**
+     * @brief      Check z 
+     * 
+     * @param x
+     * @param y
+     * @param z 
+     * @param choose 0 - z min theo mac dinh 200 mm
+     *               1 - z min theo tham so duoc set tu CCBOX    
+     */
+    double nova_check_z(double& x, double& y, double& z, int choose = 0);
+
+    int movePointRunning_;
+    bool *run_ptr_;
+
+
+private:
+    /**
+     * @brief     Robot move done notification
+     */
+    bool nova_movingDone(void);
+    /**
+     * @brief     Allow the robot to start moving
+     */
+    void nova_allowMove(void);
+    /**
+     * @brief     Dismiss the robot moving
+     */
+    void nova_dismissMove(void);
+    /**
+     * @brief         Send 6 values ​​x, y, z, Rx, Ry, Rz
+     * @param mode 0 - moveJ, const int mode = 0
+     *             1 - moveL
+     * @param Point   Coordinates to sent
+     */
+    int nova_sendCoordinates(std::vector<double>& Point);
+    /**
+     * @brief       Check point coordinates    
+     * return 1:    Point within range
+     * return 0:    Point is not range
+     * return -1:   Error
+     * @param x 
+     * @param y
+     * @param z
+     */
+    int workArealimit(double x, double y, double z);
+    /**
+     * @brief               Convert object coordinates to robot tool condinates
+     * @param Point         Condinates after conversion
+     * @param Point_initial Scan coordinates
+     * @param Coor          Object coordinates
+     */
+    int convert(std::vector<double>& Point,std::vector<double>& Point_initial, std::vector<double>& Coor);
+    /**
+     * @brief      Get z min and return z min
+     */
+    double getZmin(void);
+
+    /* data */
+    enum WorkAreaStatus {
+    WITHIN_LIMIT = 1,
+    OUT_OF_LIMIT = 0,
+    ERROR = -1
+    };
+
+    boost::shared_ptr<modbus> nova_;
+    bool inter_var;
+    
+    bool send;
+    double a1,b1,c1;
+    double a2,b2;
+    double r_max;
+    double r_min;
+    double z_min_static;
+    
+    // double safety_factor;
+
+    double x_deviation;
+    double y_deviation;
+    double z_deviation;
+
+    bool flag_move;
+    uint64_t mode_store;
+};
+
+#endif // NOVA5_CONTROL_H

Controllers/Packages/nova5_control/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>nova5_control</name>
+  <version>0.0.0</version>
+  <description>The nova5_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/nova5_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  
+  <build_depend>modbus_tcp</build_depend>
+  <build_export_depend>modbus_tcp</build_export_depend>
+  <exec_depend>modbus_tcp</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Controllers/Packages/nova5_control/src/nova5_modbus.cpp

@@ -0,0 +1,670 @@
+#include"nova5_control/nova5_modbus.h"
+
+
+
+cnt_nova5::cnt_nova5(std::string ip_address, int port) 
+ : nova_(nullptr),
+    movePointRunning_(0),
+    run_ptr_(nullptr)
+{
+    nova_ = boost::make_shared<modbus>(ip_address, port);
+    inter_var = false;
+
+
+    // Declare the values of the sphere O
+    a1 = 0.0;
+    b1 = 0.0;
+    c1 = 0.0;
+    r_max = 750.0;
+
+    // Declare the values of cylinder I
+    a2 = 0.0;
+    b2 = 0.0;
+    r_min = 250.0;
+
+    // Deviation of camera 
+    x_deviation = 8 + (350/2.0);
+    y_deviation = 100.0 + (234.5/2.0);
+    z_deviation = 140.5 + 5 - 28.0 + 3.5; // Chiều cao tool + chiều cao camera + dung sai
+
+    z_min_static = 170;
+    mode_store = 0;
+    flag_move = 0;
+}
+
+cnt_nova5::~cnt_nova5()
+{
+    nova_stopProgram();
+    nova_close();
+}
+
+int cnt_nova5::nova_connect(void)
+{
+    ROS_INFO("Nova is connecting.......");
+    try
+    {
+        
+        if(!nova_->modbus_connect()) 
+        {
+            ROS_ERROR("Nova connection is error");
+            return 0;
+        } 
+        else 
+        {
+            ROS_INFO("Nova is Connected");
+            return 1;
+        } 
+
+    }
+
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+        return -1;
+    }   
+    
+}
+
+bool cnt_nova5::nova_checkConnected(void)
+{
+    try
+    {
+        return nova_->_connected;
+    }
+    catch (const std::exception& e) 
+    {
+        ROS_ERROR_STREAM(e.what());
+    } 
+    catch (...) 
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+    }  
+}
+
+void cnt_nova5::nova_disable(void)
+{
+    nova_dismissMove();
+    nova_stopProgram();
+    nova_->modbus_write_coil(COIL_DISABLE, true);
+    ros::Duration(0.1).sleep();
+    nova_->modbus_write_coil(COIL_DISABLE, false);
+    ros::Duration(0.6).sleep();
+}
+
+void cnt_nova5::nova_close(void)
+{
+    ROS_INFO("Close robot");
+    nova_disable();
+    nova_->modbus_close();
+}
+
+int cnt_nova5::workArealimit(double x, double y, double z)
+{
+    double dO_instant = z + (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2));
+
+    // INNER BOUNDING CYLINDER
+    double dI_instant = sqrt(pow((x + a2),2) + pow((y + b2),2));
+
+    if(dO_instant < r_max)
+    {
+        return((dI_instant > r_min) ? WITHIN_LIMIT : OUT_OF_LIMIT);
+    }
+    else return ERROR;
+    
+}
+
+int cnt_nova5::nova_sendCoordinates(std::vector<double>& Point)
+{
+    try
+    {
+        if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters");
+        if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area");
+        if(workArealimit(Point[0],Point[1],Point[2]) == ERROR) throw std::runtime_error("Unknown error");
+
+        std::array<uint16_t,24> part;
+        std::vector<double> Point_send(6);
+
+        if(Point[2] < z_min_static)
+        {
+            Point_send[0] = Point[0];
+            Point_send[1] = Point[1];
+            Point_send[2] = z_min_static;
+            Point_send[3] = Point[3];
+            Point_send[4] = Point[4];
+            Point_send[5] = Point[5];
+        }
+        else 
+        {
+            Point_send[0] = Point[0];
+            Point_send[1] = Point[1];
+            Point_send[2] = Point[2];
+            Point_send[3] = Point[3];
+            Point_send[4] = Point[4];
+            Point_send[5] = Point[5];
+        }
+
+        for (int i = 0; i < 6U; ++i) 
+        {
+            std::memcpy(&part[i*4], reinterpret_cast<uint8_t*>(&Point_send[i]), sizeof(double));
+        }
+
+        if(nova_->modbus_write_registers(HOLDING_TARGET_POINT_START,24,part.data()) == 0) return 1;
+        else return 0;
+    }
+    catch(const std::invalid_argument& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    catch(const std::out_of_range& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+        catch(const std::runtime_error& e)
+    {
+        ROS_ERROR_STREAM("ERROR send: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    
+}
+
+int cnt_nova5::convert(std::vector<double>& Point, std::vector<double>& Point_initial, std::vector<double>& Coor)
+{
+    if(Point.size() == 6U)
+    {
+        for(int i = 0; i < Point.size(); ++i)
+        {
+            Point[i] = Point_initial[i];
+        }
+
+        Point[0] = Point[0] + x_deviation - Coor[1] - (x_deviation - Coor[1])*0.15;     // x
+        Point[1] = Point[1] - y_deviation + Coor[2];                                    //y
+        Point[2] = Point[2] + z_deviation - Coor[0];                                    // z
+        Point[3] = Point[3];                                                            // Rx
+        Point[4] = Point[4];                                                            // Ry
+        Point[5] = Point[5] + 90.0 - Coor[3];                                             // Rz
+
+        return 1;
+    }
+    else return ERR;
+
+
+}
+
+int cnt_nova5::nova_readCoordinates(std::vector<double>& Point,std::vector<double>& Point_initial, uint16_t& isCheck, int& Timestamp)
+{
+    try
+    {
+        std::array<uint16_t,21> register_buffer;
+        std::vector<double> Point_receive(4); 
+        
+        if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters");
+        if (register_buffer.size() < 21) throw std::runtime_error("Buffer size is too small");
+
+        int status = nova_->modbus_read_holding_registers(HOLDING_CAMERA_POINT_START,21,register_buffer.data());
+
+        if (status == 0 ) 
+        {
+            // ROS_INFO("Successfully read registers coordinates");
+
+            for (int i = 0; i < 4U; ++i) 
+            {
+                memcpy(&Point_receive[i], &register_buffer[i * 4U], sizeof(double));
+                // ROS_INFO("Double value %d: %f", i, Point_receive[i]);
+            }
+            isCheck = register_buffer[16];
+            Timestamp = (static_cast<int>(register_buffer[17]) << 16) | register_buffer[18];
+
+            // convert(Point,Point_initial,Point_receive);
+            if(Point_receive[1] == 0 && Point_receive[2] == 0 && isCheck == 0) throw std::runtime_error("Camera error");
+            if(convert(Point,Point_initial,Point_receive) == -1) throw std::invalid_argument("A point has 6 parameters");
+            if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area");
+            if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error");
+
+            return 1;
+
+        } 
+        else 
+        {
+            // ROS_ERROR("Failed to read registers coordinates");
+            return 0;
+        }
+    }
+    catch(const std::invalid_argument& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    catch(const std::out_of_range& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    catch(const std::runtime_error& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+}
+
+int cnt_nova5::nova_status(int add, bool &value)
+{
+    try
+    {
+        if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist");
+        int status = nova_->modbus_read_coil(add, value);
+        if(status == 0)
+        {
+            // ROS_INFO("Successfully read register status");
+            return 1;
+        }
+        else 
+        {
+            // ROS_ERROR("Failed to read register status %d", status);
+            return 0;
+        }
+    }
+    catch(const std::out_of_range& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    catch(...)
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+        return ERR;
+    }
+    
+}
+
+int cnt_nova5::nova_multistate(int add, int length, bool *buff)
+{
+    try
+    {
+        if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist");
+        if(length > (DISCRETE_REMOTE_CONTROL - add) || length <= 0) throw std::out_of_range("Invalid length value");
+
+        int status = nova_->modbus_read_coils(add, length, buff);
+        if(status == 0)
+        {
+            // ROS_INFO("Successfully read register multistate");
+            return 1;
+        }
+        else 
+        {
+            // ROS_ERROR("Failed to read register multistate %d", status);
+            return 0;
+        }
+    }
+    catch(const std::out_of_range& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    catch(...)
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+        return ERR;
+    }
+}
+
+int cnt_nova5::nova_timeStamp(uint64_t& time)
+{
+    try
+    {
+        std::array<uint16_t,4> register_buffer;
+        int status = nova_->modbus_read_holding_registers(INPUT_TIMESTAMP_START,4, register_buffer.data());
+        if(status == 0)
+        {
+            // ROS_INFO("Successfully read register time stamp");
+
+            memcpy(&time,&register_buffer,sizeof(uint64_t));
+
+            return 1;
+        }
+        else
+        {
+            // ROS_ERROR("Failed to read register time stamp %d", status);
+            return 0;
+        }
+    }
+    catch(const std::exception& e)
+    {
+        ROS_ERROR_STREAM("Caught unknown exception");
+        return ERR;
+    }
+
+}
+
+double cnt_nova5::nova_robotMode(void)
+{
+    uint64_t mode_uint64;
+    std::array<uint16_t,4> register_buffer;
+    int status = nova_->modbus_read_input_registers(INPUT_ROBOT_MODE_START,4,register_buffer.data());
+    
+    if(status == 0)
+    {
+        // ROS_INFO("Successfully read register robot mode");
+
+        memcpy(&mode_uint64, &register_buffer, sizeof(uint64_t));
+        if(mode_uint64 != mode_store) ROS_INFO("Mode: %ld",mode_uint64);
+        mode_store = mode_uint64;
+        double mode_double = (double)mode_uint64;
+        return mode_uint64;
+    }
+    else 
+    {
+        // ROS_ERROR("Failed to read register robot mode %d", status);
+        return ERR;
+    }
+
+}
+
+int cnt_nova5::nova_powerOn(void)
+{
+        static int count = 0;
+        if(run_ptr_ == nullptr) run_ptr_ = &inter_var;
+        while(ros::ok() && nova_robotMode() == 3 && *run_ptr_)
+        {
+            ROS_INFO("Wait 25s for robot to power on...");
+            if(nova_statusAlarm() == 1)
+            { 
+                nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
+                ros::Duration(0.01).sleep();
+            }
+
+            nova_->modbus_write_coil(COIL_POWER_ON, true);
+            ros::Duration(20).sleep();
+
+            if(nova_robotMode() == 3)
+            {
+                nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
+                ros::Duration(0.01).sleep();
+                nova_->modbus_write_coil(COIL_POWER_ON, false);
+                ros::Duration(0.01).sleep();
+                count++;
+                if(count>1)
+                {
+                     ROS_ERROR("Power on failed...");
+                     count = 0;
+                     return -1;
+                     break;
+                }
+            }
+            else
+            {
+                ROS_INFO("Power on successfully");
+                nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
+                ros::Duration(0.01).sleep();
+                nova_->modbus_write_coil(COIL_POWER_ON, false);
+                ros::Duration(0.01).sleep();
+                return 1;
+                break;
+            }
+            ros::spinOnce();
+        }
+        
+
+    return 0;
+}
+
+bool cnt_nova5::nova_enable(void)
+{
+        int status_clear_alarm = nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
+        ros::Duration(0.01).sleep();
+        int status_enable = nova_->modbus_write_coil(COIL_ENABLE, true);
+        ros::Duration(0.6).sleep();
+
+        nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
+        ros::Duration(0.01).sleep();
+        nova_->modbus_write_coil(COIL_ENABLE, false);
+        ros::Duration(0.01).sleep();
+
+        double mode = nova_robotMode();
+
+        if(mode >= 5 && status_clear_alarm == 0 && status_enable == 0 )
+        {
+            ROS_INFO("Enable done");
+            return true;
+        }
+        else 
+        {
+            ROS_ERROR("Enable failed");
+            return false;
+        }
+}
+
+void cnt_nova5::nova_allowMove(void)
+{
+    nova_->modbus_write_coil(COILD_MOVE,true);
+    ros::Duration(0.3).sleep();
+}
+
+void cnt_nova5::nova_dismissMove(void)
+{
+    nova_->modbus_write_coil(COILD_MOVE,false);
+    ros::Duration(0.01).sleep();
+}
+
+void cnt_nova5::nova_stopProgram(void)
+{
+    nova_->modbus_write_coil(COIL_STOP, true);
+    ros::Duration(0.01).sleep();
+    nova_->modbus_write_coil(COIL_STOP, false);
+    ros::Duration(0.5).sleep();
+}
+
+bool cnt_nova5::nova_startProgram(void)
+{
+    flag_move = 0;
+    nova_dismissMove();
+    nova_offTool(0.01);
+    if(run_ptr_ == nullptr) run_ptr_ = &inter_var;
+    int i = 0;
+    while(i < 4 && ros::ok() && *run_ptr_)
+    {
+        nova_->modbus_write_coil(COIL_START, true);
+        ros::Duration(0.3).sleep();
+        nova_->modbus_write_coil(COIL_START, false);
+        ros::Duration(0.02).sleep();
+        uint64_t mode = nova_robotMode();
+        if(mode == 7) break;
+        
+        i++;
+    }
+    uint64_t mode = nova_robotMode();
+    if(mode == 7) return true;
+    else return false;
+}
+
+bool cnt_nova5::nova_movingDone(void)
+{
+
+    bool status;
+    nova_->modbus_read_coil(COILD_MOVING_DONE, status);
+    // ROS_INFO("STATUS movingDone %d",status);
+    return status;
+
+}
+
+bool cnt_nova5::nova_statusAlarm(void)
+{
+    bool alarm = false;
+    nova_->modbus_read_input_bit(DISCRETE_ALARM_STATE,alarm);
+    // ROS_INFO("ALARM ST: %x",alarm);
+
+    return alarm;
+}
+
+void cnt_nova5::nova_clearAlarm()
+{
+    nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
+    ros::Duration(0.1).sleep();
+    nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
+    ros::Duration(0.01).sleep();
+}
+
+void cnt_nova5::nova_onTool(void)
+{
+    ROS_INFO("ON TOOL");
+    nova_->modbus_write_coil(COILD_TOOL, true);
+    ros::Duration(2).sleep();
+}
+
+void cnt_nova5::nova_offTool(const double delay)
+{
+    ROS_INFO("OFF TOOL");
+    nova_->modbus_write_coil(COILD_TOOL, false);
+    ros::Duration(delay).sleep();
+}
+
+void cnt_nova5::nova_speedGlobal(uint16_t speed)
+{
+    nova_->modbus_write_register(HOLDING_GLOBAL_SPEED,speed);
+    // ros::Duration(0.01).sleep();
+}
+
+int cnt_nova5::nova_getCoorcurrent(std::vector<double>& Point)
+{
+    try
+    {
+        if(Point.size() != 6U) throw std::invalid_argument("Coordinates have 6 parameters");
+        std::array<uint16_t,24> register_buffer;
+        for(int i = 0; i <= 5; i++)
+        {
+            ros::Duration(1).sleep();
+            int status = nova_->modbus_read_holding_registers(HOLDING_POINT_CURRENT_START,24,register_buffer.data());
+            if (status == 0)
+            {
+                for(int  i = 0; i < 6U; ++i)
+                {
+                    memcpy(&Point[i], &register_buffer[i * 4U], sizeof(double));
+                }
+                if((Point[0] != 0 && Point[1] != 0 && Point[2] !=0) || 
+                (!ros::ok()) || nova_robotMode() != 7 || !nova_checkConnected() && *run_ptr_ == false) break;
+                if(i == 5) return 0;
+            }
+            else return ERR;
+        }
+        return 0;
+    }
+    catch(const std::invalid_argument& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        return ERR;
+    }
+    catch(const std::exception& e)
+    {
+        std::cerr << e.what() << '\n';
+        return ERR;
+    }    
+}
+
+double cnt_nova5::getZmin(void)
+{
+    std::array<uint16_t,4> register_buffer;
+    double Z_min;
+
+    nova_->modbus_read_holding_registers(HOLDING_Z_MIN_USER_SET_START,4,register_buffer.data()); 
+    memcpy(&Z_min, &register_buffer, sizeof(double));
+    ROS_INFO("Z min : %f",Z_min);
+    return Z_min;
+}
+
+double cnt_nova5::nova_check_z(double& x, double& y, double& z, int choose)
+{
+    static double z_min_store = getZmin();
+    static double z_min;
+
+    double z_max = r_max - 10 - (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2));
+
+    if(choose == 1) z_min = z_min_static;
+    else if(choose == 0) z_min = z_min_store;
+
+    if(z_max > z_min)
+    {
+        if(z > z_min && z < z_max) return z;
+        else if(z <= z_min) return z_min;
+        else if(z >= z_max) return z_max;
+        return ERR;
+    }
+    else return z_min;
+
+
+}
+
+int cnt_nova5::nova_movePoint(std::vector<double>& Point,const int speed, const int mode)
+{
+
+    try
+    {
+        /* code */
+        if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters - move Point");
+        if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area - move Point");
+        if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error - move Point");
+
+        movePointRunning_ = 1;
+        
+        if(flag_move == 0)
+        {
+            // Chọn chế độ di chuyển moveJ move L
+            nova_->modbus_write_coil(COILD_MOVE_MODE, mode);
+
+            // Cài đặt tốc độ toàn cục cho tay máy
+            nova_speedGlobal(speed);
+
+            // Gửi tọa độ mục tiêu và xác nhận cho robot di chuyển đến tọa độ mục tiêu cho robot tay máy
+            if(nova_sendCoordinates(Point) == 1) nova_allowMove();
+            else return 0;
+            flag_move = 1; 
+        }
+        
+        // Chờ cho robot di chuyển xong
+        if(nova_movingDone() == 1)
+        {
+            nova_dismissMove();
+            flag_move = 0;
+            return 1;
+        }
+        // If no movement completed, return an error
+        return 0;
+    }
+    catch(const std::invalid_argument& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        movePointRunning_ = ERR;
+        return ERR;
+    }
+    catch(const std::out_of_range& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        movePointRunning_ = ERR;
+        return ERR;
+    }
+        catch(const std::runtime_error& e)
+    {
+        ROS_ERROR_STREAM("ERROR send: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        movePointRunning_ = ERR;
+        return ERR;
+    }
+    catch(const std::exception& e)
+    {
+        ROS_ERROR_STREAM("ERROR: " << e.what());
+        ros::Duration(TIME_ERR).sleep();
+        movePointRunning_ = ERR;
+        return ERR;
+    }
+}

Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CHANGELOG.rst

@@ -0,0 +1,237 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package diff_drive_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.21.2 (2023-09-03)
+-------------------
+
+0.21.1 (2023-01-31)
+-------------------
+* Don't hardcode plugin library path
+* Contributors: Jochen Sprickerhof
+
+0.21.0 (2022-10-21)
+-------------------
+* std::bind and placeholders instead of boost
+* use boost::placeholders::_1/_2 in remaining instances, include boost/bind/bind.hpp instead of boost/bind.hpp, eliminated unnecessary bind.hpp include
+* Contributors: Lucas Walter
+
+0.20.0 (2022-05-06)
+-------------------
+* Drop old C++ standard
+* Use new boost bind placeholders
+* Add <?xml version=1.0?> to every .launch and .test file
+* Contributors: Jochen Sprickerhof, Lucas Walter
+
+0.19.0 (2021-06-13)
+-------------------
+* fix NaN bug
+* fix test to expose NaN bug
+* Wait long enough for accumulator to be cleared
+* Add test for `#532 <https://github.com/ros-controls/ros_controllers/issues/532>`_
+  Close `#540 <https://github.com/ros-controls/ros_controllers/issues/540>`_
+* Contributors: Caio Amaral, Matt Reynolds, Melvin Wang
+
+0.18.1 (2020-12-03)
+-------------------
+* Fix null pointer checks in diff_drive_controller
+* Use version-agnostic FindBoost for headers
+* Contributors: David V. Lu, Matt Reynolds
+
+0.18.0 (2020-10-11)
+-------------------
+* Fix dependency on Boost
+* Apply consistent format to CMakeLists.txt
+* Update package.xml to format 3
+* Clean dependencies of diff_drive_controller package
+* Apply waitForController method to all diff_drive_controller tests
+* Move odom_pub setup to the end to allow consistent isControllerAlive check
+* Contributors: Mateus Amarante
+
+0.17.0 (2020-05-12)
+-------------------
+
+0.16.1 (2020-04-27)
+-------------------
+
+0.16.0 (2020-04-16)
+-------------------
+* Fix warning dynamic_reconfigure
+* Bump CMake version to prevent CMP0048
+* Add missing header guards
+* Replace header guard with #pragma once
+* Prefix every xacro tag with 'xacro:'
+* Modernize xacro
+  - Remove '--inorder'
+  - Use 'xacro' over 'xacro.py'
+* switch implementation of read and write methods of Diffbot class
+* Refactor nan test
+  EXPECT_NE(x, bool) -> EXPECT_TRUE/FALSE(x)
+  EXPECT_EQ(x, double) -> EXPECT_DOUBLE_EQ(x, double)
+  + clang default reformat
+* Check for nan cmd_vel
+* Contributors: Anas Abou Allaban, Bence Magyar, Franz, Matt Reynolds, Raffaello Bonghi
+
+0.15.1 (2020-03-09)
+-------------------
+* Use nullptr
+* add missing pluginlib deps.
+* Update null link pointer error message
+* Revert cmake include catkin_INCLUDE_DIRS as system
+* Use C++11 `std::this_thread::sleep_for`.
+* Contributors: Bence Magyar, Enrique Fernandez Perdomo, Matt Reynolds, Sean Yen
+
+0.15.0 (2019-03-26)
+-------------------
+* Default all controller builds to C++14
+* boost::chrono -> std::chrono
+* boost::assign -> C++ initializer list
+* boost::shared_ptr -> std::shared_ptr
+* Using left/right multiplies for desired vel
+* diff-drive publish joint trajectory controller state
+* fix install destination for libraries (`#403 <https://github.com/ros-controls/ros_controllers/issues/403>`_)
+* Contributors: Bence Magyar, Gennaro Raiola, James Xu, Jeremie Deray, Jordan Palacios
+
+0.14.3 (2019-02-09)
+-------------------
+* use operators instead of aliases
+* Fix typo descripion -> description
+* Contributors: Daniel Ingram, James Xu
+
+0.14.2 (2018-10-23)
+-------------------
+
+0.14.1 (2018-06-26)
+-------------------
+* Added 'multiplier' in DynamicParams ostream and changed boolean printing to 'enabled/disabled'
+* isPublishngCmdVelOut to check getNumPublisheres until timeout
+* Contributors: Kei Okada, Martin Ganeff
+
+0.14.0 (2018-04-27)
+-------------------
+* add dynamic_reconf to diff_drive_controller
+* migrate to new pluginlib headers
+* per wheel radius multiplier
+* fix xacro macro warning
+* [DiffDrive] Fix time-sensitive tests of diff_drive_controller
+* separate include_directories as SYSTEM to avoid unrelated compilation warnings
+* Contributors: Jeremie Deray, Mathias Lüdtke
+
+0.13.2 (2017-12-23)
+-------------------
+
+0.13.1 (2017-11-06)
+-------------------
+
+0.13.0 (2017-08-10)
+-------------------
+* Add test for allow_multiple_cmd_vel_publishers param
+* add check for multiple publishers on cmd_vel
+* Added tests for the odom_frame_id parameter.
+* Parameterized diff_drive_controller's odom_frame_id
+* Publish executed velocity if publish_cmd
+* refactor to remove code duplication
+* fixup pointer type for new convention
+* Allow diff_drive_controller to use spheres as well as cylinders for wheel collision geometry. Cylinders are not well behaved on Gazebo/ODE heightfields, using spheres works around the issue.
+* Contributors: Bence Magyar, Eric Tappan, Jeremie Deray, Karsten Knese, Tully Foote, mallanmba, tappan-at-git
+
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+* Add exporting include dirs
+* Contributors: Bence Magyar
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Add urdf compatibility header
+* Add --inorder to xacro calls
+* Add missing xacro tags
+* Use xacro instead of xacro.py
+* Disable angular jerk limit test
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Contributors: Bence Magyar
+
+0.11.2 (2016-08-16)
+-------------------
+
+0.11.1 (2016-05-23)
+-------------------
+
+0.11.0 (2016-05-03)
+-------------------
+
+0.10.0 (2015-11-20)
+-------------------
+* Address -Wunused-parameter warnings
+* Limit jerk
+* Add param velocity_rolling_window_size
+* Minor fixes
+  1. Coding style
+  2. Tolerance to fall-back to Runge-Kutta 2 integration
+  3. Remove unused variables
+* Fix the following bugs in the testForward test:
+  1. Check traveled distance in XY plane
+  2. Use expected speed variable on test check
+* Add test for NaN
+* Add test for bad URDF
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Enrique Fernandez, Paul Mathieu
+
+0.9.2 (2015-05-04)
+------------------
+* Allow the wheel separation and radius to be set from different sources
+  i.e. one can be set from the URDF, the other from the parameter server.
+  If wheel separation and wheel diameter is specified in the parameter server, don't look them up from urdf
+* Contributors: Bence Magyar, Nils Berg
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* Add support for multiple wheels per side
+* Odometry computation:
+  - New option to compute in open loop fashion
+  - New option to skip publishing odom frame to tf
+* Remove dependency on angles package
+* Buildsystem fixes
+* Contributors: Bence Magyar, Lukas Bulwahn, efernandez
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Add base_frame_id param (defaults to base_link)
+  The nav_msgs/Odometry message specifies the child_frame_id field,
+  which was previously not set.
+  This commit creates a parameter to replace the previously hard-coded
+  value of the child_frame_id of the published tf frame, and uses it
+  in the odom message as well.
+* Contributors: enriquefernandez
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+* Changed test-depend to build-depend for release jobs.
+* Contributors: Bence Magyar
+
+0.7.0 (2014-03-28)
+------------------
+* diff_drive_controller: New controller for differential drive wheel systems.
+* Control is in the form of a velocity command, that is split then sent on the two wheels of a differential drive
+wheel base.
+* Odometry is published to tf and to a dedicated nav__msgs/Odometry topic.
+* Realtime-safe implementation.
+* Implements task-space velocity and acceleration limits.
+* Automatic stop after command time-out.
+* Contributors: Bence Magyar, Paul Mathieu, Enrique Fernandez.