Develop proper facial expressions handler – Stint 3

README.md

@@ -45,7 +45,6 @@ cmake -DERGOCUB_MODEL_GENERATE_SIMMECHANICS=BOOL:ON -DERGOCUB_MODEL_COPY_TO_SRC=
 make
 (make install)
 ```
-
 ### Automatic generation
 [This GitHub Action](/.github/workflows/generate_models.yml) generates automatically the urdf everytime a commit in `urdf/simmechanics` on master branch is done and opens a PR containing the changes in the models.
 

src/modules/ergoCubEmotions/CMakeLists.txt

@@ -9,7 +9,7 @@ project(ergoCubEmotions)
 cmake_minimum_required(VERSION 3.12)
 
 find_package(YARP 3.7.2 REQUIRED)
-find_package(OpenCV 4.5.4 REQUIRED)
+find_package(OpenCV 4.2.0 REQUIRED)
 
 set(doc_files ${PROJECT_NAME}.xml)
 yarp_add_idl(IDL_GEN_FILES idl.thrift)
@@ -18,7 +18,8 @@ source_group("IDL Files" FILES idl.thrift)
 source_group("DOC Files" FILES ${doc_files})
 add_executable(${PROJECT_NAME} ergoCubEmotions.cpp ergoCubEmotions.h main.cpp idl.thrift ${IDL_GEN_FILES} ${doc_files})
 
-install(FILES config.ini DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}/${PROJECT_NAME})
+install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/app/conf/config.ini DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}/${PROJECT_NAME})
+install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/app/scripts/emotionHandler.xml DESTINATION ${YARP_APPLICATIONS_INSTALL_DIR}/${PROJECT_NAME})
 install(DIRECTORY expressions DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}/${PROJECT_NAME})
 
 # Link the application with YARP and OpenCV libraries

src/modules/ergoCubEmotions/README.md

@@ -0,0 +1,15 @@
+## ergoCubEmotions 
+This repo contains the `ergoCubEmotions` modul that allows to display different facial expressions on ergoCub.
+This module can be run enabling the cmake flag `COMPILE_ergoCubEmotions` and it needs [OpenCV v4.x](https://github.com/opencv/opencv) or higher to be used.
+### Usage
+```sh
+mkdir build
+cd build
+cmake -DCOMPILE_ergoCubEmotions=BOOL:ON ..
+make
+(make install)
+```
+The module is already integrated in `yarpmanager` and, in order to use it, run the following command in a console:
+```console
+yarp rpc /ergoCubEmotions/rpc
+```

src/modules/ergoCubEmotions/config.ini → src/modules/ergoCubEmotions/app/conf/config.ini

@@ -1,5 +1,5 @@
 [general]
-num_expressions 4
+num_expressions 5
 
 [expression_0]
 name angry
@@ -20,3 +20,8 @@ file expressions/images/exp_img_2.png
 name shy
 type image
 file expressions/images/exp_img_3.png
+
+[expression_4]
+name wink
+type video
+file expressions/videos/wink.mp4

src/modules/ergoCubEmotions/app/scripts/emotionHandler.xml

@@ -0,0 +1,13 @@
+<application>
+<name>ergoCubEmotions</name>
+
+        <dependencies>
+        </dependencies>
+
+        <module>
+            <name>ergoCubEmotions</name>
+            <parameters>--context ergoCubEmotions --from config.ini </parameters>
+            <node>localhost</node>
+        </module>
+
+</application>

src/modules/ergoCubEmotions/ergoCubEmotions.cpp

@@ -7,11 +7,6 @@
 
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
-#include <yarp/os/Bottle.h>
-
-#include <opencv2/core.hpp>
-#include <opencv2/highgui.hpp>
-#include <opencv2/videoio.hpp>
 
 #include "ergoCubEmotions.h"
 
@@ -33,9 +28,6 @@ bool ErgoCubEmotions::attach(RpcServer& source)
 
 bool ErgoCubEmotions::configure(ResourceFinder& rf)
 {
-    cmdPort.open("/ergoCubEmotions/rpc");
-    attach(cmdPort);
-
     this->rf=&rf;
     Bottle &bGroup = rf.findGroup("general");
     nexpressions = bGroup.find("num_expressions").asInt32();
@@ -48,23 +40,23 @@ bool ErgoCubEmotions::configure(ResourceFinder& rf)
         std::string type = bExpression.find("type").asString();
         std::string file = bExpression.find("file").asString();
 
-        std::pair<std::string, std::string> par = std::make_pair(name, type);
-        img_map[par] = file;
+        std::pair<std::string, std::string> par = std::make_pair(type, file);
+        img_map[name] = par;
     }
-
     namedWindow("emotion", WND_PROP_FULLSCREEN);
-    setWindowProperty("emotion", WND_PROP_FULLSCREEN, WINDOW_FULLSCREEN);
+    setWindowProperty("emotion", WND_PROP_FULLSCREEN, WINDOW_FULLSCREEN);  
     path = rf.findFile("expressions/images/exp_img_2.png");
-    Mat start_img = imread(path);
-    if(start_img.empty())
+    img = imread(path);
+    if(img.empty())
     {
         yError() << "Could not read the image";
         return false;
     }
-
-    imshow("emotion", start_img);
+    imshow("emotion", img);
     waitKey(1000);
-
+
+    cmdPort.open("/ergoCubEmotions/rpc");
+    attach(cmdPort);
     return true;
 }
 
@@ -82,45 +74,68 @@ double ErgoCubEmotions::getPeriod()
 
 bool ErgoCubEmotions::updateModule()
 {
-    return true;
-}
-
-bool ErgoCubEmotions::setEmotion(const std::string& command)
-{    
-    std::lock_guard<std::mutex> lg(mtx);
     for(auto it = img_map.cbegin(); it!= img_map.cend(); it++)
     {
-        if(it->first.first == command)
-        {    
-            if(it->first.second == "image")
-            {
-                path = rf->findFile(it->second);
-                Mat img = imread(path);
-                if(img.empty())
+        if(it->first == command)
+        {   
+            if(it->second.first == "image")
+            {   
+                path = rf->findFile(it->second.second);
+                Mat img_tmp = imread(path);
+                if(img_tmp.empty())
                 {
                     yDebug() << "Could not read the image!";
                 }
-
-                imshow("emotion", img);
-                pollKey();
+                else
+                {
+                    img = img_tmp;
+                    imshow("emotion", img);
+                    pollKey();
+                }
             }
 
-            else if(it->first.second == "video")
+            else if(it->second.first == "video")
             {
-                // VideoCapture cap("images/video.mp4");
-                // while(1)
-                // {
-                //     Mat frame;
-                //     cap >> frame;
-                //     imshow("emotion", frame);
-                //     char c=(char)waitKey(25);
-                //     if(c==27)
-                //         break;
-                // }
-                // cap.release();
+                path = rf->findFile(it->second.second);
+                VideoCapture cap(path);
+                while(cap.isOpened())
+                {
+                    Mat frame;
+                    cap >> frame;
+                    if(frame.empty())
+                        break;
+                    imshow("emotion", frame);
+                    pollKey();  
+                }
             }
         }
     }
-
     return true;
+}
+
+bool ErgoCubEmotions::setEmotion(const std::string& command)
+{   
+    if(img_map.count(command) < 1)
+    {
+        yError() << "Command not found!";
+        return false;
+    }
+
+    else
+    {
+        this->command = command;
+        return true;
+    }
+}
+
+std::vector<std::string> ErgoCubEmotions::availableEmotions()
+{
+    std::vector<std::string> cmd;
+
+    for(auto it = img_map.cbegin(); it!= img_map.cend(); it++)
+    {
+        cmd.push_back(it->first);
+    }
+
+    return cmd;
 }

src/modules/ergoCubEmotions/ergoCubEmotions.h

@@ -8,8 +8,6 @@
 #ifndef __ERGOCUBEMOTIONS__
 #define __ERGOCUBEMOTIONS__
 
-#include <mutex>
-
 #include <yarp/os/Vocab.h>
 #include <yarp/os/RFModule.h>
 #include <yarp/os/ResourceFinder.h>
@@ -18,26 +16,13 @@
 #include <yarp/os/BufferedPort.h>
 #include <yarp/os/ConnectionReader.h>
 
-#include "ergoCubEmotions_IDL.h"
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/videoio.hpp>
 
-// define the vocabs
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_SET = yarp::os::createVocab32('s','e','t');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_GET = yarp::os::createVocab32('g','e','t');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_IS  = yarp::os::createVocab32('i','s');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_FAILED = yarp::os::createVocab32('f','a','i','l');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_OK = yarp::os::createVocab32('o','k');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_HELP = yarp::os::createVocab32('h','e','l','p');
+#include "ergoCubEmotions_IDL.h"
 
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_NEUTRAL = yarp::os::createVocab32('n','e','u');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_HAPPY = yarp::os::createVocab32('h','a','p');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_SAD = yarp::os::createVocab32('s','a','d');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_SURPRISED = yarp::os::createVocab32('s','u','r');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_ANGRY = yarp::os::createVocab32('a','n','g');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_EVIL = yarp::os::createVocab32('e','v','i');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_SHY = yarp::os::createVocab32('s','h','y');
-constexpr  yarp::conf::vocab32_t EMOTION_VOCAB_CUN = yarp::os::createVocab32('c','u','n');
 
-// ergoCubEmotions class definition
 class ErgoCubEmotions : public yarp::os::RFModule, public ergoCubEmotions_IDL {
     protected:
         yarp::os::ResourceFinder *rf;
@@ -53,13 +38,13 @@ class ErgoCubEmotions : public yarp::os::RFModule, public ergoCubEmotions_IDL {
         double getPeriod();
 
         bool setEmotion(const std::string& command);
+        std::vector<std::string> availableEmotions();
 
-        std::mutex mtx;
         yarp::os::RpcServer cmdPort;
-
         int nexpressions;
         std::string path;
-        std::map<std::pair<std::string, std::string>, std::string> img_map;
+        std::map<std::string, std::pair<std::string, std::string>> img_map;
+        std::string command;
+        cv::Mat img;
 };
-
 #endif

src/modules/ergoCubEmotions/idl.thrift

@@ -24,4 +24,10 @@ service ergoCubEmotions_IDL
         * @return true/false on success/failure.
         */
         bool setEmotion(1:string command);
+
+        /**
+        * Print the list of all the available facial expressions.
+        * @return a list of commands.
+        */
+        list<string> availableEmotions();
 }