feat: migrate communication interfaces

Dockerfile

@@ -129,7 +129,8 @@ COPY --from=apt-dependencies /tmp/apt /
 COPY --from=base-dependencies /tmp/deps /usr
 COPY dependencies/dependencies.cmake CMakeLists.txt
 RUN --mount=type=cache,target=/build,id=cmake-deps-${TARGETPLATFORM}-${CACHEID},uid=1000 \
-  cmake -B build -Dprotobuf_BUILD_TESTS=OFF -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} && cmake --build build && cmake --install build --prefix /tmp/deps
+  cmake -B build -Dprotobuf_BUILD_TESTS=OFF -DCPPZMQ_BUILD_TESTS=OFF -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
+  && cmake --build build && cmake --install build --prefix /tmp/deps
 COPY --from=base-dependencies /tmp/deps /tmp/deps
 COPY --from=pinocchio-dependencies /tmp/deps /tmp/deps
 

apt-packages.txt

@@ -2,3 +2,4 @@ libboost-all-dev
 libeigen3-dev
 liburdfdom-dev
 libassimp-dev
+libzmq3-dev

dependencies/dependencies.cmake

@@ -18,4 +18,12 @@ FetchContent_Declare(
   SOURCE_SUBDIR  cmake
 )
 
-FetchContent_MakeAvailable(OsqpEigen protobuf)
+FetchContent_Declare(
+  cppzmq
+  GIT_REPOSITORY https://github.com/zeromq/cppzmq/
+  GIT_TAG        v4.7.1
+)
+FetchContent_MakeAvailable(cppzmq)
+
+
+FetchContent_MakeAvailable(OsqpEigen protobuf cppzmq)

python/include/communication_interfaces_bindings.hpp

@@ -0,0 +1,8 @@
+#include <pybind11/pybind11.h>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+void bind_tcp(py::module_& m);
+void bind_udp(py::module_& m);
+void bind_zmq(py::module_& m);

python/setup.py

@@ -12,13 +12,14 @@
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
 __version__ = "8.0.0"
-__libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
+__libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model', 'communicatoin_interfaces']
 __include_dirs__ = ['include']
 
 __install_clproto_module__ = True
 __install_controllers_module__ = True
 __install_dynamical_systems_module__ = True
 __install_robot_model_module__ = True
+__install_communication_interfaces_module__ = True
 
 # check that necessary libraries can be found
 try:
@@ -122,6 +123,17 @@
                           )
     )
 
