DynamicObstacleManager.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    armar6_skills::ArmarXObjects::DynamicObstacleManager
 * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
 
// STD/STL
#include <array>
#include <shared_mutex>
#include <string>
#include <tuple>
#include <vector>
 
// Eigen
#include <Eigen/Geometry>
 
// Ice
#include <Ice/Current.h>
 
// ArmarX
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
// Managed Objects
#include <RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h>
 
// ObstacleAvoidance
#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h>
 
// ArViz
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
namespace armarx
{
 
    class DynamicObstacleManager :
        virtual public Component,
        virtual public DynamicObstacleManagerInterface,
        virtual public ArVizComponentPluginUser
    {
    public:
        DynamicObstacleManager() noexcept;
 
        std::string getDefaultName() const override;
 
        void add_decayable_obstacle(const Eigen::Vector2f&,
                                    float,
                                    float,
                                    float,
                                    const Ice::Current& = Ice::Current()) override;
        void add_decayable_line_segment(const Eigen::Vector2f&,
                                        const Eigen::Vector2f&,
                                        const Ice::Current& = Ice::Current()) override;
        void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines,
                                         const Ice::Current& = Ice::Current()) override;
        void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
        void directly_update_obstacle(const std::string& name,
                                      const Eigen::Vector2f&,
                                      float,
                                      float,
                                      float,
                                      const Ice::Current& = Ice::Current()) override;
        void remove_obstacle(const std::string& name,
                             const Ice::Current& = Ice::Current()) override;
        void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
        float distanceToObstacle(const Eigen::Vector2f& agentPosition,
                                 float safetyRadius,
                                 const Eigen::Vector2f& goal,
                                 const Ice::Current& = Ice::Current()) override;
 
    protected:
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        void update_decayable_obstacles();
 
        void visualize_obstacle(viz::Layer& l,
                                const ManagedObstaclePtr& o,
                                const armarx::DrawColor& color,
                                double z_pos,
                                bool);
 
    public:
        static const std::string default_name;
 
    private:
        const std::string m_obstacle_manager_layer_name = "DynamicObstacleManagerObstacles";
 
        unsigned long m_obstacle_index;
 
        std::vector<ManagedObstaclePtr> m_managed_obstacles;
        std::shared_mutex m_managed_obstacles_mutex;
 
        PeriodicTask<DynamicObstacleManager>::pointer_type m_update_obstacles;
        unsigned int m_decay_after_ms;
        unsigned int m_periodic_task_interval;
        unsigned int m_decay_factor;
        unsigned int m_access_bonus;
        unsigned int m_min_value_for_accepting;
 
        float m_min_coverage_of_obstacles;
        float m_min_coverage_of_line_samples_in_obstacle;
        unsigned int m_min_size_of_obstacles;
        unsigned int m_min_length_of_lines;
        unsigned int m_max_size_of_obstacles;
        unsigned int m_max_length_of_lines;
        unsigned int m_thickness_of_lines;
        unsigned int m_security_margin_for_obstacles;
        unsigned int m_security_margin_for_lines;
 
        bool m_remove_enabled;
        bool m_only_visualize;
        bool m_allow_spwan_inside;
 
        ObstacleDetectionInterface::ProxyType m_obstacle_detection;
    };
} // namespace armarx