OpenPoseEstimationComponent.h

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
 */
#pragma once
 
// STD/STL
#include <mutex>
 
// IVT
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Math/Math2d.h>
#include <Math/Math3d.h>
 
// Simox
#include <VirtualRobot/Robot.h>
 
// ArmarX
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/components/RobotState/RobotStateComponent.h>
#include <VisionX/core/ImageProcessor.h>
 
// OpenPose stuff
#include <VisionX/interface/components/OpenPoseEstimationInterface.h>
#include <VisionX/libraries/OpenPose/MonocularOpenPoseEstimation/OpenPoseAdapter.h>
 
#include "OpenPoseEstimationUtil.h"
 
namespace armarx
{
    class OpenPoseEstimationComponentPluginUser :
            public OpenPoseEstimationInterface,
            public ArVizComponentPluginUser
    {
    public:
        OpenPoseEstimationComponentPluginUser();
 
        // Helper functions
        virtual void preOnInitImageProcessor();
        virtual void preOnConnectImageProcessor();
        virtual void postOnConnectImageProcessor();
        virtual void preOnDisconnectImageProcessor();
        virtual void postOnDisconnectImageProcessor();
 
        virtual void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties);
 
        // HumanPoseEstimation Interface
        void enableHumanPoseEstimation(const EnableHumanPoseEstimationInput& input,
                                       const Ice::Current& = Ice::emptyCurrent) override;
 
        // OpenPoseEstimation Interface
        void start(const Ice::Current& = Ice::emptyCurrent)  override;
        void stop(const Ice::Current& = Ice::emptyCurrent)  override;
        void start3DPoseEstimation(const Ice::Current& = Ice::emptyCurrent)  override;
        void stop3DPoseEstimation(const Ice::Current& = Ice::emptyCurrent)  override;
 
        virtual void renderOutputImage(const op::Array<float>&);
        virtual void reportEntities();
        virtual void visualize();
 
    protected:
        // main loop
        void loop();
 
    public:
        bool active_upon_startup = false;
 
        // ImageBuffer und ImageInformations
        CByteImage** imageBuffer;
        unsigned int numImages;
        visionx::ImageProviderInfo imageProviderInfo;
        armarx::MetaInfoSizeBasePtr imageMetaInfo;
        std::mutex imageBufferMutex;
 
        visionx::ImageFormatInfo rgbImageFormat;
        CByteImage* rgbImageBuffer;
        std::mutex rgbImageBufferMutex;
 
        // ErrorCounters
        std::uint64_t timeoutCounter2d{0};
        std::uint64_t readErrorCounter2d{0};
        std::uint64_t sucessCounter2d{0};
 
        // Topics
        OpenPose2DListenerPrx listener2DPrx;
 
        // OpenPose Buffers
        HumanPose2DMap openposeResult;
 
        // ImageBuffer und ImageInformations
        std::mutex openposeResultImageMutex;
        CByteImage** openposeResultImage;
        std::atomic_bool result_image_ready = false;
 
        // OpenPose renderer
        OpenPoseAdapter::OpenPoseSettings op_settings;
        float op_render_threshold = 0.05f;
        bool openpose_is_initialized = false;
 
        std::mutex openpose_initializing_mutex;
        MonocularOpenPoseEstimationPtr openposePtr = nullptr;
 
        // Visualization
        std::string layerName = "OpenPoseEstimation";
 
        // Threads and program flow information
        armarx::RunningTask<OpenPoseEstimationComponentPluginUser>::pointer_type openposeTask;
        std::atomic_bool running2D = false;
        std::atomic_bool update_ready; // Is true, if new updates are available
        std::atomic_long timestamp_of_update; // Contains the timestamp of the currently available update
    };
}