NameRecognizedObjects.cpp

Go to the documentation of this file.

/*
 * 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    RobotSkillTemplates::ScanLocationGroup
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NameRecognizedObjects.h"
 
#include <filesystem>
 
#include <boost/regex.hpp>
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
 
 
using namespace armarx;
using namespace ScanLocationGroup;
 
namespace fs = std::filesystem;
 
// DO NOT EDIT NEXT LINE
NameRecognizedObjects::SubClassRegistry
    NameRecognizedObjects::Registry(NameRecognizedObjects::GetName(),
                                    &NameRecognizedObjects::CreateInstance);
 
class InterruptedException : std::exception
{
};
 
void
NameRecognizedObjects::waitForSpeechFinished()
{
    ARMARX_INFO << "Starting to wait for speech to finish";
    TimeUtil::SleepMS(200);
    while (!isRunningTaskStopped())
    {
        ARMARX_INFO
            << deactivateSpam(1) << "Speech observer status: "
            << getSpeechObserver()->getDatafieldByName("TextToSpeech", "State")->getString();
        if (getSpeechObserver()->getDatafieldByName("TextToSpeech", "State")->getString() ==
            "FinishedSpeaking")
        {
            ARMARX_IMPORTANT
                << "Speech observer status: "
                << getSpeechObserver()->getDatafieldByName("TextToSpeech", "State")->getString();
            break;
        }
        TimeUtil::SleepMS(10);
    }
    if (isRunningTaskStopped())
    {
        throw InterruptedException();
    }
}
 
void
NameRecognizedObjects::onEnter()
{
}
 
static std::vector<std::string>
getNamesFromChannels(std::vector<ChannelRefPtr> const& channels,
                     std::map<std::string, std::string> const& humanObjectNameMap)
{
    boost::regex re("[^a-zA-Z0-9]+");
 
    auto resolveObjectName = [&](ChannelRefPtr channelRef)
    {
        std::string memoryName = channelRef->getDataField("className")->getString();
        if (humanObjectNameMap.count(memoryName))
        {
            return humanObjectNameMap.at(memoryName);
        }
        else
        {
            return boost::regex_replace(memoryName, re, std::string(" "));
        }
    };
 
    std::vector<ChannelRefPtr> const& objectInstanceChannels = channels;
    std::vector<std::string> objectNames;
 
    std::set<std::string> uniqueObjectNames;
    for (ChannelRefPtr const& p : objectInstanceChannels)
    {
        auto resolvedName = resolveObjectName(p);
        if (uniqueObjectNames.count(resolvedName))
        {
            continue;
        }
        uniqueObjectNames.insert(resolvedName);
        objectNames.push_back(resolvedName);
    }
 
    return objectNames;
}
 
void
NameRecognizedObjects::run()
{
    std::map<std::string, std::string> humanObjectNameMap = in.getHumanObjectNameMap();
 
    std::vector<std::string> recognizedObjectsVec =
        getNamesFromChannels(in.getObjectInstanceChannels(), humanObjectNameMap);
 
    if (in.getUseObjectPoseStorage())
    {
        // Also get the object names from the ObjectPoseStorage
        const objpose::ObjectPoseStorageInterfacePrx objectPoseStorage = getObjectPoseStorage();
        const std::vector<std::string> objPoseProviderNames = in.getObjectPoseProviderNames();
        const std::set<std::string> objPoseProviderNamesSet(objPoseProviderNames.begin(),
                                                            objPoseProviderNames.end());
        const objpose::ObjectPoseSeq objectPoses =
            objpose::fromIce(getObjectPoseStorage()->getObjectPoses());
        ARMARX_INFO << "Got " << objectPoses.size() << " object poses.";
 
        ObjectFinder finder;
        for (objpose::ObjectPose const& objectPose : objectPoses)
        {
            if (objPoseProviderNamesSet.count(objectPose.providerName))
            {
                const static bool includeClassName = false;
                const std::vector<std::string> names =
                    finder.loadSpokenNames(objectPose.objectID, includeClassName);
                ARMARX_IMPORTANT << "Loaded names for object " << objectPose.objectID << ": "
                                 << names;
 
                const std::string objectName =
                    names.size() > 0 ? names.front() : objectPose.objectID.className();
                recognizedObjectsVec.push_back(objectName);
            }
        }
    }
    std::set<std::string> recognizedObjects;
    for (const std::string& name : recognizedObjectsVec)
    {
        recognizedObjects.insert(simox::alg::to_lower(name));
    }
 
    std::vector<std::string> display = in.getDisplay_Text();
    if (display.size() != 2)
    {
        display.resize(2);
    }
 
    std::vector<std::string> ttsSeq;
    if (recognizedObjects.empty())
    {
        ttsSeq.push_back("I cannot see any objects that I know.");
        display[1] = "I did not find any objects.";
    }
    else if (recognizedObjects.size() == 1)
    {
        const std::string resolvedName = *recognizedObjects.begin();
        ttsSeq.push_back("I can see " + resolvedName);
        display[1] = "I can see the " + resolvedName;
    }
    else
    {
        ttsSeq.push_back("I can see the following objects: ");
        for (const std::string& objectName : recognizedObjects)
        {
            ttsSeq.push_back(" a " + objectName + "");
        }
 
        display[1] = "I found " + std::to_string(recognizedObjects.size()) + " objects.";
    }
 
    getMessageDisplay()->setMessage(display[0], display[1]);
 
    if (in.getReportObjectsWithSpeech())
    {
        for (auto tts : ttsSeq)
        {
            getTextToSpeech()->reportText(tts);
            if (in.getWaitForSpeechToFinish())
            {
                waitForSpeechFinished();
            }
            else
            {
                ARMARX_INFO << deactivateSpam(5) << "Not waiting for speech";
            }
        }
    }
 
    emitSuccess();
}
 
//void NameRecognizedObjects::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void
NameRecognizedObjects::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
NameRecognizedObjects::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new NameRecognizedObjects(stateData));
}