TopicRecorder.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    ArmarX
* @author     Mirko Waechter( mirko.waechter at kit dot edu)
* @date       2016
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include "TopicRecorder.h"
 
#include <fstream>
 
#include <Ice/ObjectAdapter.h>
#include <IceStorm/IceStorm.h> // for TopicPrx, TopicManagerPrx, etc
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include "DatabaseTopicWriter.h"
#include "FileTopicWriter.h"
#include "GenericTopicSubscriber.h"
 
namespace armarx
{
 
    TopicRecorderComponent::TopicRecorderComponent()
    {
        writer.reset();
    }
 
    PropertyDefinitionsPtr
    TopicRecorderComponent::createPropertyDefinitions()
    {
        return new TopicRecorderProperties(Component::getConfigIdentifier());
    }
 
    void
    TopicRecorderComponent::wakeUp()
    {
        idleCondition.notify_all();
    }
 
    void
    TopicRecorderComponent::onInitComponent()
    {
        isRecordingEnabled = false;
        outputFilename = getProperty<std::string>("Outputfile").getValue();
 
        offeringTopic("TopicRecorderListener");
    }
 
    void
    TopicRecorderComponent::onConnectComponent()
    {
        topicRecoderListener = getTopic<TopicRecorderListenerInterfacePrx>("TopicRecorderListener");
 
        auto allTopics = getIceManager()->getTopicManager()->retrieveAll();
        for (auto& elem : allTopics)
        {
            ARMARX_INFO << "Topic: " << elem.first;
        }
 
        auto startTimestamp = TimeUtil::GetTime();
        Ice::StringSeq topics;
        if (getProperty<std::string>("TopicsToLog").getValue() == "*")
        {
            for (auto& elem : allTopics)
            {
                topics.push_back(elem.first);
            }
        }
        else
        {
            topics = Split(getProperty<std::string>("TopicsToLog").getValue(), ",");
        }
        for (auto topic : topics)
        {
            float maxFrequency = -1.f;
            if (Contains(topic, ":"))
            {
                auto topicAndFreq = Split(topic, ":");
                maxFrequency = atof(topicAndFreq.at(1).c_str());
                topic = topicAndFreq.at(0);
            }
            GenericTopicSubscriberPtr ptr(
                new GenericTopicSubscriber(this, topic, startTimestamp, maxFrequency));
            Ice::ObjectPrx prx = getObjectAdapter()->addWithUUID(ptr);
            getIceManager()->subscribeTopic(prx, topic, true);
            topicsSubscribers[topic] = ptr;
        }
 
 
        if (getProperty<bool>("EnableRecording").getValue())
        {
            startRecording(getProperty<int>("Duration").getValue());
        }
        else
        {
            ARMARX_INFO << "automatic recording disabled.";
        }
    }
 
    void
    armarx::TopicRecorderComponent::startRecording(const int maxDuration, const Ice::Current& c)
    {
        std::lock_guard<std::mutex> lock(mutex);
 
        if (isRecordingEnabled)
        {
            ARMARX_WARNING << "topic recorder is already recording.";
            return;
        }
 
 
        this->maxDuration = maxDuration;
        outputfilePath = outputFilename;
        if (getProperty<bool>("TimestampedFilename").getValue())
        {
            std::string time = IceUtil::Time::now().toDateTime();
            time = simox::alg::replace_all(time, "/", "-");
            time = simox::alg::replace_all(time, " ", "_");
            time = simox::alg::replace_all(time, ":", "-");
            outputfilePath =
                outputfilePath.parent_path() /
                (outputfilePath.stem().string() + "-" + time + outputfilePath.extension().string());
        }
 
        std::string outputfilePathStr = outputfilePath.string();
        ArmarXDataPath::ResolveHomePath(outputfilePathStr);
        outputfilePath = outputfilePathStr;
        ARMARX_IMPORTANT << "Writing to file '" << outputfilePath.string() << "'";
 
        //Check which storage mode to use and init the writer
        std::string storageMode = getProperty<std::string>("StorageMode").getValue();
        if (!storageMode.compare("file"))
        {
            writer.reset(new FileTopicWriter(outputfilePath));
        }
        else if (!storageMode.compare("database"))
        {
            writer.reset(new DatabaseTopicWriter(outputfilePath));
        }
        else
        {
            ARMARX_WARNING
                << "StorageMode " << storageMode
                << " is not supported (database, file). Falling back to default 'database' mode.";
            writer.reset(new DatabaseTopicWriter(outputfilePath));
        }
 
        startTime = armarx::TimeUtil::GetTime();
 
        for (auto kv : topicsSubscribers)
        {
            kv.second->setTime(startTime);
            std::queue<TopicUtil::TopicData> data;
            kv.second->getData(data);
        }
 
        queueTask = new RunningTask<TopicRecorderComponent>(
            this, &TopicRecorderComponent::write, "WriterThread");
        queueTask->start();
 
        isRecordingEnabled = true;
 
        topicRecoderListener->onStartRecording();
    }
 
    void
    armarx::TopicRecorderComponent::stopRecording(const Ice::Current& c)
    {
        std::lock_guard<std::mutex> lock(mutex);
 
        if (!isRecordingEnabled)
        {
            ARMARX_WARNING << "topic recorder is not recording.";
            return;
        }
 
 
        idleCondition.notify_all();
        if (queueTask->isRunning())
        {
            queueTask->stop();
        }
 
        ARMARX_IMPORTANT << "Writing log ";
        writer.reset();
        ARMARX_IMPORTANT << "Wrote everything to file '" << outputfilePath.string() << "'";
 
        isRecordingEnabled = false;
    }
 
    void
    TopicRecorderComponent::onDisconnectComponent()
    {
        stopRecording();
    }
 
    void
    TopicRecorderComponent::onExitComponent()
    {
        topicsSubscribers.clear();
    }
 
    std::string
    TopicRecorderComponent::getDefaultName() const
    {
        return "TopicRecorder";
    }
 
    void
    TopicRecorderComponent::write()
    {
        auto sortFunc = [](const TopicUtil::TopicData& l, const TopicUtil::TopicData& r)
        { return (l.timestamp < r.timestamp); };
 
        while (!queueTask->isStopped())
        {
            if (maxDuration)
            {
                if ((startTime + IceUtil::Time::seconds(maxDuration)) < armarx::TimeUtil::GetTime())
                {
                    ARMARX_INFO << "max recording duration reached.";
                    stopRecording();
                }
            }
 
            std::unique_lock lock(queueMutex);
 
            std::vector<TopicUtil::TopicData> newDataOfAllTopics;
            for (auto& elem : topicsSubscribers)
            {
                std::queue<TopicUtil::TopicData> data;
                elem.second->getData(data);
 
                while (!data.empty())
                {
                    newDataOfAllTopics.push_back(data.front());
                    data.pop();
                }
            }
            std::sort(newDataOfAllTopics.begin(), newDataOfAllTopics.end(), sortFunc);
            for (auto& e : newDataOfAllTopics)
            {
                writer->write(e);
            }
            if (newDataOfAllTopics.empty())
            {
                usleep(10000);
            }
        }
 
        if (getProperty<int>("Duration").getValue() > 0)
        {
            getArmarXManager()->asyncShutdown();
        }
    }
 
 
} // namespace armarx