Skip to content

WIP: Handle keepalive missing events #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
fmauch wants to merge 13 commits into
base: tcp_server
Choose a base branch
from
Draft

WIP: Handle keepalive missing events #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
0cc0d32
Implement a TCPServer class
fmauch Mar 14, 2021
01ebf5b
Use new TCPServer instead of URServer
fmauch Mar 15, 2021
590fd15
Remove URServer class that has been replaced by TCPServer
fmauch Mar 15, 2021
d53b772
Added missing pipeline.stop() call in RTDEClient destructor
fmauch Mar 15, 2021
9f6ba16
Added error handling to tcp_server
fmauch Mar 15, 2021
2627d46
Removed unneeded include
fmauch Mar 15, 2021
8b1909a
Cleanup tcp_socket
fmauch Mar 15, 2021
6d010f3
Revert "Removed unneeded include"
fmauch Mar 16, 2021
ed39496
Remove unneeded include
fmauch Mar 16, 2021
4126409
Fix handle_connect in case of unlimited number of clients configured
fmauch Mar 16, 2021
341b34f
Properly disconnect when reading from socket fails.
fmauch Mar 16, 2021
618bc1c
Suppress read error on connection reset
fmauch Mar 16, 2021
5b433f2
WIP: Handle keepalive missing events
fmauch Mar 24, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
109 changes: 88 additions & 21 deletions include/ur_client_library/comm/reverse_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <cstring>
#include <endian.h>
#include <condition_variable>

namespace urcl
{
Expand All @@ -58,27 +60,29 @@ 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. For this to
* work, the URScript program will have to send keepalive signals to the \p reverse_port. I no
* keepalive signal can be read, program state will be false.
*/
ReverseInterface(uint32_t port) : server_(port)
ReverseInterface(uint32_t port, std::function<void(bool)> 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();
}

/*!
Expand All @@ -92,6 +96,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;

Expand All @@ -118,7 +126,7 @@ class ReverseInterface

size_t written;

return server_.write(buffer, sizeof(buffer), written);
return server_.write(client_fd_, buffer, sizeof(buffer), written);
}

/*!
Expand All @@ -128,23 +136,80 @@ class ReverseInterface
*/
std::string readKeepalive()
{
const size_t buf_len = 16;
char buffer[buf_len];
// If client has been disconnected / not connected yet.
if (client_fd_ == -1)
{
return "";
}

bool read_successful = server_.readLine(buffer, buf_len);
std::unique_lock<std::mutex> lk(keepalive_mutex_);
keepalive_cv_.wait(lk);
return keepalive_message_;
}

if (read_successful)
private:
void connectionCallback(const int filedescriptor)
{
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);
keepalive_watchdog_thread_ = std::thread(&ReverseInterface::runKeepalive, this);
}
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 runKeepalive()
{
while (client_fd_ != -1)
{
std::unique_lock<std::mutex> lk(keepalive_mutex_);
if (keepalive_cv_.wait_for(lk, std::chrono::milliseconds(100)) == std::cv_status::no_timeout)
{
URCL_LOG_INFO("Got keepalive signal.");
}
else
{
if (client_fd_ != -1)
{
URCL_LOG_WARN("ReverseInterface: Did not receive keepalive signal");
std::thread t(&TCPServer::disconnectClient, &server_, (int)client_fd_);
t.detach();
}
}
}
URCL_LOG_INFO("Watchdog finished");
}

void disconnectionCallback(const int filedescriptor)
{
URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor);
client_fd_ = -1;
handle_program_state_(false);
keepalive_watchdog_thread_.join();
}

void messageCallback(const int filedescriptor, char* buffer)
{
std::lock_guard<std::mutex> lk(keepalive_mutex_);
keepalive_message_ = std::string(buffer);
keepalive_cv_.notify_one();
}

std::atomic<int> client_fd_;
TCPServer server_;

std::thread keepalive_watchdog_thread_;

std::mutex keepalive_mutex_;
std::condition_variable keepalive_cv_;
std::string keepalive_message_;

static const int32_t MULT_JOINTSTATE = 1000000;

template <typename T>
Expand All @@ -154,6 +219,8 @@ class ReverseInterface
std::memcpy(buffer, &val, s);
return s;
}

std::function<void(bool)> handle_program_state_;
};

} // namespace comm
Expand Down
99 changes: 48 additions & 51 deletions include/ur_client_library/comm/script_sender.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,99 +29,96 @@
#ifndef UR_CLIENT_LIBRARY_SCRIPT_SENDER_H_INCLUDED
#define UR_CLIENT_LIBRARY_SCRIPT_SENDER_H_INCLUDED

#include "ur_client_library/comm/server.h"
#include <thread>

#include "ur_client_library/comm/tcp_server.h"
#include "ur_client_library/log.h"

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
{
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_INFO("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_INFO("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()
// bool requestRead()
//{
// const size_t buf_len = 1024;
// char buffer[buf_len];

// bool read_successful = server_.readLine(buffer, buf_len);

// if (read_successful)
//{
// if (std::string(buffer) == PROGRAM_REQUEST_)
//{
// return true;
//}
// else
//{
// URCL_LOG_WARN("Received unexpected message on script request port ");
//}
//}
// else
//{
// URCL_LOG_WARN("Could not read on script request port");
//}
// return false;
//}

void sendProgram(const int filedescriptor)
{
size_t len = program_.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(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");
}
Expand Down
Loading