DSObstacleAvoidance.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    RobotAPI::ArmarXObjects::DSObstacleAvoidance
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// STD/STL
#include <array>
#include <deque>
#include <shared_mutex>
#include <string>
#include <map>
#include <mutex>
#include <vector>
 
// Eigen
#include <Eigen/Geometry>
 
// Ice
#include <Ice/Current.h>
 
// DynamicObstacleAvoidance
#include <DynamicObstacleAvoidance/Agent.hpp>
#include <DynamicObstacleAvoidance/Environment.hpp>
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/util/tasks.h>
#include <ArmarXCore/util/time.h>
 
// RobotAPI
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.h>
 
 
namespace armarx
{
 
    class DSObstacleAvoidance :
        virtual public Component,
        virtual public DSObstacleAvoidanceInterface,
        virtual public ArVizComponentPluginUser
    {
 
    public:
 
        DSObstacleAvoidance();
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName()
        const override;
 
        dsobstacleavoidance::Config
        getConfig(const Ice::Current& = Ice::emptyCurrent)
        override;
 
        void
        updateObstacle(
            const obstacledetection::Obstacle& obstacle,
            const Ice::Current& = Ice::emptyCurrent)
        override;
 
        std::vector<obstacledetection::Obstacle>
        getObstacles(const Ice::Current& = Ice::emptyCurrent)
        override;
 
        void
        removeObstacle(
            const std::string& obstacle_name,
            const Ice::Current& = Ice::emptyCurrent)
        override;
 
        Eigen::Vector3f
        modulateVelocity(
            const obstacleavoidance::Agent& agent,
            const Ice::Current& = Ice::emptyCurrent)
        override;
 
        void
        updateEnvironment(const Ice::Current& = Ice::emptyCurrent)
        override;
 
        void
        writeDebugPlots(const std::string& filename, const Ice::Current& = Ice::emptyCurrent)
        override;
 
    protected:
 
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void
        onInitComponent()
        override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void
        onConnectComponent()
        override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void
        onDisconnectComponent()
        override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void
        onExitComponent()
        override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr
        createPropertyDefinitions()
        override;
 
    private:
 
        void
        sanity_check_obstacle(const obstacledetection::Obstacle& obstacle)
        const;
 
        void
        run_watchdog();
 
        void
        run_visualization();
 
    public:
 
        static const std::string default_name;
 
    private:
 
        struct visualization
        {
            bool enabled = true;
            PeriodicTask<DSObstacleAvoidance>::pointer_type task;
            unsigned task_interval = 20;
            mutable std::mutex mutex;
            bool needs_update = true;
        };
 
        struct watchdog
        {
            bool enabled = true;
            PeriodicTask<DSObstacleAvoidance>::pointer_type task;
            unsigned task_interval = 100;
            IceUtil::Time threshold = IceUtil::Time::milliSeconds(500);
        };
 
        struct dynamic_obstacle_avoidance
        {
            dsobstacleavoidance::Config cfg;
            DynamicObstacleAvoidance::Environment env;
            mutable std::shared_mutex mutex;
            std::string load_obstacles_from_file =
                "RobotAPI/obstacle_avoidance/r034_2d_scene_obstacles.json";
        };
 
        struct buffer
        {
            enum class update_type
            {
                update,
                removal
            };
 
            struct update
            {
                IceUtil::Time time;
                std::string name;
                DSObstacleAvoidance::buffer::update_type type;
            };
 
            mutable std::mutex mutex;
            std::vector<DSObstacleAvoidance::buffer::update> update_log;
            std::map<std::string, obstacledetection::Obstacle> obstacles;
            bool needs_env_update;
            IceUtil::Time last_env_update;
        };
 
        DSObstacleAvoidance::visualization m_vis;
 
        DSObstacleAvoidance::watchdog m_watchdog;
 
        DSObstacleAvoidance::dynamic_obstacle_avoidance m_doa;
 
        DSObstacleAvoidance::buffer m_buf;
 
    };
 
}