HumanObstacleDetection.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    Armar6Skills::ArmarXObjects::HumanAvoidance
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @author     Fabian Peller
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// STD/STL
#include <array>
#include <string>
#include <tuple>
#include <vector>
 
// Eigen
#include <Eigen/Core>
 
// Ice
#include <Ice/Current.h>
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
// Interface
#include <RobotComponents/interface/components/ObstacleDetection/HumanObstacleDetectionInterface.h>
 
// ObstacleAvoidance
#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
 
namespace armarx
{
 
    class HumanObstacleDetection :
        virtual public armarx::Component,
        virtual public armarx::HumanObstacleDetectionInterface
    {
 
    private:
        struct OpenPoseResult
        {
            bool valid = false;
            unsigned int index;
            float distance;
            float confidence;
            Eigen::Vector3f mean_position;
            armarx::Keypoint3DMap keypoints;
            IceUtil::Time timestamp;
        };
 
        struct HumanApproximation
        {
            bool valid = false;
            std::map<std::string, std::pair<IceUtil::Time, Eigen::Vector2f>> transformed_keypoints;
            double posX;
            double posY;
            double yaw;
            double axisLengthX;
            double axisLengthY;
            IceUtil::Time timestamp;
        };
 
    public:
        HumanObstacleDetection() noexcept;
 
        std::string getDefaultName() const override;
 
        void setEnabled(bool enable, const Ice::Current& = Ice::emptyCurrent) override;
        void enable(const Ice::Current& = Ice::emptyCurrent) override;
        void disable(const Ice::Current& = Ice::emptyCurrent) override;
        void report3DKeypoints(const armarx::HumanPose3DMap& kpml,
                               long timestamp,
                               const Ice::Current& = Ice::emptyCurrent) override;
 
        void checkHumanVisibility();
 
    protected:
        void onInitComponent() override;
 
        void onConnectComponent() override;
 
        void onDisconnectComponent() override;
 
        void onExitComponent() override;
 
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        OpenPoseResult find_closest_human(const std::vector<armarx::Keypoint3DMap>& kpml,
                                          const IceUtil::Time& time);
 
        HumanApproximation approximate_human(const OpenPoseResult& human);
 
    public:
        static const std::string default_name;
 
    private:
        bool m_enabled;
        int m_onlyUseFirstNResults;
 
        unsigned int m_warn_distance;
 
        float m_human_confidence_filter_value;
        float m_min_velocity_treshold;
        IceUtil::Time m_keypoint_after;
 
        unsigned int m_pose_buffer_fillctr = 0;
        unsigned int m_pose_buffer_index = 0;
        WriteBufferedTripleBuffer<std::array<HumanApproximation, 10>> m_pose_buffer;
 
        PeriodicTask<HumanObstacleDetection>::pointer_type m_check_human_task;
 
        HumanApproximation last_human_approximation;
 
 
        DynamicObstacleManagerInterface::ProxyType m_obstacle_manager;
 
        std::mutex m_human_mutex;
        std::mutex m_enabled_mutex;
    };
 
} // namespace armarx