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 <map>
#include <mutex>
#include <shared_mutex>
#include <string>
#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/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.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;
    };
 
} // namespace armarx