Add visitors to RobotUnitDataStreamingReceiver

source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h

@@ -200,6 +200,16 @@ namespace armarx
             const std::string& rnsName = "",
             const std::string& nodeName = "");
 
+        template<class...Ts>
+        VirtualRobot::RobotPtr addOrGetRobot(const std::string& id, Ts&& ...ts)
+        {
+            if (hasRobot(id))
+            {
+                return getRobot(id);
+            }
+            return addRobot(id, std::forward<Ts>(ts)...);
+        }
+
         VirtualRobot::RobotPtr getRobot(const std::string& id) const;
         RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const;
         void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h

@@ -49,7 +49,9 @@ namespace armarx
     class RobotUnitDataStreamingReceiver
     {
     public:
-        using clock_t = std::chrono::high_resolution_clock;
+        using clock_t    = std::chrono::high_resolution_clock;
+        using timestep_t = RobotUnitDataStreaming::TimeStep;
+        using entry_t    = RobotUnitDataStreaming::DataEntry;
 
         RobotUnitDataStreamingReceiver(
             const ManagedIceObjectPtr& obj,
@@ -57,9 +59,34 @@ namespace armarx
             const RobotUnitDataStreaming::Config& cfg);
         ~RobotUnitDataStreamingReceiver();
 
-        std::deque<RobotUnitDataStreaming::TimeStep>& getDataBuffer();
+        std::deque<timestep_t>& getDataBuffer();
         const RobotUnitDataStreaming::DataStreamingDescription& getDataDescription() const;
 
+        static void visitEntry(auto&& f, const timestep_t& st, const entry_t& e)
+        {
+            using enum_t = RobotUnitDataStreaming::DataEntryType;
+            // *INDENT-OFF*
+            switch (e.type)
+            {
+                case enum_t::NodeTypeBool  : f(st.bools  .at(e.index)); break;
+                case enum_t::NodeTypeByte  : f(st.bytes  .at(e.index)); break;
+                case enum_t::NodeTypeShort : f(st.shorts .at(e.index)); break;
+                case enum_t::NodeTypeInt   : f(st.ints   .at(e.index)); break;
+                case enum_t::NodeTypeLong  : f(st.longs  .at(e.index)); break;
+                case enum_t::NodeTypeFloat : f(st.floats .at(e.index)); break;
+                case enum_t::NodeTypeDouble: f(st.doubles.at(e.index)); break;
+            };
+            // *INDENT-ON*
+        }
+
+        static void visitEntries(auto&& f, const timestep_t& st, const auto& cont)
+        {
+            for (const entry_t& e : cont)
+            {
+                visitEntry(f, st, e);
+            }
+        }
+
     private:
         ManagedIceObjectPtr                                 _obj;
         RobotUnitInterfacePrx                               _ru;