+if __install_communication_interfaces_module__:
+    ext_modules.append(
+        Pybind11Extension('communication_interfaces',
+                          sorted(glob('source/communication_interfaces/*.cpp')),
+                          cxx_std=17,
+                          include_dirs=__include_dirs__,
+                          libraries=['communication_interfaces'],
+                          define_macros=[('MODULE_VERSION_INFO', __version__)],
+                          )
+    )
+
 setup(
     name='control-libraries',
     version=__version__,

python/source/communication_interfaces/bind_tcp.cpp

@@ -0,0 +1,27 @@
+#include "communication_interfaces_bindings.hpp"
+
+#include <communication_interfaces/sockets/TCPClient.hpp>
+#include <communication_interfaces/sockets/TCPServer.hpp>
+
+using namespace communication_interfaces::sockets;
+
+void bind_tcp(py::module_& m) {
+  py::class_<TCPClientConfiguration>(m, "TCPClientConfiguration")
+      .def(
+          py::init<std::string, int, int>(), "TCPClientConfiguration struct", "ip_address"_a, "port"_a, "buffer_size"_a)
+      .def_readwrite("ip_address", &TCPClientConfiguration::ip_address)
+      .def_readwrite("port", &TCPClientConfiguration::port)
+      .def_readwrite("buffer_size", &TCPClientConfiguration::buffer_size);
+
+  py::class_<TCPClient, std::shared_ptr<TCPClient>, ISocket>(m, "TCPClient")
+      .def(py::init<TCPClientConfiguration>(), "Constructor taking the configuration struct", "configuration"_a);
+
+  py::class_<TCPServerConfiguration>(m, "TCPServerConfiguration")
+      .def(py::init<int, int, bool>(), "TCPServerConfiguration struct", "port"_a, "buffer_size"_a, "enable_reuse"_a)
+      .def_readwrite("port", &TCPServerConfiguration::port)
+      .def_readwrite("buffer_size", &TCPServerConfiguration::buffer_size)
+      .def_readwrite("enable_reuse", &TCPServerConfiguration::enable_reuse);
+
+  py::class_<TCPServer, std::shared_ptr<TCPServer>, ISocket>(m, "TCPServer")
+      .def(py::init<TCPServerConfiguration>(), "Constructor taking the configuration struct", "configuration"_a);
+}

python/source/communication_interfaces/bind_udp.cpp

@@ -0,0 +1,24 @@
+#include "communication_interfaces_bindings.hpp"
+
+#include <communication_interfaces/sockets/UDPClient.hpp>
+#include <communication_interfaces/sockets/UDPServer.hpp>
+
+using namespace communication_interfaces::sockets;
+
+void bind_udp(py::module_& m) {
+  py::class_<UDPSocketConfiguration>(m, "UDPSocketConfiguration")
+      .def(
+          py::init<std::string, int, int, bool, double>(), "UDPSocketConfiguration struct", "ip_address"_a, "port"_a,
+          "buffer_size"_a, "enable_reuse"_a = false, "timeout_duration_sec"_a = 0.0)
+      .def_readwrite("ip_address", &UDPSocketConfiguration::ip_address)
+      .def_readwrite("port", &UDPSocketConfiguration::port)
+      .def_readwrite("buffer_size", &UDPSocketConfiguration::buffer_size)
+      .def_readwrite("enable_reuse", &UDPSocketConfiguration::enable_reuse)
+      .def_readwrite("timeout_duration_sec", &UDPSocketConfiguration::timeout_duration_sec);
+
+  py::class_<UDPClient, std::shared_ptr<UDPClient>, ISocket>(m, "UDPClient")
+      .def(py::init<UDPSocketConfiguration>(), "Constructor taking the configuration struct", "configuration"_a);
+
+  py::class_<UDPServer, std::shared_ptr<UDPServer>, ISocket>(m, "UDPServer")
+      .def(py::init<UDPSocketConfiguration>(), "Constructor taking the configuration struct", "configuration"_a);
+}

python/source/communication_interfaces/bind_zmq.cpp

@@ -0,0 +1,87 @@
+#include "communication_interfaces_bindings.hpp"
+
+#include <communication_interfaces/sockets/ZMQPublisherSubscriber.hpp>
+
+using namespace communication_interfaces::sockets;
+
+class PyZMQContext {
+public:
+  PyZMQContext(int io_threads = 1) { context = std::make_shared<zmq::context_t>(io_threads); }
+
+  std::shared_ptr<zmq::context_t> context;
+};
+
+struct PyZMQSocketConfiguration {
+  PyZMQContext context;
+  std::string ip_address;
+  std::string port;
+  bool bind = true;
+  bool wait = false;
+};
+
+struct PyZMQCombinedSocketsConfiguration {
+  PyZMQContext context;
+  std::string ip_address;
+  std::string publisher_port;
+  std::string subscriber_port;
+  bool bind_publisher = true;
+  bool bind_subscriber = true;
+  bool wait = false;
+};
+
+class PyZMQPublisher : public ZMQPublisher, public std::enable_shared_from_this<PyZMQPublisher> {
+public:
+  PyZMQPublisher(PyZMQSocketConfiguration config)
+      : ZMQPublisher({config.context.context, config.ip_address, config.port, config.bind, config.wait}) {}
+};
+
+class PyZMQSubscriber : public ZMQSubscriber, public std::enable_shared_from_this<PyZMQSubscriber> {
+public:
+  PyZMQSubscriber(PyZMQSocketConfiguration config)
+      : ZMQSubscriber({config.context.context, config.ip_address, config.port, config.bind, config.wait}) {}
+};
+
+class PyZMQPublisherSubscriber : public ZMQPublisherSubscriber,
+                                 public std::enable_shared_from_this<PyZMQPublisherSubscriber> {
+public:
+  PyZMQPublisherSubscriber(PyZMQCombinedSocketsConfiguration config)
+      : ZMQPublisherSubscriber(
+          {config.context.context, config.ip_address, config.publisher_port, config.subscriber_port,
+           config.bind_publisher, config.bind_subscriber, config.wait}) {}
+};
+
+void bind_zmq(py::module_& m) {
+  py::class_<PyZMQContext>(m, "ZMQContext").def(py::init<int>(), "Create a ZMQ context", "io_threads"_a = 1);
+
+  py::class_<PyZMQSocketConfiguration>(m, "ZMQSocketConfiguration")
+      .def(
+          py::init<PyZMQContext, std::string, std::string, bool, bool>(), "ZMQSocketConfiguration struct", "context"_a,
+          "ip_address"_a, "port"_a, "bind"_a = true, "wait"_a = false)
+      .def_readwrite("ip_address", &PyZMQSocketConfiguration::ip_address)
+      .def_readwrite("port", &PyZMQSocketConfiguration::port)
+      .def_readwrite("bind", &PyZMQSocketConfiguration::bind)
+      .def_readwrite("wait", &PyZMQSocketConfiguration::wait);
+
+  py::class_<PyZMQCombinedSocketsConfiguration>(m, "ZMQCombinedSocketsConfiguration")
+      .def(
+          py::init<PyZMQContext, std::string, std::string, std::string, bool, bool, bool>(),
+          "ZMQCombinedSocketsConfiguration struct", "context"_a, "ip_address"_a, "publisher_port"_a,
+          "subscriber_port"_a, "bind_publisher"_a = true, "bind_subscriber"_a = true, "wait"_a = false)
+      .def_readwrite("ip_address", &PyZMQCombinedSocketsConfiguration::ip_address)
+      .def_readwrite("publisher_port", &PyZMQCombinedSocketsConfiguration::publisher_port)
+      .def_readwrite("subscriber_port", &PyZMQCombinedSocketsConfiguration::subscriber_port)
+      .def_readwrite("bind_publisher", &PyZMQCombinedSocketsConfiguration::bind_publisher)
+      .def_readwrite("bind_subscriber", &PyZMQCombinedSocketsConfiguration::bind_subscriber)
+      .def_readwrite("wait", &PyZMQCombinedSocketsConfiguration::wait);
+
+  py::class_<PyZMQPublisher, std::shared_ptr<PyZMQPublisher>, ISocket>(m, "ZMQPublisher")
+      .def(py::init<PyZMQSocketConfiguration>(), "Constructor taking the configuration struct", "configuration"_a);
+
+  py::class_<PyZMQSubscriber, std::shared_ptr<PyZMQSubscriber>, ISocket>(m, "ZMQSubscriber")
+      .def(py::init<PyZMQSocketConfiguration>(), "Constructor taking the configuration struct", "configuration"_a);
+
+  py::class_<PyZMQPublisherSubscriber, std::shared_ptr<PyZMQPublisherSubscriber>, ISocket>(m, "ZMQPublisherSubscriber")
+      .def(
+          py::init<PyZMQCombinedSocketsConfiguration>(), "Constructor taking the configuration struct",
+          "configuration"_a);
+}

python/source/communication_interfaces/communication_interfaces_bindings.cpp

@@ -0,0 +1,46 @@
+#include "communication_interfaces_bindings.hpp"
+
+#include <communication_interfaces/exceptions/SocketConfigurationException.hpp>
+#include <communication_interfaces/sockets/ISocket.hpp>
+
+#define STRINGIFY(x) #x
+#define MACRO_STRINGIFY(x) STRINGIFY(x)
+
+using namespace communication_interfaces;
+
+PYBIND11_MODULE(communication_interfaces, m) {
+  m.doc() = "Python bindings for communication interfaces";
+
+#ifdef MODULE_VERSION_INFO
+  m.attr("__version__") = MACRO_STRINGIFY(MODULE_VERSION_INFO);
+#else
+  m.attr("__version__") = "dev";
+#endif
+
+  auto m_sub_ex = m.def_submodule("exceptions", "Submodule for custom communication interfaces exceptions");
+  py::register_exception<exceptions::SocketConfigurationException>(
+      m_sub_ex, "SocketConfigurationError", PyExc_RuntimeError);
+
+  auto m_sub_sock = m.def_submodule("sockets", "Submodule for communication interfaces sockets");
+
+  py::class_<sockets::ISocket, std::shared_ptr<sockets::ISocket>>(m_sub_sock, "ISocket")
+      .def("open", &sockets::ISocket::open, "Perform configuration steps to open the socket for communication")
+      .def(
+          "receive_bytes",
+          [](sockets::ISocket& socket) -> py::object {
+            std::string buffer;
+            auto res = socket.receive_bytes(buffer);
+            if (res) {
+              return py::bytes(buffer);
+            } else {
+              return py::none();
+            }
+          },
+          "Receive bytes from the socket")
+      .def("send_bytes", &sockets::ISocket::send_bytes, "Receive bytes from the socket", "buffer"_a)
+      .def("close", &sockets::ISocket::close, "Perform steps to disconnect and close the socket communication");
+
+  bind_tcp(m_sub_sock);
+  bind_udp(m_sub_sock);
+  bind_zmq(m_sub_sock);
+}

python/test/communication_interfaces/test_tcp.py

@@ -0,0 +1,56 @@
+import multiprocessing
+import pytest
+import time
+
+from communication_interfaces.exceptions import SocketConfigurationError
+from communication_interfaces.sockets import TCPClientConfiguration, TCPClient, TCPServerConfiguration, TCPServer
+
+
+class TestTCPCommunication:
+    server = TCPServer(TCPServerConfiguration(6000, 50, True))
+    client = TCPClient(TCPClientConfiguration("127.0.0.1", 6000, 50))
+    server_message = "Hello client"
+    client_message = "Hello server"
+
+    def serve(self):
+        self.server.open()
+        response = self.server.receive_bytes()
+        if response is None:
+            pytest.fail("No response from client")
+        assert response.decode("utf-8").rstrip("\x00") == self.client_message
+        assert self.server.send_bytes(self.server_message)
+
+    def test_comm(self):
+        t = multiprocessing.Process(target=self.serve)
+        t.start()
+        time.sleep(1.0)
+
+        self.client.open()
+        assert self.client.send_bytes(self.client_message)
+        response = self.client.receive_bytes()
+        assert response
+        assert response.decode("utf-8").rstrip("\x00") == self.server_message
+
+        buffer = ""
+        self.server.close()
+        with pytest.raises(SocketConfigurationError):
+            self.server.receive_bytes()
+        with pytest.raises(SocketConfigurationError):
+            self.server.send_bytes(buffer)
+        self.client.close()
+        with pytest.raises(SocketConfigurationError):
+            self.client.receive_bytes()
+        with pytest.raises(SocketConfigurationError):
+            self.client.send_bytes(buffer)
+
+    def test_not_open(self):
+        buffer = ""
+        with pytest.raises(SocketConfigurationError):
+            self.server.receive_bytes()
+        with pytest.raises(SocketConfigurationError):
+            self.server.send_bytes(buffer)
+
+        with pytest.raises(SocketConfigurationError):
+            self.client.receive_bytes()
+        with pytest.raises(SocketConfigurationError):
+            self.client.send_bytes(buffer)