From cc885a5ffc04a08e481f4b600b1dbde7e514ff23 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sun, 14 Mar 2021 02:07:16 +0100 Subject: [PATCH 1/4] Implement a TCPServer class This implements an event-based tcp server for easier use. It is decoupled from the TCPSocket class to remove one layer of complexity. Also, the event-based approach allows interrupting this server as it won't be blocking inside an accept or recv call if not applicable. --- CMakeLists.txt | 2 +- .../comm/reverse_interface.h | 74 ++-- .../ur_client_library/comm/script_sender.h | 74 ++-- include/ur_client_library/comm/server.h | 104 ------ include/ur_client_library/comm/tcp_server.h | 193 ++++++++++ include/ur_client_library/comm/tcp_socket.h | 12 +- include/ur_client_library/rtde/rtde_client.h | 2 +- include/ur_client_library/ur/ur_driver.h | 10 - resources/external_control.urscript | 1 - src/comm/server.cpp | 156 -------- src/comm/tcp_server.cpp | 339 ++++++++++++++++++ src/comm/tcp_socket.cpp | 11 +- src/rtde/rtde_client.cpp | 5 + src/ur/ur_driver.cpp | 73 +--- 14 files changed, 616 insertions(+), 440 deletions(-) delete mode 100644 include/ur_client_library/comm/server.h create mode 100644 include/ur_client_library/comm/tcp_server.h delete mode 100644 src/comm/server.cpp create mode 100644 src/comm/tcp_server.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 664a4000..c238303c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic") add_library(urcl SHARED src/comm/tcp_socket.cpp - src/comm/server.cpp + src/comm/tcp_server.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp diff --git a/include/ur_client_library/comm/reverse_interface.h b/include/ur_client_library/comm/reverse_interface.h index 3a07842f..66b75272 100644 --- a/include/ur_client_library/comm/reverse_interface.h +++ b/include/ur_client_library/comm/reverse_interface.h @@ -28,10 +28,12 @@ #ifndef UR_CLIENT_LIBRARY_REVERSE_INTERFACE_H_INCLUDED #define UR_CLIENT_LIBRARY_REVERSE_INTERFACE_H_INCLUDED -#include "ur_client_library/comm/server.h" +#include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/types.h" +#include "ur_client_library/log.h" #include #include +#include namespace urcl { @@ -58,27 +60,27 @@ class ReverseInterface public: ReverseInterface() = delete; /*! - * \brief Creates a ReverseInterface object including a URServer. + * \brief Creates a ReverseInterface object including a TCPServer. * * \param port Port the Server is started on + * \param handle_program_state Function handle to a callback on program state changes. */ - ReverseInterface(uint32_t port) : server_(port) + ReverseInterface(uint32_t port, std::function handle_program_state) + : client_fd_(-1), server_(port), handle_program_state_(handle_program_state) { - if (!server_.bind()) - { - throw std::runtime_error("Could not bind to server"); - } - if (!server_.accept()) - { - throw std::runtime_error("Failed to accept robot connection"); - } + handle_program_state_(false); + server_.setMessageCallback( + std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2)); + server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1)); + server_.setDisconnectCallback(std::bind(&ReverseInterface::disconnectionCallback, this, std::placeholders::_1)); + server_.setMaxClientsAllowed(1); + server_.start(); } /*! * \brief Disconnects possible clients so the reverse interface object can be safely destroyed. */ ~ReverseInterface() { - server_.disconnectClient(); } /*! @@ -92,6 +94,10 @@ class ReverseInterface */ bool write(const vector6d_t* positions, const ControlMode control_mode = ControlMode::MODE_IDLE) { + if (client_fd_ == -1) + { + return false; + } uint8_t buffer[sizeof(int32_t) * 8]; uint8_t* b_pos = buffer; @@ -118,33 +124,41 @@ class ReverseInterface size_t written; - return server_.write(buffer, sizeof(buffer), written); + return server_.write(client_fd_, buffer, sizeof(buffer), written); } - /*! - * \brief Reads a keepalive signal from the robot. - * - * \returns The received keepalive string or the empty string, if nothing was received - */ - std::string readKeepalive() +private: + void connectionCallback(const int filedescriptor) { - const size_t buf_len = 16; - char buffer[buf_len]; - - bool read_successful = server_.readLine(buffer, buf_len); - - if (read_successful) + if (client_fd_ < 0) { - return std::string(buffer); + URCL_LOG_INFO("Robot connected to reverse interface. Ready to receive control commands."); + client_fd_ = filedescriptor; + handle_program_state_(true); } else { - return std::string(""); + URCL_LOG_ERROR("Connection request to ReverseInterface received while connection already established. Only one " + "connection is allowed at a time. Ignoring this request."); } } -private: - URServer server_; + void disconnectionCallback(const int filedescriptor) + { + URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor); + client_fd_ = -1; + handle_program_state_(false); + } + + void messageCallback(const int filedescriptor, char* buffer) + { + URCL_LOG_WARN("Messge on ReverseInterface received. The reverse interface currently does not support any message " + "handling. This message will be ignored."); + } + + int client_fd_; + TCPServer server_; + static const int32_t MULT_JOINTSTATE = 1000000; template @@ -154,6 +168,8 @@ class ReverseInterface std::memcpy(buffer, &val, s); return s; } + + std::function handle_program_state_; }; } // namespace comm diff --git a/include/ur_client_library/comm/script_sender.h b/include/ur_client_library/comm/script_sender.h index 5ae6a322..3e3c7b7c 100644 --- a/include/ur_client_library/comm/script_sender.h +++ b/include/ur_client_library/comm/script_sender.h @@ -29,7 +29,9 @@ #ifndef UR_CLIENT_LIBRARY_SCRIPT_SENDER_H_INCLUDED #define UR_CLIENT_LIBRARY_SCRIPT_SENDER_H_INCLUDED -#include "ur_client_library/comm/server.h" +#include + +#include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/log.h" namespace urcl @@ -37,7 +39,7 @@ namespace urcl namespace comm { /*! - * \brief The ScriptSender class starts a URServer for a robot to connect to and waits for a + * \brief The ScriptSender class starts a TCPServer for a robot to connect to and waits for a * request to receive a program. This program is then delivered to the requesting robot. */ class ScriptSender @@ -45,83 +47,53 @@ class ScriptSender public: ScriptSender() = delete; /*! - * \brief Creates a ScriptSender object, including a new URServer and not yet started thread. + * \brief Creates a ScriptSender object, including a new TCPServer * * \param port Port to start the server on * \param program Program to send to the robot upon request */ ScriptSender(uint32_t port, const std::string& program) : server_(port), script_thread_(), program_(program) { - if (!server_.bind()) - { - throw std::runtime_error("Could not bind to server"); - } - } - - /*! - * \brief Starts the thread that handles program requests by a robot. - */ - void start() - { - script_thread_ = std::thread(&ScriptSender::runScriptSender, this); + server_.setMessageCallback( + std::bind(&ScriptSender::messageCallback, this, std::placeholders::_1, std::placeholders::_2)); + server_.setConnectCallback(std::bind(&ScriptSender::connectionCallback, this, std::placeholders::_1)); + server_.setDisconnectCallback(std::bind(&ScriptSender::disconnectionCallback, this, std::placeholders::_1)); + server_.start(); } private: - URServer server_; + TCPServer server_; std::thread script_thread_; std::string program_; const std::string PROGRAM_REQUEST_ = std::string("request_program\n"); - void runScriptSender() + void connectionCallback(const int filedescriptor) { - while (true) - { - if (!server_.accept()) - { - throw std::runtime_error("Failed to accept robot connection"); - } - if (requestRead()) - { - URCL_LOG_INFO("Robot requested program"); - sendProgram(); - } - server_.disconnectClient(); - } + URCL_LOG_DEBUG("New client connected at FD %d.", filedescriptor); } - bool requestRead() + void disconnectionCallback(const int filedescriptor) { - const size_t buf_len = 1024; - char buffer[buf_len]; - - bool read_successful = server_.readLine(buffer, buf_len); + URCL_LOG_DEBUG("Client at FD %d disconnected.", filedescriptor); + } - if (read_successful) - { - if (std::string(buffer) == PROGRAM_REQUEST_) - { - return true; - } - else - { - URCL_LOG_WARN("Received unexpected message on script request port "); - } - } - else + void messageCallback(const int filedescriptor, char* buffer) + { + if (std::string(buffer) == PROGRAM_REQUEST_) { - URCL_LOG_WARN("Could not read on script request port"); + URCL_LOG_INFO("Robot requested program"); + sendProgram(filedescriptor); } - return false; } - void sendProgram() + void sendProgram(const int filedescriptor) { size_t len = program_.size(); const uint8_t* data = reinterpret_cast(program_.c_str()); size_t written; - if (server_.write(data, len, written)) + if (server_.write(filedescriptor, data, len, written)) { URCL_LOG_INFO("Sent program to robot"); } diff --git a/include/ur_client_library/comm/server.h b/include/ur_client_library/comm/server.h deleted file mode 100644 index 3ab4e67c..00000000 --- a/include/ur_client_library/comm/server.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright 2019, FZI Forschungszentrum Informatik (refactor) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * 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. - */ - -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include "ur_client_library/comm/tcp_socket.h" - -namespace urcl -{ -namespace comm -{ -#define MAX_SERVER_BUF_LEN 50 - -/*! - * \brief The URServer class abstracts communication with the robot. It opens a socket on a given - * port and waits for a robot to connect, at which point two way communication can be established. - */ -class URServer : private comm::TCPSocket -{ -private: - int port_; - comm::TCPSocket client_; - -protected: - virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len); - -public: - /*! - * \brief Creates a URServer object with a given port. - * - * \param port The port to open a socket on - */ - URServer(int port); - /*! - * \brief Closes the socket to allow for destruction of the object. - */ - ~URServer(); - /*! - * \brief Getter for the server IP. - * - * \returns The IP of the server - */ - std::string getIP(); - /*! - * \brief Binds to server's port, setting up a socket if possible. - * - * \returns Success of setting up the socket - */ - bool bind(); - /*! - * \brief Waits for a robot to connect to the socket. - * - * \returns True, if a robot successfully connected, false otherwise. - */ - bool accept(); - /*! - * \brief Triggers a disconnect of the currently connected robot. - */ - void disconnectClient(); - /*! - * \brief Reads the byte-stream from the robot to the next linebreak. - * - * \param buffer The buffer to write the received bytes to - * \param buf_len Size of the buffer - * - * \returns True if a successful read occurred, false otherwise - */ - bool readLine(char* buffer, size_t buf_len); - /*! - * \brief Writes a buffer to the robot. - * - * \param buf The buffer to write from - * \param buf_len The length to write - * \param written A reference used to indicate how many bytes were written - * - * \returns Success of the write - */ - bool write(const uint8_t* buf, size_t buf_len, size_t& written); -}; -} // namespace comm -} // namespace urcl diff --git a/include/ur_client_library/comm/tcp_server.h b/include/ur_client_library/comm/tcp_server.h new file mode 100644 index 00000000..6e478717 --- /dev/null +++ b/include/ur_client_library/comm/tcp_server.h @@ -0,0 +1,193 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner mauch@fzi.de + * \date 2021-03-13 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED +#define UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED + +#include +#include +#include +#include + +#include +#include +#include + +namespace urcl +{ +namespace comm +{ +/*! + * \brief Wrapper class for a TCP socket server. + * + * The server can be created given a port and it can register callbacks for 3 events: + * - connect: A new client connected + * - disconnect: A client disconnected + * - message: Data sent from one of the clients + * + * Please note that start() has to be called manually after initialization and callback + * registering in order to start handling socket events. + * + * While this server implementation supports multiple (number limited by system's socket + * implementation) clients by default, a maximum number of allowed clients can be configured. + */ +class TCPServer +{ +public: + TCPServer() = delete; + TCPServer(const int port); + virtual ~TCPServer(); + + /*! + * \brief This callback will be triggered on clients connecting to the server + * + * \param func Function handling the event information. The file descriptor created by the + * connection event will be passed to the function. + */ + void setConnectCallback(std::function func) + { + new_connection_callback_ = func; + } + + /*! + * \brief This callback will be triggered on clients disconnecting from the server + * + * \param func Function handling the event information. The file descriptor created by the + * connection event will be passed to the function. + */ + void setDisconnectCallback(std::function func) + { + disconnect_callback_ = func; + } + + /*! + * \brief This callback will be triggered on messages received on the socket + * + * \param func Function handling the event information. The file client's file_descriptor will be + * passed to the function as well as the actual message received from the client. + */ + void setMessageCallback(std::function func) + { + message_callback_ = func; + } + + /*! + * \brief Start event handling. + * + * Without calling this function the socket will be advertised and bound to a tcp port, but no + * handling of connection requests will be performed. + */ + void start(); + + /*! + * \brief Shut down the event listener thread. After calling this, no events will be handled + * anymore, but the socket will remain open and bound to the port. Call start() in order to + * restart event handling. + */ + void shutdown(); + + /*! + * \brief Writes to a client + * + * \param[in] fd File descriptor belonging to the client the data should be sent to. The file + * descriptor will be given from the connection callback. + * \param[in] buf Buffer of bytes to write + * \param[in] buf_len Number of bytes in the buffer + * \param[out] written Number of bytes actually written + * + * \returns True on success, false otherwise + */ + bool write(const int fd, const uint8_t* buf, const size_t buf_len, size_t& written); + + /*! + * \brief Get the maximum number of clients allowed to connect to this server + * + * \returns The currently configured client limit. 0 means unlimited amount of clients allowed. + */ + uint32_t getMaxClientsAllowed() const + { + return max_clients_allowed_; + } + + /*! + * \brief Set the maximum number of clients allowed to connect to this server. + * + * 0 means unlimited number of clients allowed. + * + */ + void setMaxClientsAllowed(const uint32_t& max_clients_allowed) + { + max_clients_allowed_ = max_clients_allowed; + } + +private: + void init(); + void bind(); + void startListen(); + + //! Handles connection events + void handleConnect(); + + void handleDisconnect(const int fd); + + //! read data from socket + void readData(const int fd); + + //! Event handler. Blocks until activity on any client or connection attempt + void spin(); + + //! Runs spin() as long as keep_running_ is set to true. + void worker(); + + std::atomic keep_running_; + std::thread worker_thread_; + + std::atomic listen_fd_; + int port_; + + int maxfd_; + fd_set masterfds_; + fd_set tempfds_; + + uint32_t max_clients_allowed_; + std::vector client_fds_; + + // Pipe for the self-pipe trick (https://cr.yp.to/docs/selfpipe.html) + int self_pipe_[2]; + + static const int INPUT_BUFFER_SIZE = 100; + char input_buffer_[INPUT_BUFFER_SIZE]; + + std::function new_connection_callback_; + std::function disconnect_callback_; + std::function message_callback_; +}; + +} // namespace comm +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED diff --git a/include/ur_client_library/comm/tcp_socket.h b/include/ur_client_library/comm/tcp_socket.h index 591186c2..7c8e3016 100644 --- a/include/ur_client_library/comm/tcp_socket.h +++ b/include/ur_client_library/comm/tcp_socket.h @@ -88,19 +88,11 @@ class TCPSocket { return socket_fd_; } - /*! - * \brief Setter for the file descriptor of the socket. - * - * \param socket_fd The new value - * - * \returns False, if the socket is in state connected - */ - bool setSocketFD(int socket_fd); /*! - * \brief Determines the IP address of the local machine + * \brief Determines the local IP address of the currently configured socket * - * \returns The IP address of the local machine. + * \returns The local IP address of the socket associated to the current file descriptor */ std::string getIP() const; diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 528b1754..1637a2a4 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -93,7 +93,7 @@ class RTDEClient */ RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, const std::string& input_recipe_file); - ~RTDEClient() = default; + ~RTDEClient(); /*! * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the * used protocol version and setting of input and output recipes. diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index e8322064..1add8552 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -174,12 +174,6 @@ class UrDriver */ bool stopControl(); - /*! - * \brief Starts the watchdog checking if the URCaps program is running on the robot and it is - * ready to receive control commands. - */ - void startWatchdog(); - /*! * \brief Checks if the kinematics information in the used model fits the actual robot. * @@ -231,7 +225,6 @@ class UrDriver private: std::string readScriptFile(const std::string& filename); - std::string readKeepalive(); int rtde_frequency_; comm::INotifier notifier_; @@ -245,9 +238,6 @@ class UrDriver uint32_t servoj_gain_; double servoj_lookahead_time_; - std::thread watchdog_thread_; - bool reverse_interface_active_; - uint32_t reverse_port_; std::function handle_program_state_; std::string robot_ip_; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8968f391..2d1e3161 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -103,7 +103,6 @@ textmsg("ExternalControl: External control active") keepalive = params_mult[1] while keepalive > 0 and control_mode > MODE_STOPPED: enter_critical - socket_send_line(1, "reverse_socket") params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation if params_mult[0] > 0: keepalive = params_mult[1] diff --git a/src/comm/server.cpp b/src/comm/server.cpp deleted file mode 100644 index 3ba317fb..00000000 --- a/src/comm/server.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Copyright 2019, FZI Forschungszentrum Informatik (refactor) - * - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * 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 "ur_client_library/comm/server.h" -#include -#include -#include -#include -#include "ur_client_library/log.h" - -namespace urcl -{ -namespace comm -{ -URServer::URServer(int port) : port_(port) -{ -} - -URServer::~URServer() -{ - TCPSocket::close(); -} - -std::string URServer::getIP() -{ - sockaddr_in name; - socklen_t len = sizeof(name); - int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len); - - if (res < 0) - { - URCL_LOG_ERROR("Could not get local IP"); - return std::string(); - } - - char buf[128]; - inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf)); - return std::string(buf); -} - -bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len) -{ - int flag = 1; - setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - return ::bind(socket_fd, address, address_len) == 0; -} - -bool URServer::bind() -{ - std::string empty; - bool res = TCPSocket::setup(empty, port_); - - if (!res) - return false; - - if (::listen(getSocketFD(), 1) < 0) - return false; - - return true; -} - -bool URServer::accept() -{ - if (TCPSocket::getState() != comm::SocketState::Connected || client_.getSocketFD() > 0) - return false; - - struct sockaddr addr; - socklen_t addr_len = sizeof(struct sockaddr); - int client_fd = -1; - - int retry = 0; - while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1) - { - URCL_LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno); - if (retry++ >= 5) - return false; - } - - TCPSocket::setOptions(client_fd); - - return client_.setSocketFD(client_fd); -} - -void URServer::disconnectClient() -{ - client_.close(); -} - -bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written) -{ - return client_.write(buf, buf_len, written); -} - -bool URServer::readLine(char* buffer, size_t buf_len) -{ - char* current_pointer = buffer; - char ch; - size_t total_read; - - if (buf_len <= 0 || buffer == NULL) - { - return false; - } - - total_read = 0; - for (;;) - { - if (client_.read(&ch)) - { - if (total_read < buf_len - 1) // just in case ... - { - total_read++; - *current_pointer++ = ch; - } - if (ch == '\n') - { - break; - } - } - else - { - if (total_read == 0) - { - return false; - } - else - { - break; - } - } - } - - *current_pointer = '\0'; - return true; -} -} // namespace comm -} // namespace urcl diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp new file mode 100644 index 00000000..3443c223 --- /dev/null +++ b/src/comm/tcp_server.cpp @@ -0,0 +1,339 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner mauch@fzi.de + * \date 2021-03-13 + * + */ +//---------------------------------------------------------------------- + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace urcl +{ +namespace comm +{ +TCPServer::TCPServer(const int port) : port_(port), maxfd_(0), max_clients_allowed_(0) +{ + init(); + bind(); + startListen(); +} + +TCPServer::~TCPServer() +{ + URCL_LOG_DEBUG("Destroying TCPServer object."); + shutdown(); + close(listen_fd_); +} + +void TCPServer::init() +{ + int err = (listen_fd_ = socket(AF_INET, SOCK_STREAM, 0)); + if (err == -1) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "Failed to create socket endpoint"); + } + int flag = 1; + setsockopt(listen_fd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); + setsockopt(listen_fd_, SOL_SOCKET, SO_KEEPALIVE, &flag, sizeof(int)); + + URCL_LOG_DEBUG("Created socket with FD %d", (int)listen_fd_); + + FD_ZERO(&masterfds_); + FD_ZERO(&tempfds_); + + // Create self-pipe for interrupting the worker loop + if (pipe(self_pipe_) == -1) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "Error creating self-pipe"); + } + URCL_LOG_DEBUG("Created read pipe at FD %d", self_pipe_[0]); + FD_SET(self_pipe_[0], &masterfds_); + + // Make read and write ends of pipe nonblocking + int flags; + flags = fcntl(self_pipe_[0], F_GETFL); + if (flags == -1) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "fcntl-F_GETFL"); + } + flags |= O_NONBLOCK; // Make read end nonblocking + if (fcntl(self_pipe_[0], F_SETFL, flags) == -1) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "fcntl-F_SETFL"); + } + + flags = fcntl(self_pipe_[1], F_GETFL); + if (flags == -1) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "fcntl-F_GETFL"); + } + flags |= O_NONBLOCK; // Make write end nonblocking + if (fcntl(self_pipe_[1], F_SETFL, flags) == -1) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "fcntl-F_SETFL"); + } +} + +void TCPServer::shutdown() +{ + keep_running_ = false; + + // This is basically the self-pipe trick. Writing to the pipe will trigger an event for the event + // handler which will stop the select() call from blocking. + if (::write(self_pipe_[1], "x", 1) == -1 && errno != EAGAIN) + { + throw std::system_error(std::error_code(errno, std::generic_category()), "Writing to self-pipe failed."); + } + + // After the event loop has finished the thread will be joinable. + if (worker_thread_.joinable()) + { + worker_thread_.join(); + URCL_LOG_DEBUG("Worker thread joined."); + } +} + +void TCPServer::bind() +{ + struct sockaddr_in server_addr; + server_addr.sin_family = AF_INET; + + // INADDR_ANY is a special constant that signalizes "ANY IFACE", + server_addr.sin_addr.s_addr = htonl(INADDR_ANY); + server_addr.sin_port = htons(port_); + int err = ::bind(listen_fd_, (struct sockaddr*)&server_addr, sizeof(server_addr)); + if (err == -1) + { + std::ostringstream ss; + ss << "Failed to bind socket for port " << port_ << " to address. Reason: " << strerror(errno); + throw std::system_error(std::error_code(errno, std::generic_category()), ss.str()); + } + URCL_LOG_DEBUG("Bound %d:%d to FD %d", server_addr.sin_addr.s_addr, port_, (int)listen_fd_); + + FD_SET(listen_fd_, &masterfds_); + maxfd_ = std::max((int)listen_fd_, self_pipe_[0]); +} + +void TCPServer::startListen() +{ + int err = listen(listen_fd_, 1); + if (err == -1) + { + std::ostringstream ss; + ss << "Failed to start listen on port " << port_; + throw std::system_error(std::error_code(errno, std::generic_category()), ss.str()); + } + URCL_LOG_DEBUG("Listening on port %d", port_); +} + +void TCPServer::handleConnect() +{ + struct sockaddr_storage client_addr; + socklen_t addrlen = sizeof(client_addr); + int client_fd = accept(listen_fd_, (struct sockaddr*)&client_addr, &addrlen); + if (client_fd < 0) + { + std::ostringstream ss; + ss << "Failed to accept connection request on port " << port_; + throw std::system_error(std::error_code(errno, std::generic_category()), ss.str()); + } + + if (client_fds_.size() < max_clients_allowed_ || max_clients_allowed_ == 0) + { + client_fds_.push_back(client_fd); + FD_SET(client_fd, &masterfds_); + if (client_fd > maxfd_) + { + maxfd_ = std::max(client_fd, self_pipe_[0]); + } + if (new_connection_callback_) + { + new_connection_callback_(client_fd); + } + } + else + { + URCL_LOG_WARN("Connection attempt on port %d while maximum number of clients (%d) is already connected. Closing " + "connection.", + port_, max_clients_allowed_); + close(client_fd); + } +} + +void TCPServer::spin() +{ + tempfds_ = masterfds_; + + // blocks until activity on any socket from tempfds + int sel = select(maxfd_ + 1, &tempfds_, NULL, NULL, NULL); + if (sel < 0) + { + URCL_LOG_ERROR("select() failed. Shutting down socket event handler."); + keep_running_ = false; + return; + } + + // Read part if pipe-trick. This will help interrupting the event handler thread. + if (FD_ISSET(self_pipe_[0], &masterfds_)) + { + URCL_LOG_DEBUG("Activity on self-pipe"); + char buffer; + if (read(self_pipe_[0], &buffer, 1) == -1) + { + while (true) + { + if (errno == EAGAIN) + break; + else + URCL_LOG_ERROR("read failed"); + } + } + else + { + URCL_LOG_DEBUG("Self-pipe triggered"); + return; + } + } + + // Check which fd has an activity + for (int i = 0; i <= maxfd_; i++) + { + if (FD_ISSET(i, &tempfds_)) + { + URCL_LOG_DEBUG("Activity on FD %d", i); + if (listen_fd_ == i) + { + // Activity on the listen_fd means we have a new connection + handleConnect(); + } + else + { + readData(i); + } + } + } +} + +void TCPServer::handleDisconnect(const int fd) +{ + URCL_LOG_DEBUG("%d disconnected.", fd); + close(fd); + if (disconnect_callback_) + { + disconnect_callback_(fd); + } + FD_CLR(fd, &masterfds_); + + for (size_t i = 0; i < client_fds_.size(); ++i) + { + if (client_fds_[i] == fd) + { + client_fds_.erase(client_fds_.begin() + i); + break; + } + } +} + +void TCPServer::readData(const int fd) +{ + bzero(&input_buffer_, INPUT_BUFFER_SIZE); // clear input buffer + int nbytesrecv = recv(fd, input_buffer_, INPUT_BUFFER_SIZE, 0); + if (nbytesrecv > 0) + { + if (message_callback_) + { + message_callback_(fd, input_buffer_); + } + } + else + { + if (0 < nbytesrecv) + { + if (errno == ECONNRESET) // if connection gets reset by client, we want to suppress this output + { + URCL_LOG_DEBUG("client from FD %s sent a connection reset package.", fd); + } + { + URCL_LOG_ERROR("recv() on FD %d failed.", fd); + } + } + else + { + // normal disconnect + } + handleDisconnect(fd); + } +} + +void TCPServer::worker() +{ + while (keep_running_) + { + spin(); + } + URCL_LOG_DEBUG("Finished worker thread of TCPServer"); +} + +void TCPServer::start() +{ + URCL_LOG_DEBUG("Starting worker thread"); + keep_running_ = true; + worker_thread_ = std::thread(&TCPServer::worker, this); +} + +bool TCPServer::write(const int fd, const uint8_t* buf, const size_t buf_len, size_t& written) +{ + written = 0; + + size_t remaining = buf_len; + + // handle partial sends + while (written < buf_len) + { + ssize_t sent = ::send(fd, buf + written, remaining, 0); + + if (sent <= 0) + { + URCL_LOG_ERROR("Sending data through socket failed."); + return false; + } + + written += sent; + remaining -= sent; + } + + return true; +} + +} // namespace comm +} // namespace urcl diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index bed8b3b5..586cb721 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -61,7 +61,7 @@ bool TCPSocket::setup(std::string& host, int port) URCL_LOG_DEBUG("Setting up connection: %s:%d", host.c_str(), port); // gethostbyname() is deprecated so use getadderinfo() as described in: - // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo + // https://beej.us/guide/bgnet/html/#getaddrinfoprepare-to-launch const char* host_name = host.empty() ? nullptr : host.c_str(); std::string service = std::to_string(port); @@ -107,15 +107,6 @@ bool TCPSocket::setup(std::string& host, int port) return connected; } -bool TCPSocket::setSocketFD(int socket_fd) -{ - if (state_ == SocketState::Connected) - return false; - socket_fd_ = socket_fd; - state_ = SocketState::Connected; - return true; -} - void TCPSocket::close() { if (socket_fd_ >= 0) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index dfbcfee1..b01ab0f1 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -45,6 +45,11 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st { } +RTDEClient::~RTDEClient() +{ + pipeline_.stop(); +} + bool RTDEClient::init() { // A running pipeline is needed inside setup diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 5dd5919f..c138e0a7 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -56,8 +56,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ : servoj_time_(0.008) , servoj_gain_(servoj_gain) , servoj_lookahead_time_(servoj_lookahead_time) - , reverse_interface_active_(false) - , reverse_port_(reverse_port) , handle_program_state_(handle_program_state) , robot_ip_(robot_ip) { @@ -147,12 +145,10 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ else { script_sender_.reset(new comm::ScriptSender(script_sender_port, prog)); - script_sender_->start(); URCL_LOG_DEBUG("Created script sender"); } - reverse_port_ = reverse_port; - watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this); + reverse_interface_.reset(new comm::ReverseInterface(reverse_port, handle_program_state)); URCL_LOG_DEBUG("Initialization done"); } @@ -169,21 +165,13 @@ std::unique_ptr urcl::UrDriver::getDataPackage() bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode) { - if (reverse_interface_active_) - { - return reverse_interface_->write(&values, control_mode); - } - return false; + return reverse_interface_->write(&values, control_mode); } bool UrDriver::writeKeepalive() { - if (reverse_interface_active_) - { - vector6d_t* fake = nullptr; - return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE); - } - return false; + vector6d_t* fake = nullptr; + return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE); } void UrDriver::startRTDECommunication() @@ -193,46 +181,8 @@ void UrDriver::startRTDECommunication() bool UrDriver::stopControl() { - if (reverse_interface_active_) - { - vector6d_t* fake = nullptr; - return reverse_interface_->write(fake, comm::ControlMode::MODE_STOPPED); - } - return false; -} - -void UrDriver::startWatchdog() -{ - handle_program_state_(false); - reverse_interface_.reset(new comm::ReverseInterface(reverse_port_)); - reverse_interface_active_ = true; - URCL_LOG_DEBUG("Created reverse interface"); - - while (true) - { - URCL_LOG_INFO("Robot ready to receive control commands."); - handle_program_state_(true); - while (reverse_interface_active_ == true) - { - std::string keepalive = readKeepalive(); - - if (keepalive == std::string("")) - { - reverse_interface_active_ = false; - } - } - - URCL_LOG_INFO("Connection to robot dropped, waiting for new connection."); - handle_program_state_(false); - // We explicitly call the destructor here, as unique_ptr.reset() creates a new object before - // replacing the pointer and destroying the old object. This will result in a resource conflict - // when trying to bind the socket. - // TODO: It would probably make sense to keep the same instance alive for the complete runtime - // instead of killing it all the time. - reverse_interface_->~ReverseInterface(); - reverse_interface_.reset(new comm::ReverseInterface(reverse_port_)); - reverse_interface_active_ = true; - } + vector6d_t* fake = nullptr; + return reverse_interface_->write(fake, comm::ControlMode::MODE_STOPPED); } std::string UrDriver::readScriptFile(const std::string& filename) @@ -242,17 +192,6 @@ std::string UrDriver::readScriptFile(const std::string& filename) return content; } -std::string UrDriver::readKeepalive() -{ - if (reverse_interface_active_) - { - return reverse_interface_->readKeepalive(); - } - else - { - return std::string(""); - } -} void UrDriver::checkCalibration(const std::string& checksum) { From d1d64b10c5e27fd5721f47d77e14a57506c40cb6 Mon Sep 17 00:00:00 2001 From: urmahp Date: Wed, 24 Mar 2021 15:39:22 +0100 Subject: [PATCH 2/4] Added unit test for tcp server --- tests/CMakeLists.txt | 7 + tests/test_tcp_server.cpp | 341 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 348 insertions(+) create mode 100644 tests/test_tcp_server.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4534d599..71de76ca 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -61,3 +61,10 @@ target_include_directories(rtde_parser_tests PRIVATE ${GTEST_INCLUDE_DIRS}) target_link_libraries(rtde_parser_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) gtest_add_tests(TARGET rtde_parser_tests ) + +add_executable(tcp_server_tests test_tcp_server.cpp) +target_compile_options(tcp_server_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(tcp_server_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(tcp_server_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET tcp_server_tests +) diff --git a/tests/test_tcp_server.cpp b/tests/test_tcp_server.cpp new file mode 100644 index 00000000..a851c659 --- /dev/null +++ b/tests/test_tcp_server.cpp @@ -0,0 +1,341 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 Universal Robots A/S +// +// 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. + +// All source code contained in and/or linked to in this message (the “Source Code”) is subject to the copyright of +// Universal Robots A/S and/or its licensors. THE SOURCE CODE IS PROVIDED “AS IS” WITHOUT WARRANTY OF ANY KIND, EXPRESS +// OR IMPLIED, INCLUDING – BUT NOT LIMITED TO – WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR +// NONINFRINGEMENT. USE OF THE SOURCE CODE IS AT YOUR OWN RISK AND UNIVERSAL ROBOTS A/S AND ITS LICENSORS SHALL, TO THE +// MAXIMUM EXTENT PERMITTED BY LAW, NOT BE LIABLE FOR ANY ERRORS OR MALICIOUS CODE IN THE SOURCE CODE, ANY THIRD-PARTY +// CLAIMS, OR ANY OTHER CLAIMS AND DAMAGES, INCLUDING INDIRECT, INCIDENTAL, SPECIAL, CONSEQUENTIAL OR PUNITIVE DAMAGES, +// OR ANY LOSS OF PROFITS, EXPECTED SAVINGS, OR REVENUES, WHETHER INCURRED DIRECTLY OR INDIRECTLY, OR ANY LOSS OF DATA, +// USE, GOODWILL, OR OTHER INTANGIBLE LOSSES, RESULTING FROM YOUR USE OF THE SOURCE CODE. You may make copies of the +// Source Code for use in connection with a Universal Robots or UR+ product, provided that you include (i) an +// appropriate copyright notice (“© [the year in which you received the Source Code or the Source Code was first +// published, e.g. “2021”] Universal Robots A/S and/or its licensors”) along with the capitalized section of this notice +// in all copies of the Source Code. By using the Source Code, you agree to the above terms. For more information, +// please contact legal@universal-robots.com. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +#include +#include + +using namespace urcl; + +class TCPServerTest : public ::testing::Test +{ +protected: + class Client : public comm::TCPSocket + { + public: + Client(const int& port) + { + std::string host = "127.0.0.1"; + TCPSocket::setup(host, port); + } + + void send(const std::string& text) + { + size_t len = text.size(); + const uint8_t* data = reinterpret_cast(text.c_str()); + size_t written; + TCPSocket::write(data, len, written); + } + + std::string recv() + { + std::stringstream result; + char character; + size_t read_chars = 99; + while (read_chars > 0) + { + TCPSocket::read((uint8_t*)&character, 1, read_chars); + result << character; + if (character == '\n') + { + break; + } + } + return result.str(); + } + + protected: + virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len) + { + return ::connect(socket_fd, address, address_len) == 0; + } + }; + + // callback functions + void connectionCallback(const int filedescriptor) + { + std::lock_guard lk(connect_mutex_); + client_fd_ = filedescriptor; + connect_cv_.notify_one(); + connection_callback_ = true; + } + + void disconnectionCallback(const int filedescriptor) + { + std::lock_guard lk(disconnect_mutex_); + client_fd_ = -1; + disconnect_cv_.notify_one(); + disconnection_callback_ = true; + } + + void messageCallback(const int filedescriptor, char* buffer) + { + std::lock_guard lk(message_mutex_); + message_ = std::string(buffer); + message_cv_.notify_one(); + message_callback_ = true; + } + + bool waitForConnectionCallback(int milliseconds = 100) + { + std::unique_lock lk(connect_mutex_); + if (connect_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + connection_callback_ == true) + { + connection_callback_ = false; + return true; + } + else + { + return false; + } + } + + bool waitForDisconnectionCallback(int milliseconds = 100) + { + std::unique_lock lk(disconnect_mutex_); + if (disconnect_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + disconnection_callback_ == true) + { + disconnection_callback_ = false; + return true; + } + else + { + return false; + } + } + + bool waitForMessageCallback(int milliseconds = 100) + { + std::unique_lock lk(message_mutex_); + if (message_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + message_callback_ == true) + { + message_callback_ = false; + return true; + } + else + { + return false; + } + } + + int port_ = 50001; + std::string message_ = ""; + int client_fd_ = -1; + +private: + std::condition_variable connect_cv_; + std::mutex connect_mutex_; + + std::condition_variable disconnect_cv_; + std::mutex disconnect_mutex_; + + std::condition_variable message_cv_; + std::mutex message_mutex_; + + bool connection_callback_ = false; + bool disconnection_callback_ = false; + bool message_callback_ = false; +}; + +TEST_F(TCPServerTest, socket_creation) +{ + comm::TCPServer server(port_); + + // Shouldn't be able to create antoher server on same port + EXPECT_THROW(comm::TCPServer server2(port_), std::system_error); + + server.start(); + + // We should be able to connect to the server even though the callbacks haven't been configured + ASSERT_NO_THROW(Client client(port_)); + Client client(port_); + + // We should also be able to send message and disconnect. We wait to be absolutely sure no exception is thrown + EXPECT_NO_THROW(client.send("message\n")); + EXPECT_NO_THROW(waitForMessageCallback()); + + EXPECT_NO_THROW(client.close()); + EXPECT_NO_THROW(waitForDisconnectionCallback()); +} + +TEST_F(TCPServerTest, callback_functions) +{ + comm::TCPServer server(port_); + server.setMessageCallback(std::bind(&TCPServerTest_callback_functions_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + server.setConnectCallback( + std::bind(&TCPServerTest_callback_functions_Test::connectionCallback, this, std::placeholders::_1)); + server.setDisconnectCallback( + std::bind(&TCPServerTest_callback_functions_Test::disconnectionCallback, this, std::placeholders::_1)); + server.start(); + + // Check that the appropriate callback functions are called + Client client(port_); + EXPECT_TRUE(waitForConnectionCallback()); + + client.send("message\n"); + EXPECT_TRUE(waitForMessageCallback()); + + client.close(); + EXPECT_TRUE(waitForDisconnectionCallback()); +} + +TEST_F(TCPServerTest, unlimited_clients_allowed) +{ + comm::TCPServer server(port_); + server.setMessageCallback(std::bind(&TCPServerTest_unlimited_clients_allowed_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + server.setConnectCallback( + std::bind(&TCPServerTest_unlimited_clients_allowed_Test::connectionCallback, this, std::placeholders::_1)); + server.setDisconnectCallback( + std::bind(&TCPServerTest_unlimited_clients_allowed_Test::disconnectionCallback, this, std::placeholders::_1)); + server.start(); + + // Test that a large number of clients can connect to the server + std::vector clients; + Client* client; + for (unsigned int i = 0; i < 100; ++i) + { + client = new Client(port_); + ASSERT_TRUE(waitForConnectionCallback()); + clients.push_back(client); + } +} + +TEST_F(TCPServerTest, max_clients_allowed) +{ + comm::TCPServer server(port_); + server.setMessageCallback(std::bind(&TCPServerTest_max_clients_allowed_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + server.setConnectCallback( + std::bind(&TCPServerTest_max_clients_allowed_Test::connectionCallback, this, std::placeholders::_1)); + server.setDisconnectCallback( + std::bind(&TCPServerTest_max_clients_allowed_Test::disconnectionCallback, this, std::placeholders::_1)); + server.start(); + server.setMaxClientsAllowed(1); + + // Test that only one client can connect + Client client1(port_); + EXPECT_TRUE(waitForConnectionCallback()); + Client client2(port_); + EXPECT_FALSE(waitForConnectionCallback()); +} + +TEST_F(TCPServerTest, message_transmission) +{ + comm::TCPServer server(port_); + server.setMessageCallback(std::bind(&TCPServerTest_message_transmission_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + server.setConnectCallback( + std::bind(&TCPServerTest_message_transmission_Test::connectionCallback, this, std::placeholders::_1)); + server.setDisconnectCallback( + std::bind(&TCPServerTest_message_transmission_Test::disconnectionCallback, this, std::placeholders::_1)); + server.start(); + + Client client(port_); + EXPECT_TRUE(waitForConnectionCallback()); + + // Test that messages are transmitted corectly between client and server + std::string message = "test message\n"; + client.send(message); + + EXPECT_TRUE(waitForMessageCallback()); + EXPECT_EQ(message, message_); + + size_t len = message.size(); + const uint8_t* data = reinterpret_cast(message.c_str()); + size_t written; + + ASSERT_TRUE(server.write(client_fd_, data, len, written)); + EXPECT_EQ(client.recv(), message); +} + +TEST_F(TCPServerTest, client_connections) +{ + comm::TCPServer server(port_); + server.setMessageCallback(std::bind(&TCPServerTest_client_connections_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + server.setConnectCallback( + std::bind(&TCPServerTest_client_connections_Test::connectionCallback, this, std::placeholders::_1)); + server.setDisconnectCallback( + std::bind(&TCPServerTest_client_connections_Test::disconnectionCallback, this, std::placeholders::_1)); + server.start(); + + std::string message = "text message\n"; + size_t len = message.size(); + const uint8_t* data = reinterpret_cast(message.c_str()); + size_t written; + + // Test that we can connect multiple clients + Client client1(port_); + EXPECT_TRUE(waitForConnectionCallback()); + int client1_fd = client_fd_; + + Client client2(port_); + EXPECT_TRUE(waitForConnectionCallback()); + int client2_fd = client_fd_; + + Client client3(port_); + EXPECT_TRUE(waitForConnectionCallback()); + int client3_fd = client_fd_; + + // Test that the correct clients are disconnected on the server side. + client1.close(); + EXPECT_TRUE(waitForDisconnectionCallback()); + + EXPECT_FALSE(server.write(client1_fd, data, len, written)); + EXPECT_TRUE(server.write(client2_fd, data, len, written)); + EXPECT_TRUE(server.write(client3_fd, data, len, written)); + + client2.close(); + EXPECT_TRUE(waitForDisconnectionCallback()); + EXPECT_FALSE(server.write(client1_fd, data, len, written)); + EXPECT_FALSE(server.write(client2_fd, data, len, written)); + EXPECT_TRUE(server.write(client3_fd, data, len, written)); + + client3.close(); + EXPECT_TRUE(waitForDisconnectionCallback()); + EXPECT_FALSE(server.write(client1_fd, data, len, written)); + EXPECT_FALSE(server.write(client2_fd, data, len, written)); + EXPECT_FALSE(server.write(client3_fd, data, len, written)); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From a06f566307e7bb9092c44085b2cd80432339c7ba Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 24 Mar 2021 23:20:09 +0100 Subject: [PATCH 3/4] Run coverage on tests --- .github/workflows/ci.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 573b20a3..6a78ff64 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -28,10 +28,18 @@ jobs: run: sudo apt-get install -y libboost-dev - name: configure run: mkdir build && cd build && cmake .. -DBUILDING_TESTS=1 -DINTEGRATION_TESTS=1 + env: + CXXFLAGS: -g -O2 -fprofile-arcs -ftest-coverage + CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage + LDFLAGS: -fprofile-arcs -ftest-coverage - name: build run: cmake --build build --config Debug - name: test run: cd build && make test + - name: install gcovr + run: sudo apt-get install -y gcovr + - name: gcovr + run: cd build && gcovr -r .. check_links: runs-on: ubuntu-latest From ed16661f169e43aa7132a50c9da1633b780d9eaa Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 25 Mar 2021 14:56:49 +0100 Subject: [PATCH 4/4] Update documentation on ReverseInterface --- README.md | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index ae5b146d..8cccab13 100644 --- a/README.md +++ b/README.md @@ -219,13 +219,9 @@ meant to send joint positions or velocities together with a mode that tells the interpret those values (e.g. `SERVOJ`, `SPEEDJ`). Therefore, this interface can be used to do motion command streaming to the robot. -Simultaneously this class offers a function to receive a keepalive signal from the robot. This -function expects to read a terminated string from the opened socket and returns the string that has -been read. If no string could be read, an empty string is returned instead. - In order to use this class in an application together with a robot, make sure that a corresponding -URScript is running on the robot that can interpret the commands sent and sends keepalive signals to -the interface. See [this example script](examples/resources/scriptfile.urscript) for reference. +URScript is running on the robot that can interpret the commands sent. See [this example +script](resources/external_control.urscript) for reference. Also see the [ScriptSender](#scriptsender) for a way to define the corresponding URScript on the control PC and sending it to the robot upon request.