Update FakeRawValuesPublisher device

robotology · Sep 26, 2024 · 37b6e55 · 37b6e55
1 parent 6f5a7d8
commit 37b6e55
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 14 deletions.
diff --git a/src/libraries/icubmod/fakeRawValuesPublisher/CMakeLists.txt b/src/libraries/icubmod/fakeRawValuesPublisher/CMakeLists.txt
@@ -4,8 +4,6 @@
 # This software may be modified and distributed under the terms of the
 # BSD-3-Clause license. See the accompanying LICENSE file for details.
 
-# project(fakeRawValuesPublisher)
-
 yarp_prepare_plugin(fakeRawValuesPublisher
                     CATEGORY device
                     TYPE iCub::debugLibrary::FakeRawValuesPublisher

diff --git a/src/libraries/icubmod/fakeRawValuesPublisher/FakeRawValuesPublisher.cpp b/src/libraries/icubmod/fakeRawValuesPublisher/FakeRawValuesPublisher.cpp
@@ -38,31 +38,48 @@ FakeRawValuesPublisher::FakeRawValuesPublisher()
     m_numberOfJomos = 4;
     // set sawtooth amplitude to default value of 128. It will be overridden by the conf value
     m_sawtoothThreshold = 128;
+    m_sawthootTestVal = 0;
     m_rawValuesVectorTag = "rawJomoEncoderValues";
+    m_rawDataAuxVector = {};
 }
 
 bool FakeRawValuesPublisher::getRawDataMap(std::map<std::string, std::vector<std::int32_t>> &map)
 {
-    return NOT_YET_IMPLEMENTED("getRawDataMap");
+    for (auto it = m_rawValuesMetadataMap.begin(); it != m_rawValuesMetadataMap.end(); it++)
+    {
+        m_rawDataAuxVector.clear();
+        if(!getRawData(it->first, m_rawDataAuxVector))
+        {
+            yCError(FAKERAWVALUESPUBLISHER) << "getRawData() failed. Cannot retrieve all data from local memory";
+            return false;
+        }
+        map.insert({it->first, m_rawDataAuxVector});
+    }
+    return true;
 }
 
 bool FakeRawValuesPublisher::getRawData(std::string key, std::vector<std::int32_t> &data)
 {   
-    if (m_rawValuesVectorsMap.find(key) != m_rawValuesVectorsMap.end())
+    if (m_rawValuesMetadataMap.find(key) != m_rawValuesMetadataMap.end())
     {
-        for (uint8_t i = 0; i < m_rawValuesVectorsMap[key].size(); i++)
+        m_sawthootTestVal = (m_sawthootTestVal < m_sawtoothThreshold) ? (++m_sawthootTestVal) : 0;
+        for (uint8_t i = 0; i < m_rawValuesMetadataMap[key].size; i++)
         {
-            m_rawValuesVectorsMap[key].at(i) = (m_rawValuesVectorsMap[key].at(i) < m_sawtoothThreshold) ? (m_rawValuesVectorsMap[key].at(i)+(i+1)) : 0;
+            data.push_back(m_sawthootTestVal);
         }
-
+    }
+    else
+    {
+        yCError(FAKERAWVALUESPUBLISHER) << "Request key:" << key << "is not available. Cannot retrieve get raw data.";
+        return false;
     }
 
     return true;
 }
 
 bool FakeRawValuesPublisher::getKeys(std::vector<std::string> &keys)
 {
-    for (const auto &p : m_rawValuesVectorsMap)
+    for (const auto &p : m_rawValuesMetadataMap)
     {
         keys.push_back(p.first);
     }
@@ -72,17 +89,43 @@ bool FakeRawValuesPublisher::getKeys(std::vector<std::string> &keys)
 
 int FakeRawValuesPublisher::getNumberOfKeys()
 {
-    return m_rawValuesVectorsMap.size();
+    return m_rawValuesMetadataMap.size();
 }
 
 
 bool FakeRawValuesPublisher::getMetadataMAP(rawValuesKeyMetadataMap &metamap)
 {
-    return NOT_YET_IMPLEMENTED("getMetadataMAP");
+    for (auto [k, v] : m_rawValuesMetadataMap)
+    {
+        yCDebug(FAKERAWVALUESPUBLISHER) << "size of elements name at key:" << k << "is:" << v.rawValueNames.size();
+        for (size_t e = 0; e < v.rawValueNames.size(); e++)
+        {
+            yCDebug(FAKERAWVALUESPUBLISHER) << "GOT to rawValueNames:" << v.rawValueNames[e];
+        }
+
+    }
+
+    if (m_rawValuesMetadataMap.empty())
+    {
+        yCError(FAKERAWVALUESPUBLISHER) << "embObjMotionControl Map is empty. No reason to proceed...";
+        return false;
+    }
+
+    metamap.metadataMap = m_rawValuesMetadataMap;
+    return true;
 }
 bool FakeRawValuesPublisher::getKeyMetadata(std::string key, rawValuesKeyMetadata &meta)
 {
-    return NOT_YET_IMPLEMENTED("getKeyMetadata");
+    if(m_rawValuesMetadataMap.find(key) != m_rawValuesMetadataMap.end())
+    {
+        meta = m_rawValuesMetadataMap[key];
+    }
+    else
+    {
+        yCError(FAKERAWVALUESPUBLISHER) << "Requested key" << key << "is not available in the map. Exiting";
+        return false;
+    }
+    return true;
 }
 
 bool FakeRawValuesPublisher::open(yarp::os::Searchable& config)
@@ -98,8 +141,11 @@ bool FakeRawValuesPublisher::open(yarp::os::Searchable& config)
     m_sawtoothThreshold = m_threshold;
 
     // Instantiate map of raw values vectors
-    m_rawValuesVectorsMap.insert({m_rawValuesVectorTag, std::vector<std::int32_t>(m_numberOfJomos * 2)});
-
+    m_rawValuesMetadataMap.insert({m_rawValuesVectorTag, rawValuesKeyMetadata({}, m_numberOfJomos)});
+    for (int i = 0; i < m_numberOfJomos;  i++)
+    {
+        m_rawValuesMetadataMap[m_rawValuesVectorTag].rawValueNames.push_back("fake_jomo_"+std::to_string(i));
+    }
     return true;
 }
 

diff --git a/src/libraries/icubmod/fakeRawValuesPublisher/FakeRawValuesPublisher.h b/src/libraries/icubmod/fakeRawValuesPublisher/FakeRawValuesPublisher.h
@@ -50,6 +50,7 @@ class iCub::debugLibrary::FakeRawValuesPublisher :
         FakeRawValuesPublisher();
         ~FakeRawValuesPublisher()  = default;
 
+        // IRawValuesPublisher
         virtual bool getRawDataMap(std::map<std::string, std::vector<std::int32_t>> &map) override;
         virtual bool getRawData(std::string key, std::vector<std::int32_t> &data) override;
         virtual bool getKeys(std::vector<std::string> &keys) override;
@@ -66,8 +67,10 @@ class iCub::debugLibrary::FakeRawValuesPublisher :
         int m_numberOfJomos = 0;
         int m_sawtoothThreshold = 0;
         std::string m_rawValuesVectorTag = "";
+        std::vector<std::int32_t> m_rawDataAuxVector;
+        int m_sawthootTestVal = 0;
 
-        std::map<std::string, std::vector<std::int32_t>> m_rawValuesVectorsMap;
+        std::map<std::string, rawValuesKeyMetadata> m_rawValuesMetadataMap;
 };
 
 #endif //_ICUB_FAKERAWVALUESPUBLISHER_H_