diff --git a/.github/ISSUE_TEMPLATE/general-issue.md b/.github/ISSUE_TEMPLATE/general-issue.md new file mode 100644 index 000000000..92c0824f7 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/general-issue.md @@ -0,0 +1,24 @@ +--- +name: General issue +about: General issue template for micro XRCE-DDS Agent +title: '' +labels: '' +assignees: '' + +--- + +## Issue template + +- Hardware description: +- OS: +- Installation type: +- Version or commit hash: + +#### Steps to reproduce the issue + + +#### Expected behavior + +#### Actual behavior + +#### Additional information diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c3c57122..4c464f655 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,14 +27,18 @@ option(UAGENT_ISOLATED_INSTALL "Install the project and dependencies into separa option(UAGENT_USE_INTERNAL_GTEST "Enable internal GTest libraries." OFF) option(BUILD_SHARED_LIBS "Control shared/static building." ON) +option(UAGENT_USE_SYSTEM_FASTDDS "Force find and use system installed Fast-DDS." OFF) option(UAGENT_FAST_PROFILE "Build FastMiddleware profile." ON) option(UAGENT_CED_PROFILE "Build CedMiddleware profile." ON) option(UAGENT_DISCOVERY_PROFILE "Build Discovery profile." ON) option(UAGENT_P2P_PROFILE "Build P2P discovery profile." ON) option(UAGENT_LOGGER_PROFILE "Build logger profile." ON) option(UAGENT_SECURITY_PROFILE "Build security profile." OFF) -option(UAGENT_CLI_PROFILE "Build CLI profile." ON) -option(UAGENT_BUILD_EXECUTABLE "Build MicroXRCE-DDS Agent provided executable." ON) +option(UAGENT_BUILD_EXECUTABLE "Build Micro XRCE-DDS Agent provided executable." ON) +option(UAGENT_BUILD_USAGE_EXAMPLES "Build Micro XRCE-DDS Agent built-in usage examples" OFF) + +set(UAGENT_P2P_CLIENT_VERSION 2.0.0 CACHE STRING "Sets Micro XRCE-DDS client version for P2P") +set(UAGENT_P2P_CLIENT_TAG develop CACHE STRING "Sets Micro XRCE-DDS client tag for P2P") option(UAGENT_BUILD_CI_TESTS "Build CI test cases.") if(UAGENT_BUILD_CI_TESTS) @@ -67,23 +71,21 @@ set(_fastcdr_version 1.0.13) set(_fastcdr_tag v1.0.13) list(APPEND _deps "fastcdr\;${_fastcdr_version}") -if(UAGENT_CLI_PROFILE) - set(_cli11_version 1.7.1) - set(_cli11_tag v1.7.1) - list(APPEND _deps "CLI11\;${_cli11_version}") -endif() - if(UAGENT_P2P_PROFILE) - set(_microxrcedds_client_version 1.2.3) - set(_microxrcedds_client_tag v1.2.3) + set(_microxrcedds_client_version ${UAGENT_P2P_CLIENT_VERSION}) + set(_microxrcedds_client_tag ${UAGENT_P2P_CLIENT_TAG}) list(APPEND _deps "microxrcedds_client\;${_microxrcedds_client_version}") endif() if(UAGENT_FAST_PROFILE) - set(_fastdds_version 2.0) - set(_fastdds_tag 2.0.x) + if(UAGENT_USE_SYSTEM_FASTDDS) + set(_fastdds_version 2) + else() + set(_fastdds_version 2.0.2) + set(_fastdds_tag v2.0.2) + set(_foonathan_memory_tag c619113) # This tag should be updated every time it gets updated in foonathan_memory_vendor eProsima's package + endif() list(APPEND _deps "fastrtps\;${_fastdds_version}") - set(_foonathan_memory_tag c619113) # This tag should be updated every time it gets updated in foonathan_memory_vendor eProsima's package endif() if(UAGENT_LOGGER_PROFILE) @@ -97,7 +99,7 @@ endif() ############################################################################### set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules) if(NOT UAGENT_SUPERBUILD) - project(microxrcedds_agent VERSION "1.4.2" LANGUAGES C CXX) + project(microxrcedds_agent VERSION "2.0.0" LANGUAGES C CXX) else() project(uagent_superbuild NONE) include(${PROJECT_SOURCE_DIR}/cmake/SuperBuild.cmake) @@ -129,6 +131,7 @@ set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${PROJECT_BINARY_DIR}/temp_install) foreach(d ${_deps}) list(GET d 0 _name) list(GET d 1 _version) + set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${PROJECT_BINARY_DIR}/temp_install/${_name}-${_version}) find_package(${_name} ${_version} REQUIRED) endforeach() @@ -188,9 +191,10 @@ set(SRCS src/cpp/types/SubMessageHeader.cpp src/cpp/message/InputMessage.cpp src/cpp/message/OutputMessage.cpp - $<$>:src/cpp/utils/ArgumentParser.cpp> + src/cpp/utils/ArgumentParser.cpp src/cpp/transport/Server.cpp - src/cpp/transport/serial/SerialProtocol.cpp + src/cpp/transport/stream_framing/StreamFramingProtocol.cpp + src/cpp/transport/custom/CustomAgent.cpp ${TRANSPORT_SRCS} $<$:src/cpp/transport/discovery/DiscoveryServer.cpp> $<$:src/cpp/types/TopicPubSubType.cpp> @@ -284,7 +288,6 @@ target_link_libraries(${PROJECT_NAME} $<$:ws2_32> $<$:iphlpapi> $<$:spdlog::spdlog> - $<$:CLI11::CLI11> PRIVATE $<$:fastrtps> $<$:microxrcedds_client> @@ -324,6 +327,11 @@ if(UAGENT_BUILD_EXECUTABLE) ) endif() +# Examples +if(UAGENT_BUILD_USAGE_EXAMPLES) + add_subdirectory(examples/custom_agent) +endif() + # XML default profile used to launch exec in the building folder file(COPY ${PROJECT_SOURCE_DIR}/agent.refs DESTINATION ${PROJECT_BINARY_DIR} diff --git a/CTestConfig.cmake b/CTestConfig.cmake index 4aef1028c..16d3d6aa0 100644 --- a/CTestConfig.cmake +++ b/CTestConfig.cmake @@ -15,8 +15,7 @@ if(CMAKE_SYSTEM_NAME STREQUAL "Linux") # MemoryCheck configuration. find_program(MEMORYCHECK_COMMAND NAMES valgrind) - set(MEMORYCHECK_COMMAND_OPTIONS "${MEMORYCHECK_COMMAND_OPTIONS} --quiet --tool=memcheck --leak-check=yes --show-reachable=yes --num-callers=50 --xml=yes --xml-file=test_%p_memcheck.xml \"--suppressions=${CMAKE_CURRENT_SOURCE_DIR}/ci/valgrind.supp\"") - + set(MEMORYCHECK_COMMAND_OPTIONS "${MEMORYCHECK_COMMAND_OPTIONS} --log-fd=2 --quiet --tool=memcheck --leak-check=yes --show-reachable=yes --error-exitcode=1 --num-callers=50 \"--suppressions=${CMAKE_CURRENT_SOURCE_DIR}/ci/valgrind.supp\"") # Coverage configuration. if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") find_program(COVERAGE_COMMAND NAMES gcov) diff --git a/Dockerfile b/Dockerfile index e1621880f..4d8e681d8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -31,9 +31,8 @@ ADD . /agent/ # Build Micro XRCE-DDS Agent and install RUN cd /agent/build && \ cmake -DCMAKE_INSTALL_PREFIX=../install \ - -DUAGENT_ISOLATED_INSTALL=OFF \ .. &&\ - make && make install + make -j $(nproc) && make install # Prepare Micro XRCE-DDS Agent artifacts RUN cd /agent && \ diff --git a/README.md b/README.md index 5f9af35ee..be219e164 100644 --- a/README.md +++ b/README.md @@ -20,13 +20,13 @@ This is made possible by the creation of *DDS Entities* on the *Agent* as a resu The communication between a *Micro XRCE-DDS Client* and a *Micro XRCE-DDS Agent* is achieved by means of several kinds of built-in transports: **UDPv4**, **UDPv6**, **TCPv4**, **TCPv6** and **Serial** communication. In addition, there is the possibility for the user to generate its own **Custom** transport. You can use an *Agent* with these transports by means of the standalone executable generated when building the project, which comes with a built-in CLI tool to select one of the transports listed above. + This built-in *Agent* can also be installed and launched using the provided [Snap package](https://snapcraft.io/micro-xrce-dds-agent) or the provided [Docker image](https://hub.docker.com/r/eprosima/micro-xrce-dds-agent/). -![Architecture](docs/agent_architecture.png) ## Documentation -You can access *Micro XRCE-DDS* documentation online, which is hosted on Read the Docs. +You can access the *eProsima Micro XRCE-DDS* user documentation online, which is hosted on Read the Docs. * [Start Page](http://micro-xrce-dds.readthedocs.io) * [Installation manual](http://micro-xrce-dds.readthedocs.io/en/latest/installation.html) diff --git a/ci/linux/CMakeLists.txt b/ci/linux/CMakeLists.txt index ce93c36ab..262959f75 100644 --- a/ci/linux/CMakeLists.txt +++ b/ci/linux/CMakeLists.txt @@ -24,10 +24,10 @@ include(ExternalProject) include(CheckCXXCompilerFlag) include(CheckCCompilerFlag) -set(_c_flags "-fwrapv -fprofile-arcs -ftest-coverage") -set(_cxx_flags "-fwrapv -fprofile-arcs -ftest-coverage") -set(_exe_linker_flags "-fprofile-arcs -ftest-coverage") -set(_shared_linker_flags "-fprofile-arcs -ftest-coverage") +set(_c_flags "-fwrapv -fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") +set(_cxx_flags "-fwrapv -fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") +set(_exe_linker_flags "-fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") +set(_shared_linker_flags "-fprofile-arcs -ftest-coverage --coverage -fno-inline -fno-inline-small-functions -fno-default-inline") check_cxx_compiler_flag("-fprofile-abs-path" _have_cxx_fprofile_abs_path) if(_have_cxx_fprofile_abs_path) diff --git a/cmake/SuperBuild.cmake b/cmake/SuperBuild.cmake index 0f653cf2a..ef4da2353 100644 --- a/cmake/SuperBuild.cmake +++ b/cmake/SuperBuild.cmake @@ -32,7 +32,7 @@ if(UAGENT_P2P_PROFILE) PREFIX ${PROJECT_BINARY_DIR}/microxrcedds_client INSTALL_DIR - ${PROJECT_BINARY_DIR}/temp_install + ${PROJECT_BINARY_DIR}/temp_install/microxrcedds_client-${_microxrcedds_client_version} CMAKE_CACHE_ARGS -DCMAKE_INSTALL_PREFIX:PATH= -DCMAKE_PREFIX_PATH:PATH=${CMAKE_PREFIX_PATH} @@ -77,7 +77,7 @@ if(NOT fastcdr_FOUND) list(APPEND _deps fastcdr) endif() -if(UAGENT_FAST_PROFILE) +if(UAGENT_FAST_PROFILE AND NOT UAGENT_USE_SYSTEM_FASTDDS) # Foonathan memory. unset(foonathan_memory_DIR CACHE) find_package(foonathan_memory QUIET) @@ -104,7 +104,7 @@ if(UAGENT_FAST_PROFILE) # Fast DDS. unset(fastdds_DIR CACHE) find_package(fastrtps ${_fastdds_version} EXACT QUIET) - if(NOT fastdds_FOUND) + if(NOT fastrtps_FOUND) ExternalProject_Add(fastdds GIT_REPOSITORY https://github.com/eProsima/Fast-DDS.git @@ -126,6 +126,7 @@ if(UAGENT_FAST_PROFILE) -DTHIRDPARTY:BOOL=ON -DCOMPILE_TOOLS:BOOL=OFF -DSECURITY:BOOL=${UAGENT_SECURITY_PROFILE} + -DSHM_TRANSPORT_DEFAULT:BOOL=OFF DEPENDS fastcdr foonathan_memory @@ -139,35 +140,6 @@ if(UAGENT_FAST_PROFILE) endif() endif() -# CLI11. -if(UAGENT_CLI_PROFILE) - unset(CLI11_DIR CACHE) - find_package(CLI11 ${_cli11_version} EXACT QUIET) - if(NOT CLI11_FOUND) - ExternalProject_Add(cli11 - GIT_REPOSITORY - https://github.com/CLIUtils/CLI11.git - GIT_TAG - ${_cli11_tag} - PREFIX - ${PROJECT_BINARY_DIR}/CLI11 - INSTALL_DIR - ${PROJECT_BINARY_DIR}/temp_install/cli11-${_cli11_version} - CMAKE_CACHE_ARGS - -DCMAKE_INSTALL_PREFIX:PATH= - -DCMAKE_PREFIX_PATH:PATH=${CMAKE_PREFIX_PATH};${CMAKE_INSTALL_PREFIX} - -DBUILD_SHARED_LIBS:BOOL=${BUILD_SHARED_LIBS} - -DCMAKE_TOOLCHAIN_FILE:PATH=${CMAKE_TOOLCHAIN_FILE} - -DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE} - -DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER} - -DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER} - -DCLI11_TESTING:BOOL=OFF - -DCLI11_EXAMPLES:BOOL=OFF - ) - list(APPEND _deps cli11) - endif() -endif() - if(UAGENT_LOGGER_PROFILE) # spdlog. unset(spdlog_DIR CACHE) @@ -248,6 +220,8 @@ if(NOT Sanitizers_FOUND) "" INSTALL_COMMAND "" + CMAKE_ARGS + $<$:-DCMAKE_POLICY_DEFAULT_CMP0077=OLD> # Disable CMP0077 unset warning CMAKE_CACHE_ARGS -DCMAKE_TOOLCHAIN_FILE:PATH=${CMAKE_TOOLCHAIN_FILE} ) diff --git a/colcon.pkg b/colcon.pkg index 0bf0590bd..d2b1d6aec 100644 --- a/colcon.pkg +++ b/colcon.pkg @@ -7,6 +7,6 @@ ], "cmake-args":[ "-DUAGENT_ISOLATED_INSTALL=OFF", - "-DUAGENT_CLI_PROFILE=OFF" + "-DUAGENT_USE_SYSTEM_FASTDDS=ON" ] } \ No newline at end of file diff --git a/docs/Agent.png b/docs/Agent.png new file mode 100644 index 000000000..ba32b1a31 Binary files /dev/null and b/docs/Agent.png differ diff --git a/docs/General.png b/docs/General.png new file mode 100644 index 000000000..f07336411 Binary files /dev/null and b/docs/General.png differ diff --git a/docs/agent_architecture.png b/docs/agent_architecture.png deleted file mode 100644 index c50c5837f..000000000 Binary files a/docs/agent_architecture.png and /dev/null differ diff --git a/examples/custom_agent/CMakeLists.txt b/examples/custom_agent/CMakeLists.txt new file mode 100644 index 000000000..2dd21d0bc --- /dev/null +++ b/examples/custom_agent/CMakeLists.txt @@ -0,0 +1,30 @@ +# Copyright 2021-present Proyectos y Sistemas de Mantenimiento SL (eProsima). +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +if (UAGENT_BUILD_USAGE_EXAMPLES) + add_executable(CustomXRCEAgent custom_agent.cpp) + target_link_libraries(CustomXRCEAgent + PRIVATE + ${PROJECT_NAME} + $<$>:rt> + $<$>:dl> + ) + + set_target_properties(CustomXRCEAgent + PROPERTIES + CXX_STANDARD + 11 + CXX_STANDARD_REQUIRED + YES + ) +endif() \ No newline at end of file diff --git a/examples/custom_agent/custom_agent.cpp b/examples/custom_agent/custom_agent.cpp new file mode 100644 index 000000000..745be33c7 --- /dev/null +++ b/examples/custom_agent/custom_agent.cpp @@ -0,0 +1,251 @@ +// Copyright 2021-present Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include +#include +#include + +/** + * This custom XRCE Agent example attempts to show how easy is for the user to define a custom + * Micro XRCE-DDS Agent behaviour, in terms of transport initialization and closing, and also + * regarding read and write operations. + * For this simple case, an UDP socket is opened on port 8888. Additionally, some information + * messages are being printed to demonstrate the custom behaviour. + * As the endpoint is already defined, we are using the provided + * `eprosima::uxr::IPv4EndPoint` by the library. + * Other transport protocols might need to implement their own endpoint struct. + */ + +int main(int argc, char** argv) +{ + eprosima::uxr::Middleware::Kind mw_kind(eprosima::uxr::Middleware::Kind::FASTDDS); + uint16_t agent_port(8888); + struct pollfd poll_fd; + + /** + * @brief Agent's initialization behaviour description. + */ + eprosima::uxr::CustomAgent::InitFunction init_function = [&]() -> bool + { + bool rv = false; + poll_fd.fd = socket(PF_INET, SOCK_DGRAM, 0); + + if (-1 != poll_fd.fd) + { + struct sockaddr_in address{}; + + address.sin_family = AF_INET; + address.sin_port = htons(agent_port); + address.sin_addr.s_addr = INADDR_ANY; + memset(address.sin_zero, '\0', sizeof(address.sin_zero)); + + if (-1 != bind(poll_fd.fd, + reinterpret_cast(&address), + sizeof(address))) + { + poll_fd.events = POLLIN; + rv = true; + + UXR_AGENT_LOG_INFO( + UXR_DECORATE_GREEN( + "This is an example of a custom Micro XRCE-DDS Agent INIT function"), + "port: {}", + agent_port); + } + } + + return rv; + }; + + /** + * @brief Agent's destruction actions. + */ + eprosima::uxr::CustomAgent::FiniFunction fini_function = [&]() -> bool + { + if (-1 == poll_fd.fd) + { + return true; + } + + if (0 == ::close(poll_fd.fd)) + { + poll_fd.fd = -1; + + UXR_AGENT_LOG_INFO( + UXR_DECORATE_GREEN( + "This is an example of a custom Micro XRCE-DDS Agent FINI function"), + "port: {}", + agent_port); + + return true; + } + else + { + return false; + } + }; + + /** + * @brief Agent's incoming data functionality. + */ + eprosima::uxr::CustomAgent::RecvMsgFunction recv_msg_function = [&]( + eprosima::uxr::CustomEndPoint* source_endpoint, + uint8_t* buffer, + size_t buffer_length, + int timeout, + eprosima::uxr::TransportRc& transport_rc) -> ssize_t + { + struct sockaddr_in client_addr{}; + socklen_t client_addr_len = sizeof(struct sockaddr_in); + ssize_t bytes_received = -1; + + int poll_rv = poll(&poll_fd, 1, timeout); + + if (0 < poll_rv) + { + bytes_received = recvfrom( + poll_fd.fd, + buffer, + buffer_length, + 0, + reinterpret_cast(&client_addr), + &client_addr_len); + + transport_rc = (-1 != bytes_received) + ? eprosima::uxr::TransportRc::ok + : eprosima::uxr::TransportRc::server_error; + } + else + { + transport_rc = (0 == poll_rv) + ? eprosima::uxr::TransportRc::timeout_error + : eprosima::uxr::TransportRc::server_error; + bytes_received = 0; + } + + if (eprosima::uxr::TransportRc::ok == transport_rc) + { + UXR_AGENT_LOG_INFO( + UXR_DECORATE_GREEN( + "This is an example of a custom Micro XRCE-DDS Agent RECV_MSG function"), + "port: {}", + agent_port); + source_endpoint->set_member_value("address", + static_cast(client_addr.sin_addr.s_addr)); + source_endpoint->set_member_value("port", + static_cast(client_addr.sin_port)); + } + + + return bytes_received; + }; + + /** + * @brief Agent's outcoming data flow definition. + */ + eprosima::uxr::CustomAgent::SendMsgFunction send_msg_function = [&]( + const eprosima::uxr::CustomEndPoint* destination_endpoint, + uint8_t* buffer, + size_t message_length, + eprosima::uxr::TransportRc& transport_rc) -> ssize_t + { + struct sockaddr_in client_addr{}; + + memset(&client_addr, 0, sizeof(client_addr)); + client_addr.sin_family = AF_INET; + client_addr.sin_port = destination_endpoint->get_member("port"); + client_addr.sin_addr.s_addr = destination_endpoint->get_member("address"); + + ssize_t bytes_sent = + sendto( + poll_fd.fd, + buffer, + message_length, + 0, + reinterpret_cast(&client_addr), + sizeof(client_addr)); + + transport_rc = (-1 != bytes_sent) + ? eprosima::uxr::TransportRc::ok + : eprosima::uxr::TransportRc::server_error; + + if (eprosima::uxr::TransportRc::ok == transport_rc) + { + UXR_AGENT_LOG_INFO( + UXR_DECORATE_GREEN( + "This is an example of a custom Micro XRCE-DDS Agent SEND_MSG function"), + "port: {}", + agent_port); + } + + return bytes_sent; + }; + + /** + * Run the main application. + */ + try + { + /** + * EndPoint definition for this transport. We define an address and a port. + */ + eprosima::uxr::CustomEndPoint custom_endpoint; + custom_endpoint.add_member("address"); + custom_endpoint.add_member("port"); + + /** + * Create a custom agent instance. + */ + eprosima::uxr::CustomAgent custom_agent( + "UDPv4_CUSTOM", + &custom_endpoint, + mw_kind, + false, + init_function, + fini_function, + send_msg_function, + recv_msg_function); + + /** + * Set verbosity level + */ + custom_agent.set_verbose_level(6); + + /** + * Run agent and wait until receiving an stop signal. + */ + custom_agent.start(); + + int n_signal = 0; + sigset_t signals; + sigwait(&signals, &n_signal); + + /** + * Stop agent, and exit. + */ + custom_agent.stop(); + return 0; + } + catch (const std::exception& e) + { + std::cout << e.what() << std::endl; + return 1; + } +} \ No newline at end of file diff --git a/include/uxr/agent/Agent.hpp b/include/uxr/agent/Agent.hpp index 8b36af78f..c1c0370ec 100644 --- a/include/uxr/agent/Agent.hpp +++ b/include/uxr/agent/Agent.hpp @@ -22,8 +22,15 @@ #include #include -namespace eprosima{ -namespace uxr{ +namespace eprosima { +namespace uxr { +namespace middleware { +/** + * @brief Forward declarations. + */ +enum class CallbackKind : uint8_t; +class CallbackFactory; +} // namespace middleware class Root; @@ -546,6 +553,22 @@ class Agent */ UXR_AGENT_EXPORT void set_verbose_level(uint8_t verbose_level); + /** + * @brief Sets a callback function for an specific create/delete middleware entity operation. + * Note that not some middlewares might not implement every defined operation, or even + * no operation at all. + * @param middleware_kind Enumeration class defining all the supported pluggable middlewares for the agent. + * @param callback_kind Enumeration class defining all the different operations available to which + * set a callback to. + * @param callback_function std::function rvalue variable implementing the callback logic. Desirable + * to be implemented using lambda expressions wrapped inside a std::function descriptor. + */ + template + UXR_AGENT_EXPORT void add_middleware_callback( + const Middleware::Kind& middleware_kind, + const middleware::CallbackKind& callback_kind, + std::function&& callback_function); + private: template bool create_object( @@ -562,9 +585,9 @@ class Agent uint16_t raw_id, Agent::OpResult& op_result); - protected: std::unique_ptr root_; + middleware::CallbackFactory& callback_factory_; }; } // uxr diff --git a/include/uxr/agent/AgentInstance.hpp b/include/uxr/agent/AgentInstance.hpp index d3bd9b9e5..ac6c67c27 100644 --- a/include/uxr/agent/AgentInstance.hpp +++ b/include/uxr/agent/AgentInstance.hpp @@ -17,23 +17,26 @@ #include -#ifdef UAGENT_CLI_PROFILE -#include -#else #include -#endif // UAGENT_CLI_PROFILE #include namespace eprosima { namespace uxr { +namespace middleware { +/** + * @brief Forward declaration. + */ +class CallbackFactory; +} // middleware + /** * @brief Singleton class to manage the launch process of a MicroXRCE-DDS Agent. */ class AgentInstance { -public: +private: /** * @brief Default constructor. */ @@ -56,6 +59,7 @@ class AgentInstance UXR_AGENT_EXPORT AgentInstance& operator =( AgentInstance &&) = delete; +public: /** * @brief Get instance associated to this class. * @return Static reference to the singleton AgentInstance object. @@ -77,24 +81,28 @@ class AgentInstance */ UXR_AGENT_EXPORT void run(); + /** + * @brief Sets a callback function for a specific create/delete middleware entity operation. + * Note that not some middlewares might not implement every defined operation, or even + * no operation at all. + * @param middleware_kind Enumeration class defining all the supported pluggable middlewares for the agent. + * @param callback_kind Enumeration class defining all the different operations available to which + * set a callback to. + * @param callback_function std::function rvalue variable implementing the callback logic. Desirable + * to be implemented using lambda expressions wrapped inside a std::function descriptor. + */ + template + UXR_AGENT_EXPORT void add_middleware_callback( + const Middleware::Kind& middleware_kind, + const middleware::CallbackKind& callback_kind, + std::function&& callback_function); + private: -#ifdef UAGENT_CLI_PROFILE - CLI::App app_; - cli::UDPv4Subcommand udpv4_subcmd_; - cli::UDPv6Subcommand udpv6_subcmd_; - cli::TCPv4Subcommand tcpv4_subcmd_; - cli::TCPv6Subcommand tcpv6_subcmd_; -#ifndef _WIN32 - cli::TermiosSubcommand termios_subcmd_; - cli::PseudoTerminalSubcommand pseudo_serial_subcmd_; -#endif // _WIN32 - cli::ExitSubcommand exit_subcmd_; -#else std::thread agent_thread_; -#endif // UAGENT_CLI_PROFILE #ifndef _WIN32 sigset_t signals_; #endif // _WIN32 + middleware::CallbackFactory& callback_factory_; }; } // uxr } // eprosima diff --git a/include/uxr/agent/client/ProxyClient.hpp b/include/uxr/agent/client/ProxyClient.hpp index 45886e64b..9464553bd 100644 --- a/include/uxr/agent/client/ProxyClient.hpp +++ b/include/uxr/agent/client/ProxyClient.hpp @@ -35,7 +35,8 @@ class ProxyClient : public std::enable_shared_from_this explicit ProxyClient( const dds::xrce::CLIENT_Representation& representation, - Middleware::Kind middleware_kind = Middleware::Kind(0)); + Middleware::Kind middleware_kind = Middleware::Kind(0), + std::unordered_map&& properties = {}); ~ProxyClient() = default; @@ -132,6 +133,7 @@ class ProxyClient : public std::enable_shared_from_this std::mutex state_mtx_; State state_; std::chrono::time_point timestamp_; + std::unordered_map properties_; }; } // namespace uxr diff --git a/include/uxr/agent/client/session/stream/OutputStream.hpp b/include/uxr/agent/client/session/stream/OutputStream.hpp index c6ef4113a..ddf8be600 100644 --- a/include/uxr/agent/client/session/stream/OutputStream.hpp +++ b/include/uxr/agent/client/session/stream/OutputStream.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -177,7 +178,16 @@ inline bool BestEffortOutputStream::push_submessage( /* Create message. */ OutputMessagePtr output_message(new OutputMessage(message_header, session_info.mtu)); - if (output_message->append_submessage(submessage_id, submessage)) + if (session_info.mtu < submessage.getCdrSerializedSize()) + { + UXR_AGENT_LOG_WARN( + UXR_DECORATE_YELLOW("serialization warning"), + "Trying to serialize {:d} in {:d} MTU stream", + submessage.getCdrSerializedSize(), + session_info.mtu); + rv = true; + } + else if (output_message->append_submessage(submessage_id, submessage)) { /* Push message. */ messages_.push(std::move(output_message)); diff --git a/include/uxr/agent/config.hpp.in b/include/uxr/agent/config.hpp.in index a3abbc022..d752d383b 100644 --- a/include/uxr/agent/config.hpp.in +++ b/include/uxr/agent/config.hpp.in @@ -28,7 +28,6 @@ namespace uxr { #cmakedefine UAGENT_P2P_PROFILE #endif #cmakedefine UAGENT_LOGGER_PROFILE -#cmakedefine UAGENT_CLI_PROFILE const uint16_t DISCOVERY_PORT = 7400; const char* const DISCOVERY_IP = "239.255.0.2"; diff --git a/include/uxr/agent/middleware/Middleware.hpp b/include/uxr/agent/middleware/Middleware.hpp index 2b905c3b1..29e18d840 100644 --- a/include/uxr/agent/middleware/Middleware.hpp +++ b/include/uxr/agent/middleware/Middleware.hpp @@ -44,6 +44,11 @@ class Middleware }; Middleware() = default; + Middleware( + bool intraprocess_enabled) + : intraprocess_enabled_(intraprocess_enabled) + { + }; virtual ~Middleware() = default; /********************************************************************************************************************** @@ -222,6 +227,12 @@ class Middleware virtual bool matched_replier_from_xml( uint16_t replier_id, const std::string& xml) const = 0; + +/********************************************************************************************************************** + * Members. + **********************************************************************************************************************/ +protected: + bool intraprocess_enabled_; }; } // namespace uxr diff --git a/include/uxr/agent/middleware/ced/CedMiddleware.hpp b/include/uxr/agent/middleware/ced/CedMiddleware.hpp index 8f4a5e00d..8923d572c 100644 --- a/include/uxr/agent/middleware/ced/CedMiddleware.hpp +++ b/include/uxr/agent/middleware/ced/CedMiddleware.hpp @@ -348,7 +348,7 @@ class CedMiddleware : public Middleware std::chrono::milliseconds) override { return false; }; /** - * @brief + * @brief Not implemented. */ bool read_reply( uint16_t, diff --git a/include/uxr/agent/middleware/fast/FastEntities.hpp b/include/uxr/agent/middleware/fast/FastEntities.hpp index f93f91c2a..c1b3e2b7a 100644 --- a/include/uxr/agent/middleware/fast/FastEntities.hpp +++ b/include/uxr/agent/middleware/fast/FastEntities.hpp @@ -101,6 +101,8 @@ class FastParticipant int16_t domain_id() const; + const fastrtps::rtps::GUID_t& get_guid() const; + private: fastrtps::Participant* impl_; std::unordered_map> type_register_; @@ -208,6 +210,10 @@ class FastDataWriter const fastrtps::rtps::GUID_t& get_guid() const; + const fastrtps::Participant* get_participant() const; + + const fastrtps::Publisher* get_ptr() const; + private: fastrtps::Publisher* impl_; std::shared_ptr topic_; @@ -239,6 +245,12 @@ class FastDataReader fastrtps::SampleInfo_t& info, std::chrono::milliseconds timeout); + const fastrtps::rtps::GUID_t& get_guid() const; + + const fastrtps::Participant* get_participant() const; + + const fastrtps::Subscriber* get_ptr() const; + private: fastrtps::Subscriber* impl_; std::shared_ptr topic_; @@ -270,6 +282,12 @@ class FastRequester std::vector& data, std::chrono::milliseconds timeout); + const fastrtps::Participant* get_participant() const; + + const fastrtps::Publisher* get_request_datawriter() const; + + const fastrtps::Subscriber* get_reply_datareader() const; + private: std::shared_ptr datawriter_; std::shared_ptr datareader_; @@ -298,6 +316,13 @@ class FastReplier bool read( std::vector& data, std::chrono::milliseconds timeout); + + const fastrtps::Participant* get_participant() const; + + const fastrtps::Subscriber* get_request_datareader() const; + + const fastrtps::Publisher* get_reply_datawriter() const; + private: std::shared_ptr datawriter_; std::shared_ptr datareader_; diff --git a/include/uxr/agent/middleware/fast/FastMiddleware.hpp b/include/uxr/agent/middleware/fast/FastMiddleware.hpp index 4ebc700f8..638560840 100644 --- a/include/uxr/agent/middleware/fast/FastMiddleware.hpp +++ b/include/uxr/agent/middleware/fast/FastMiddleware.hpp @@ -27,11 +27,14 @@ namespace eprosima { namespace uxr { +namespace middleware { +class CallbackFactory; +} // namespace middleware class FastMiddleware : public Middleware { public: - FastMiddleware() = default; + FastMiddleware(); ~FastMiddleware() final = default; /********************************************************************************************************************** @@ -221,6 +224,8 @@ class FastMiddleware : public Middleware std::unordered_map> datareaders_; std::unordered_map> requesters_; std::unordered_map> repliers_; + + middleware::CallbackFactory& callback_factory_; }; } // namespace uxr diff --git a/include/uxr/agent/middleware/fastdds/FastDDSEntities.hpp b/include/uxr/agent/middleware/fastdds/FastDDSEntities.hpp index d37022300..386872bb0 100644 --- a/include/uxr/agent/middleware/fastdds/FastDDSEntities.hpp +++ b/include/uxr/agent/middleware/fastdds/FastDDSEntities.hpp @@ -57,6 +57,8 @@ class FastDDSParticipant // Proxy methods int16_t domain_id() const { return domain_id_; } + fastrtps::rtps::GUID_t guid() const { return ptr_->guid(); } + fastdds::dds::DomainParticipant* get_ptr() const { return ptr_; } ReturnCode_t unregister_type( const std::string& typeName); @@ -107,6 +109,10 @@ class FastDDSParticipant std::shared_ptr find_local_topic( const std::string& topic_name) const; + fastdds::dds::DomainParticipant* operator * (); + + const fastdds::dds::DomainParticipant* operator * () const; + private: fastdds::dds::DomainParticipant* ptr_; fastdds::dds::DomainParticipantFactory* factory_; @@ -210,7 +216,7 @@ class FastDDSPublisher class FastDDSSubscriber { public: - FastDDSSubscriber(const std::shared_ptr& participant) + FastDDSSubscriber(const std::shared_ptr& participant) : participant_{participant} , ptr_{nullptr} {} @@ -230,7 +236,7 @@ class FastDDSSubscriber std::shared_ptr get_participant() const { return participant_; } - + private: std::shared_ptr participant_; fastdds::dds::Subscriber* ptr_; @@ -251,10 +257,15 @@ class FastDDSDataWriter ~FastDDSDataWriter(); + fastrtps::rtps::GUID_t guid() const { return ptr_->guid(); } + bool create_by_ref(const std::string& ref); bool create_by_xml(const std::string& xml); bool match(const fastrtps::PublisherAttributes& attrs) const; bool write(const std::vector& data); + const fastdds::dds::DataWriter* ptr() const; + const fastdds::dds::DomainParticipant* participant() const; + private: std::shared_ptr publisher_; std::shared_ptr topic_; @@ -274,13 +285,19 @@ class FastDDSDataReader ~FastDDSDataReader(); + fastrtps::rtps::GUID_t guid() const { return ptr_->guid(); } + bool create_by_ref(const std::string& ref); bool create_by_xml(const std::string& xml); bool match_from_ref(const std::string& ref) const; bool match_from_xml(const std::string& xml) const; bool read( std::vector& data, - std::chrono::milliseconds timeout); + std::chrono::milliseconds timeout, + fastdds::dds::SampleInfo& sample_info); + const fastdds::dds::DataReader* ptr() const; + const fastdds::dds::DomainParticipant* participant() const; + private: std::shared_ptr subscriber_; std::shared_ptr topic_; @@ -308,6 +325,9 @@ class FastDDSRequester ~FastDDSRequester(); + fastrtps::rtps::GUID_t guid_datareader() const { return datareader_ptr_->guid(); } + fastrtps::rtps::GUID_t guid_datawriter() const { return datawriter_ptr_->guid(); } + bool create_by_attributes( const fastrtps::RequesterAttributes& attrs); bool match_from_ref(const std::string& ref) const; @@ -322,9 +342,15 @@ class FastDDSRequester std::vector& data, std::chrono::milliseconds timeout); + const fastdds::dds::DomainParticipant* get_participant() const; + + const fastdds::dds::DataWriter* get_request_datawriter() const; + + const fastdds::dds::DataReader* get_reply_datareader() const; + private: bool match(const fastrtps::RequesterAttributes& attrs) const; - + private: std::shared_ptr participant_; @@ -363,6 +389,9 @@ class FastDDSReplier ~FastDDSReplier(); + fastrtps::rtps::GUID_t guid_datareader() const { return datareader_ptr_->guid(); } + fastrtps::rtps::GUID_t guid_datawriter() const { return datawriter_ptr_->guid(); } + bool create_by_attributes( const fastrtps::ReplierAttributes& attrs); bool match_from_ref(const std::string& ref) const; @@ -372,6 +401,12 @@ class FastDDSReplier bool read(std::vector& data, std::chrono::milliseconds timeout); + const fastdds::dds::DomainParticipant* get_participant() const; + + const fastdds::dds::DataReader* get_request_datareader() const; + + const fastdds::dds::DataWriter* get_reply_datawriter() const; + private: bool match(const fastrtps::ReplierAttributes& attrs) const; void transform_sample_identity( @@ -380,7 +415,7 @@ class FastDDSReplier void transport_sample_identity( const dds::SampleIdentity& dds_identity, fastrtps::rtps::SampleIdentity& fast_identity); - + private: std::shared_ptr participant_; diff --git a/include/uxr/agent/middleware/fastdds/FastDDSMiddleware.hpp b/include/uxr/agent/middleware/fastdds/FastDDSMiddleware.hpp index 395204d64..cc3dabd63 100644 --- a/include/uxr/agent/middleware/fastdds/FastDDSMiddleware.hpp +++ b/include/uxr/agent/middleware/fastdds/FastDDSMiddleware.hpp @@ -24,11 +24,15 @@ namespace eprosima { namespace uxr { +namespace middleware { +class CallbackFactory; +} // namespace middleware class FastDDSMiddleware : public Middleware { public: - FastDDSMiddleware() = default; + FastDDSMiddleware(); + FastDDSMiddleware(bool intraprocess_enabled); ~FastDDSMiddleware() final = default; /********************************************************************************************************************** @@ -209,6 +213,14 @@ class FastDDSMiddleware : public Middleware const std::string& xml) const override; private: + std::shared_ptr create_requester( + std::shared_ptr& participant, + const fastrtps::RequesterAttributes& attrs); + + std::shared_ptr create_replier( + std::shared_ptr& participant, + const fastrtps::ReplierAttributes& attrs); + std::unordered_map> participants_; std::unordered_map> topics_; std::unordered_map> publishers_; @@ -217,6 +229,8 @@ class FastDDSMiddleware : public Middleware std::unordered_map> datareaders_; std::unordered_map> requesters_; std::unordered_map> repliers_; + + middleware::CallbackFactory& callback_factory_; }; } // namespace uxr diff --git a/include/uxr/agent/middleware/utils/Callbacks.hpp b/include/uxr/agent/middleware/utils/Callbacks.hpp new file mode 100644 index 000000000..06ddbc170 --- /dev/null +++ b/include/uxr/agent/middleware/utils/Callbacks.hpp @@ -0,0 +1,1063 @@ +// Copyright 2020-present Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UXR__AGENT__MIDDLEWARE__UTILS__CALLBACKS_HPP_ +#define UXR__AGENT__MIDDLEWARE__UTILS__CALLBACKS_HPP_ + +#include + +#ifdef UAGENT_FAST_PROFILE +#include +#include + +#endif // UAGENT_FAST_PROFILE +#ifdef UAGENT_CED_PROFILE +#include +#endif // UAGENT_CED_PROFILE + +#include + +#include + +namespace eprosima { +namespace uxr { +namespace middleware { + +/********************************************************************************************************************** + * CallbackKind + **********************************************************************************************************************/ +/** + * @brief Enumeration class defining the different types of callbacks available for a middleware. + */ +enum class CallbackKind : uint8_t +{ + CREATE_PARTICIPANT, + CREATE_DATAWRITER, + CREATE_DATAREADER, + CREATE_REQUESTER, + CREATE_REPLIER, + DELETE_PARTICIPANT, + DELETE_DATAWRITER, + DELETE_DATAREADER, + DELETE_REQUESTER, + DELETE_REPLIER +}; + +/********************************************************************************************************************** + * Callback + **********************************************************************************************************************/ +/** + * @brief Represents a callback function, which can be called anytime. + */ +template +class Callback +{ +public: + /** + * @brief Constructor. + * @param callback_function Implementation of the callback function. Must be passed as rvalue. + */ + Callback( + std::function&& callback_function) + : callback_(std::move(callback_function)) + { + } + + /** + * @brief Default destructor. + */ + ~Callback() = default; + + /** + * @brief Const version of the call () operator. + * @param args Variable number of arguments to be passed to the callback function. + */ + inline void operator ()( + Args ... args) const + { + callback_(std::forward(args)...); + } + + /** + * @brief Non-const version of the call () operator. + * @param args Variable number of arguments to be passed to the callback function. + */ + inline void operator ()( + Args ... args) + { + callback_(std::forward(args)...); + } + +private: + + std::function callback_; +}; + +/********************************************************************************************************************** + * CallbackFactory + **********************************************************************************************************************/ +/** + * @brief Represents a factory of all the defined callbacks for each active communication middleware. + */ +class CallbackFactory +{ +public: + /** + * @brief Default destructor. + */ + ~CallbackFactory() = default; + + /** + * @brief Get instance associated to this singleton class. + * @return Reference to the CallbackFactory database static object. + */ + static CallbackFactory& getInstance() + { + static CallbackFactory instance; + return instance; + } + + /** + * @brief Add callback function for a specific middleware kind and operation. + * @param middleware_kind Enumeration class defining all the supported middlewares. + * @param callback_kind Enumeration class defining all the different operations available to which + * set a callback to. + * @param callback_function std::function rvalue variable implementing the callback method logic to be + * included in the callback database. + */ + template + void add_callback( + const Middleware::Kind& middleware_kind, + const CallbackKind& callback_kind, + std::function&& callback_function) + { + switch (middleware_kind) + { +#ifdef UAGENT_FAST_PROFILE + case Middleware::Kind::FASTRTPS: + { + fast_callback_factory_.add_callback(callback_kind, std::move(callback_function)); + break; + } + case Middleware::Kind::FASTDDS: + { + fast_dds_callback_factory_.add_callback(callback_kind, std::move(callback_function)); + break; + } +#endif // UAGENT_FAST_PROFILE +#ifdef UAGENT_CED_PROFILE + case Middleware::Kind::CED: + case Middleware::Kind::NONE: + { + // Callbacks not implemented + break; + } +#endif // UAGENT_CED_PROFILE + } + } + + /** + * @brief Execute specific callback type functions registered for a certain middleware kind. + * @param middleware_kind Enumeration class defining all the supported middlewares. + * @param callback_kind Enumerated element from class CallbackKind indicating which callback + * functions need to be executed. + * @param args Variadic number of arguments to be passed to the registered callbacks. + * Note that this means that are registered callbacks for a certain Middleware::Kind + * and CallbackKind must receive the same type of input parameters. + */ + template + void execute_callbacks( + const Middleware::Kind& middleware_kind, + const CallbackKind callback_kind, + Args ... args) + { + switch (middleware_kind) + { +#ifdef UAGENT_FAST_PROFILE + case Middleware::Kind::FASTRTPS: + { + fast_callback_factory_.execute_callbacks(callback_kind, std::forward(args)...); + break; + } + case Middleware::Kind::FASTDDS: + { + fast_dds_callback_factory_.execute_callbacks(callback_kind, std::forward(args)...); + break; + } +#endif // UAGENT_FAST_PROFILE +#ifdef UAGENT_CED_PROFILE + case Middleware::Kind::CED: + case Middleware::Kind::NONE: + { + // Callbacks not implemented + break; + } +#endif // UAGENT_CED_PROFILE + } + } + +private: + /** + * @brief Default constructor. + */ + CallbackFactory() = default; + + /** + * @brief CallbackFactory shall not be copy constructible. + */ + CallbackFactory( + const CallbackFactory &) = delete; + + CallbackFactory( + CallbackFactory &&) = delete; + + /** + * @brief CallbackFactory shall not be copy assignable. + */ + CallbackFactory& operator = ( + const CallbackFactory &) = delete; + + CallbackFactory& operator = ( + CallbackFactory &&) = delete; + +#ifdef UAGENT_FAST_PROFILE + /********************************************************************************************************************** + * FastCallbackFactory + **********************************************************************************************************************/ + class FastCallbackFactory + { + public: + /** + * @brief Default constructor. + */ + FastCallbackFactory() = default; + + /** + * @brief Default destructor. + */ + ~FastCallbackFactory() = default; + + /** + * @brief FastCallbackFactory shall not be copy constructible. + */ + FastCallbackFactory( + const FastCallbackFactory &) = delete; + + FastCallbackFactory( + FastCallbackFactory &&) = delete; + + /** + * @brief FastCallbackFactory shall not be copy assignable. + */ + FastCallbackFactory& operator = ( + const FastCallbackFactory &) = delete; + + FastCallbackFactory& operator = ( + FastCallbackFactory &&) = delete; + + /** + * @brief Adds a callback function to the database. + * @param callback_kind Enumerated element from class CallbackKind indicating which callback + * functions need to be executed. + * @param callback_function std::function rvalue variable implementing the callback method logic to be + * included in the callback database. + */ + template + void add_callback( + const CallbackKind& callback_kind, + std::function&& callback_function) + { + switch (callback_kind) + { + case CallbackKind::CREATE_PARTICIPANT: + { + on_create_participant_callbacks_.emplace_back(Callback(std::move(callback_function))); + break; + } + case CallbackKind::CREATE_DATAWRITER: + case CallbackKind::DELETE_DATAWRITER: + case CallbackKind::CREATE_DATAREADER: + case CallbackKind::DELETE_DATAREADER: + case CallbackKind::CREATE_REQUESTER: + case CallbackKind::DELETE_REQUESTER: + case CallbackKind::CREATE_REPLIER: + case CallbackKind::DELETE_REPLIER: + { + // Only implemented in template specialization + break; + } + case CallbackKind::DELETE_PARTICIPANT: + { + on_delete_participant_callbacks_.emplace_back(Callback(std::move(callback_function))); + break; + } + } + } + + /** + * @brief Executes a certain kind of callback functions. + * @param callback_kind Kind of callback functions to be executed. + */ + void execute_callbacks( + const CallbackKind callback_kind ...) const + { + va_list args; + va_start(args, callback_kind); + + switch (callback_kind) + { + case CallbackKind::CREATE_PARTICIPANT: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + for (const auto& on_create_participant_callback : on_create_participant_callbacks_) + { + on_create_participant_callback(participant); + } + break; + } + case CallbackKind::CREATE_DATAWRITER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Publisher* datawriter = va_arg(args, fastrtps::Publisher*); + for (const auto& on_create_datawriter_callback : on_create_datawriter_callbacks_) + { + on_create_datawriter_callback(participant, datawriter); + } + break; + } + case CallbackKind::CREATE_DATAREADER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Subscriber* datareader = va_arg(args, fastrtps::Subscriber*); + for (const auto& on_create_datareader_callback : on_create_datareader_callbacks_) + { + on_create_datareader_callback(participant, datareader); + } + break; + } + case CallbackKind::CREATE_REQUESTER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Publisher* req_datawriter = va_arg(args, fastrtps::Publisher*); + fastrtps::Subscriber* repl_datareader = va_arg(args, fastrtps::Subscriber*); + for (const auto& on_create_requester_callback : on_create_requester_callbacks_) + { + on_create_requester_callback(participant, req_datawriter, repl_datareader); + } + break; + } + case CallbackKind::CREATE_REPLIER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Publisher* repl_datawriter = va_arg(args, fastrtps::Publisher*); + fastrtps::Subscriber* req_datareader = va_arg(args, fastrtps::Subscriber*); + for (const auto& on_create_replier_callback : on_create_replier_callbacks_) + { + on_create_replier_callback(participant, repl_datawriter, req_datareader); + } + break; + } + case CallbackKind::DELETE_PARTICIPANT: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + for (const auto& on_delete_participant_callback : on_delete_participant_callbacks_) + { + on_delete_participant_callback(participant); + } + break; + } + case CallbackKind::DELETE_DATAWRITER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Publisher* datawriter = va_arg(args, fastrtps::Publisher*); + for (const auto& on_delete_datawriter_callback : on_delete_datawriter_callbacks_) + { + on_delete_datawriter_callback(participant, datawriter); + } + break; + } + case CallbackKind::DELETE_DATAREADER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Subscriber* datareader = va_arg(args, fastrtps::Subscriber*); + for (const auto& on_delete_datareader_callback : on_delete_datareader_callbacks_) + { + on_delete_datareader_callback(participant, datareader); + } + break; + } + case CallbackKind::DELETE_REQUESTER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Publisher* req_datawriter = va_arg(args, fastrtps::Publisher*); + fastrtps::Subscriber* repl_datareader = va_arg(args, fastrtps::Subscriber*); + for (const auto& on_delete_requester_callback : on_delete_requester_callbacks_) + { + on_delete_requester_callback(participant, req_datawriter, repl_datareader); + } + break; + } + case CallbackKind::DELETE_REPLIER: + { + fastrtps::Participant* participant = va_arg(args, fastrtps::Participant*); + fastrtps::Publisher* repl_datawriter = va_arg(args, fastrtps::Publisher*); + fastrtps::Subscriber* req_datareader = va_arg(args, fastrtps::Subscriber*); + for (const auto& on_delete_replier_callback : on_delete_replier_callbacks_) + { + on_delete_replier_callback(participant, repl_datawriter, req_datareader); + } + break; + } + } + va_end(args); + } + + private: + + using CreateParticipantCallback = Callback< + const fastrtps::Participant*>; + using DeleteParticipantCallback = Callback< + const fastrtps::Participant*>; + using CreateDataWriterCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*>; + + using DeleteDataWriterCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*>; + + using CreateDataReaderCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Subscriber*>; + + using DeleteDataReaderCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Subscriber*>; + + using CreateRequesterCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>; + + using DeleteRequesterCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>; + + using CreateReplierCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>; + + using DeleteReplierCallback = Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>; + + std::vector on_create_participant_callbacks_; + std::vector on_delete_participant_callbacks_; + std::vector on_create_datawriter_callbacks_; + std::vector on_delete_datawriter_callbacks_; + std::vector on_create_datareader_callbacks_; + std::vector on_delete_datareader_callbacks_; + std::vector on_create_requester_callbacks_; + std::vector on_delete_requester_callbacks_; + std::vector on_create_replier_callbacks_; + std::vector on_delete_replier_callbacks_; + }; + FastCallbackFactory fast_callback_factory_; + + /********************************************************************************************************************** + * FastDDSCallbackFactory + **********************************************************************************************************************/ + class FastDDSCallbackFactory + { + public: + /** + * @brief Default constructor. + */ + FastDDSCallbackFactory() = default; + + /** + * @brief Default destructor. + */ + ~FastDDSCallbackFactory() = default; + + /** + * @brief FastDDSCallbackFactory shall not be copy constructible. + */ + FastDDSCallbackFactory( + const FastDDSCallbackFactory &) = delete; + + FastDDSCallbackFactory( + FastDDSCallbackFactory &&) = delete; + + /** + * @brief FastDDSCallbackFactory shall not be copy assignable. + */ + FastDDSCallbackFactory& operator = ( + const FastDDSCallbackFactory &) = delete; + + FastDDSCallbackFactory& operator = ( + FastDDSCallbackFactory &&) = delete; + + /** + * @brief Adds a callback function to the database. + * @param callback_kind Enumerated element from class CallbackKind indicating which callback + * functions need to be executed. + * @param callback_function std::function rvalue variable implementing the callback method logic to be + * included in the callback database. + */ + template + void add_callback( + const CallbackKind& callback_kind, + std::function&& callback_function) + { + switch (callback_kind) + { + case CallbackKind::CREATE_PARTICIPANT: + { + on_create_participant_callbacks_.emplace_back(Callback(std::move(callback_function))); + break; + } + case CallbackKind::CREATE_DATAWRITER: + case CallbackKind::DELETE_DATAWRITER: + case CallbackKind::CREATE_DATAREADER: + case CallbackKind::DELETE_DATAREADER: + case CallbackKind::CREATE_REQUESTER: + case CallbackKind::DELETE_REQUESTER: + case CallbackKind::CREATE_REPLIER: + case CallbackKind::DELETE_REPLIER: + { + // Only implemented in template specialization + break; + } + case CallbackKind::DELETE_PARTICIPANT: + { + on_delete_participant_callbacks_.emplace_back(Callback(std::move(callback_function))); + break; + } + } + } + + /** + * @brief Executes a certain kind of callback functions. + * @param callback_kind Kind of callback functions to be executed. + */ + void execute_callbacks( + const CallbackKind callback_kind ...) const + { + va_list args; + va_start(args, callback_kind); + + switch (callback_kind) + { + case CallbackKind::CREATE_PARTICIPANT: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + for (const auto& on_create_participant_callback : on_create_participant_callbacks_) + { + on_create_participant_callback(participant); + } + break; + } + case CallbackKind::CREATE_DATAWRITER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataWriter* datawriter = va_arg(args, fastdds::dds::DataWriter*); + for (const auto& on_create_datawriter_callback : on_create_datawriter_callbacks_) + { + on_create_datawriter_callback(participant, datawriter); + } + break; + } + case CallbackKind::CREATE_DATAREADER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataReader* datareader = va_arg(args, fastdds::dds::DataReader*); + for (const auto& on_create_datareader_callback : on_create_datareader_callbacks_) + { + on_create_datareader_callback(participant, datareader); + } + break; + } + case CallbackKind::CREATE_REQUESTER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataWriter* req_datawriter = va_arg(args, fastdds::dds::DataWriter*); + fastdds::dds::DataReader* repl_datareader = va_arg(args, fastdds::dds::DataReader*); + for (const auto& on_create_requester_callback : on_create_requester_callbacks_) + { + on_create_requester_callback(participant, req_datawriter, repl_datareader); + } + break; + } + case CallbackKind::CREATE_REPLIER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataWriter* repl_datawriter = va_arg(args, fastdds::dds::DataWriter*); + fastdds::dds::DataReader* req_datareader = va_arg(args, fastdds::dds::DataReader*); + for (const auto& on_create_replier_callback : on_create_replier_callbacks_) + { + on_create_replier_callback(participant, repl_datawriter, req_datareader); + } + break; + } + case CallbackKind::DELETE_PARTICIPANT: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + for (const auto& on_delete_participant_callback : on_delete_participant_callbacks_) + { + on_delete_participant_callback(participant); + } + break; + } + case CallbackKind::DELETE_DATAWRITER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataWriter* datawriter = va_arg(args, fastdds::dds::DataWriter*); + for (const auto& on_delete_datawriter_callback : on_delete_datawriter_callbacks_) + { + on_delete_datawriter_callback(participant, datawriter); + } + break; + } + case CallbackKind::DELETE_DATAREADER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataReader* datareader = va_arg(args, fastdds::dds::DataReader*); + for (const auto& on_delete_datareader_callback : on_delete_datareader_callbacks_) + { + on_delete_datareader_callback(participant, datareader); + } + break; + } + case CallbackKind::DELETE_REQUESTER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataWriter* req_datawriter = va_arg(args, fastdds::dds::DataWriter*); + fastdds::dds::DataReader* repl_datareader = va_arg(args, fastdds::dds::DataReader*); + for (const auto& on_delete_requester_callback : on_delete_requester_callbacks_) + { + on_delete_requester_callback(participant, req_datawriter, repl_datareader); + } + break; + } + case CallbackKind::DELETE_REPLIER: + { + fastdds::dds::DomainParticipant* participant = va_arg(args, fastdds::dds::DomainParticipant*); + fastdds::dds::DataWriter* repl_datawriter = va_arg(args, fastdds::dds::DataWriter*); + fastdds::dds::DataReader* req_datareader = va_arg(args, fastdds::dds::DataReader*); + for (const auto& on_delete_replier_callback : on_delete_replier_callbacks_) + { + on_delete_replier_callback(participant, repl_datawriter, req_datareader); + } + break; + } + } + va_end(args); + } + + private: + + using CreateParticipantCallback = Callback< + const fastdds::dds::DomainParticipant*>; + using DeleteParticipantCallback = Callback< + const fastdds::dds::DomainParticipant*>; + using CreateDataWriterCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*>; + + using DeleteDataWriterCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*>; + + using CreateDataReaderCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataReader*>; + + using DeleteDataReaderCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataReader*>; + + using CreateRequesterCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>; + + using DeleteRequesterCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>; + + using CreateReplierCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>; + + using DeleteReplierCallback = Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>; + + std::vector on_create_participant_callbacks_; + std::vector on_delete_participant_callbacks_; + std::vector on_create_datawriter_callbacks_; + std::vector on_delete_datawriter_callbacks_; + std::vector on_create_datareader_callbacks_; + std::vector on_delete_datareader_callbacks_; + std::vector on_create_requester_callbacks_; + std::vector on_delete_requester_callbacks_; + std::vector on_create_replier_callbacks_; + std::vector on_delete_replier_callbacks_; + }; + + FastDDSCallbackFactory fast_dds_callback_factory_; +#endif // UAGENT_FAST_PROFILE + +#ifdef UAGENT_CED_PROFILE + /********************************************************************************************************************** + * CEDCallbackFactory (skeleton class, not implemented) + **********************************************************************************************************************/ + class CEDCallbackFactory + { + public: + /** + * @brief Default constructor. + */ + CEDCallbackFactory() = default; + + /** + * @brief Default destructor. + */ + ~CEDCallbackFactory() = default; + + /** + * @brief CEDCallbackFactory shall not be copy constructible. + */ + CEDCallbackFactory( + const CEDCallbackFactory &) = delete; + + CEDCallbackFactory( + CEDCallbackFactory &&) = delete; + + /** + * @brief CEDCallbackFactory shall not be copy assignable. + */ + CEDCallbackFactory& operator = ( + const CEDCallbackFactory &) = delete; + + CEDCallbackFactory& operator = ( + CEDCallbackFactory &&) = delete; + }; +#endif // UAGENT_CED_PROFILE +}; + +/********************************************************************************************************************** + * Template specializations for callback methods + **********************************************************************************************************************/ +#ifdef UAGENT_FAST_PROFILE +template <> +inline void CallbackFactory::FastCallbackFactory::add_callback< + const fastrtps::Participant*, + const fastrtps::Publisher*>( + const CallbackKind& callback_kind, + std::function&& callback_function) +{ + switch (callback_kind) + { + case CallbackKind::CREATE_DATAWRITER: + { + on_create_datawriter_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_DATAWRITER: + { + on_delete_datawriter_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*>(std::move(callback_function))); + break; + } + default: + { + break; + } + } +} + +template <> +inline void CallbackFactory::FastCallbackFactory::add_callback< + const fastrtps::Participant*, + const fastrtps::Subscriber*>( + const CallbackKind& callback_kind, + std::function&& callback_function) +{ + switch (callback_kind) + { + case CallbackKind::CREATE_DATAREADER: + { + on_create_datareader_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Subscriber*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_DATAREADER: + { + on_delete_datareader_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Subscriber*>(std::move(callback_function))); + break; + } + default: + { + break; + } + } +} + +template <> +inline void CallbackFactory::FastCallbackFactory::add_callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>( + const CallbackKind& callback_kind, + std::function&& callback_function) +{ + switch (callback_kind) + { + case CallbackKind::CREATE_REQUESTER: + { + on_create_requester_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>(std::move(callback_function))); + break; + } + case CallbackKind::CREATE_REPLIER: + { + on_create_replier_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_REQUESTER: + { + on_delete_requester_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_REPLIER: + { + on_delete_replier_callbacks_.emplace_back(Callback< + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*>(std::move(callback_function))); + break; + } + default: + { + break; + } + } +} + +template <> +inline void CallbackFactory::FastDDSCallbackFactory::add_callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*>( + const CallbackKind& callback_kind, + std::function&& callback_function) +{ + switch (callback_kind) + { + case CallbackKind::CREATE_DATAWRITER: + { + on_create_datawriter_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_DATAWRITER: + { + on_delete_datawriter_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*>(std::move(callback_function))); + break; + } + default: + { + break; + } + } +} + +template <> +inline void CallbackFactory::FastDDSCallbackFactory::add_callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataReader*>( + const CallbackKind& callback_kind, + std::function&& callback_function) +{ + switch (callback_kind) + { + case CallbackKind::CREATE_DATAREADER: + { + on_create_datareader_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataReader*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_DATAREADER: + { + on_delete_datareader_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataReader*>(std::move(callback_function))); + break; + } + default: + { + break; + } + } +} + +template <> +inline void CallbackFactory::FastDDSCallbackFactory::add_callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>( + const CallbackKind& callback_kind, + std::function&& callback_function) +{ + switch (callback_kind) + { + case CallbackKind::CREATE_REQUESTER: + { + on_create_requester_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>(std::move(callback_function))); + break; + } + case CallbackKind::CREATE_REPLIER: + { + on_create_replier_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_REQUESTER: + { + on_delete_requester_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>(std::move(callback_function))); + break; + } + case CallbackKind::DELETE_REPLIER: + { + on_delete_replier_callbacks_.emplace_back(Callback< + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*>(std::move(callback_function))); + break; + } + default: + { + break; + } + } +} +#endif // UAGENT_FAST_PROFILE + +#define CALLBACK_FACTORY_ADD_CALLBACK(MIDDLEWARE, CB_FACTORY, ...) \ +template <> \ +inline void CallbackFactory::add_callback<__VA_ARGS__>( \ + const Middleware::Kind& middleware_kind, \ + const CallbackKind& callback_kind, \ + std::function&& callback_function) \ +{ \ + switch (middleware_kind) \ + { \ + case MIDDLEWARE: \ + { \ + CB_FACTORY.add_callback(callback_kind, std::move(callback_function)); \ + break; \ + } \ + default: \ + { \ + break; \ + } \ + } \ +} + +#ifdef UAGENT_FAST_PROFILE +#define CALLBACK_FACTORY_ADD_FASTRTPS_CALLBACK(...) \ + CALLBACK_FACTORY_ADD_CALLBACK(Middleware::Kind::FASTRTPS, fast_callback_factory_, __VA_ARGS__) + +CALLBACK_FACTORY_ADD_FASTRTPS_CALLBACK( + const fastrtps::Participant*) + +CALLBACK_FACTORY_ADD_FASTRTPS_CALLBACK( + const fastrtps::Participant*, + const fastrtps::Publisher*) + +CALLBACK_FACTORY_ADD_FASTRTPS_CALLBACK( + const fastrtps::Participant*, + const fastrtps::Subscriber*) + +CALLBACK_FACTORY_ADD_FASTRTPS_CALLBACK( + const fastrtps::Participant*, + const fastrtps::Publisher*, + const fastrtps::Subscriber*) + +#define CALLBACK_FACTORY_ADD_FASTDDS_CALLBACK(...) \ + CALLBACK_FACTORY_ADD_CALLBACK(Middleware::Kind::FASTDDS, fast_dds_callback_factory_, __VA_ARGS__) + +CALLBACK_FACTORY_ADD_FASTDDS_CALLBACK( + const fastdds::dds::DomainParticipant*) + +CALLBACK_FACTORY_ADD_FASTDDS_CALLBACK( + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*) + +CALLBACK_FACTORY_ADD_FASTDDS_CALLBACK( + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataReader*) + +CALLBACK_FACTORY_ADD_FASTDDS_CALLBACK( + const fastdds::dds::DomainParticipant*, + const fastdds::dds::DataWriter*, + const fastdds::dds::DataReader*) +#endif // UAGENT_FAST_PROFILE + +} // namespace middleware +} // namespace uxr +} // namespace eprosima + +#endif // UXR__AGENT__MIDDLEWARE__UTILS__CALLBACKS_HPP_ \ No newline at end of file diff --git a/include/uxr/agent/p2p/InternalClient.hpp b/include/uxr/agent/p2p/InternalClient.hpp index 275551d48..2a5fc1abd 100644 --- a/include/uxr/agent/p2p/InternalClient.hpp +++ b/include/uxr/agent/p2p/InternalClient.hpp @@ -76,7 +76,6 @@ class InternalClient /* Transport. */ uxrUDPTransport transport_; - uxrUDPPlatform platform_; /* Client. */ uint32_t remote_client_key_; diff --git a/include/uxr/agent/processor/Processor.hpp b/include/uxr/agent/processor/Processor.hpp index f9f9676f1..bf7c7f1e6 100644 --- a/include/uxr/agent/processor/Processor.hpp +++ b/include/uxr/agent/processor/Processor.hpp @@ -62,6 +62,10 @@ class Processor void process_input_packet( InputPacket&& input_packet); + bool process_get_info_packet( + InputPacket&& input_packet, + OutputPacket& output_packet) const; + bool process_get_info_packet( InputPacket&& input_packet, std::vector& address, diff --git a/include/uxr/agent/transport/Server.hpp b/include/uxr/agent/transport/Server.hpp index a4f8924c5..53af654af 100644 --- a/include/uxr/agent/transport/Server.hpp +++ b/include/uxr/agent/transport/Server.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -55,7 +54,7 @@ class Server : public Agent, public SessionManager private: void push_output_packet( - OutputPacket output_packet); + OutputPacket&& output_packet); virtual bool init() = 0; diff --git a/include/uxr/agent/transport/custom/CustomAgent.hpp b/include/uxr/agent/transport/custom/CustomAgent.hpp new file mode 100644 index 000000000..e145d75d6 --- /dev/null +++ b/include/uxr/agent/transport/custom/CustomAgent.hpp @@ -0,0 +1,199 @@ +// Copyright 2021-present Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UXR_AGENT_TRANSPORT_CUSTOM_AGENT_HPP_ +#define UXR_AGENT_TRANSPORT_CUSTOM_AGENT_HPP_ + +#include +#include +#include + +#include +#include + +#ifdef _WIN32 +#include +typedef SSIZE_T ssize_t; +#endif + +namespace eprosima { +namespace uxr { + +/** + * @brief This class allows final users to easily implement a custom + * Micro XRCE-DDS Agent, in terms of transport behaviour. + * To do so, several methods must be implemented, in order to init + * an agent instance, close it, and send and receive messages using + * the desired transport mechanism. + */ + +class CustomAgent : public Server +{ +public: + /** + * @brief Init function signature, to be implemented by final users. + * return true if successful transport initialization; false otherwise. + */ + using InitFunction = std::function; + + /** + * @brief Fini function signature, to be implemented by final users. + * return true if successful transport shutdown; false otherwise. + */ + using FiniFunction = std::function; + + /** + * @brief Receive message function signature, to be implemented by final users. + * @param source_endpoint User-defined, it should be filled accordingly + * with the source metadata acquired from receiving a new packet/ + * byte stream through the middleware. + * @param buffer Pointer to octec buffer used to receive the information. + * @param buffer_length Reception buffer size. + * @param timeout Connection timeout for receiving a new message. + * @param transport_rc Transport return code, to be filled by the user. + * @return ssize_t Number of received bytes. + */ + using RecvMsgFunction = std::function; + /** + * @brief Send message function signature, to be implemented by final users. + * @param destination_endpoint Allows to retrieve the required endpoint + * information to send the message back to the client. + * @param buffer Holds the message to be sent back to the client. + * @param message_length Number of bytes to be sent. + * @param transport_rc Transport return code, to be filled by the user. + * @return ssize_t Number of sent bytes. + */ + using SendMsgFunction = std::function; + + /** + * @brief Constructor. + * @param name Name of the middleware to be implemented by this CustomAgent. + * @param middleware_kind The middleware selected to represent the XRCE entities + * in the DDS world (FastDDS, FastRTPS, CED...) + * @param framing Whether this agent transport shall use framing or not. + * @param init_function Custom user-defined function, called during initialization. + * @param fini_function Custom user-defined function, called upon agent's destruction. + * @param recv_msg_function Custom user-defined function, called when receiving some data. + * @param send_msg_function Custom user-defined function, called when sending some information. + */ + UXR_AGENT_EXPORT CustomAgent( + const std::string& name, + CustomEndPoint* endpoint, + Middleware::Kind middleware_kind, + bool framing, + InitFunction& init_function, + FiniFunction& fini_function, + SendMsgFunction& send_msg_function, + RecvMsgFunction& recv_msg_function); + + /** + * @brief Destructor. + */ + UXR_AGENT_EXPORT ~CustomAgent() final; + +private: + /** + * @brief Override virtual Server operations. + */ + bool init() final; + + bool fini() final; + +#ifdef UAGENT_DISCOVERY_PROFILE + inline bool init_discovery( + uint16_t /*discovery_port*/) final + { + return false; + } + + inline bool fini_discovery() final + { + return true; + } +#endif // UAGENT_DISCOVERY_PROFILE + +#ifdef UAGENT_P2P_PROFILE + inline bool init_p2p( + uint16_t /*p2p_port*/) final + { + return false; + } + + inline bool fini_p2p() final + { + return true; + } +#endif // UAGENT_P2P_PROFILE + + bool recv_message( + InputPacket& input_packet, + int timeout, + TransportRc& transport_rc) final; + + bool send_message( + OutputPacket output_packet, + TransportRc& transport_rc) final; + + bool handle_error( + TransportRc transport_rc) final; + + /** + * @brief Internal buffer used for receiving messages. + */ + uint8_t buffer_[SERVER_BUFFER_SIZE]; + + /** + * @brief Custom agent middleware's name. + */ + const std::string name_; + + /** + * @brief Pointers to this custom agent's endpoint definition. + * They are used for receive and send operations, respectively. + */ + CustomEndPoint* recv_endpoint_; + CustomEndPoint* send_endpoint_; + + /** + * @brief Reference to user-defined operations for the custom agent server. + */ + InitFunction& custom_init_func_; + FiniFunction& custom_fini_func_; + SendMsgFunction& custom_send_msg_func_; + RecvMsgFunction& custom_recv_msg_func_; + + /** + * @brief Indicates the usage or non-usage of framing for R/W operations. + */ + bool framing_; + + /** + * @brief Holds the framing logics, if framing is used. + */ + FramingIO framing_io_; +}; + +} // namespace uxr +} // namespace eprosima + +#endif // UXR_AGENT_TRANSPORT_CUSTOM_AGENT_HPP_ \ No newline at end of file diff --git a/include/uxr/agent/transport/endpoint/CustomEndPoint.hpp b/include/uxr/agent/transport/endpoint/CustomEndPoint.hpp new file mode 100644 index 000000000..96483a4d1 --- /dev/null +++ b/include/uxr/agent/transport/endpoint/CustomEndPoint.hpp @@ -0,0 +1,507 @@ +// Copyright 2021 - present Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UXR_AGENT_TRANSPORT_ENDPOINT_CUSTOM_ENDPOINT_HPP_ +#define UXR_AGENT_TRANSPORT_ENDPOINT_CUSTOM_ENDPOINT_HPP_ + +#include +#include +#include + +namespace eprosima { +namespace uxr { + +/** + * @brief Class to allow user to easily create an endpoint for their Custom Agent + * implementation, if applicable. + * A certain set of values are permitted, including unsigned integers and + * strings, which usually are more than enough to characterize an endpoint. + */ +class CustomEndPoint +{ +private: + /** + * @brief Enum class which holds all the available type kinds for an endpoint member. + */ + enum class MemberKind + { + UINT8, + UINT16, + UINT32, + UINT64, +#ifdef __SIZEOF_UINT128__ + UINT128, +#endif // __SIZEOF_UINT128__ + STRING + }; + + /** + * @brief Struct defining a member in terms of its kind and an opaque pointer, + * containing the member data itself. + */ + typedef struct Member + { + MemberKind kind; + std::shared_ptr data; + } Member; + + /** + * @brief Exception to be launched when trying to insert two elements + * with the same key on the CustomEndPoint map. + */ + class SameKeyException : public std::exception + { + public: + SameKeyException( + const char* file, + int line, + const char* func, + const std::string& key) + { + std::stringstream what; + what << file << ":" << line << ":" << func + << ": Cannot add two members with the key '" + << key << "'."; + message_ = what.str(); + } + + const char* what() const noexcept + { + return message_.c_str(); + } + + private: + std::string message_; + }; + + class NoExistingMemberException : public std::exception + { + public: + NoExistingMemberException( + const char* file, + int line, + const char* func, + const std::string& key) + { + std::stringstream what; + what << file << ":" << line << ":" << func + << ": Member '" + << key << "' does not exist."; + message_ = what.str(); + } + + const char* what() const noexcept + { + return message_.c_str(); + } + + private: + std::string message_; + }; + + class EmptyMemberException : public std::exception + { + public: + EmptyMemberException( + const std::string& member_name) + { + std::stringstream what; + what << ": Member '" + << member_name + << "' is empty."; + message_ = what.str(); + } + + const char* what() const noexcept + { + return message_.c_str(); + } + + private: + std::string message_; + }; +public: + /** + * @brief Default constructor. + */ + CustomEndPoint() = default; + + /** + * @brief Default destructor. + */ + ~CustomEndPoint() = default; + + /** + * @brief Adds a member to the member list. + * The member must not already exist in the EndPoint. + * @param name The member name, + * @param kind The member kind. + * @throw SameKeyException if member already exists. + * @return true if the insert was successful, or false otherwise. + */ + bool add_member( + const std::string& name, + const MemberKind& kind) + { + if (members_.end() != members_.find(name)) + { + throw SameKeyException(__FILE__, __LINE__, __FUNCTION__, name); + } + + Member member; + member.kind = kind; + member.data.reset(); + + auto res = members_.emplace(name, std::move(member)); + return res.second; + } + + /** + * @brief Allows to add a member without specifying the MemberKind. + * This template must be specialized for every supported type. + * @param name The new member name. + * @returns true if the insert was successful, false otherwise. + */ + template + bool add_member( + const std::string& name); + + /** + * @brief Helper method to reset all the contained data within members. + */ + void reset() + { + for (auto& member : members_) + { + member.second.data.reset(); + } + } + + /** + * @brief Allows setting a value for a given member. + * @param name Key value to be searched in the members map. + * @param value A const reference to the value to be set. + * @throw NoExistingMemberException if trying to set a value not registered in the map. + */ + template + void set_member_value( + const std::string& name, + const T& value) + { + if (members_.end() == members_.find(name)) + { + throw NoExistingMemberException(__FILE__, __LINE__, __FUNCTION__, name); + } + else + { + members_.at(name).data.reset(new T(value)); + } + } + + /** + * @brief Allows setting a value for a given member. + * @param name Key value to be searched in the members map. + * @param value A movable reference to the value to be set. + * @throw NoExistingMemberException if trying to set a value not registered in the map. + */ + template + void set_member_value( + const std::string& name, + T&& value) + { + if (members_.end() == members_.find(name)) + { + throw NoExistingMemberException(__FILE__, __LINE__, __FUNCTION__, name); + } + else + { + members_.at(name).data.reset(new T(std::move(value))); + } + } + + /** + * + */ + void check_non_empty_members() + { + for (const auto& member : members_) + { + if (nullptr == member.second.data.get()) + { + throw EmptyMemberException(member.first); + } + } + } + + /** + * @brief Helper static method that allows to compare two members of the same type. + * @param first First member to be compared. + * @param second Second member to be compared against the first. + * @return 0 if the members are equal, -1 if first < second, 1 if second > first. + */ + template + static int8_t less_than_members( + const Member& first, + const Member& second) + { + T first_data = *static_cast(first.data.get()); + T second_data = *static_cast(second.data.get()); + + if (first_data == second_data) + { + return 0; + } + else if (first_data < second_data) + { + return -1; + } + else + { + return 1; + } + } + + /** + * @brief Operator < overload. + * @param other The CustomEndPoint to be checked against this one. + * @return True if this < other, false otherwise. + */ + bool operator <( + const CustomEndPoint& other) const + { + for (const auto& member : members_) + { + const std::string& member_name = member.first; + + auto other_member = other.members_.at(member_name); + switch (member.second.kind) + { + case MemberKind::UINT8: + { + int8_t res = less_than_members(member.second, other_member); + if (0 == res) + { + break; + } + return (res < 0) ? true : false; + } + case MemberKind::UINT16: + { + int8_t res = less_than_members(member.second, other_member); + if (0 == res) + { + break; + } + return (res < 0) ? true : false; + } + case MemberKind::UINT32: + { + int8_t res = less_than_members(member.second, other_member); + if (0 == res) + { + break; + } + return (res < 0) ? true : false; + } + case MemberKind::UINT64: + { + int8_t res = less_than_members(member.second, other_member); + if (0 == res) + { + break; + } + return (res < 0) ? true : false; + } +#ifdef __SIZEOF_UINT128__ + case MemberKind::UINT128: + { + int8_t res = less_than_members(member.second, other_member); + if (0 == res) + { + break; + } + return (res < 0) ? true : false; + } +#endif // __SIZEOF_UINT128__ + case MemberKind::STRING: + { + int8_t res = less_than_members(member.second, other_member); + if (0 == res) + { + break; + } + return (res < 0) ? true : false; + } + } + } + return false; + } + + /** + * @brief Operator << overload for ostream operations. + * @param os The ostream object to which the output is sent. + * @param endpoint The CustomEndPoint to be sent to the ostream. + * @return Reference to the modified ostream. + */ + friend std::ostream& operator <<( + std::ostream& os, + const CustomEndPoint& endpoint) + { + for (const auto& member : endpoint.members_) + { + os << member.first << ": "; + + if (nullptr == member.second.data.get()) + { + os << ""; + } + else + { + switch (member.second.kind) + { + case MemberKind::UINT8: + { + os << *static_cast(member.second.data.get()); + break; + } + case MemberKind::UINT16: + { + os << *static_cast(member.second.data.get()); + break; + } + case MemberKind::UINT32: + { + os << *static_cast(member.second.data.get()); + break; + } + case MemberKind::UINT64: + { + os << *static_cast(member.second.data.get()); + break; + } +#ifdef __SIZEOF_UINT128__ + case MemberKind::UINT128: + { + os << *static_cast(member.second.data.get()); + break; + } +#endif // __SIZEOF_UINT128__ + case MemberKind::STRING: + { + os << "'" + << *static_cast(member.second.data.get()) + << "'"; + break; + } + } + } + + if (&(*endpoint.members_.rbegin()) != &member) + { + os << ", "; + } + } + + return os; + } + + /** + * @brief Get a member's value, given its key. + * @param key The member's key. + * @throw NoExistingMemberException if the member is not found. + * @return Const reference to the requested value. + */ + template + const T& get_member( + const char* key) const + { + if (members_.end() == members_.find(key)) + { + throw NoExistingMemberException(__FILE__, __LINE__, __FUNCTION__, key); + } + else + { + return *static_cast(members_.at(key).data.get()); + } + } + + /** + * @brief Get a member's value, given its key. + * @param key The member's key. + * @throw NoExistingMemberException if the member is not found. + * @return Const reference to the requested value. + */ + template + const T& get_member( + const std::string& key) const + { + return this->get_member(key.c_str()); + } + +private: + std::map members_; +}; + +/** + * @brief CustomEndPoint::add_member template method specializations, + * for each of the available MemberKind types. + */ +template <> +inline bool CustomEndPoint::add_member( + const std::string& name) +{ + return add_member(name, MemberKind::UINT8); +} + +template <> +inline bool CustomEndPoint::add_member( + const std::string& name) +{ + return add_member(name, MemberKind::UINT16); +} + +template <> +inline bool CustomEndPoint::add_member( + const std::string& name) +{ + return add_member(name, MemberKind::UINT32); +} + +template <> +inline bool CustomEndPoint::add_member( + const std::string& name) +{ + return add_member(name, MemberKind::UINT64); +} + +#ifdef __SIZEOF_UINT128__ +template <> +inline bool CustomEndPoint::add_member( + const std::string& name) +{ + return add_member(name, MemberKind::UINT128); +} +#endif // __SIZEOF_UINT128__ + +template <> +inline bool CustomEndPoint::add_member( + const std::string& name) +{ + return add_member(name, MemberKind::STRING); +} + +} // namespace uxr +} // namespace eprosima + +#endif // UXR_AGENT_TRANSPORT_ENDPOINT_IPV4_ENDPOINT_HPP_ diff --git a/include/uxr/agent/transport/endpoint/EndPoint.hpp b/include/uxr/agent/transport/endpoint/EndPoint.hpp deleted file mode 100644 index 13a1d454f..000000000 --- a/include/uxr/agent/transport/endpoint/EndPoint.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UXR_AGENT_TRANSPORT_ENDPOINT_ENDPOINT_HPP_ -#define UXR_AGENT_TRANSPORT_ENDPOINT_ENDPOINT_HPP_ - -#include - -namespace eprosima { -namespace uxr { - -class EndPoint -{ -public: - EndPoint() = default; - virtual ~EndPoint() = default; - - virtual std::ostream& print(std::ostream& os) const = 0; -}; - -inline std::ostream& operator<<(std::ostream& os, const EndPoint& endpoint) -{ - return endpoint.print(os); -} - -} // namespace uxr -} // namespace eprosima - -#endif // UXR_AGENT_TRANSPORT_ENDPOINT_ENDPOINT_HPP_ diff --git a/include/uxr/agent/transport/endpoint/IPv4EndPoint.hpp b/include/uxr/agent/transport/endpoint/IPv4EndPoint.hpp index 5d4405f96..77447d927 100644 --- a/include/uxr/agent/transport/endpoint/IPv4EndPoint.hpp +++ b/include/uxr/agent/transport/endpoint/IPv4EndPoint.hpp @@ -15,8 +15,6 @@ #ifndef UXR_AGENT_TRANSPORT_ENDPOINT_IPV4_ENDPOINT_HPP_ #define UXR_AGENT_TRANSPORT_ENDPOINT_IPV4_ENDPOINT_HPP_ -#include - #include #include @@ -44,10 +42,10 @@ class IPv4EndPoint friend std::ostream& operator<<(std::ostream& os, const IPv4EndPoint& endpoint) { - os << int(uint8_t(endpoint.addr_)) << "." - << int(uint8_t(endpoint.addr_ >> 8)) << "." - << int(uint8_t(endpoint.addr_ >> 16)) << "." - << int(uint8_t(endpoint.addr_ >> 24)) << ":" + os << static_cast(static_cast(endpoint.addr_)) << "." + << static_cast(static_cast(endpoint.addr_ >> 8)) << "." + << static_cast(static_cast(endpoint.addr_ >> 16)) << "." + << static_cast(static_cast(endpoint.addr_ >> 24)) << ":" << endpoint.get_port(); return os; } diff --git a/include/uxr/agent/transport/endpoint/IPv6EndPoint.hpp b/include/uxr/agent/transport/endpoint/IPv6EndPoint.hpp index a3048bcba..9e1dad37a 100644 --- a/include/uxr/agent/transport/endpoint/IPv6EndPoint.hpp +++ b/include/uxr/agent/transport/endpoint/IPv6EndPoint.hpp @@ -15,8 +15,6 @@ #ifndef UXR_AGENT_TRANSPORT_ENDPOINT_IPV6_ENDPOINT_HPP_ #define UXR_AGENT_TRANSPORT_ENDPOINT_IPV6_ENDPOINT_HPP_ -#include - #include #include #include diff --git a/include/uxr/agent/transport/endpoint/SerialEndPoint.hpp b/include/uxr/agent/transport/endpoint/SerialEndPoint.hpp index 93b89dbbf..d14420c60 100644 --- a/include/uxr/agent/transport/endpoint/SerialEndPoint.hpp +++ b/include/uxr/agent/transport/endpoint/SerialEndPoint.hpp @@ -15,14 +15,12 @@ #ifndef _UXR_AGENT_TRANSPORT_SERIAL_ENDPOINT_HPP_ #define _UXR_AGENT_TRANSPORT_SERIAL_ENDPOINT_HPP_ -#include - #include namespace eprosima { namespace uxr { -class SerialEndPoint : public EndPoint +class SerialEndPoint { public: SerialEndPoint() = default; @@ -39,9 +37,10 @@ class SerialEndPoint : public EndPoint return (addr_ < other.addr_); } - std::ostream& print(std::ostream& os) const final + friend std::ostream& operator<<(std::ostream& os, const SerialEndPoint& endpoint) { - return os << int(addr_); + os << static_cast(endpoint.addr_); + return os; } uint8_t get_addr() const { return addr_; } diff --git a/include/uxr/agent/transport/serial/SerialAgentLinux.hpp b/include/uxr/agent/transport/serial/SerialAgentLinux.hpp index 478889bc4..a296a4d6c 100644 --- a/include/uxr/agent/transport/serial/SerialAgentLinux.hpp +++ b/include/uxr/agent/transport/serial/SerialAgentLinux.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -61,12 +61,12 @@ class SerialAgent : public Server OutputPacket output_packet, TransportRc& transport_rc) final; - size_t write_data( + ssize_t write_data( uint8_t* buf, size_t len, TransportRc& transport_rc); - size_t read_data( + ssize_t read_data( uint8_t* buf, size_t len, int timeout, @@ -76,7 +76,7 @@ class SerialAgent : public Server const uint8_t addr_; struct pollfd poll_fd_; uint8_t buffer_[SERVER_BUFFER_SIZE]; - SerialIO serial_io_; + FramingIO framing_io_; }; } // namespace uxr diff --git a/include/uxr/agent/transport/serial/SerialProtocol.hpp b/include/uxr/agent/transport/serial/SerialProtocol.hpp deleted file mode 100644 index fbfe4ad8b..000000000 --- a/include/uxr/agent/transport/serial/SerialProtocol.hpp +++ /dev/null @@ -1,532 +0,0 @@ -// Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UXR_AGENT_TRANSPORT_SERIAL_PROTOCOL_HPP_ -#define UXR_AGENT_TRANSPORT_SERIAL_PROTOCOL_HPP_ - -#include - -#include -#include -#include - -namespace eprosima { -namespace uxr { - -typedef const std::function WriteCallback; -typedef const std::function ReadCallback; - -class SerialIO -{ - static constexpr uint16_t crc16_table[256] = { - 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, - 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, - 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, - 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, - 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, - 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, - 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, - 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, - 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, - 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, - 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, - 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, - 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, - 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, - 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, - 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, - 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, - 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, - 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, - 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, - 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, - 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, - 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, - 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, - 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, - 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, - 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, - 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, - 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, - 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, - 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, - 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 - }; - static constexpr uint8_t framing_begin_flag = 0x7E; - static constexpr uint8_t framing_esc_flag = 0x7D; - static constexpr uint8_t framing_xor_flag = 0x20; - - enum class InputState : uint8_t - { - UXR_SERIAL_UNINITIALIZED, - UXR_SERIAL_READING_SRC_ADDR, - UXR_SERIAL_READING_DST_ADDR, - UXR_SERIAL_READING_LEN_LSB, - UXR_SERIAL_READING_LEN_MSB, - UXR_SERIAL_READING_PAYLOAD, - UXR_SERIAL_READING_CRC_LSB, - UXR_SERIAL_READING_CRC_MSB, - - }; - -public: - SerialIO( - uint8_t local_addr, - WriteCallback write_callback, - ReadCallback read_callback); - - size_t write_msg( - const uint8_t* buf, - size_t len, - uint8_t remote_addr, - TransportRc& transport_rc); - - size_t read_msg( - uint8_t* buf, - size_t len, - uint8_t& remote_addr, - int timeout, - TransportRc& transport_rc); - -private: - static void update_crc( - uint16_t& crc, - const uint8_t data); - - bool get_next_octet( - uint8_t& octet); - - bool add_next_octet( - uint8_t octet); - -private: - InputState state_; - uint8_t local_addr_; - uint8_t read_buffer_[42]; - uint8_t read_buffer_head_; - uint8_t read_buffer_tail_; - uint8_t remote_addr_; - uint16_t msg_len_; - uint16_t msg_pos_; - uint16_t msg_crc_; - uint16_t cmp_crc_; - uint8_t write_buffer_[42]; - uint8_t write_buffer_pos_; - WriteCallback write_callback_; - ReadCallback read_callback_; -}; - -inline SerialIO::SerialIO( - uint8_t local_addr, - WriteCallback write_callback, - ReadCallback read_callback) - : state_{InputState::UXR_SERIAL_UNINITIALIZED} - , local_addr_{local_addr} - , write_callback_{write_callback} - , read_callback_{read_callback} -{} - -inline void SerialIO::update_crc( - uint16_t& crc, - const uint8_t data) -{ - crc = (crc >> 8) ^ SerialIO::crc16_table[(crc ^ data) & 0xFF]; -} - -inline bool SerialIO::get_next_octet( - uint8_t& octet) -{ - bool rv = false; - octet = 0; - if (read_buffer_head_ != read_buffer_tail_) - { - if (framing_esc_flag != read_buffer_[read_buffer_tail_]) - { - octet = read_buffer_[read_buffer_tail_]; - read_buffer_tail_ = uint8_t(size_t(read_buffer_tail_ + 1) % sizeof(read_buffer_)); - rv = (framing_begin_flag != octet); - } - else - { - uint8_t temp_tail = uint8_t(size_t(read_buffer_tail_ + 1) % sizeof(read_buffer_)); - if (temp_tail != read_buffer_head_) - { - octet = read_buffer_[temp_tail]; - read_buffer_tail_ = uint8_t(size_t(read_buffer_tail_ + 2) % sizeof(read_buffer_)); - if (framing_begin_flag != octet) - { - octet ^= framing_xor_flag; - rv = true; - } - } - } - } - return rv; -} - -inline bool SerialIO::add_next_octet( - uint8_t octet) -{ - bool rv = false; - - if (framing_begin_flag == octet || framing_esc_flag == octet) - { - if (uint8_t(write_buffer_pos_ + 1) < sizeof(write_buffer_)) - { - write_buffer_[write_buffer_pos_] = framing_esc_flag; - write_buffer_[write_buffer_pos_ + 1] = octet ^ framing_xor_flag; - write_buffer_pos_ += 2; - rv = true; - } - } - else - { - if (write_buffer_pos_ < sizeof(write_buffer_)) - { - write_buffer_[write_buffer_pos_] = octet; - ++write_buffer_pos_; - rv = true; - } - } - - return rv; -} - -inline size_t SerialIO::write_msg( - const uint8_t *buf, - size_t len, - uint8_t remote_addr, - TransportRc& transport_rc) -{ - /* Buffer being flag. */ - write_buffer_[0] = framing_begin_flag; - write_buffer_pos_ = 1; - - /* Buffer header. */ - add_next_octet(local_addr_); - add_next_octet(remote_addr); - add_next_octet(uint8_t(len & 0xFF)); - add_next_octet(uint8_t(len >> 8)); - - /* Write payload. */ - uint8_t octet = 0; - uint16_t written_len = 0; - uint16_t crc = 0; - bool cond = true; - while (written_len < len && cond) - { - octet = *(buf + written_len); - if (add_next_octet(octet)) - { - update_crc(crc, octet); - ++written_len; - } - else - { - size_t bytes_written = write_callback_(write_buffer_, write_buffer_pos_, transport_rc); - if (0 < bytes_written) - { - cond = true; - write_buffer_pos_ = uint8_t(write_buffer_pos_ - bytes_written); - } - else - { - cond = false; - } - } - } - - /* Write CRC. */ - uint8_t tmp_crc[2]; - tmp_crc[0] = uint8_t(crc & 0xFF); - tmp_crc[1] = uint8_t(crc >> 8); - written_len = 0; - while (written_len < sizeof(tmp_crc) && cond) - { - octet = *(tmp_crc + written_len); - if (add_next_octet(octet)) - { - update_crc(crc, octet); - ++written_len; - } - else - { - size_t bytes_written = write_callback_(write_buffer_, write_buffer_pos_, transport_rc); - if (0 < bytes_written) - { - cond = true; - write_buffer_pos_ -= (uint8_t)bytes_written; - } - else - { - cond = false; - } - } - } - - /* Flus write buffer. */ - if (cond && (0 < write_buffer_pos_)) - { - size_t bytes_written = write_callback_(write_buffer_, write_buffer_pos_, transport_rc); - if (0 < bytes_written) - { - cond = true; - write_buffer_pos_ -= (uint8_t)bytes_written; - } - else - { - cond = false; - } - } - - return cond ? uint16_t(len) : 0; -} - -inline size_t SerialIO::read_msg( - uint8_t* buf, - size_t len, - uint8_t& remote_addr, - int timeout, - TransportRc& transport_rc) -{ - size_t rv = 0; - - /* Compute read-buffer available size. */ - uint8_t av_len[2] = {0, 0}; - if (read_buffer_head_ == read_buffer_tail_) - { - read_buffer_head_ = 0; - read_buffer_tail_ = 0; - av_len[0] = sizeof(read_buffer_) - 1; - } - else if (read_buffer_head_ > read_buffer_tail_) - { - if (0 < read_buffer_tail_) - { - av_len[0] = uint8_t(sizeof(read_buffer_) - read_buffer_head_); - av_len[1] = uint8_t(read_buffer_tail_ - 1); - } - else - { - av_len[0] = uint8_t(sizeof(read_buffer_) - read_buffer_head_ - 1); - } - } - else - { - av_len[0] = uint8_t(read_buffer_tail_ - read_buffer_head_ - 1); - } - - /* Read from serial. */ - size_t bytes_read[2] = {0}; - if (0 < av_len[0]) - { - bytes_read[0] = read_callback_(&read_buffer_[read_buffer_head_], av_len[0], timeout, transport_rc); - read_buffer_head_ = uint8_t(size_t(read_buffer_head_ + bytes_read[0]) % sizeof(read_buffer_)); - if (0 < bytes_read[0]) - { - if ((bytes_read[0] == av_len[0]) && (0 < av_len[1])) - { - bytes_read[1] = read_callback_(&read_buffer_[read_buffer_head_], av_len[1], 0, transport_rc); - read_buffer_head_ = uint8_t(size_t(read_buffer_head_ + bytes_read[1]) % sizeof(read_buffer_)); - } - } - } - - if (0 < (bytes_read[0] + bytes_read[1]) || (read_buffer_tail_ != read_buffer_head_)) - { - /* State Machine. */ - bool exit_cond = false; - while (!exit_cond) - { - uint8_t octet = 0; - switch (state_) - { - case InputState::UXR_SERIAL_UNINITIALIZED: - { - octet = 0; - while ((framing_begin_flag != octet) && (read_buffer_head_ != read_buffer_tail_)) - { - octet = read_buffer_[read_buffer_tail_]; - read_buffer_tail_ = uint8_t(size_t(read_buffer_tail_ + 1) % sizeof(read_buffer_)); - } - - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - break; - } - case InputState::UXR_SERIAL_READING_SRC_ADDR: - { - if (get_next_octet(remote_addr_)) - { - state_ = InputState::UXR_SERIAL_READING_DST_ADDR; - } - else - { - if (framing_begin_flag != remote_addr_) - { - exit_cond = true; - } - } - break; - } - case InputState::UXR_SERIAL_READING_DST_ADDR: - if (get_next_octet(octet)) - { - state_ = (octet == local_addr_) - ? InputState::UXR_SERIAL_READING_LEN_LSB - : InputState::UXR_SERIAL_UNINITIALIZED; - } - else - { - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - } - break; - case InputState::UXR_SERIAL_READING_LEN_LSB: - if (get_next_octet(octet)) - { - msg_len_ = octet; - state_ = InputState::UXR_SERIAL_READING_LEN_MSB; - } - else - { - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - } - break; - case InputState::UXR_SERIAL_READING_LEN_MSB: - if (get_next_octet(octet)) - { - msg_len_ += (octet << 8); - msg_pos_ = 0; - cmp_crc_ = 0; - if (len < msg_len_) - { - state_ = InputState::UXR_SERIAL_UNINITIALIZED; - exit_cond = true; - } - else - { - state_ = InputState::UXR_SERIAL_READING_PAYLOAD; - } - } - else - { - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - } - break; - case InputState::UXR_SERIAL_READING_PAYLOAD: - { - while ((msg_pos_ < msg_len_) && get_next_octet(octet)) - { - buf[size_t(msg_pos_)] = octet; - ++msg_pos_; - update_crc(cmp_crc_, octet); - } - - if (msg_pos_ == msg_len_) - { - state_ = InputState::UXR_SERIAL_READING_CRC_LSB; - } - else - { - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - } - break; - } - case InputState::UXR_SERIAL_READING_CRC_LSB: - if (get_next_octet(octet)) - { - msg_crc_ = octet; - state_ = InputState::UXR_SERIAL_READING_CRC_MSB; - } - else - { - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - } - break; - case InputState::UXR_SERIAL_READING_CRC_MSB: - if (get_next_octet(octet)) - { - msg_crc_ += (octet << 8); - state_ = InputState::UXR_SERIAL_UNINITIALIZED; - if (cmp_crc_ == msg_crc_) - { - remote_addr = remote_addr_; - rv = msg_len_; - } - exit_cond = true; - } - else - { - if (framing_begin_flag == octet) - { - state_ = InputState::UXR_SERIAL_READING_SRC_ADDR; - } - else - { - exit_cond = true; - } - } - break; - } - } - } - - return rv; -} - -} // namespace uxr -} // namespace eprosima - -#endif // UXR_AGENT_TRANSPORT_SERIAL_PROTOCOL_HPP_ diff --git a/include/uxr/agent/transport/serial/SerialServerBase.hpp b/include/uxr/agent/transport/serial/SerialServerBase.hpp deleted file mode 100644 index dc60911c3..000000000 --- a/include/uxr/agent/transport/serial/SerialServerBase.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UXR_AGENT_TRANSPORT_SERIAL_SERVER_BASE_HPP_ -#define UXR_AGENT_TRANSPORT_SERIAL_SERVER_BASE_HPP_ - -#include -#include -#include - -#include - -namespace eprosima { -namespace uxr { - -class SerialServerBase : public Server -{ -public: - SerialServerBase( - uint8_t addr, - Middleware::Kind middleware_kind); - - virtual ~SerialServerBase() override = default; - - void on_create_client( - EndPoint* source, - const dds::xrce::CLIENT_Representation& representation) override; - - void on_delete_client(EndPoint* source) override; - - const dds::xrce::ClientKey get_client_key(EndPoint* source) override; - - std::unique_ptr get_source(const dds::xrce::ClientKey& client_key) override; - -protected: - uint8_t addr_; - -private: - std::unordered_map source_to_client_map_; - std::unordered_map client_to_source_map_; - std::mutex clients_mtx_; -}; - -} // namespace uxr -} // namespace eprosima - -#endif // UXR_AGENT_TRANSPORT_SERIAL_SERVER_BASE_HPP_ diff --git a/include/uxr/agent/transport/stream_framing/StreamFramingProtocol.hpp b/include/uxr/agent/transport/stream_framing/StreamFramingProtocol.hpp new file mode 100644 index 000000000..8e402b415 --- /dev/null +++ b/include/uxr/agent/transport/stream_framing/StreamFramingProtocol.hpp @@ -0,0 +1,223 @@ +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UXR_AGENT_TRANSPORT_STREAM_FRAMING_PROTOCOL_HPP_ +#define UXR_AGENT_TRANSPORT_STREAM_FRAMING_PROTOCOL_HPP_ + +#include + +#include +#include +#include + +#ifdef _WIN32 +#include +typedef SSIZE_T ssize_t; +#endif + +namespace eprosima { +namespace uxr { + +class FramingIO +{ +private: + static constexpr uint16_t crc16_table[256] = + { + 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, + 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, + 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, + 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, + 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, + 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, + 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, + 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, + 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, + 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, + 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, + 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, + 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, + 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, + 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, + 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, + 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, + 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, + 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, + 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, + 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, + 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, + 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, + 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, + 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, + 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, + 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, + 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, + 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, + 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, + 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, + 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 + }; + + static constexpr uint8_t framing_begin_flag = 0x7E; + static constexpr uint8_t framing_esc_flag = 0x7D; + static constexpr uint8_t framing_xor_flag = 0x20; + + /** + * @brief Possible states for the framing protocol. + */ + enum class InputState : uint8_t + { + UXR_FRAMING_UNINITIALIZED, + UXR_FRAMING_READING_SRC_ADDR, + UXR_FRAMING_READING_DST_ADDR, + UXR_FRAMING_READING_LEN_LSB, + UXR_FRAMING_READING_LEN_MSB, + UXR_FRAMING_READING_PAYLOAD, + UXR_FRAMING_READING_CRC_LSB, + UXR_FRAMING_READING_CRC_MSB, + }; + +public: + /** + * @brief Write callback function signature. + * @param buffer Raw octet buffer to write into. + * @param message_length Number of bytes to be written. + * @param transport_rc Return code to be set by the callback logic. + * @return size_t Number of written bytes. + */ + using WriteCallback = std::function; + + /** + * @brief Read callback function signature. + * @param buffer Raw octet buffer to read data from. + * @param buffer_length Length of the buffer. + * @param timeout Read timeout. + * @param transport_rc Return code to be set by the callback logic. + * @return size_t Number of read bytes. + */ + using ReadCallback = std::function; + + FramingIO( + uint8_t local_addr, + WriteCallback write_callback, + ReadCallback read_callback); + + /** + * @brief Write message using the stream framing protocol + * and the previously user provided WriteCallback method. + * @param buf Buffer to write data into. + * @param len Length of the message to be written. + * @param remote_addr Remote address to where the message will be sent. + * @param transport_rc Return code of the write operation. + * @return size_t Number of written bytes. + */ + size_t write_framed_msg( + const uint8_t* buf, + size_t len, + uint8_t remote_addr, + TransportRc& transport_rc); + + /** + * @brief Read message using the stream framing protocol + * and the previously user provided ReadCallback method. + * @param buf Buffer to read data from. + * @param len Length of the buffer. + * @param remote_addr Remote address tfrom which the message will be read. + * @param timeout Timeout in milliseconds. + * @param transport_rc Return code of the read operation. + * @return size_t Number of read bytes. + */ + size_t read_framed_msg( + uint8_t* buf, + size_t len, + uint8_t& remote_addr, + int timeout, + TransportRc& transport_rc); + +private: + /** + * @brief Static method to update CRC. + * @param crc CRC code to be updated. + * @param data New data to be loaded into the CRC code. + */ + static void update_crc( + uint16_t& crc, + const uint8_t data); + + /** + * @brief Get next octet from the read buffer. + * @param octet Octet to which the data will be written. + * @return True if successful read, false otherwise. + */ + bool get_next_octet( + uint8_t& octet); + + /** + * @brief Add next octet to the write buffer. + * @param octet Data to be written. + * @return true if successful write, false otherwise. + */ + bool add_next_octet( + uint8_t octet); + + /** + * @brief Internal write method implemented using circular write buffer. + * @param transport_rc Return code of the write operation. + * @return True if success, false otherwise. + */ + bool transport_write( + TransportRc& transport_rc); + + /** + * @brief Internal read method implemented using circular read buffer. + * @param timeout Read timeout in milliseconds. + * @param transport_rc Return code of the read operation. + * @return Number of Bytes read. + */ + size_t transport_read( + int& timeout, + TransportRc& transport_rc); + + InputState state_; + + uint8_t local_addr_; + uint8_t remote_addr_; + + uint8_t read_buffer_[42]; + uint8_t read_buffer_head_; + uint8_t read_buffer_tail_; + + ReadCallback read_callback_; + + uint16_t msg_len_; + uint16_t msg_pos_; + uint16_t msg_crc_; + uint16_t cmp_crc_; + + uint8_t write_buffer_[42]; + uint8_t write_buffer_pos_; + + WriteCallback write_callback_; +}; + +} // namespace uxr +} // namespace eprosima + +#endif // UXR_AGENT_TRANSPORT_STREAM_FRAMING_PROTOCOL_HPP_ \ No newline at end of file diff --git a/include/uxr/agent/transport/util/Interface.hpp b/include/uxr/agent/transport/util/Interface.hpp deleted file mode 100644 index 414a9fd0d..000000000 --- a/include/uxr/agent/transport/util/Interface.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2017-present Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UXR__AGENT__TRANSPORT__UTIL__INTERFACE_HPP_ -#define UXR__AGENT__TRANSPORT__UTIL__INTERFACE_HPP_ - -#include - -#include - -inline -std::ostream & operator<<(std::ostream & os, dds::xrce::TransportAddress const & address) -{ - switch (address._d()) - { - case dds::xrce::ADDRESS_FORMAT_MEDIUM: - os << int(address.medium_locator().address().at(0)) << "." - << int(address.medium_locator().address().at(1)) << "." - << int(address.medium_locator().address().at(2)) << "." - << int(address.medium_locator().address().at(3)) << ":" - << address.medium_locator().port(); - break; - case dds::xrce::ADDRESS_FORMAT_LARGE: - os << std::hex << "[" - << int(address.large_locator().address().at(0)) - << int(address.large_locator().address().at(1)) - << ":" - << int(address.large_locator().address().at(2)) - << int(address.large_locator().address().at(3)) - << ":" - << int(address.large_locator().address().at(4)) - << int(address.large_locator().address().at(5)) - << ":" - << int(address.large_locator().address().at(6)) - << int(address.large_locator().address().at(7)) - << ":" - << int(address.large_locator().address().at(8)) - << int(address.large_locator().address().at(9)) - << ":" - << int(address.large_locator().address().at(10)) - << int(address.large_locator().address().at(11)) - << ":" - << int(address.large_locator().address().at(12)) - << int(address.large_locator().address().at(13)) - << ":" - << int(address.large_locator().address().at(14)) - << int(address.large_locator().address().at(15)) - << "]:" << std::dec - << address.large_locator().port(); - default: - break; - } - return os; -} - -#endif // UXR__AGENT__TRANSPORT__UTIL__INTERFACE_HPP_ \ No newline at end of file diff --git a/include/uxr/agent/transport/util/InterfaceLinux.hpp b/include/uxr/agent/transport/util/InterfaceLinux.hpp index fb49f3b61..151797f40 100644 --- a/include/uxr/agent/transport/util/InterfaceLinux.hpp +++ b/include/uxr/agent/transport/util/InterfaceLinux.hpp @@ -15,7 +15,6 @@ #ifndef UXR_AGENT_TRANSPORT_UTIL_INTERFACELINUX_HPP_ #define UXR_AGENT_TRANSPORT_UTIL_INTERFACELINUX_HPP_ -#include #include #include #include diff --git a/include/uxr/agent/transport/util/InterfaceWindows.hpp b/include/uxr/agent/transport/util/InterfaceWindows.hpp index 5a25e5a5f..26a7d0894 100644 --- a/include/uxr/agent/transport/util/InterfaceWindows.hpp +++ b/include/uxr/agent/transport/util/InterfaceWindows.hpp @@ -15,7 +15,6 @@ #ifndef UXR_AGENT_TRANSPORT_UTIL_INTERFACEWINDOWS_HPP_ #define UXR_AGENT_TRANSPORT_UTIL_INTERFACEWINDOWS_HPP_ -#include #include #include #include diff --git a/include/uxr/agent/types/XRCETypes.hpp b/include/uxr/agent/types/XRCETypes.hpp index 5a8682e2f..2dba1564e 100644 --- a/include/uxr/agent/types/XRCETypes.hpp +++ b/include/uxr/agent/types/XRCETypes.hpp @@ -29,6 +29,7 @@ #include #include #include +#include namespace eprosima { namespace fastcdr { @@ -943,6 +944,56 @@ class TransportAddress */ virtual void deserialize(eprosima::fastcdr::Cdr &cdr); + /*! + * @brief This function formats the IP address and port to ostream, for logging purpuses. + * @param os ostream object. + * @param address TransportAddress object to format. + * @return ostream object with formated address. + */ + friend std::ostream & operator<<(std::ostream & os, TransportAddress const & address) + { + switch (address._d()) + { + case ADDRESS_FORMAT_MEDIUM: + os << int(address.medium_locator().address().at(0)) << "." + << int(address.medium_locator().address().at(1)) << "." + << int(address.medium_locator().address().at(2)) << "." + << int(address.medium_locator().address().at(3)) << ":" + << address.medium_locator().port(); + break; + case ADDRESS_FORMAT_LARGE: + os << std::hex << "[" + << int(address.large_locator().address().at(0)) + << int(address.large_locator().address().at(1)) + << ":" + << int(address.large_locator().address().at(2)) + << int(address.large_locator().address().at(3)) + << ":" + << int(address.large_locator().address().at(4)) + << int(address.large_locator().address().at(5)) + << ":" + << int(address.large_locator().address().at(6)) + << int(address.large_locator().address().at(7)) + << ":" + << int(address.large_locator().address().at(8)) + << int(address.large_locator().address().at(9)) + << ":" + << int(address.large_locator().address().at(10)) + << int(address.large_locator().address().at(11)) + << ":" + << int(address.large_locator().address().at(12)) + << int(address.large_locator().address().at(13)) + << ":" + << int(address.large_locator().address().at(14)) + << int(address.large_locator().address().at(15)) + << "]:" << std::dec + << address.large_locator().port(); + default: + break; + } + return os; + } + private: TransportAddressFormat m__d; diff --git a/include/uxr/agent/utils/ArgumentParser.hpp b/include/uxr/agent/utils/ArgumentParser.hpp index c78aa5694..aa496cb2f 100644 --- a/include/uxr/agent/utils/ArgumentParser.hpp +++ b/include/uxr/agent/utils/ArgumentParser.hpp @@ -60,6 +60,7 @@ enum class TransportKind SERIAL, PSEUDOTERMINAL, #endif // _WIN32 + HELP }; template @@ -77,6 +78,7 @@ namespace parser { namespace utils { bool usage( + const char* executable_name, bool no_help = true); TransportKind check_transport( @@ -86,7 +88,7 @@ Middleware::Kind get_mw_kind( const std::string& kind); } // namespace utils -using dummy_type = bool; +using dummy_type = uint8_t; enum class ArgumentKind { @@ -105,6 +107,7 @@ enum class ParseResult /************************************************************************************************* * Generic command line argument representation *************************************************************************************************/ +// TODO(jamoralp): add proper documentation for this whole file. template class Argument { @@ -113,11 +116,14 @@ class Argument const char* short_alias, const char* long_alias, ArgumentKind kind = ArgumentKind::VALUE, - const std::set allowed_values = {}) + const std::set allowed_values = {}, + bool enabled_by_default = true) : argument_kind_(kind) , short_alias_(short_alias) , long_alias_(long_alias) , value_() + , has_default_value_(false) + , enabled_by_default_(enabled_by_default) , parse_found_(false) , allowed_values_(allowed_values) { @@ -127,21 +133,31 @@ class Argument const char* short_alias, const char* long_alias, const T& default_value, - const std::set allowed_values = {}) + const std::set allowed_values = {}, + bool enabled_by_default = true) : argument_kind_(ArgumentKind::VALUE) , short_alias_(short_alias) , long_alias_(long_alias) , value_(default_value) + , has_default_value_(true) + , enabled_by_default_(enabled_by_default) , parse_found_(false) , allowed_values_(allowed_values) { } + ~Argument() = default; + const T& value() const { return value_; } + T& value() + { + return value_; + } + bool found() { return parse_found_; @@ -174,6 +190,29 @@ class Argument { exists_value = true; value_str = argv[position]; + + /** + * It might occur that an argument is specified to enable some behaviour but we want + * to use the default value. Handle that case. + */ + if (std::string::npos != value_str.find("-")) + { + if (has_default_value_) + { + parse_found_ = true; + return ParseResult::VALID; + } + else + { + parse_found_ = false; + return ParseResult::INVALID; + } + } + } + else if (has_default_value_) + { + parse_found_ = true; + return ParseResult::VALID; } } else if (0 == opt.find(short_alias_)) @@ -197,10 +236,10 @@ class Argument { std::stringstream ss; ss << "Warning: introduced value for argument '" << long_alias_; - ss << "' is not allowed. Please choose between { "; + ss << "' is not allowed. Please choose between {"; for (const auto& allowed_value_ : allowed_values_) { - ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : " }."); + ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : "}."); } ss << std::endl; std::cerr << ss.str(); @@ -215,6 +254,16 @@ class Argument } } } + + /** + * Argument not found, but maybe it is enabled by default. + */ + if (enabled_by_default_ && has_default_value_) + { + parse_found_ = true; + return ParseResult::VALID; + } + return ParseResult::NOT_FOUND; } @@ -228,11 +277,11 @@ class Argument } if (!allowed_values_.empty()) { - ss << " ("; - for (const auto& allowed_value_ : allowed_values_) - { - ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : " )"); - } + ss << " (" << *allowed_values_.begin() << " - " << *allowed_values_.rbegin() << ")"; + } + if (has_default_value_) + { + ss << " [default: '" << value_ << "']"; } ss << "."; return ss.str(); @@ -243,6 +292,8 @@ class Argument std::string short_alias_; std::string long_alias_; T value_; + bool has_default_value_; + bool enabled_by_default_; bool parse_found_; std::set allowed_values_; }; @@ -258,11 +309,14 @@ class Argument const char* short_alias, const char* long_alias, const std::set allowed_values = {}, - ArgumentKind kind = ArgumentKind::VALUE) + ArgumentKind kind = ArgumentKind::VALUE, + bool enabled_by_default = true) : argument_kind_(kind) , short_alias_(short_alias) , long_alias_(long_alias) , value_() + , has_default_value_(false) + , enabled_by_default_(enabled_by_default) , parse_found_(false) , allowed_values_(allowed_values) { @@ -272,21 +326,31 @@ class Argument const char* short_alias, const char* long_alias, const std::string& default_value, - const std::set allowed_values = {}) + const std::set allowed_values = {}, + bool enabled_by_default = true) : argument_kind_(ArgumentKind::VALUE) , short_alias_(short_alias) , long_alias_(long_alias) , value_(default_value) + , has_default_value_(true) + , enabled_by_default_(enabled_by_default) , parse_found_(false) , allowed_values_(allowed_values) { } + ~Argument() = default; + const std::string& value() const { return value_; } + std::string& value() + { + return value_; + } + bool found() { return parse_found_; @@ -311,10 +375,10 @@ ParseResult parse_argument( { std::stringstream ss; ss << "Warning: introduced value for argument '" << long_alias_; - ss << "' is not allowed. Please choose between { "; + ss << "' is not allowed. Please choose between {"; for (const auto& allowed_value_ : allowed_values_) { - ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : " }."); + ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : "}."); } ss << std::endl; std::cerr << ss.str(); @@ -335,6 +399,16 @@ ParseResult parse_argument( } } } + + /** + * Argument not found, but maybe it is enabled by default. + */ + if (enabled_by_default_ && has_default_value_) + { + parse_found_ = true; + return ParseResult::VALID; + } + return ParseResult::NOT_FOUND; } @@ -348,12 +422,16 @@ ParseResult parse_argument( } if (!allowed_values_.empty()) { - ss << " ( "; + ss << " ("; for (const auto& allowed_value_ : allowed_values_) { - ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : " )"); + ss << allowed_value_ << ((*allowed_values_.rbegin() != allowed_value_) ? ", " : ")"); } } + if (has_default_value_) + { + ss << " [default: '" << value_ << "']"; + } ss << "."; return ss.str(); } @@ -363,6 +441,8 @@ ParseResult parse_argument( std::string short_alias_; std::string long_alias_; std::string value_; + bool has_default_value_; + bool enabled_by_default_; bool parse_found_; std::set allowed_values_; }; @@ -382,7 +462,7 @@ class CommonArgs , verbose_("-v", "--verbose", static_cast(DEFAULT_VERBOSE_LEVEL), {0, 1, 2, 3, 4, 5, 6}) #ifdef UAGENT_DISCOVERY_PROFILE - , discovery_("-d", "--discovery", static_cast(DEFAULT_DISCOVERY_PORT)) + , discovery_("-d", "--discovery", static_cast(DEFAULT_DISCOVERY_PORT), {}, false) #endif #ifdef UAGENT_P2P_PROFILE , p2p_("-P", "--p2p") @@ -412,7 +492,12 @@ class CommonArgs result.first = false; return result; } - result.first &= static_cast(middleware_.parse_argument(argc, argv)); + + if (ParseResult::INVALID == middleware_.parse_argument(argc, argv)) + { + result.first = false; + return result; + } ParseResult refs_arg = refs_.parse_argument(argc, argv); if (ParseResult::VALID == refs_arg) { @@ -424,13 +509,30 @@ class CommonArgs return result; } } - result.first &= static_cast(refs_arg); - result.first &= static_cast(verbose_.parse_argument(argc, argv)); + else if (ParseResult::INVALID == refs_arg) + { + result.first = false; + return result; + } + + if (ParseResult::INVALID == verbose_.parse_argument(argc, argv)) + { + result.first = false; + return result; + } #ifdef UAGENT_DISCOVERY_PROFILE - result.first &= static_cast(discovery_.parse_argument(argc, argv)); + if (ParseResult::INVALID == discovery_.parse_argument(argc, argv)) + { + result.first = false; + return result; + } #endif #ifdef UAGENT_P2P_PROFILE - result.first &= static_cast(p2p_.parse_argument(argc, argv)); + if (ParseResult::INVALID == p2p_.parse_argument(argc, argv)) + { + result.first = false; + return result; + } #endif return result; } @@ -480,7 +582,7 @@ class CommonArgs Argument help_; Argument middleware_; Argument refs_; - Argument verbose_; + Argument verbose_; #ifdef UAGENT_DISCOVERY_PROFILE Argument discovery_; #endif @@ -604,7 +706,6 @@ class SerialArgs : public PseudoTerminalArgs { std::stringstream ss; ss << " " << dev_.get_help(); - ss << " (only available with 'serial' transport)" << std::endl; return ss.str(); } @@ -772,16 +873,20 @@ class ArgumentParser void show_help() { - utils::usage(false); + utils::usage(argv_[0], false); std::stringstream ss; ss << std::endl << "Available arguments (per transport):" << std::endl; ss << " * COMMON" << std::endl; ss << common_args_.get_help(); ss << " * IPvX (udp4, udp6, tcp4, tcp6)" << std::endl; ss << ip_args_.get_help(); - ss << " * SERIAL" << std::endl; + ss << " * SERIAL (serial, pseudoterminal)" << std::endl; +#ifndef _WIN32 ss << pseudoterminal_args_.get_help(); ss << serial_args_.get_help(); +#endif // _WIN32 + ss << std::endl; + // TODO(@jamoralp): Once documentation is updated with proper CLI section, add here an hyperlink to that section std::cout << ss.str(); } @@ -823,7 +928,7 @@ inline std::thread create_agent_thread( case parser::ParseResult::INVALID: case parser::ParseResult::NOT_FOUND: { - parser::utils::usage(); + parser::utils::usage(argv[0]); break; } case parser::ParseResult::VALID: @@ -875,7 +980,7 @@ inline std::thread create_agent_thread( case parser::ParseResult::INVALID: case parser::ParseResult::NOT_FOUND: { - parser::utils::usage(); + parser::utils::usage(argv[0]); break; } case parser::ParseResult::VALID: @@ -915,7 +1020,7 @@ inline std::thread create_agent_thread( case parser::ParseResult::INVALID: case parser::ParseResult::NOT_FOUND: { - parser::utils::usage(); + parser::utils::usage(argv[0]); break; } case parser::ParseResult::VALID: diff --git a/include/uxr/agent/utils/CLI.hpp b/include/uxr/agent/utils/CLI.hpp deleted file mode 100644 index 7e3e50c5f..000000000 --- a/include/uxr/agent/utils/CLI.hpp +++ /dev/null @@ -1,667 +0,0 @@ -// Copyright 2019 Proyectos y Sistemas de Mantenimiento SL (eProsima). -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UXR_AGENT_UTILS_CLI_HPP_ -#define UXR_AGENT_UTILS_CLI_HPP_ - -#ifdef _WIN32 -#include -#include -#include -#include -#else -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#endif //_WIN32 - -#include - -#include - -namespace eprosima{ -namespace uxr{ -namespace cli { - -/************************************************************************************************* - * Middleware CLI Option - *************************************************************************************************/ -class MiddlewareOpt -{ -public: - MiddlewareOpt(CLI::App& subcommand) - : kind_{"none"} - , set_{} - , cli_opt_{} - { -#ifdef UAGENT_CED_PROFILE - set_.insert("ced"); - kind_ = "ced"; -#endif -#ifdef UAGENT_FAST_PROFILE - set_.insert("dds"); - set_.insert("rtps"); - kind_ = "dds"; -#endif - cli_opt_ = subcommand.add_set("-m,--middleware", kind_, set_, "Select the kind of Middleware", true); - } - - eprosima::uxr::Middleware::Kind get_kind() const - { -#ifdef UAGENT_FAST_PROFILE - if ("dds" == kind_) - { - return eprosima::uxr::Middleware::Kind::FASTDDS; - } - if ("rtps" == kind_) - { - return eprosima::uxr::Middleware::Kind::FASTRTPS; - } -#endif -#ifdef UAGENT_CED_PROFILE - if ("ced" == kind_) - { - return eprosima::uxr::Middleware::Kind::CED; - } -#endif - return eprosima::uxr::Middleware::Kind::NONE; - } - -protected: - std::string kind_; - std::set set_; - CLI::Option* cli_opt_; -}; - -/************************************************************************************************* - * Discovery CLI Option - *************************************************************************************************/ -#ifdef UAGENT_DISCOVERY_PROFILE -class DiscoveryOpt -{ -public: - DiscoveryOpt(CLI::App& subcommand) - : port_(eprosima::uxr::DISCOVERY_PORT) - , cli_flag_{subcommand.add_flag("-d,--discovery", "Activate the Discovery server")} - , cli_opt_{subcommand.add_option("--disport", port_, "Select the port for the Discovery server", true)} - { - cli_opt_->needs(cli_flag_); - } - - bool is_enable() const { return bool(*cli_flag_); } - uint16_t get_port() const { return port_; } - -protected: - uint16_t port_; - CLI::Option* cli_flag_; - CLI::Option* cli_opt_; -}; -#endif - -/************************************************************************************************* - * P2P CLI Option - *************************************************************************************************/ -#ifdef UAGENT_P2P_PROFILE -class P2POpt -{ -public: - P2POpt(CLI::App& subcommand) - : port_(0) - , cli_opt_{subcommand.add_option("--p2p", port_, "Activate the P2P profile using the given port")} - { - } - - bool is_enable() const { return bool(*cli_opt_); } - uint16_t get_port() const { return port_; } - -protected: - uint16_t port_; - CLI::Option* cli_opt_; -}; -#endif - -/************************************************************************************************* - * Baudrate CLI Option - *************************************************************************************************/ -class BaudrateOpt -{ -public: - BaudrateOpt(CLI::App& subcommand) - : baudrate_("115200") - , cli_opt_{subcommand.add_option("-b,--baudrate", baudrate_, "Select the baudrate", true)} - {} - - bool is_enable() const { return bool(*cli_opt_); } - const std::string& get_baudrate() const { return baudrate_; } - -protected: - std::string baudrate_; - CLI::Option* cli_opt_; -}; - -/************************************************************************************************* - * Refernce CLI Option - *************************************************************************************************/ -class ReferenceOpt -{ -public: - ReferenceOpt(CLI::App& subcommand) - : file_{} - , cli_opt_{subcommand.add_option("-r,--refs", file_, "Load a reference file", true)} - { - cli_opt_->check(CLI::ExistingFile); - } - - bool is_enable() const { return bool(*cli_opt_); } - const std::string& get_file() const { return file_; } - -protected: - std::string file_; - CLI::Option* cli_opt_; -}; - -/************************************************************************************************* - * Logger CLI Option - *************************************************************************************************/ -class VerboseOpt -{ -public: - VerboseOpt(CLI::App& subcommand) - : level_{4} - , set_{0, 1, 2, 3, 4, 5, 6} - , cli_opt_{subcommand.add_set("-v,--verbose", level_, set_, "Select log level from less to more verbose", true)} - {} - - bool is_enable() const { return bool(*cli_opt_); } - uint8_t get_level() const { return uint8_t(level_); } - -protected: - uint16_t level_; - std::set set_; - CLI::Option* cli_opt_; -}; - -/************************************************************************************************* - * Common CLI Opts - *************************************************************************************************/ -class CommonOpts -{ -public: - CommonOpts(CLI::App& subcommand) - : middleware_opt_{subcommand} - , reference_opt_{subcommand} - , verbose_opt_{subcommand} -#ifdef UAGENT_DISCOVERY_PROFILE - , discovery_opt_{subcommand} -#endif -#ifdef UAGENT_P2P_PROFILE - , p2p_opt_{subcommand} -#endif - {} - - MiddlewareOpt middleware_opt_; - ReferenceOpt reference_opt_; - VerboseOpt verbose_opt_; -#ifdef UAGENT_DISCOVERY_PROFILE - DiscoveryOpt discovery_opt_; -#endif -#ifdef UAGENT_P2P_PROFILE - P2POpt p2p_opt_; -#endif -}; - -/************************************************************************************************* - * Exit Subcommand - *************************************************************************************************/ -class ExitSubcommand -{ -public: - ExitSubcommand( - CLI::App& app) - : exit_subcommand_{app.add_subcommand("q", "Exit")} - { - exit_subcommand_->callback(std::bind(&ExitSubcommand::exit, this)); - } - -private: - void exit() - { - std::exit(EXIT_SUCCESS); - } - -private: - CLI::App* exit_subcommand_; -}; - -/************************************************************************************************* - * Server Subcommand - *************************************************************************************************/ -class ServerSubcommand -{ -public: - ServerSubcommand( - CLI::App& app, - const std::string& name, - const std::string& description, - const CommonOpts& common_opts) - : cli_subcommand_{app.add_subcommand(name, description)} - , opts_ref_{common_opts} - { - cli_subcommand_->callback(std::bind(&ServerSubcommand::server_callback, this)); - } - - virtual ~ServerSubcommand() = default; - -private: - void server_callback() - { -#ifdef _WIN32 - std::cout << "Enter 'q' for exit" << std::endl; -#else - std::cout << "Press CTRL+C to exit" << std::endl; -#endif - launch_server(); - } - - virtual void launch_server() = 0; - -protected: - CLI::App* cli_subcommand_; - const CommonOpts& opts_ref_; -}; - - -/************************************************************************************************* - * UDPv6 Subcommand - *************************************************************************************************/ -class UDPv4Subcommand : public ServerSubcommand -{ -public: - UDPv4Subcommand(CLI::App& app) - : ServerSubcommand{app, "udp4", "Launch a UDP/IPv4 server", common_opts_} - , cli_opt_{cli_subcommand_->add_option("-p,--port", port_, "Select the port")} - , common_opts_{*cli_subcommand_} - { - cli_opt_->required(true); - } - - ~UDPv4Subcommand() final = default; - -private: - void launch_server() - { - server_.reset(new eprosima::uxr::UDPv4Agent(port_, common_opts_.middleware_opt_.get_kind())); - if (server_->start()) - { -#ifdef UAGENT_DISCOVERY_PROFILE - if (opts_ref_.discovery_opt_.is_enable()) - { - server_->enable_discovery(opts_ref_.discovery_opt_.get_port()); - } -#endif - -#ifdef UAGENT_P2P_PROFILE - if ((eprosima::uxr::Middleware::Kind::CED == opts_ref_.middleware_opt_.get_kind()) - && opts_ref_.p2p_opt_.is_enable()) - { - server_->enable_p2p(opts_ref_.p2p_opt_.get_port()); - } -#endif - if (opts_ref_.reference_opt_.is_enable()) - { - server_->load_config_file(opts_ref_.reference_opt_.get_file()); - } - - if (opts_ref_.verbose_opt_.is_enable()) - { - server_->set_verbose_level(opts_ref_.verbose_opt_.get_level()); - } - } - } - -private: - std::unique_ptr server_; - uint16_t port_; - CLI::Option* cli_opt_; - CommonOpts common_opts_; -}; - -/************************************************************************************************* - * UDPv6 Subcommand - *************************************************************************************************/ -class UDPv6Subcommand : public ServerSubcommand -{ -public: - UDPv6Subcommand(CLI::App& app) - : ServerSubcommand{app, "udp6", "Launch a UDP/IPv6 server", common_opts_} - , cli_opt_{cli_subcommand_->add_option("-p,--port", port_, "Select the port")} - , common_opts_{*cli_subcommand_} - { - cli_opt_->required(true); - } - - ~UDPv6Subcommand() final = default; - -private: - void launch_server() - { - server_.reset(new eprosima::uxr::UDPv6Agent(port_, common_opts_.middleware_opt_.get_kind())); - if (server_->start()) - { -#ifdef UAGENT_DISCOVERY_PROFILE - if (opts_ref_.discovery_opt_.is_enable()) - { - server_->enable_discovery(opts_ref_.discovery_opt_.get_port()); - } -#endif - -#ifdef UAGENT_P2P_PROFILE - if ((eprosima::uxr::Middleware::Kind::CED == opts_ref_.middleware_opt_.get_kind()) - && opts_ref_.p2p_opt_.is_enable()) - { - server_->enable_p2p(opts_ref_.p2p_opt_.get_port()); - } -#endif - if (opts_ref_.reference_opt_.is_enable()) - { - server_->load_config_file(opts_ref_.reference_opt_.get_file()); - } - - if (opts_ref_.verbose_opt_.is_enable()) - { - server_->set_verbose_level(opts_ref_.verbose_opt_.get_level()); - } - } - } - -private: - std::unique_ptr server_; - uint16_t port_; - CLI::Option* cli_opt_; - CommonOpts common_opts_; -}; - -/************************************************************************************************* - * TCPv4 Subcommand - *************************************************************************************************/ -class TCPv4Subcommand : public ServerSubcommand -{ -public: - TCPv4Subcommand(CLI::App& app) - : ServerSubcommand{app, "tcp4", "Launch a TCP/IPv4 server", common_opts_} - , cli_opt_{cli_subcommand_->add_option("-p,--port", port_, "Select the port")} - , common_opts_{*cli_subcommand_} - { - cli_opt_->required(true); - } - - ~TCPv4Subcommand() final = default; - -private: - void launch_server() - { - server_.reset(new eprosima::uxr::TCPv4Agent(port_, common_opts_.middleware_opt_.get_kind())); - if (server_->start()) - { -#ifdef UAGENT_DISCOVERY_PROFILE - if (opts_ref_.discovery_opt_.is_enable()) - { - server_->enable_discovery(opts_ref_.discovery_opt_.get_port()); - } -#endif - -#ifdef UAGENT_P2P_PROFILE - if ((eprosima::uxr::Middleware::Kind::CED == opts_ref_.middleware_opt_.get_kind()) - && opts_ref_.p2p_opt_.is_enable()) - { - server_->enable_p2p(opts_ref_.p2p_opt_.get_port()); - } -#endif - if (opts_ref_.reference_opt_.is_enable()) - { - server_->load_config_file(opts_ref_.reference_opt_.get_file()); - } - - if (opts_ref_.verbose_opt_.is_enable()) - { - server_->set_verbose_level(opts_ref_.verbose_opt_.get_level()); - } - } - } - -private: - std::unique_ptr server_; - uint16_t port_; - CLI::Option* cli_opt_; - CommonOpts common_opts_; -}; - -/************************************************************************************************* - * TCPv6 Subcommand - *************************************************************************************************/ -class TCPv6Subcommand : public ServerSubcommand -{ -public: - TCPv6Subcommand(CLI::App& app) - : ServerSubcommand{app, "tcp6", "Launch a TCP/IPv6 server", common_opts_} - , cli_opt_{cli_subcommand_->add_option("-p,--port", port_, "Select the port")} - , common_opts_{*cli_subcommand_} - { - cli_opt_->required(true); - } - - ~TCPv6Subcommand() final = default; - -private: - void launch_server() - { - server_.reset(new eprosima::uxr::TCPv6Agent(port_, common_opts_.middleware_opt_.get_kind())); - if (server_->start()) - { -#ifdef UAGENT_DISCOVERY_PROFILE - if (opts_ref_.discovery_opt_.is_enable()) - { - server_->enable_discovery(opts_ref_.discovery_opt_.get_port()); - } -#endif - -#ifdef UAGENT_P2P_PROFILE - if ((eprosima::uxr::Middleware::Kind::CED == opts_ref_.middleware_opt_.get_kind()) - && opts_ref_.p2p_opt_.is_enable()) - { - server_->enable_p2p(opts_ref_.p2p_opt_.get_port()); - } -#endif - if (opts_ref_.reference_opt_.is_enable()) - { - server_->load_config_file(opts_ref_.reference_opt_.get_file()); - } - - if (opts_ref_.verbose_opt_.is_enable()) - { - server_->set_verbose_level(opts_ref_.verbose_opt_.get_level()); - } - } - } - -private: - std::unique_ptr server_; - uint16_t port_; - CLI::Option* cli_opt_; - CommonOpts common_opts_; -}; - -/************************************************************************************************* - * Serial Subcommand - *************************************************************************************************/ -#ifndef _WIN32 -class TermiosSubcommand : public ServerSubcommand -{ -public: - TermiosSubcommand(CLI::App& app) - : ServerSubcommand{app, "serial", "Launch a Termios server", common_opts_} - , cli_opt_{cli_subcommand_->add_option("--dev", dev_, "Select the serial device")} - , baudrate_opt_{*cli_subcommand_} - , common_opts_{*cli_subcommand_} - { - cli_opt_->required(true); - cli_opt_->check(CLI::ExistingFile); - } - -private: - void launch_server() final - { - struct termios attr = {}; - - /* Setting CONTROL OPTIONS. */ - attr.c_cflag |= unsigned(CREAD); // Enable read. - attr.c_cflag |= unsigned(CLOCAL); // Set local mode. - attr.c_cflag &= unsigned(~PARENB); // Disable parity. - attr.c_cflag &= unsigned(~CSTOPB); // Set one stop bit. - attr.c_cflag &= unsigned(~CSIZE); // Mask the character size bits. - attr.c_cflag |= unsigned(CS8); // Set 8 data bits. - attr.c_cflag &= unsigned(~CRTSCTS); // Disable hardware flow control. - - /* Setting LOCAL OPTIONS. */ - attr.c_lflag &= unsigned(~ICANON); // Set non-canonical input. - attr.c_lflag &= unsigned(~ECHO); // Disable echoing of input characters. - attr.c_lflag &= unsigned(~ECHOE); // Disable echoing the erase character. - attr.c_lflag &= unsigned(~ISIG); // Disable SIGINTR, SIGSUSP, SIGDSUSP and SIGQUIT signals. - - /* Setting INPUT OPTIONS. */ - attr.c_iflag &= unsigned(~IXON); // Disable output software flow control. - attr.c_iflag &= unsigned(~IXOFF); // Disable input software flow control. - attr.c_iflag &= unsigned(~INPCK); // Disable parity check. - attr.c_iflag &= unsigned(~ISTRIP); // Disable strip parity bits. - attr.c_iflag &= unsigned(~IGNBRK); // No ignore break condition. - attr.c_iflag &= unsigned(~IGNCR); // No ignore carrier return. - attr.c_iflag &= unsigned(~INLCR); // No map NL to CR. - attr.c_iflag &= unsigned(~ICRNL); // No map CR to NL. - - /* Setting OUTPUT OPTIONS. */ - attr.c_oflag &= unsigned(~OPOST); // Set raw output. - - /* Setting OUTPUT CHARACTERS. */ - attr.c_cc[VMIN] = 10; - attr.c_cc[VTIME] = 1; - - /* Setting baudrate. */ - speed_t baudrate = getBaudRate(baudrate_opt_.get_baudrate().c_str()); - attr.c_ispeed = baudrate; - attr.c_ospeed = baudrate; - - server_.reset( - new eprosima::uxr::TermiosAgent( - dev_.c_str(), O_RDWR | O_NOCTTY, attr, 0, common_opts_.middleware_opt_.get_kind())); - if (server_->start()) - { -#ifdef UAGENT_DISCOVERY_PROFILE - if (opts_ref_.discovery_opt_.is_enable()) - { - server_->enable_discovery(opts_ref_.discovery_opt_.get_port()); - } -#endif - -#ifdef UAGENT_P2P_PROFILE - if ((eprosima::uxr::Middleware::Kind::CED == opts_ref_.middleware_opt_.get_kind()) - && opts_ref_.p2p_opt_.is_enable()) - { - server_->enable_p2p(opts_ref_.p2p_opt_.get_port()); - } -#endif - if (opts_ref_.reference_opt_.is_enable()) - { - server_->load_config_file(opts_ref_.reference_opt_.get_file()); - } - - if (opts_ref_.verbose_opt_.is_enable()) - { - server_->set_verbose_level(opts_ref_.verbose_opt_.get_level()); - } - } - } - -private: - std::unique_ptr server_; - std::string dev_; - CLI::Option* cli_opt_; - BaudrateOpt baudrate_opt_; - CommonOpts common_opts_; -}; - -/************************************************************************************************* - * PseudoTerminal Subcommand - *************************************************************************************************/ -class PseudoTerminalSubcommand : public ServerSubcommand -{ -public: - PseudoTerminalSubcommand(CLI::App& app) - : ServerSubcommand{app, "pseudoterminal", "Launch a pseudoterminal server", common_opts_} - , baudrate_opt_{*cli_subcommand_} - , common_opts_{*cli_subcommand_} - {} - -private: - void launch_server() final - { - server_.reset( - new eprosima::uxr::PseudoTerminalAgent( - O_RDWR | O_NOCTTY, baudrate_opt_.get_baudrate().c_str(), 0, common_opts_.middleware_opt_.get_kind())); - if (server_->start()) - { -#ifdef UAGENT_DISCOVERY_PROFILE - if (opts_ref_.discovery_opt_.is_enable()) - { - server_->enable_discovery(opts_ref_.discovery_opt_.get_port()); - } -#endif - -#ifdef UAGENT_P2P_PROFILE - if ((eprosima::uxr::Middleware::Kind::CED == opts_ref_.middleware_opt_.get_kind()) - && opts_ref_.p2p_opt_.is_enable()) - { - server_->enable_p2p(opts_ref_.p2p_opt_.get_port()); - } -#endif - if (opts_ref_.reference_opt_.is_enable()) - { - server_->load_config_file(opts_ref_.reference_opt_.get_file()); - } - - if (opts_ref_.verbose_opt_.is_enable()) - { - server_->set_verbose_level(opts_ref_.verbose_opt_.get_level()); - } - } - } - -private: - std::unique_ptr server_; - BaudrateOpt baudrate_opt_; - CommonOpts common_opts_; -}; -#endif - -} // cli -} // uxr -} // eprosima - -#endif // UXR_AGENT_UTILS_CLI_HPP_ diff --git a/src/cpp/Agent.cpp b/src/cpp/Agent.cpp index 0082b0a70..575d4e36b 100644 --- a/src/cpp/Agent.cpp +++ b/src/cpp/Agent.cpp @@ -16,7 +16,7 @@ #include #include #include - +#include namespace eprosima { namespace uxr { @@ -214,6 +214,7 @@ void fill_object_variant( **********************************************************************************************************************/ Agent::Agent() : root_(new Root()) + , callback_factory_(callback_factory_.getInstance()) {} Agent::~Agent() = default; @@ -638,6 +639,56 @@ bool Agent::delete_object( return rv; } +template +void Agent::add_middleware_callback( + const Middleware::Kind& middleware_kind, + const middleware::CallbackKind& callback_kind, + std::function&& callback_function) +{ + callback_factory_.add_callback(middleware_kind, callback_kind, std::move(callback_function)); +} + +// Specific template specializations for used callback signatures. +#define AGENT_ADD_MW_CB(...) \ +template \ +UXR_AGENT_EXPORT void Agent::add_middleware_callback<__VA_ARGS__>( \ + const Middleware::Kind &, \ + const middleware::CallbackKind &, \ + std::function &&); + +#ifdef UAGENT_FAST_PROFILE +AGENT_ADD_MW_CB( + const eprosima::fastrtps::Participant *) + +AGENT_ADD_MW_CB( + const eprosima::fastrtps::Participant *, + const eprosima::fastrtps::Publisher *) + +AGENT_ADD_MW_CB( + const eprosima::fastrtps::Participant *, + const eprosima::fastrtps::Subscriber *) + +AGENT_ADD_MW_CB( + const eprosima::fastrtps::Participant *, + const eprosima::fastrtps::Publisher *, + const eprosima::fastrtps::Subscriber *) + +AGENT_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *) + +AGENT_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataWriter *) + +AGENT_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataReader *) + +AGENT_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataWriter *, + const eprosima::fastdds::dds::DataReader *) +#endif // UAGENT_FAST_PROFILE } // namespace uxr } // namespace eprosima diff --git a/src/cpp/AgentInstance.cpp b/src/cpp/AgentInstance.cpp index c9cff41a4..464a5f547 100644 --- a/src/cpp/AgentInstance.cpp +++ b/src/cpp/AgentInstance.cpp @@ -13,28 +13,14 @@ // limitations under the License. #include +#include namespace eprosima { namespace uxr { AgentInstance::AgentInstance() -#ifdef UAGENT_CLI_PROFILE - : app_("eProsima Micro XRCE-DDS Agent") - , udpv4_subcmd_(app_) - , udpv6_subcmd_(app_) - , tcpv4_subcmd_(app_) - , tcpv6_subcmd_(app_) -#ifndef _WIN32 - , termios_subcmd_(app_) - , pseudo_serial_subcmd_(app_) -#endif // _WIN32 - , exit_subcmd_(app_) -#endif // UAGENT_CLI_PROFILE + : callback_factory_(callback_factory_.getInstance()) { -#ifdef UAGENT_CLI_PROFILE - app_.require_subcommand(1, 1); - app_.get_formatter()->column_width(42); -#endif } AgentInstance& AgentInstance::getInstance() @@ -55,35 +41,10 @@ bool AgentInstance::create( return false; } #endif // _WIN32 -#ifdef UAGENT_CLI_PROFILE - // Parse CLI arguments. - std::stringstream ss; - for (int i = 1; i < argc; ++i) - { - ss << argv[i] << " "; - } - std::string cli_input(ss.str()); - while (true) - { - try - { - app_.parse(cli_input); - break; - } - catch(const CLI::ParseError& e) - { - app_.exit(e); - std::cin.clear(); - std::cout << std::endl; - std::cout << "Enter command: "; - std::getline(std::cin, cli_input); - } - } -#else - // Use built-in argument parser + if (2 > argc) { - agent::parser::utils::usage(); + agent::parser::utils::usage(argv[0]); return false; } const char* chosen_transport(argv[1]); @@ -93,36 +54,36 @@ bool AgentInstance::create( { case agent::TransportKind::UDP4: { - agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport, + agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport #ifndef _WIN32 - &signals_ + , &signals_ #endif // _WIN32 )); break; } case agent::TransportKind::UDP6: { - agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport, + agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport #ifndef _WIN32 - &signals_ + , &signals_ #endif // _WIN32 )); break; } case agent::TransportKind::TCP4: { - agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport, + agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport #ifndef _WIN32 - &signals_ + , &signals_ #endif // _WIN32 )); break; } case agent::TransportKind::TCP6: { - agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport, + agent_thread_ = std::move(agent::create_agent_thread(argc, argv, valid_transport #ifndef _WIN32 - &signals_ + , &signals_ #endif // _WIN32 )); break; @@ -141,36 +102,79 @@ bool AgentInstance::create( break; } #endif // _WIN32 + case agent::TransportKind::HELP: + { + // TODO(jamoralp): avoid creating this object just to show help + agent::parser::ArgumentParser(argc, argv, valid_transport).show_help(); + return false; + break; + } case agent::TransportKind::INVALID: { std::cerr << "Error: chosen transport '" << chosen_transport << "' is invalid!" << std::endl; - agent::parser::utils::usage(); + agent::parser::utils::usage(argv[0]); return false; } } -#endif // UAGENT_CLI_PROFILE + return true; } void AgentInstance::run() { -#ifdef UAGENT_CLI_PROFILE - // Wait until exit. -#ifndef _WIN32 - int n_signal = 0; - sigwait(&signals_, &n_signal); -#else - std::cin.clear(); - char exit_flag = 0; - while ('q' != exit_flag) - { - std::cin >> exit_flag; - } -#endif // _WIN32 -#else agent_thread_.join(); -#endif // UAGENT_CLI_PROFILE } +template +void AgentInstance::add_middleware_callback( + const Middleware::Kind& middleware_kind, + const middleware::CallbackKind& callback_kind, + std::function&& callback_function) +{ + callback_factory_.add_callback(middleware_kind, callback_kind, std::move(callback_function)); +} + +// Specific template specializations for used callback signatures. +#define AGENTINSTANCE_ADD_MW_CB(...) \ +template \ +UXR_AGENT_EXPORT void AgentInstance::add_middleware_callback<__VA_ARGS__>( \ + const Middleware::Kind &, \ + const middleware::CallbackKind &, \ + std::function &&); + +#ifdef UAGENT_FAST_PROFILE +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastrtps::Participant *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastrtps::Participant *, + const eprosima::fastrtps::Publisher *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastrtps::Participant *, + const eprosima::fastrtps::Subscriber *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastrtps::Participant *, + const eprosima::fastrtps::Publisher *, + const eprosima::fastrtps::Subscriber *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataWriter *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataReader *) + +AGENTINSTANCE_ADD_MW_CB( + const eprosima::fastdds::dds::DomainParticipant *, + const eprosima::fastdds::dds::DataWriter *, + const eprosima::fastdds::dds::DataReader *) +#endif // UAGENT_FAST_PROFILE + } // namespace uxr } // namespace eprosima \ No newline at end of file diff --git a/src/cpp/Root.cpp b/src/cpp/Root.cpp index c1d5b81fd..23d07f216 100644 --- a/src/cpp/Root.cpp +++ b/src/cpp/Root.cpp @@ -83,8 +83,21 @@ dds::xrce::ResultStatus Root::create_client( auto it = clients_.find(client_key); if (it == clients_.end()) { - std::shared_ptr new_client - = std::make_shared(client_representation, middleware_kind); + std::unordered_map client_properties; + + if (client_representation.properties()) + { + auto v = *client_representation.properties(); + for (auto it_props = v.begin(); it_props != v.end(); ++it_props) + { + client_properties.insert(std::pair(it_props->name(), it_props->value())); + } + } + + std::shared_ptr new_client = std::make_shared( + client_representation, + middleware_kind, + std::move(client_properties)); if (clients_.emplace(client_key, std::move(new_client)).second) { UXR_AGENT_LOG_INFO( @@ -108,7 +121,9 @@ dds::xrce::ResultStatus Root::create_client( std::shared_ptr client = clients_.at(client_key); if (session_id != client->get_session_id()) { - it->second = std::make_shared(client_representation, middleware_kind); + it->second = std::make_shared( + client_representation, + middleware_kind); } else { diff --git a/src/cpp/client/ProxyClient.cpp b/src/cpp/client/ProxyClient.cpp index 138647838..a29a97577 100644 --- a/src/cpp/client/ProxyClient.cpp +++ b/src/cpp/client/ProxyClient.cpp @@ -36,12 +36,14 @@ namespace uxr { ProxyClient::ProxyClient( const dds::xrce::CLIENT_Representation& representation, - Middleware::Kind middleware_kind) + Middleware::Kind middleware_kind, + std::unordered_map&& properties) : representation_(representation) , objects_() , session_(SessionInfo{representation.client_key(), representation.session_id(), representation.mtu()}) , state_{State::alive} , timestamp_{std::chrono::steady_clock::now()} + , properties_(std::move(properties)) { switch (middleware_kind) { @@ -60,7 +62,10 @@ ProxyClient::ProxyClient( } case Middleware::Kind::FASTDDS: { - middleware_.reset(new FastDDSMiddleware()); + bool intraprocess_enabled = + properties_.find("uxr_sm") != properties_.end() && + properties_["uxr_sm"] == "1"; + middleware_.reset(new FastDDSMiddleware(intraprocess_enabled)); break; } #endif diff --git a/src/cpp/middleware/fast/FastEntities.cpp b/src/cpp/middleware/fast/FastEntities.cpp index 1da9db65e..d2a0302ef 100644 --- a/src/cpp/middleware/fast/FastEntities.cpp +++ b/src/cpp/middleware/fast/FastEntities.cpp @@ -173,6 +173,11 @@ int16_t FastParticipant::domain_id() const return (int16_t) impl_->getAttributes().domainId; } +const fastrtps::rtps::GUID_t& FastParticipant::get_guid() const +{ + return impl_->getGuid(); +} + /********************************************************************************************************************** * FastTopic **********************************************************************************************************************/ @@ -249,6 +254,16 @@ const fastrtps::rtps::GUID_t& FastDataWriter::get_guid() const return impl_->getGuid(); } +const fastrtps::Participant* FastDataWriter::get_participant() const +{ + return publisher_->get_participant()->get_ptr(); +} + +const fastrtps::Publisher* FastDataWriter::get_ptr() const +{ + return impl_; +} + /********************************************************************************************************************** * FastDataReader **********************************************************************************************************************/ @@ -302,6 +317,21 @@ bool FastDataReader::read( return rv; } +const fastrtps::rtps::GUID_t& FastDataReader::get_guid() const +{ + return impl_->getGuid(); +} + +const fastrtps::Participant* FastDataReader::get_participant() const +{ + return subscriber_->get_participant()->get_ptr(); +} + +const fastrtps::Subscriber* FastDataReader::get_ptr() const +{ + return impl_; +} + /********************************************************************************************************************** * FastRequester **********************************************************************************************************************/ @@ -396,6 +426,27 @@ bool FastRequester::read( return rv; } +const fastrtps::Participant* FastRequester::get_participant() const +{ + const fastrtps::Participant* participant = datawriter_->get_participant(); + if (participant != datareader_->get_participant()) + { + participant = nullptr; + } + + return participant; +} + +const fastrtps::Publisher* FastRequester::get_request_datawriter() const +{ + return datawriter_->get_ptr(); +} + +const fastrtps::Subscriber* FastRequester::get_reply_datareader() const +{ + return datareader_->get_ptr(); +} + /********************************************************************************************************************** * FastReplier **********************************************************************************************************************/ @@ -503,5 +554,26 @@ bool FastReplier::read( return rv; } +const fastrtps::Participant* FastReplier::get_participant() const +{ + const fastrtps::Participant* participant = datawriter_->get_participant(); + if (participant != datareader_->get_participant()) + { + participant = nullptr; + } + + return participant; +} + +const fastrtps::Subscriber* FastReplier::get_request_datareader() const +{ + return datareader_->get_ptr(); +} + +const fastrtps::Publisher* FastReplier::get_reply_datawriter() const +{ + return datawriter_->get_ptr(); +} + } // namespace uxr } // namespace eprosima diff --git a/src/cpp/middleware/fast/FastMiddleware.cpp b/src/cpp/middleware/fast/FastMiddleware.cpp index 0315842b0..d80ac4b7b 100644 --- a/src/cpp/middleware/fast/FastMiddleware.cpp +++ b/src/cpp/middleware/fast/FastMiddleware.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -23,6 +24,19 @@ namespace uxr { using namespace fastrtps::xmlparser; +FastMiddleware::FastMiddleware() + : participants_() + , topics_() + , publishers_() + , subscribers_() + , datawriters_() + , datareaders_() + , requesters_() + , repliers_() + , callback_factory_(callback_factory_.getInstance()) +{ +} + /********************************************************************************************************************** * Create functions. **********************************************************************************************************************/ @@ -40,7 +54,13 @@ bool FastMiddleware::create_participant_by_ref( if (nullptr != impl) { std::shared_ptr participant(new FastParticipant(impl)); - rv = participants_.emplace(participant_id, std::move(participant)).second; + auto emplace_res = participants_.emplace(participant_id, std::move(participant)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_PARTICIPANT, impl); + } } } return rv; @@ -61,7 +81,13 @@ bool FastMiddleware::create_participant_by_xml( if (nullptr != impl) { std::shared_ptr participant(new FastParticipant(impl)); - rv = participants_.emplace(participant_id, std::move(participant)).second; + auto emplace_res = participants_.emplace(participant_id, std::move(participant)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_PARTICIPANT, impl); + } } } return rv; @@ -211,7 +237,19 @@ bool FastMiddleware::create_datawriter_by_ref( { std::shared_ptr datawriter = create_datawriter(attrs, &listener_, it_publisher->second); - rv = datawriter && datawriters_.emplace(datawriter_id, std::move(datawriter)).second; + if (nullptr == datawriter) + { + return false; + } + auto emplace_res = datawriters_.emplace(datawriter_id, std::move(datawriter)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_DATAWRITER, + emplace_res.first->second->get_participant(), + emplace_res.first->second->get_ptr()); + } } } return rv; @@ -231,7 +269,19 @@ bool FastMiddleware::create_datawriter_by_xml( { std::shared_ptr datawriter = create_datawriter(attrs, &listener_, it_publisher->second); - rv = datawriter && datawriters_.emplace(datawriter_id, std::move(datawriter)).second; + if (nullptr == datawriter) + { + return false; + } + auto emplace_res = datawriters_.emplace(datawriter_id, std::move(datawriter)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_DATAWRITER, + emplace_res.first->second->get_participant(), + emplace_res.first->second->get_ptr()); + } } } return rv; @@ -273,7 +323,19 @@ bool FastMiddleware::create_datareader_by_ref( { std::shared_ptr datareader = create_datareader(attrs, &listener_, it_subscriber->second); - rv = datareader && datareaders_.emplace(datareader_id, std::move(datareader)).second; + if (nullptr == datareader) + { + return false; + } + auto emplace_res = datareaders_.emplace(datareader_id, std::move(datareader)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_DATAREADER, + emplace_res.first->second->get_participant(), + emplace_res.first->second->get_ptr()); + } } } return rv; @@ -293,7 +355,19 @@ bool FastMiddleware::create_datareader_by_xml( { std::shared_ptr datareader = create_datareader(attrs, &listener_, it_subscriber->second); - rv = datareader && datareaders_.emplace(datareader_id, std::move(datareader)).second; + if (nullptr == datareader) + { + return false; + } + auto emplace_res = datareaders_.emplace(datareader_id, std::move(datareader)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_DATAREADER, + emplace_res.first->second->get_participant(), + emplace_res.first->second->get_ptr()); + } } } return rv; @@ -333,7 +407,20 @@ bool FastMiddleware::create_requester_by_ref( if (XMLP_ret::XML_OK == XMLProfileManager::fillRequesterAttributes(ref, attrs)) { std::shared_ptr requester = create_requester(attrs, &listener_, participant); - rv = requester && requesters_.emplace(requester_id, std::move(requester)).second; + if (nullptr == requester) + { + return false; + } + auto emplace_res = requesters_.emplace(requester_id, std::move(requester)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_REQUESTER, + participant->get_ptr(), + emplace_res.first->second->get_request_datawriter(), + emplace_res.first->second->get_reply_datareader()); + } } } return rv; @@ -353,7 +440,20 @@ bool FastMiddleware::create_requester_by_xml( if (xmlobjects::parse_requester(xml.data(), xml.size(), attrs)) { std::shared_ptr requester = create_requester(attrs, &listener_, participant); - rv = requester && requesters_.emplace(requester_id, std::move(requester)).second; + if (nullptr == requester) + { + return false; + } + auto emplace_res = requesters_.emplace(requester_id, std::move(requester)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_REQUESTER, + participant->get_ptr(), + emplace_res.first->second->get_request_datawriter(), + emplace_res.first->second->get_reply_datareader()); + } } } return rv; @@ -393,7 +493,20 @@ bool FastMiddleware::create_replier_by_ref( if (XMLP_ret::XML_OK == XMLProfileManager::fillReplierAttributes(ref, attrs)) { std::shared_ptr replier = create_replier(attrs, &listener_, participant); - rv = replier && repliers_.emplace(replier_id, std::move(replier)).second; + if (nullptr == replier) + { + return false; + } + auto emplace_res = repliers_.emplace(replier_id, std::move(replier)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_REPLIER, + participant->get_ptr(), + emplace_res.first->second->get_reply_datawriter(), + emplace_res.first->second->get_request_datareader()); + } } } return rv; @@ -413,7 +526,20 @@ bool FastMiddleware::create_replier_by_xml( if (xmlobjects::parse_replier(xml.data(), xml.size(), attrs)) { std::shared_ptr replier = create_replier(attrs, &listener_, participant); - rv = replier && repliers_.emplace(replier_id, std::move(replier)).second; + if (nullptr == replier) + { + return false; + } + auto emplace_res = repliers_.emplace(replier_id, std::move(replier)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_REPLIER, + participant->get_ptr(), + emplace_res.first->second->get_reply_datawriter(), + emplace_res.first->second->get_request_datareader()); + } } } return rv; @@ -425,7 +551,21 @@ bool FastMiddleware::create_replier_by_xml( bool FastMiddleware::delete_participant( uint16_t participant_id) { - return (0 != participants_.erase(participant_id)); + auto it = participants_.find(participant_id); + if (it == participants_.end()) + { + return false; + } + else + { + auto participant = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::DELETE_PARTICIPANT, + participant->get_ptr()); + + participants_.erase(participant_id); + return true; + } } bool FastMiddleware::delete_topic( @@ -449,25 +589,87 @@ bool FastMiddleware::delete_subscriber( bool FastMiddleware::delete_datawriter( uint16_t datawriter_id) { - return (0 != datawriters_.erase(datawriter_id)); + auto it = datawriters_.find(datawriter_id); + if (it == datawriters_.end()) + { + return false; + } + else + { + auto datawriter = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::DELETE_DATAWRITER, + datawriter->get_participant(), + datawriter->get_ptr()); + + datawriters_.erase(datawriter_id); + return true; + } } bool FastMiddleware::delete_datareader( uint16_t datareader_id) { - return (0 != datareaders_.erase(datareader_id)); + auto it = datareaders_.find(datareader_id); + if (it == datareaders_.end()) + { + return false; + } + else + { + auto datareader = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::DELETE_DATAREADER, + datareader->get_participant(), + datareader->get_ptr()); + + datareaders_.erase(datareader_id); + return true; + } } bool FastMiddleware::delete_requester( uint16_t requester_id) { - return (0 != requesters_.erase(requester_id)); + auto it = requesters_.find(requester_id); + if (it == requesters_.end()) + { + return false; + } + else + { + auto requester = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::DELETE_REQUESTER, + requester->get_participant(), + requester->get_request_datawriter(), + requester->get_reply_datareader()); + + requesters_.erase(requester_id); + return true; + } } bool FastMiddleware::delete_replier( uint16_t replier_id) { - return (0 != repliers_.erase(replier_id)); + auto it = repliers_.find(replier_id); + if (it == repliers_.end()) + { + return false; + } + else + { + auto replier = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTRTPS, + middleware::CallbackKind::DELETE_REPLIER, + replier->get_participant(), + replier->get_reply_datawriter(), + replier->get_request_datareader()); + + repliers_.erase(replier_id); + return true; + } } /********************************************************************************************************************** diff --git a/src/cpp/middleware/fastdds/FastDDSEntities.cpp b/src/cpp/middleware/fastdds/FastDDSEntities.cpp index 4135cbb69..892492e7b 100644 --- a/src/cpp/middleware/fastdds/FastDDSEntities.cpp +++ b/src/cpp/middleware/fastdds/FastDDSEntities.cpp @@ -300,7 +300,7 @@ bool FastDDSParticipant::register_local_topic( bool FastDDSParticipant::unregister_local_topic( const std::string& topic_name) -{ +{ ptr_->unregister_type(topic_name); return (1 == topic_register_.erase(topic_name)); } @@ -317,6 +317,16 @@ std::shared_ptr FastDDSParticipant::find_local_topic( return topic; } +const fastdds::dds::DomainParticipant* FastDDSParticipant::operator * () const +{ + return ptr_; +} + +fastdds::dds::DomainParticipant* FastDDSParticipant::operator * () +{ + return ptr_; +} + /********************************************************************************************************************** * FastDDSTopic **********************************************************************************************************************/ @@ -327,7 +337,7 @@ FastDDSType::~FastDDSType() } FastDDSTopic::~FastDDSTopic() -{ +{ participant_->unregister_local_topic(ptr_->get_name()); participant_->delete_topic(ptr_); } @@ -337,7 +347,7 @@ bool FastDDSTopic::create_by_ref(const std::string& ref) bool rv = false; fastrtps::TopicAttributes attrs; if (XMLP_ret::XML_OK == XMLProfileManager::fillTopicAttributes(ref, attrs)) - { + { rv = create_by_attributes(attrs); } return rv; @@ -348,7 +358,7 @@ bool FastDDSTopic::create_by_xml(const std::string& xml) bool rv = false; fastrtps::TopicAttributes attrs; if (xmlobjects::parse_topic(xml.data(), xml.size(), attrs)) - { + { rv = create_by_attributes(attrs); } return rv; @@ -361,7 +371,7 @@ bool FastDDSTopic::create_by_attributes(const fastrtps::TopicAttributes& attrs) { fastdds::dds::TopicQos qos; set_qos_from_attributes(qos, attrs); - + ptr_ = participant_->create_topic(attrs.getTopicName().to_string(), attrs.getTopicDataType().to_string(), qos); @@ -387,7 +397,6 @@ bool FastDDSTopic::create_by_name_type( { type_ = type; } - } return rv; } @@ -437,7 +446,7 @@ bool FastDDSTopic::match(const fastrtps::TopicAttributes& attrs) const * FastDDSPublisher **********************************************************************************************************************/ FastDDSPublisher::~FastDDSPublisher() -{ +{ if (ptr_) { participant_->delete_publisher(ptr_); @@ -449,11 +458,11 @@ bool FastDDSPublisher::create_by_xml( { bool rv = false; if (nullptr == ptr_) - { + { fastdds::dds::PublisherQos qos; fastrtps::PublisherAttributes attrs; if (0 != xml.size() && xmlobjects::parse_publisher(xml.data(), xml.size(), attrs)) - { + { set_qos_from_attributes(qos, attrs); } ptr_ = participant_->create_publisher(qos); @@ -481,7 +490,7 @@ ReturnCode_t FastDDSPublisher::delete_datawriter( * FastDDSSubscriber **********************************************************************************************************************/ FastDDSSubscriber::~FastDDSSubscriber() -{ +{ if (ptr_) { participant_->delete_subscriber(ptr_); @@ -498,11 +507,11 @@ bool FastDDSSubscriber::create_by_xml( fastrtps::SubscriberAttributes attrs; fastdds::dds::SubscriberQos qos; if (0 != xml.size() && xmlobjects::parse_subscriber(xml.data(), xml.size(), attrs)) - { + { set_qos_from_attributes(qos, attrs); } ptr_ = participant_->create_subscriber(qos); - rv = (nullptr != ptr_); + rv = (nullptr != ptr_); } return rv; } @@ -530,16 +539,16 @@ FastDDSDataWriter::~FastDDSDataWriter() if (ptr_) { publisher_->delete_datawriter(ptr_); - } + } } bool FastDDSDataWriter::create_by_ref(const std::string& ref) -{ +{ bool rv = false; if (nullptr == ptr_){ fastrtps::PublisherAttributes attrs; if (XMLP_ret::XML_OK == XMLProfileManager::fillPublisherAttributes(ref, attrs)) - { + { topic_ = publisher_->get_participant()->find_local_topic(attrs.topic.topicName.c_str()); if(topic_){ fastdds::dds::DataWriterQos qos; @@ -554,12 +563,12 @@ bool FastDDSDataWriter::create_by_ref(const std::string& ref) } bool FastDDSDataWriter::create_by_xml(const std::string& xml) -{ +{ bool rv = false; if (nullptr == ptr_){ fastrtps::PublisherAttributes attrs; if (xmlobjects::parse_publisher(xml.data(), xml.size(), attrs)) - { + { topic_ = publisher_->get_participant()->find_local_topic(attrs.topic.topicName.c_str()); if(topic_){ fastdds::dds::DataWriterQos qos; @@ -574,7 +583,7 @@ bool FastDDSDataWriter::create_by_xml(const std::string& xml) } bool FastDDSDataWriter::match(const fastrtps::PublisherAttributes& attrs) const -{ +{ fastdds::dds::DataWriterQos qos; set_qos_from_attributes(qos, attrs); return (ptr_->get_qos() == qos); @@ -586,6 +595,16 @@ bool FastDDSDataWriter::write(const std::vector& data) return ptr_->write(&const_cast&>(data)); } +const fastdds::dds::DataWriter* FastDDSDataWriter::ptr() const +{ + return ptr_; +} + +const fastdds::dds::DomainParticipant* FastDDSDataWriter::participant() const +{ + return publisher_->get_participant()->get_ptr(); +} + /********************************************************************************************************************** * FastDDSDataReader **********************************************************************************************************************/ @@ -594,16 +613,16 @@ FastDDSDataReader::~FastDDSDataReader() if (ptr_) { subscriber_->delete_datareader(ptr_); - } + } } bool FastDDSDataReader::create_by_ref(const std::string& ref) -{ +{ bool rv = false; if (nullptr == ptr_){ fastrtps::SubscriberAttributes attrs; if (XMLP_ret::XML_OK == XMLProfileManager::fillSubscriberAttributes(ref, attrs)) - { + { topic_ = subscriber_->get_participant()->find_local_topic(attrs.topic.topicName.c_str()); if(topic_){ fastdds::dds::DataReaderQos qos; @@ -618,12 +637,12 @@ bool FastDDSDataReader::create_by_ref(const std::string& ref) } bool FastDDSDataReader::create_by_xml(const std::string& xml) -{ +{ bool rv = false; if (nullptr == ptr_){ fastrtps::SubscriberAttributes attrs; if (xmlobjects::parse_subscriber(xml.data(), xml.size(), attrs)) - { + { topic_ = subscriber_->get_participant()->find_local_topic(attrs.topic.topicName.c_str()); if(topic_){ fastdds::dds::DataReaderQos qos; @@ -673,21 +692,31 @@ bool FastDDSDataReader::match_from_xml( bool FastDDSDataReader::read( std::vector& data, - std::chrono::milliseconds timeout) -{ + std::chrono::milliseconds timeout, + fastdds::dds::SampleInfo& sample_info) +{ bool rv = false; fastrtps::Duration_t d((long double) timeout.count()/1000.0); - + if(ptr_->wait_for_unread_message(d)){ - fastdds::dds::SampleInfo info; - rv = ReturnCode_t::RETCODE_OK == ptr_->take_next_sample(&data, &info); + rv = ReturnCode_t::RETCODE_OK == ptr_->take_next_sample(&data, &sample_info); } return rv; } +const fastdds::dds::DataReader* FastDDSDataReader::ptr() const +{ + return ptr_; +} + +const fastdds::dds::DomainParticipant* FastDDSDataReader::participant() const +{ + return subscriber_->get_participant()->get_ptr(); +} + /********************************************************************************************************************** * FastDDSRequester **********************************************************************************************************************/ @@ -707,7 +736,7 @@ bool FastDDSRequester::create_by_attributes( const fastrtps::RequesterAttributes& attrs) { bool rv = false; - + fastdds::dds::PublisherQos qos_publisher; set_qos_from_attributes(qos_publisher, attrs.publisher); publisher_ptr_ = participant_->create_publisher(qos_publisher); @@ -812,7 +841,7 @@ bool FastDDSRequester::read( fastrtps::Duration_t d((long double) timeout.count()/1000.0); fastdds::dds::SampleInfo info; - + if(datareader_ptr_->wait_for_unread_message(d)){ rv = ReturnCode_t::RETCODE_OK == datareader_ptr_->take_next_sample(&data, &info); } @@ -850,6 +879,21 @@ bool FastDDSRequester::read( return rv; } +const fastdds::dds::DomainParticipant* FastDDSRequester::get_participant() const +{ + return participant_->get_ptr(); +} + +const fastdds::dds::DataWriter* FastDDSRequester::get_request_datawriter() const +{ + return datawriter_ptr_; +} + +const fastdds::dds::DataReader* FastDDSRequester::get_reply_datareader() const +{ + return datareader_ptr_; +} + /********************************************************************************************************************** * FastDDSReplier **********************************************************************************************************************/ @@ -916,7 +960,7 @@ bool FastDDSReplier::match_from_xml(const std::string& xml) const } bool FastDDSReplier::match(const fastrtps::ReplierAttributes& attrs) const -{ +{ fastdds::dds::PublisherQos qos_publisher; set_qos_from_attributes(qos_publisher, attrs.publisher); fastdds::dds::SubscriberQos qos_subscriber; @@ -992,7 +1036,7 @@ bool FastDDSReplier::read( fastrtps::Duration_t d((long double) timeout.count()/1000.0); fastdds::dds::SampleInfo info; - + if(datareader_ptr_->wait_for_unread_message(d)){ rv = ReturnCode_t::RETCODE_OK == datareader_ptr_->take_next_sample(&temp_data, &info); } @@ -1022,5 +1066,20 @@ bool FastDDSReplier::read( return rv; } +const fastdds::dds::DomainParticipant* FastDDSReplier::get_participant() const +{ + return participant_->get_ptr(); +} + +const fastdds::dds::DataReader* FastDDSReplier::get_request_datareader() const +{ + return datareader_ptr_; +} + +const fastdds::dds::DataWriter* FastDDSReplier::get_reply_datawriter() const +{ + return datawriter_ptr_; +} + } // namespace uxr } // namespace eprosima \ No newline at end of file diff --git a/src/cpp/middleware/fastdds/FastDDSMiddleware.cpp b/src/cpp/middleware/fastdds/FastDDSMiddleware.cpp index 265258fa4..d11a87126 100644 --- a/src/cpp/middleware/fastdds/FastDDSMiddleware.cpp +++ b/src/cpp/middleware/fastdds/FastDDSMiddleware.cpp @@ -15,13 +15,43 @@ #include #include +#include #include "../../xmlobjects/xmlobjects.h" +#include + namespace eprosima { namespace uxr { using namespace fastrtps::xmlparser; +FastDDSMiddleware::FastDDSMiddleware() + : participants_() + , topics_() + , publishers_() + , subscribers_() + , datawriters_() + , datareaders_() + , requesters_() + , repliers_() + , callback_factory_(callback_factory_.getInstance()) +{ +} + +FastDDSMiddleware::FastDDSMiddleware(bool intraprocess_enabled) + : Middleware(intraprocess_enabled) + , participants_() + , topics_() + , publishers_() + , subscribers_() + , datawriters_() + , datareaders_() + , requesters_() + , repliers_() + , callback_factory_(callback_factory_.getInstance()) +{ +} + /********************************************************************************************************************** * Create functions. **********************************************************************************************************************/ @@ -34,8 +64,14 @@ bool FastDDSMiddleware::create_participant_by_ref( std::shared_ptr participant(new FastDDSParticipant(domain_id)); if (participant->create_by_ref(ref)) { - participants_.emplace(participant_id, std::move(participant)); - rv = true; + auto emplace_res = participants_.emplace(participant_id, std::move(participant)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_PARTICIPANT, + **(emplace_res.first->second)); + } } return rv; } @@ -49,8 +85,14 @@ bool FastDDSMiddleware::create_participant_by_xml( std::shared_ptr participant(new FastDDSParticipant(domain_id)); if (participant->create_by_xml(xml)) { - participants_.emplace(participant_id, std::move(participant)); - rv = true; + auto emplace_res = participants_.emplace(participant_id, std::move(participant)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_PARTICIPANT, + **(emplace_res.first->second)); + } } return rv; } @@ -139,7 +181,7 @@ bool FastDDSMiddleware::create_publisher_by_xml( uint16_t publisher_id, uint16_t participant_id, const std::string& xml) -{ +{ bool rv = false; auto it_participant = participants_.find(participant_id); if (participants_.end() != it_participant) @@ -181,12 +223,19 @@ bool FastDDSMiddleware::create_datawriter_by_ref( bool rv = false; auto it_publisher = publishers_.find(publisher_id); if (publishers_.end() != it_publisher) - { + { std::shared_ptr datawriter(new FastDDSDataWriter(it_publisher->second)); if (datawriter->create_by_ref(ref)) { - datawriters_.emplace(datawriter_id, std::move(datawriter)); - rv = true; + auto emplace_res = datawriters_.emplace(datawriter_id, std::move(datawriter)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_DATAWRITER, + **it_publisher->second->get_participant(), + emplace_res.first->second->ptr()); + } } } return rv; @@ -200,12 +249,19 @@ bool FastDDSMiddleware::create_datawriter_by_xml( bool rv = false; auto it_publisher = publishers_.find(publisher_id); if (publishers_.end() != it_publisher) - { + { std::shared_ptr datawriter(new FastDDSDataWriter(it_publisher->second)); if (datawriter->create_by_xml(xml)) { - datawriters_.emplace(datawriter_id, std::move(datawriter)); - rv = true; + auto emplace_res = datawriters_.emplace(datawriter_id, std::move(datawriter)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_DATAWRITER, + **it_publisher->second->get_participant(), + emplace_res.first->second->ptr()); + } } } return rv; @@ -219,12 +275,19 @@ bool FastDDSMiddleware::create_datareader_by_ref( bool rv = false; auto it_subscriber = subscribers_.find(subscriber_id); if (subscribers_.end() != it_subscriber) - { + { std::shared_ptr datareader(new FastDDSDataReader(it_subscriber->second)); if (datareader->create_by_ref(ref)) { - datareaders_.emplace(datareader_id, std::move(datareader)); - rv = true; + auto emplace_res = datareaders_.emplace(datareader_id, std::move(datareader)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_DATAREADER, + **it_subscriber->second->get_participant(), + emplace_res.first->second->ptr()); + } } } return rv; @@ -238,19 +301,25 @@ bool FastDDSMiddleware::create_datareader_by_xml( bool rv = false; auto it_subscriber = subscribers_.find(subscriber_id); if (subscribers_.end() != it_subscriber) - { + { std::shared_ptr datareader(new FastDDSDataReader(it_subscriber->second)); if (datareader->create_by_xml(xml)) { - datareaders_.emplace(datareader_id, std::move(datareader)); - rv = true; + auto emplace_res = datareaders_.emplace(datareader_id, std::move(datareader)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_DATAREADER, + **it_subscriber->second->get_participant(), + emplace_res.first->second->ptr()); + } } } return rv; } -static -std::shared_ptr create_requester( +std::shared_ptr FastDDSMiddleware::create_requester( std::shared_ptr& participant, const fastrtps::RequesterAttributes& attrs) { @@ -283,7 +352,20 @@ bool FastDDSMiddleware::create_requester_by_ref( if (XMLP_ret::XML_OK == XMLProfileManager::fillRequesterAttributes(ref, attrs)) { std::shared_ptr requester = create_requester(participant, attrs); - rv = requester && requesters_.emplace(requester_id, std::move(requester)).second; + if (nullptr == requester) + { + return false; + } + auto emplace_res = requesters_.emplace(requester_id, std::move(requester)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_REQUESTER, + participant->get_ptr(), + emplace_res.first->second->get_request_datawriter(), + emplace_res.first->second->get_reply_datareader()); + } } } return rv; @@ -303,14 +385,26 @@ bool FastDDSMiddleware::create_requester_by_xml( if (xmlobjects::parse_requester(xml.data(), xml.size(), attrs)) { std::shared_ptr requester = create_requester(participant, attrs); - rv = requester && requesters_.emplace(requester_id, std::move(requester)).second; + if (nullptr == requester) + { + return false; + } + auto emplace_res = requesters_.emplace(requester_id, std::move(requester)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_REQUESTER, + participant->get_ptr(), + emplace_res.first->second->get_request_datawriter(), + emplace_res.first->second->get_reply_datareader()); + } } } return rv; } -static -std::shared_ptr create_replier( +std::shared_ptr FastDDSMiddleware::create_replier( std::shared_ptr& participant, const fastrtps::ReplierAttributes& attrs) { @@ -343,7 +437,20 @@ bool FastDDSMiddleware::create_replier_by_ref( if (XMLP_ret::XML_OK == XMLProfileManager::fillReplierAttributes(ref, attrs)) { std::shared_ptr replier = create_replier(participant, attrs); - rv = replier && repliers_.emplace(replier_id, std::move(replier)).second; + if (nullptr == replier) + { + return false; + } + auto emplace_res = repliers_.emplace(replier_id, std::move(replier)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_REPLIER, + participant->get_ptr(), + emplace_res.first->second->get_reply_datawriter(), + emplace_res.first->second->get_request_datareader()); + } } } return rv; @@ -363,7 +470,20 @@ bool FastDDSMiddleware::create_replier_by_xml( if (xmlobjects::parse_replier(xml.data(), xml.size(), attrs)) { std::shared_ptr replier = create_replier(participant, attrs); - rv = replier && repliers_.emplace(replier_id, std::move(replier)).second; + if (nullptr == replier) + { + return false; + } + auto emplace_res = repliers_.emplace(replier_id, std::move(replier)); + rv = emplace_res.second; + if (rv) + { + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_REPLIER, + participant->get_ptr(), + emplace_res.first->second->get_reply_datawriter(), + emplace_res.first->second->get_request_datareader()); + } } } return rv; @@ -375,49 +495,125 @@ bool FastDDSMiddleware::create_replier_by_xml( bool FastDDSMiddleware::delete_participant( uint16_t participant_id) { - return (0 != participants_.erase(participant_id)); + auto it = participants_.find(participant_id); + if (it == participants_.end()) + { + return false; + } + else + { + auto participant = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::DELETE_PARTICIPANT, + participant->get_ptr()); + + participants_.erase(participant_id); + return true; + } } bool FastDDSMiddleware::delete_topic( uint16_t topic_id) { - return (0 != topics_.erase(topic_id)); + return (0 != topics_.erase(topic_id)); } bool FastDDSMiddleware::delete_publisher( uint16_t publisher_id) { - return (0 != publishers_.erase(publisher_id)); + return (0 != publishers_.erase(publisher_id)); } bool FastDDSMiddleware::delete_subscriber( uint16_t subscriber_id) { - return (0 != subscribers_.erase(subscriber_id)); + return (0 != subscribers_.erase(subscriber_id)); } bool FastDDSMiddleware::delete_datawriter( uint16_t datawriter_id) { - return (0 != datawriters_.erase(datawriter_id)); + auto it = datawriters_.find(datawriter_id); + if (it == datawriters_.end()) + { + return false; + } + else + { + auto datawriter = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::DELETE_DATAWRITER, + datawriter->participant(), + datawriter->ptr()); + + datawriters_.erase(datawriter_id); + return true; + } } bool FastDDSMiddleware::delete_datareader( uint16_t datareader_id) { - return (0 != datareaders_.erase(datareader_id)); + auto it = datareaders_.find(datareader_id); + if (it == datareaders_.end()) + { + return false; + } + else + { + auto datareader = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::DELETE_DATAREADER, + datareader->participant(), + datareader->ptr()); + + datareaders_.erase(datareader_id); + return true; + } } bool FastDDSMiddleware::delete_requester( uint16_t requester_id) { - return (0 != requesters_.erase(requester_id)); + auto it = requesters_.find(requester_id); + if (it == requesters_.end()) + { + return false; + } + else + { + auto requester = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::DELETE_REQUESTER, + requester->get_participant(), + requester->get_request_datawriter(), + requester->get_reply_datareader()); + + requesters_.erase(requester_id); + return true; + } } bool FastDDSMiddleware::delete_replier( uint16_t replier_id) { - return (0 != repliers_.erase(replier_id)); + auto it = repliers_.find(replier_id); + if (it == repliers_.end()) + { + return false; + } + else + { + auto replier = it->second; + callback_factory_.execute_callbacks(Middleware::Kind::FASTDDS, + middleware::CallbackKind::DELETE_REPLIER, + replier->get_participant(), + replier->get_reply_datawriter(), + replier->get_request_datareader()); + + repliers_.erase(replier_id); + return true; + } } /********************************************************************************************************************** @@ -472,7 +668,20 @@ bool FastDDSMiddleware::read_data( auto it = datareaders_.find(datareader_id); if (datareaders_.end() != it) { - rv = it->second->read(data, timeout); + fastdds::dds::SampleInfo sample_info; + rv = it->second->read(data, timeout, sample_info); + + if (intraprocess_enabled_) + { + for (auto dw = datawriters_.begin(); dw != datawriters_.end(); dw++) + { + if (dw->second->guid() == sample_info.sample_identity.writer_guid()) + { + rv = false; + break; + } + } + } } return rv; } diff --git a/src/cpp/p2p/InternalClient.cpp b/src/cpp/p2p/InternalClient.cpp index c8a3869b2..acff48475 100644 --- a/src/cpp/p2p/InternalClient.cpp +++ b/src/cpp/p2p/InternalClient.cpp @@ -37,7 +37,6 @@ InternalClient::InternalClient( , topics_{} , topic_counter_{0} , transport_{} - , platform_{} , remote_client_key_{remote_client_key} , local_client_key_{local_client_key} , session_{} @@ -99,7 +98,7 @@ bool InternalClient::run() if (agent_.create_client(INTERNAL_CLIENT_KEY, 0x00, UXR_CONFIG_UDP_TRANSPORT_MTU, Middleware::Kind::CED, result)) { /* Transport. */ - if (uxr_init_udp_transport(&transport_, &platform_, UXR_IPv4, ip.c_str(), port.c_str())) + if (uxr_init_udp_transport(&transport_, UXR_IPv4, ip.c_str(), port.c_str())) { /* Session. */ uxr_init_session(&session_, &transport_.comm, local_client_key_); diff --git a/src/cpp/processor/Processor.cpp b/src/cpp/processor/Processor.cpp index bd07cffc4..8bc63f96a 100644 --- a/src/cpp/processor/Processor.cpp +++ b/src/cpp/processor/Processor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace eprosima { namespace uxr { @@ -47,9 +48,24 @@ void Processor::process_input_packet( { if (input_packet.message->prepare_next_submessage()) { - if (input_packet.message->get_subheader().submessage_id() == dds::xrce::CREATE_CLIENT) + switch (input_packet.message->get_subheader().submessage_id()) { - process_create_client_submessage(input_packet); + case dds::xrce::CREATE_CLIENT: + { + process_create_client_submessage(input_packet); + break; + } + case dds::xrce::GET_INFO: + { + OutputPacket output_packet; + process_get_info_packet(std::move(input_packet), output_packet); + server_.push_output_packet(std::move(output_packet)); + break; + } + default: + { + break; + } } } } @@ -110,7 +126,7 @@ void Processor::process_input_packet( output_packet.message.reset(new OutputMessage(acknack_header, message_size)); output_packet.message->append_submessage(dds::xrce::ACKNACK, acknack_payload); - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } else @@ -137,7 +153,7 @@ void Processor::process_input_packet( output_packet.message.reset(new OutputMessage(input_packet.message->get_header(), message_size)); output_packet.message->append_submessage(dds::xrce::STATUS, status_payload); - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } } @@ -268,7 +284,7 @@ bool Processor::process_create_client_submessage( output_packet.message = std::shared_ptr(new OutputMessage(status_header, message_size)); output_packet.message->append_submessage(dds::xrce::STATUS_AGENT, status_agent); - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } else @@ -309,7 +325,7 @@ bool Processor::process_create_submessage( output_packet.destination = input_packet.source; while (client.session().get_next_output_message(dds::xrce::STREAMID_BUILTIN_RELIABLE, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } return rv; @@ -348,7 +364,7 @@ bool Processor::process_delete_submessage( while (client.session().get_next_output_message(dds::xrce::STREAMID_NONE, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } else @@ -363,7 +379,7 @@ bool Processor::process_delete_submessage( while (client.session().get_next_output_message(dds::xrce::STREAMID_BUILTIN_RELIABLE, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } } @@ -534,7 +550,7 @@ bool Processor::process_read_data_submessage( output_packet.destination = input_packet.source; while (client.session().get_next_output_message(dds::xrce::STREAMID_BUILTIN_RELIABLE, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } } @@ -570,14 +586,14 @@ bool Processor::process_acknack_submessage( { if (client.session().get_output_message(stream_id, first_message + i, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } if ((nack_bitmap.at(0) & mask) == mask) { if (client.session().get_output_message(stream_id, first_message + i + 8, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } } @@ -623,7 +639,7 @@ bool Processor::process_heartbeat_submessage( output_packet.destination = input_packet.source; if (client.session().get_next_output_message(dds::xrce::STREAMID_NONE, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } else @@ -688,7 +704,7 @@ bool Processor::process_timestamp_submessage( output_packet.destination = input_packet.source; if (client.session().get_next_output_message(dds::xrce::STREAMID_NONE, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } else @@ -722,7 +738,7 @@ bool Processor::read_data_callback( while (cb_args.client->session().get_next_output_message(cb_args.stream_id, output_packet.message)) { - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } else @@ -732,6 +748,51 @@ bool Processor::read_data_callback( return rv; } +template +bool Processor::process_get_info_packet( + InputPacket&& input_packet, + OutputPacket& output_packet) const +{ + bool rv = false; + + dds::xrce::GET_INFO_Payload get_info_payload; + input_packet.message->get_payload(get_info_payload); + + dds::xrce::ObjectInfo object_info; + dds::xrce::ResultStatus result_status = root_.get_info(object_info); + if (dds::xrce::STATUS_OK == result_status.status()) + { + dds::xrce::AGENT_ActivityInfo agent_info; + agent_info.availability(1); + + dds::xrce::ActivityInfoVariant info_variant; + info_variant.agent(agent_info); + object_info.activity(info_variant); + + dds::xrce::INFO_Payload info_payload; + info_payload.related_request().request_id(get_info_payload.request_id()); + info_payload.related_request().object_id(get_info_payload.object_id()); + info_payload.result(result_status); + info_payload.object_info(object_info); + + dds::xrce::SubmessageHeader info_subheader; + info_subheader.submessage_id(dds::xrce::INFO); + info_subheader.flags(dds::xrce::FLAG_LITTLE_ENDIANNESS); + info_subheader.submessage_length(uint16_t(info_payload.getCdrSerializedSize())); + + const size_t message_size = input_packet.message->get_header().getCdrSerializedSize() + + info_subheader.getCdrSerializedSize() + + info_payload.getCdrSerializedSize(); + + output_packet.destination = input_packet.source; + output_packet.message = OutputMessagePtr(new OutputMessage(input_packet.message->get_header(), + message_size)); + rv = output_packet.message->append_submessage(dds::xrce::INFO, info_payload); + } + + return rv; +} + template bool Processor::process_get_info_packet( InputPacket&& input_packet, @@ -825,7 +886,7 @@ void Processor::check_heartbeats() output_packet.message = OutputMessagePtr(new OutputMessage(header, message_size)); output_packet.message->append_submessage(dds::xrce::HEARTBEAT, heartbeat); - server_.push_output_packet(output_packet); + server_.push_output_packet(std::move(output_packet)); } } } @@ -835,6 +896,7 @@ void Processor::check_heartbeats() template class Processor; template class Processor; template class Processor; +template class Processor; } // namespace uxr } // namespace eprosima diff --git a/src/cpp/transport/Server.cpp b/src/cpp/transport/Server.cpp index a0c2812e0..1df335c71 100644 --- a/src/cpp/transport/Server.cpp +++ b/src/cpp/transport/Server.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -32,6 +33,7 @@ namespace uxr { extern template class Processor; extern template class Processor; extern template class Processor; +extern template class Processor; template Server::Server(Middleware::Kind middleware_kind) @@ -112,6 +114,7 @@ bool Server::stop() /* Close servers. */ bool rv = true; + // TODO: check at run time if P2P and discovery are implemented #ifdef UAGENT_DISCOVERY_PROFILE rv = fini_discovery() && rv; #endif @@ -162,7 +165,7 @@ bool Server::disable_p2p() template void Server::push_output_packet( - OutputPacket output_packet) + OutputPacket&& output_packet) { if (output_packet.message) { @@ -262,6 +265,7 @@ void Server::error_handler_loop() template class Server; template class Server; template class Server; +template class Server; } // namespace uxr } // namespace eprosima diff --git a/src/cpp/transport/custom/CustomAgent.cpp b/src/cpp/transport/custom/CustomAgent.cpp new file mode 100644 index 000000000..86425beb0 --- /dev/null +++ b/src/cpp/transport/custom/CustomAgent.cpp @@ -0,0 +1,333 @@ +// Copyright 2021-present Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace eprosima { +namespace uxr { + +namespace { +const std::string transport_rc_to_str( + const TransportRc& transport_rc) +{ + switch (transport_rc) + { + case TransportRc::connection_error: + { + return std::string("connection error"); + } + case TransportRc::timeout_error: + { + return std::string("timeout error"); + } + case TransportRc::server_error: + { + return std::string("server error"); + } + default: + { + return std::string(); + } + } +} + +} // anonymous namespace + +CustomAgent::CustomAgent( + const std::string& name, + CustomEndPoint* endpoint, + Middleware::Kind middleware_kind, + bool framing, + InitFunction& init_function, + FiniFunction& fini_function, + SendMsgFunction& send_msg_function, + RecvMsgFunction& recv_msg_function) + : Server(middleware_kind) + , name_(name) + , recv_endpoint_(endpoint) + , send_endpoint_(nullptr) + , custom_init_func_(init_function) + , custom_fini_func_(fini_function) + , custom_send_msg_func_(send_msg_function) + , custom_recv_msg_func_(recv_msg_function) + , framing_(framing) + , framing_io_(0x00, + [&]( + uint8_t* buffer, + size_t message_length, + TransportRc& transport_rc) -> ssize_t + { + return custom_send_msg_func_( + send_endpoint_, + buffer, + message_length, + transport_rc); + }, + [&]( + uint8_t* buffer, + size_t buffer_length, + int timeout, + TransportRc& transport_rc) -> ssize_t + { + return custom_recv_msg_func_( + recv_endpoint_, + buffer, + buffer_length, + timeout, + transport_rc); + }) +{ +} + +CustomAgent::~CustomAgent() +{ + try + { + this->stop(); + } + catch (const std::exception& e) + { + UXR_AGENT_LOG_CRITICAL( + UXR_DECORATE_RED("Error stopping custom agent server"), + "{} agent exception: {}", + name_, e.what()); + } +} + +bool CustomAgent::init() +{ + try + { + bool user_init_res = custom_init_func_(); + + if (user_init_res) + { + UXR_AGENT_LOG_INFO( + UXR_DECORATE_GREEN("Custom agent status: opened"), + "{} agent running", + name_); + } + else + { + UXR_AGENT_LOG_ERROR( + UXR_DECORATE_RED("Error initializing custom agent server"), + "{} agent error", + name_); + } + + return user_init_res; + } + catch (const std::exception& e) + { + UXR_AGENT_LOG_CRITICAL( + UXR_DECORATE_RED("Error initializing custom agent server"), + "{} agent exception: {}", + name_, e.what()); + + return false; + } +} + +bool CustomAgent::fini() +{ + try + { + bool user_fini_res = custom_fini_func_(); + + if (user_fini_res) + { + UXR_AGENT_LOG_INFO( + UXR_DECORATE_GREEN("Custom agent status: closed"), + "{} agent closed", + name_); + } + else + { + UXR_AGENT_LOG_ERROR( + UXR_DECORATE_RED("Error terminating custom agent server"), + "{} agent error", + name_); + } + + return user_fini_res; + } + catch (const std::exception& e) + { + UXR_AGENT_LOG_CRITICAL( + UXR_DECORATE_RED("Error terminating custom agent server"), + "{} agent exception: {}", + name_, e.what()); + + return false; + } +} + +bool CustomAgent::recv_message( + InputPacket& input_packet, + int timeout, + TransportRc& transport_rc) +{ + // Reset recv_endpoint_ members before receiving a new message. + recv_endpoint_->reset(); + + try + { + ssize_t recv_bytes; + if (framing_) + { + uint8_t remote_addr = 0x00; + recv_bytes = framing_io_.read_framed_msg( + buffer_, SERVER_BUFFER_SIZE, remote_addr, timeout, transport_rc); + } + else + { + recv_bytes = custom_recv_msg_func_( + recv_endpoint_, buffer_, SERVER_BUFFER_SIZE, timeout, transport_rc); + } + + bool success = (0 <= recv_bytes && TransportRc::ok == transport_rc); + if (success) + { + // User must have filled all the members of the endpoint. + recv_endpoint_->check_non_empty_members(); + + input_packet.message.reset( + new eprosima::uxr::InputMessage( + buffer_, static_cast(recv_bytes))); + input_packet.source = *recv_endpoint_; + + uint32_t raw_client_key = 0u; + this->get_client_key(input_packet.source, raw_client_key); + + std::stringstream ss; + ss << UXR_COLOR_YELLOW << "[==>> " << name_ << " <<==]" << UXR_COLOR_RESET; + UXR_AGENT_LOG_MESSAGE( + ss.str(), + raw_client_key, + input_packet.message->get_buf(), + input_packet.message->get_len()); + } + else if (TransportRc::timeout_error != transport_rc) + { + // Printing a trace for timeout_error would fill the log with too much messages. + std::stringstream ss; + ss << UXR_COLOR_RED << "Error while receiving message: " + << transport_rc_to_str(transport_rc) << UXR_COLOR_RESET; + UXR_AGENT_LOG_ERROR( + ss.str(), + "{} agent error", + name_); + } + + return success; + } + catch (const std::exception& e) + { + UXR_AGENT_LOG_ERROR( + UXR_DECORATE_RED("Error while receiving message"), + "custom {} agent, exception: {}", + name_, e.what()); + + return false; + } +} + +bool CustomAgent::send_message( + OutputPacket output_packet, + TransportRc& transport_rc) +{ + try + { + ssize_t sent_bytes; + if (framing_) + { + send_endpoint_ = &output_packet.destination; + sent_bytes = framing_io_.write_framed_msg( + output_packet.message->get_buf(), + output_packet.message->get_len(), + 0x00, + transport_rc); + send_endpoint_ = nullptr; + } + else + { + sent_bytes = custom_send_msg_func_( + &output_packet.destination, + output_packet.message->get_buf(), + output_packet.message->get_len(), + transport_rc); + } + + bool success = (output_packet.message->get_len() == static_cast(sent_bytes)); + if (success) + { + uint32_t raw_client_key = 0u; + this->get_client_key(output_packet.destination, raw_client_key); + + std::stringstream ss; + ss << UXR_COLOR_YELLOW << "[** <<" << name_ << ">> **]" << UXR_COLOR_RESET; + UXR_AGENT_LOG_MESSAGE( + ss.str(), + raw_client_key, + output_packet.message->get_buf(), + output_packet.message->get_len()); + } + else + { + std::stringstream ss; + ss << UXR_COLOR_RED + << "Error while sending message: " + << transport_rc_to_str(transport_rc) + << ". Expected to send " + << output_packet.message->get_len() + << " bytes, but sent " + << sent_bytes + << "instead" + << UXR_COLOR_RESET; + UXR_AGENT_LOG_ERROR( + ss.str(), + "{} agent error", + name_); + } + + return success; + } + catch (const std::exception& e) + { + UXR_AGENT_LOG_ERROR( + UXR_DECORATE_RED("Error while sending message"), + "custom {} agent, exception: {}", + name_, e.what()); + + return false; + } +} + +bool CustomAgent::handle_error( + TransportRc transport_rc) +{ + std::stringstream ss; + ss << UXR_COLOR_RED << "Recovering from error: " + << transport_rc_to_str(transport_rc) << UXR_COLOR_RESET; + UXR_AGENT_LOG_ERROR( + ss.str(), + "{} agent error", + name_); + return fini() && init(); +} + +} // namespace uxr +} // namespace eprosima \ No newline at end of file diff --git a/src/cpp/transport/serial/SerialAgentLinux.cpp b/src/cpp/transport/serial/SerialAgentLinux.cpp index 29f9aaa74..bc0082ea9 100644 --- a/src/cpp/transport/serial/SerialAgentLinux.cpp +++ b/src/cpp/transport/serial/SerialAgentLinux.cpp @@ -28,13 +28,13 @@ SerialAgent::SerialAgent( , addr_{addr} , poll_fd_{} , buffer_{0} - , serial_io_( + , framing_io_( addr, std::bind(&SerialAgent::write_data, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), std::bind(&SerialAgent::read_data, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)) {} -size_t SerialAgent::write_data( +ssize_t SerialAgent::write_data( uint8_t* buf, size_t len, TransportRc& transport_rc) @@ -52,22 +52,18 @@ size_t SerialAgent::write_data( return rv; } -size_t SerialAgent::read_data( +ssize_t SerialAgent::read_data( uint8_t* buf, size_t len, int timeout, TransportRc& transport_rc) { - size_t rv = 0; + ssize_t bytes_read = 0; int poll_rv = poll(&poll_fd_, 1, timeout); if (0 < poll_rv) { - ssize_t bytes_read = read(poll_fd_.fd, buf, len); - if (0 < bytes_read) - { - rv = size_t(bytes_read); - } - else + bytes_read = read(poll_fd_.fd, buf, len); + if (0 > bytes_read) { transport_rc = TransportRc::server_error; } @@ -76,7 +72,7 @@ size_t SerialAgent::read_data( { transport_rc = (poll_rv == 0) ? TransportRc::timeout_error : TransportRc::server_error; } - return rv; + return bytes_read; } bool SerialAgent::recv_message( @@ -86,7 +82,7 @@ bool SerialAgent::recv_message( { bool rv = false; uint8_t remote_addr; - size_t bytes_read = serial_io_.read_msg(buffer_,sizeof (buffer_), remote_addr, timeout, transport_rc); + ssize_t bytes_read = framing_io_.read_framed_msg(buffer_,sizeof (buffer_), remote_addr, timeout, transport_rc); if (0 < bytes_read) { input_packet.message.reset(new InputMessage(buffer_, static_cast(bytes_read))); @@ -111,13 +107,14 @@ bool SerialAgent::send_message( TransportRc& transport_rc) { bool rv = false; - size_t bytes_written = - serial_io_.write_msg( + ssize_t bytes_written = + framing_io_.write_framed_msg( output_packet.message->get_buf(), output_packet.message->get_len(), output_packet.destination.get_addr(), transport_rc); - if ((0 < bytes_written) && (bytes_written == output_packet.message->get_len())) + if ((0 < bytes_written) && ( + static_cast(bytes_written) == output_packet.message->get_len())) { rv = true; diff --git a/src/cpp/transport/serial/SerialProtocol.cpp b/src/cpp/transport/serial/SerialProtocol.cpp deleted file mode 100644 index 32a2d5063..000000000 --- a/src/cpp/transport/serial/SerialProtocol.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -namespace eprosima { -namespace uxr { - -constexpr uint16_t SerialIO::crc16_table[256]; - -} // namespace uxr -} // namespace eprosima diff --git a/src/cpp/transport/stream_framing/StreamFramingProtocol.cpp b/src/cpp/transport/stream_framing/StreamFramingProtocol.cpp new file mode 100644 index 000000000..5833f76de --- /dev/null +++ b/src/cpp/transport/stream_framing/StreamFramingProtocol.cpp @@ -0,0 +1,493 @@ +// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace eprosima { +namespace uxr { + +constexpr uint16_t FramingIO::crc16_table[256]; + +FramingIO::FramingIO( + uint8_t local_addr, + WriteCallback write_callback, + ReadCallback read_callback) + : state_(InputState::UXR_FRAMING_UNINITIALIZED) + , local_addr_(local_addr) + , remote_addr_(0) + , read_buffer_() + , read_buffer_head_(0) + , read_buffer_tail_(0) + , read_callback_(read_callback) + , msg_len_(0) + , msg_pos_(0) + , msg_crc_(0) + , cmp_crc_(0) + , write_buffer_() + , write_buffer_pos_(0) + , write_callback_(write_callback) +{ +} + +size_t FramingIO::write_framed_msg( + const uint8_t* buf, + size_t len, + uint8_t remote_addr, + TransportRc& transport_rc) +{ + /* Buffer being flag. */ + write_buffer_[0] = framing_begin_flag; + write_buffer_pos_ = 1; + + /* Buffer header. */ + add_next_octet(local_addr_); + add_next_octet(remote_addr); + add_next_octet(static_cast(len & 0xFF)); + add_next_octet(static_cast(len >> 8)); + + /* Write payload. */ + uint8_t octet = 0; + uint16_t written_len = 0; + uint16_t crc = 0; + bool cond = true; + while (written_len < len && cond) + { + octet = static_cast(*(buf + written_len)); + if (add_next_octet(octet)) + { + update_crc(crc, octet); + ++written_len; + } + else + { + cond = transport_write(transport_rc); + } + } + + /* Write CRC. */ + uint8_t tmp_crc[2]; + tmp_crc[0] = static_cast(crc & 0xFF); + tmp_crc[1] = static_cast(crc >> 8); + written_len = 0; + while (written_len < sizeof(tmp_crc) && cond) + { + octet = *(tmp_crc + written_len); + if (add_next_octet(octet)) + { + update_crc(crc, octet); + ++written_len; + } + else + { + cond = transport_write(transport_rc); + } + } + + /* Flus write buffer. */ + if (cond && (0 < write_buffer_pos_)) + { + cond = transport_write(transport_rc); + } + + return cond ? len : 0; +} + +size_t FramingIO::read_framed_msg( + uint8_t* buf, + size_t len, + uint8_t& remote_addr, + int timeout, + TransportRc& transport_rc) +{ + size_t rv = 0; + + size_t read_bytes = transport_read(timeout, transport_rc); + + if (0 < read_bytes || (read_buffer_tail_ != read_buffer_head_)) + { + /** + * State Machine. + */ + bool exit_cond = false; + while (!exit_cond) + { + uint8_t octet = 0; + switch (state_) + { + case InputState::UXR_FRAMING_UNINITIALIZED: + { + octet = 0; + while ((framing_begin_flag != octet) && + (read_buffer_head_ != read_buffer_tail_)) + { + octet = read_buffer_[read_buffer_tail_]; + read_buffer_tail_ = + static_cast( + static_cast( + read_buffer_tail_ + 1) % + sizeof(read_buffer_)); + } + + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else + { + exit_cond = true; + } + break; + } + case InputState::UXR_FRAMING_READING_SRC_ADDR: + { + if (get_next_octet(remote_addr_)) + { + state_ = InputState::UXR_FRAMING_READING_DST_ADDR; + } + else + { + if (framing_begin_flag != remote_addr_) + { + exit_cond = true; + } + } + break; + } + case InputState::UXR_FRAMING_READING_DST_ADDR: + { + if (get_next_octet(octet)) + { + state_ = (octet == local_addr_) + ? InputState::UXR_FRAMING_READING_LEN_LSB + : InputState::UXR_FRAMING_UNINITIALIZED; + } + else + { + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else + { + exit_cond = true; + } + } + break; + } + case InputState::UXR_FRAMING_READING_LEN_LSB: + { + if (get_next_octet(octet)) + { + msg_len_ = octet; + state_ = InputState::UXR_FRAMING_READING_LEN_MSB; + } + else + { + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else + { + exit_cond = true; + } + } + break; + } + case InputState::UXR_FRAMING_READING_LEN_MSB: + { + if (get_next_octet(octet)) + { + msg_len_ += (octet << 8); + msg_pos_ = 0; + cmp_crc_ = 0; + if (len < msg_len_) + { + state_ = InputState::UXR_FRAMING_UNINITIALIZED; + exit_cond = true; + } + else + { + state_ = InputState::UXR_FRAMING_READING_PAYLOAD; + } + } + else + { + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else + { + exit_cond = true; + } + } + break; + } + case InputState::UXR_FRAMING_READING_PAYLOAD: + { + while ((msg_pos_ < msg_len_) && get_next_octet(octet)) + { + buf[static_cast(msg_pos_)] = octet; + ++msg_pos_; + update_crc(cmp_crc_, octet); + } + + if (msg_pos_ == msg_len_) + { + state_ = InputState::UXR_FRAMING_READING_CRC_LSB; + } + else + { + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else if (0 < transport_read(timeout, transport_rc)) + { + /* Do nothing */ + } + else + { + exit_cond = true; + } + } + break; + } + case InputState::UXR_FRAMING_READING_CRC_LSB: + { + if (get_next_octet(octet)) + { + msg_crc_ = octet; + state_ = InputState::UXR_FRAMING_READING_CRC_MSB; + } + else + { + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else + { + exit_cond = true; + } + } + break; + } + case InputState::UXR_FRAMING_READING_CRC_MSB: + if (get_next_octet(octet)) + { + msg_crc_ += (octet << 8); + state_ = InputState::UXR_FRAMING_UNINITIALIZED; + if (cmp_crc_ == msg_crc_) + { + remote_addr = remote_addr_; + rv = msg_len_; + } + exit_cond = true; + } + else + { + if (framing_begin_flag == octet) + { + state_ = InputState::UXR_FRAMING_READING_SRC_ADDR; + } + else + { + exit_cond = true; + } + } + break; + } + } + } + + return rv; +} + +void FramingIO::update_crc( + uint16_t& crc, + const uint8_t data) +{ + crc = (crc >> 8) ^ crc16_table[(crc ^ data) & 0xFF]; +} + +bool FramingIO::get_next_octet( + uint8_t& octet) +{ + bool rv = false; + octet = 0; + + if (read_buffer_head_ != read_buffer_tail_) + { + if (framing_esc_flag != read_buffer_[read_buffer_tail_]) + { + octet = read_buffer_[read_buffer_tail_]; + read_buffer_tail_ = static_cast( + static_cast(read_buffer_tail_ + 1) % sizeof(read_buffer_)); + + rv = (framing_begin_flag != octet); + } + else + { + uint8_t temp_tail = static_cast( + static_cast(read_buffer_tail_ + 1) % sizeof(read_buffer_)); + + if (temp_tail != read_buffer_head_) + { + octet = read_buffer_[temp_tail]; + read_buffer_tail_ = static_cast( + static_cast(read_buffer_tail_ + 2) % sizeof(read_buffer_)); + + if (framing_begin_flag != octet) + { + octet ^= framing_xor_flag; + rv = true; + } + } + } + } + + return rv; +} + +bool FramingIO::add_next_octet( + uint8_t octet) +{ + bool rv = false; + + if (framing_begin_flag == octet || framing_esc_flag == octet) + { + if (static_cast(write_buffer_pos_ + 1) < sizeof(write_buffer_)) + { + write_buffer_[write_buffer_pos_] = framing_esc_flag; + write_buffer_[write_buffer_pos_ + 1] = octet ^ framing_xor_flag; + write_buffer_pos_ += 2; + rv = true; + } + } + else if (write_buffer_pos_ < sizeof(write_buffer_)) + { + write_buffer_[write_buffer_pos_] = octet; + ++write_buffer_pos_; + rv = true; + } + + return rv; +} + +bool FramingIO::transport_write( + TransportRc& transport_rc) +{ + size_t bytes_written = 0; + size_t last_written = 0; + + do + { + ssize_t write_res = write_callback_(write_buffer_, write_buffer_pos_, transport_rc); + last_written = (0 < write_res) ? write_res : 0; + bytes_written += last_written; + } while (bytes_written < write_buffer_pos_ && 0 < last_written); + + if (write_buffer_pos_ == bytes_written) + { + write_buffer_pos_ = 0; + return true; + } + + return false; +} + +size_t FramingIO::transport_read( + int& timeout, + TransportRc& transport_rc) +{ + const auto time_init = std::chrono::system_clock::now(); + + /** + * Compute read-buffer available size. + * We keep track of two possible fragments to handle the case of + * some intermediate section of the circular buffer being written, + * that is, head > tail. + */ + uint8_t available_length[2] = {0, 0}; + if (read_buffer_head_ == read_buffer_tail_) + { + read_buffer_head_ = 0; + read_buffer_tail_ = 0; + available_length[0] = sizeof(read_buffer_) - 1; + } + else if (read_buffer_head_ > read_buffer_tail_) + { + if (0 < read_buffer_tail_) + { + available_length[0] = + static_cast(sizeof(read_buffer_) - read_buffer_head_); + available_length[1] = + static_cast(read_buffer_tail_ - 1); + } + else + { + available_length[0] = + static_cast(sizeof(read_buffer_) - read_buffer_head_ - 1); + } + } + else + { + available_length[0] = + static_cast(read_buffer_tail_ - read_buffer_head_ - 1); + } + + /** + * Read from serial, according to the fragment size previously calculated. + */ + size_t bytes_read[2] = {0, 0}; + if (0 < available_length[0]) + { + ssize_t read_res = read_callback_(&read_buffer_[read_buffer_head_], + available_length[0], + timeout, + transport_rc); + bytes_read[0] = (0 < read_res) ? read_res : 0; + + read_buffer_head_ = static_cast( + static_cast(read_buffer_head_ + bytes_read[0]) % sizeof(read_buffer_)); + + if (0 < bytes_read[0]) + { + if ((bytes_read[0] == available_length[0]) && (0 < available_length[1])) + { + read_res = read_callback_(&read_buffer_[read_buffer_head_], + available_length[1], + 0, + transport_rc); + bytes_read[1] = (0 < read_res) ? read_res : 0; + + read_buffer_head_ = static_cast( + static_cast(read_buffer_head_ + bytes_read[1]) % sizeof(read_buffer_)); + } + } + } + + timeout -= static_cast( + std::chrono::duration_cast( + time_init - std::chrono::system_clock::now()) + .count()); + + return bytes_read[0] + bytes_read[1]; +} + +} // namespace uxr +} // namespace eprosima \ No newline at end of file diff --git a/src/cpp/types/XRCETypes.cpp b/src/cpp/types/XRCETypes.cpp index 956051ccc..f82d238bb 100644 --- a/src/cpp/types/XRCETypes.cpp +++ b/src/cpp/types/XRCETypes.cpp @@ -8799,6 +8799,18 @@ void dds::EntityId_t::deserialize(eprosima::fastcdr::Cdr &dcdr) dcdr >> m_entityKind; } +dds::GUID_t::GUID_t(const GUID_t &x) +{ + m_guidPrefix = x.m_guidPrefix; + m_entityId = x.m_entityId; +} + +dds::GUID_t::GUID_t(GUID_t &&x) +{ + m_guidPrefix = std::move(x.m_guidPrefix); + m_entityId = std::move(x.m_entityId); +} + size_t dds::GUID_t::getMaxCdrSerializedSize(size_t current_alignment) { size_t initial_alignment = current_alignment; diff --git a/src/cpp/utils/ArgumentParser.cpp b/src/cpp/utils/ArgumentParser.cpp index e649a06b4..eea3737ea 100644 --- a/src/cpp/utils/ArgumentParser.cpp +++ b/src/cpp/utils/ArgumentParser.cpp @@ -20,10 +20,17 @@ // TODO(jamoralp): move definitions of ArgumentParser.hpp into this file, to maintain code coherence. bool eprosima::uxr::agent::parser::utils::usage( + const char* executable_name, bool no_help) { + std::string executable_name_str(executable_name); + size_t pos = executable_name_str.rfind('/'); + if (std::string::npos != pos) + { + executable_name_str = executable_name_str.substr(pos + 1); + } std::stringstream ss; - ss << "Usage: 'MicroXRCEAgent && properties = {}) {}; ~ProxyClient() = default; diff --git a/test/unittest/agent/AgentUnitTests.cpp b/test/unittest/agent/AgentUnitTests.cpp index 18b460af3..353112de1 100644 --- a/test/unittest/agent/AgentUnitTests.cpp +++ b/test/unittest/agent/AgentUnitTests.cpp @@ -14,6 +14,7 @@ #include #include +#include #include @@ -23,7 +24,7 @@ namespace testing { using namespace eprosima::uxr; -class AgentUnitTests : public ::testing::TestWithParam +class AgentUnitTests : public ::testing::TestWithParam { protected: AgentUnitTests() @@ -949,7 +950,7 @@ TEST_P(AgentUnitTests, CreateReplierByRef) uint8_t flag = 0x00; /* - * Create Requester. + * Create replier. */ agent_.create_participant_by_ref(client_key_, participant_id, domain_id, participant_ref, flag, result); EXPECT_TRUE(agent_.create_replier_by_ref(client_key_, replier_id, participant_id, ref_one, flag, result)); @@ -980,7 +981,7 @@ TEST_P(AgentUnitTests, CreateReplierByXML) uint8_t flag = 0x00; /* - * Create requester. + * Create replier. */ agent_.create_participant_by_ref(client_key_, participant_id, domain_id, participant_ref, flag, result); EXPECT_TRUE(agent_.create_replier_by_xml(client_key_, replier_id, participant_id, xml_one, flag, result)); @@ -988,6 +989,92 @@ TEST_P(AgentUnitTests, CreateReplierByXML) // TODO (julianbermudez): complete tests. } +TEST_P(AgentUnitTests, RegisterCallbackFunctions) +{ + Agent::OpResult result; + EXPECT_TRUE(agent_.create_client(client_key_, 0x01, 512, GetParam(), result)); + + bool participant_callback_flag(false); + + switch (GetParam()) + { + case Middleware::Kind::FASTRTPS: + { + std::function on_create_participant + ([&]( + const fastrtps::Participant* /*participant*/) -> void + { + participant_callback_flag = true; + }); + agent_.add_middleware_callback( + Middleware::Kind::FASTRTPS, + middleware::CallbackKind::CREATE_PARTICIPANT, + std::move(on_create_participant)); + + std::function on_delete_participant + ([&]( + const fastrtps::Participant* /*participant*/) -> void + { + participant_callback_flag = false; + }); + agent_.add_middleware_callback( + Middleware::Kind::FASTRTPS, + middleware::CallbackKind::DELETE_PARTICIPANT, + std::move(on_delete_participant)); + break; + } + case Middleware::Kind::FASTDDS: + { + std::function on_create_participant + ([&]( + const fastdds::dds::DomainParticipant* /*participant*/) -> void + { + participant_callback_flag = true; + }); + agent_.add_middleware_callback( + Middleware::Kind::FASTDDS, + middleware::CallbackKind::CREATE_PARTICIPANT, + std::move(on_create_participant)); + + std::function on_delete_participant + ([&]( + const fastdds::dds::DomainParticipant* /*participant*/) -> void + { + participant_callback_flag = false; + }); + agent_.add_middleware_callback( + Middleware::Kind::FASTDDS, + middleware::CallbackKind::DELETE_PARTICIPANT, + std::move(on_delete_participant)); + break; + } + default: + { + break; + } + } + + const char* participant_ref = "default_xrce_participant"; + const uint16_t domain_id = 0x00; + const uint16_t participant_id = 0x00; + uint8_t flag = 0x00; + + EXPECT_FALSE(participant_callback_flag); + + EXPECT_TRUE(agent_.create_participant_by_ref( + client_key_, participant_id, domain_id, participant_ref, flag, result)); + EXPECT_TRUE(participant_callback_flag); + + EXPECT_TRUE(agent_.delete_participant(client_key_, participant_id, result)); + EXPECT_FALSE(participant_callback_flag); + + // TODO (jamoralp): shall we test for all defined callback types? +} + INSTANTIATE_TEST_CASE_P(AgentUnitTestsParams, AgentUnitTests, ::testing::Values(Middleware::Kind::FASTRTPS,Middleware::Kind::FASTDDS)); diff --git a/test/unittest/agent/CMakeLists.txt b/test/unittest/agent/CMakeLists.txt index a6db3fea3..a4d9b4281 100644 --- a/test/unittest/agent/CMakeLists.txt +++ b/test/unittest/agent/CMakeLists.txt @@ -43,6 +43,7 @@ target_include_directories(${TEST_NAME} target_link_libraries(${TEST_NAME} PRIVATE + fastrtps microxrcedds_agent ${GTEST_LIBRARIES} ${GMOCK_LIBRARIES}