git commit -m "first commit for v2"
This commit is contained in:
246
Controllers/Packages/amr_comunication/CMakeLists.txt
Normal file
246
Controllers/Packages/amr_comunication/CMakeLists.txt
Normal file
@@ -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)
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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 ¤t_order);
|
||||
|
||||
static bool acceptingInstantActions(const vda_5050::InstantAction &received_instantAction, vda_5050::InstantAction ¤t_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_
|
||||
67
Controllers/Packages/amr_comunication/package.xml
Normal file
67
Controllers/Packages/amr_comunication/package.xml
Normal file
@@ -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>
|
||||
155
Controllers/Packages/amr_comunication/src/opc_ua/ua_server.cpp
Normal file
155
Controllers/Packages/amr_comunication/src/opc_ua/ua_server.cpp
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user