OpenPoseSimulation.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    VisionX::ArmarXObjects::OpenPoseEstimation
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "OpenPoseSimulation.h"
 
#include <stdexcept>
 
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/interface/components/RGBDImageProvider.h>
#include <VisionX/libraries/ArViz/HumanPoseBody25.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/math/ColorUtils.h>
 
#include <Image/IplImageAdaptor.h>
#include <Image/PrimitivesDrawer.h>
 
 
 
// json
#include <SimoxUtility/json/json.hpp>
 
using namespace armarx;
 
armarx::PropertyDefinitionsPtr OpenPoseSimulation::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new OpenPoseSimulationPropertyDefinitions(
            getConfigIdentifier()));
}
 
 
void OpenPoseSimulation::onInitComponent()
{
 
}
 
 
void OpenPoseSimulation::onConnectComponent()
{
 
    offeringTopic(getProperty<std::string>("OpenPoseEstimation2DTopicName").getValue());
    listener2DPrx = getTopic<OpenPose2DListenerPrx>(getProperty<std::string>("OpenPoseEstimation2DTopicName").getValue());
    offeringTopic(getProperty<std::string>("OpenPoseEstimation3DTopicName").getValue());
    listener3DPrx = getTopic<OpenPose3DListenerPrx>(getProperty<std::string>("OpenPoseEstimation3DTopicName").getValue());
 
    ARMARX_VERBOSE << "Trying to get RobotStateComponent proxy";
    // Trying to access robotStateComponent if it is avaiable
    robotStateInterface = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue(), false, "", false);
    if (robotStateInterface)
    {
        localRobot = RemoteRobot::createLocalClone(robotStateInterface);
        ARMARX_VERBOSE << "RobotStateComponent available";
    }
    else
    {
        ARMARX_ERROR << "RobotStateCompontent is not avaiable, 3D-Estimation will be deactivated!!";
    }
 
    start3DPoseEstimation();
}
 
 
void OpenPoseSimulation::onDisconnectComponent()
{
    stop3DPoseEstimation();
}
 
 
void OpenPoseSimulation::onExitComponent()
{
 
}
 
void OpenPoseSimulation::start(const Ice::Current&)
{
    if (running2D)
    {
        ARMARX_INFO << "already running";
        return;
    }
    else
    {
        ARMARX_INFO << "Starting OpenPoseEstimation";
        running2D = true;
        runTask = new RunningTask<OpenPoseSimulation>(this, &OpenPoseSimulation::run);
        runTask->start();
    }
}
 
void OpenPoseSimulation::stop(const Ice::Current&)
{
    if (running2D)
    {
        ARMARX_INFO << "Stopping OpenPoseEstimation";
        running2D = false;
        runTask->stop(true);
        stop3DPoseEstimation();
    }
    else
    {
        ARMARX_INFO << "not running";
    }
}
 
void OpenPoseSimulation::start3DPoseEstimation(const Ice::Current&)
{
    if (running3D)
    {
        return;
    }
    else
    {
        ARMARX_INFO << "Starting OpenPoseEstimation -- 3D";
 
        running3D = true;
        start();
    }
}
 
void OpenPoseSimulation::stop3DPoseEstimation(const Ice::Current&)
{
    if (running3D)
    {
        ARMARX_INFO << "Stopping OpenPoseEstimation -- 3D";
        running3D = false;
    }
    {
        // In any case, let's clear the layer.
        viz::Layer openPoseArVizLayer = arviz.layer(layerName);
        openPoseArVizLayer.clear();
        arviz.commit(openPoseArVizLayer);
    }
}
 
void OpenPoseSimulation::onMessage(const Texting::TextMessage& text, const Ice::Current&)
{
    using json = nlohmann::json;
    // parsing message
    json jsonView = json::parse(text.message);
 
    // Reading values
    long timestamp = jsonView["timestamp"].get<long>();
    json jsonValues = jsonView["values"];
    // TODO
}
 
void OpenPoseSimulation::run()
{
    ARMARX_VERBOSE << "Worker thread started.";
    ARMARX_DEBUG << "Transitioning into worker loop.";
 
    while (running2D && !runTask->isStopped())
    {
 
        ARMARX_DEBUG << "Done calculating 2D keypoints.";
        long timeProvidedImage = IceUtil::Time::now().toMilliSeconds();
 
        if (running3D && localRobot)
        {
            ARMARX_TRACE;
 
            // 2. Calculate 3D values
            RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateInterface, timeProvidedImage);
 
            ARMARX_TRACE;
 
 
 
            ARMARX_VERBOSE << deactivateSpam(0.5) << "Reporting 2Dkeypoints for " << 1 << " objects";
            listener2DPrx->report2DKeypoints({}, timeProvidedImage);
            listener2DPrx->report2DKeypointsNormalized({}, timeProvidedImage);
            ARMARX_VERBOSE << deactivateSpam(0.5) << "Reporting 3Dkeypoints for " << 1 << " objects";
            listener3DPrx->report3DKeypoints({}, timeProvidedImage);
        }
        else
        {
            ARMARX_VERBOSE << deactivateSpam(0.5) << "Reporting 2Dkeypoints for " << 1 << " objects";
            listener2DPrx->report2DKeypoints({}, timeProvidedImage);
            listener2DPrx->report2DKeypointsNormalized({}, timeProvidedImage);
        }
 
        ARMARX_TRACE;
        sleep(1);
    }
}
 
OpenPoseSimulationPropertyDefinitions::OpenPoseSimulationPropertyDefinitions(std::string prefix) :
    armarx::ComponentPropertyDefinitions(prefix)
{
    defineOptionalProperty<std::string>("OpenPoseEstimation2DTopicName", "OpenPoseEstimation2D");
    defineOptionalProperty<std::string>("OpenPoseEstimation3DTopicName", "OpenPoseEstimation3D");
 
    defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent");
    defineOptionalProperty<std::string>("CameraNodeName", "DepthCamera", "Name of the robot node for the input camera");
}