Add RobotUnitDataStreamingReceiver

This object helps the client with data sreaming

source/RobotAPI/libraries/CMakeLists.txt

@@ -21,4 +21,5 @@ add_subdirectory(armem)
 add_subdirectory(aron)
 
 add_subdirectory(NJointControllerGuiPluginUtility)
-add_subdirectory(RobotAPINJointControllerWidgets)
\ No newline at end of file
+add_subdirectory(RobotAPINJointControllerWidgets)
+add_subdirectory(RobotUnitDataStreamingReceiver)
\ No newline at end of file

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/CMakeLists.txt

+set(LIB_NAME       RobotUnitDataStreamingReceiver)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+    RobotAPIInterfaces 
+    RobotAPICore
+)
+
+set(LIB_FILES RobotUnitDataStreamingReceiver.cpp)
+set(LIB_HEADERS RobotUnitDataStreamingReceiver.h )
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::RobotUnitDataStreamingReceiver
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <Ice/ObjectAdapter.h>
+
+#include <ArmarXCore/core/ArmarXManager.h>
+
+#include "RobotUnitDataStreamingReceiver.h"
+
+namespace armarx::detail::RobotUnitDataStreamingReceiver
+{
+    class Receiver :
+        virtual public ManagedIceObject,
+        virtual public RobotUnitDataStreaming::Receiver
+    {
+    public:
+        std::string getDefaultName() const override
+        {
+            return "RobotUnitDataStreamingReceiver";
+        }
+        void onInitComponent() override {}
+        void onConnectComponent() override {}
+        void onExitComponent() override {}
+
+        void update(const RobotUnitDataStreaming::TimeStepSeq& data, const Ice::Current&) override
+        {
+            std::lock_guard{_data_mutex};
+            ARMARX_INFO << deactivateSpam()
+                        << "received " << data.size() << " timesteps";
+            _data.emplace_back(data);
+        }
+
+        std::mutex                                       _data_mutex;
+        std::deque<RobotUnitDataStreaming::TimeStepSeq>  _data;
+        Ice::Identity                                    _identity;
+    };
+}
+
+namespace armarx
+{
+    RobotUnitDataStreamingReceiver::RobotUnitDataStreamingReceiver(
+        const ManagedIceObjectPtr& obj,
+        const RobotUnitInterfacePrx& ru,
+        const RobotUnitDataStreaming::Config& cfg
+    ) : _obj{obj}, _ru{ru}
+    {
+        ARMARX_CHECK_NOT_NULL(_obj);
+        ARMARX_CHECK_NOT_NULL(_ru);
+        _receiver = make_shared<detail::RobotUnitDataStreamingReceiver::Receiver>();
+
+        _receiver->_identity.name =
+            _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
+            std::to_string(clock_t::now().time_since_epoch().count());
+
+        auto adapter = _obj->getArmarXManager()->getAdapter();
+        _proxy = RobotUnitDataStreaming::ReceiverPrx::uncheckedCast(
+                     adapter->add(_receiver,  _receiver->_identity));
+
+        _description = _ru->startDataStreaming(_proxy, cfg);
+    }
+
+    RobotUnitDataStreamingReceiver::~RobotUnitDataStreamingReceiver()
+    {
+        if (_proxy)
+        {
+            if (!_description.entries.empty())
+            {
+                _ru->stopDataStreaming(_proxy);
+            }
+            auto adapter = _obj->getArmarXManager()->getAdapter();
+            adapter->remove(_receiver->_identity);
+        }
+        _proxy    = nullptr;
+        _receiver = nullptr;
+    }
+
+    std::deque<RobotUnitDataStreaming::TimeStep>& RobotUnitDataStreamingReceiver::getDataBuffer()
+    {
+        ARMARX_CHECK_NOT_NULL(_receiver);
+        std::deque<RobotUnitDataStreaming::TimeStepSeq> data;
+        {
+            std::lock_guard{_receiver->_data_mutex};
+            std::swap(data, _receiver->_data);
+        }
+        for (auto& chunk : data)
+        {
+            for (auto& step : chunk)
+            {
+                if (
+                    _last_iteration_id != -1 &&
+                    _last_iteration_id + 1 != step.iterationId
+                )
+                {
+                    ARMARX_ERROR << "Missing Iterations or iterations out of order!"
+                                 " This should not happen";
+                }
+                _last_iteration_id = step.iterationId;
+                _data_buffer.emplace_back(std::move(step));
+            }
+        }
+        return _data_buffer;
+    }
+
+    const RobotUnitDataStreaming::DataStreamingDescription& RobotUnitDataStreamingReceiver::getDataDescription() const
+    {
+        return _description;
+    }
+}

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::RobotUnitDataStreamingReceiver
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+namespace armarx::detail::RobotUnitDataStreamingReceiver
+{
+    TYPEDEF_PTRS_HANDLE(Receiver);
+}
+namespace armarx
+{
+    TYPEDEF_PTRS_SHARED(RobotUnitDataStreamingReceiver);
+    /**
+    * @defgroup Library-RobotUnitDataStreamingReceiver RobotUnitDataStreamingReceiver
+    * @ingroup RobotAPI
+    * A description of the library RobotUnitDataStreamingReceiver.
+    *
+    * @class RobotUnitDataStreamingReceiver
+    * @ingroup Library-RobotUnitDataStreamingReceiver
+    * @brief Brief description of class RobotUnitDataStreamingReceiver.
+    *
+    * Detailed description of class RobotUnitDataStreamingReceiver.
+    */
+    class RobotUnitDataStreamingReceiver
+    {
+    public:
+        using clock_t = std::chrono::high_resolution_clock;
+
+        RobotUnitDataStreamingReceiver(
+            const ManagedIceObjectPtr& obj,
+            const RobotUnitInterfacePrx& ru,
+            const RobotUnitDataStreaming::Config& cfg);
+        ~RobotUnitDataStreamingReceiver();
+
+        std::deque<RobotUnitDataStreaming::TimeStep>& getDataBuffer();
+        const RobotUnitDataStreaming::DataStreamingDescription& getDataDescription() const;
+
+    private:
+        ManagedIceObjectPtr                                 _obj;
+        RobotUnitInterfacePrx                               _ru;
+        detail::RobotUnitDataStreamingReceiver::ReceiverPtr _receiver;
+        RobotUnitDataStreaming::ReceiverPrx                 _proxy;
+        RobotUnitDataStreaming::DataStreamingDescription    _description;
+        std::deque<RobotUnitDataStreaming::TimeStep>        _data_buffer;
+        long                                                _last_iteration_id = -1;
+    };
+}

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/CMakeLists.txt

+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotUnitDataStreamingReceiver)
+ 
+armarx_add_test(RobotUnitDataStreamingReceiverTest RobotUnitDataStreamingReceiverTest.cpp "${LIBS}")
\ No newline at end of file

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::RobotUnitDataStreamingReceiver
+ * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date       2020
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::RobotUnitDataStreamingReceiver
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../RobotUnitDataStreamingReceiver.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+    BOOST_CHECK_EQUAL(true, true);
+}