ManagedObstacle.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 <tuple>
#include <shared_mutex>
#include <string>
#include <vector>
#include <memory>
 
// Eigen
#include <Eigen/Geometry>
 
// ArmarX
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h>
 
 
namespace armarx
{
 
    class ManagedObstacle;
    typedef std::shared_ptr<ManagedObstacle> ManagedObstaclePtr;
 
    class ManagedObstacle
    {
    public:
 
        static bool ComparatorDESCPrt(ManagedObstaclePtr& i, ManagedObstaclePtr& j)
        {
            return (ManagedObstacle::calculateObstacleArea(i->m_obstacle) > ManagedObstacle::calculateObstacleArea(j->m_obstacle));
        }
 
        void update_position(const unsigned int);
 
        static double calculateObstacleArea(const obstacledetection::Obstacle& o);
 
        static bool point_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f point);
 
        static float ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin, const float e1_rx, const float e1_ry, const float e1_yaw, const Eigen::Vector2f e2_origin, const float e2_rx, const float e2_ry, const float e2_yaw);
 
        static float line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end);
 
 
        obstacledetection::Obstacle m_obstacle;
        IceUtil::Time m_last_matched;
        bool m_published;
        bool m_updated;
        float m_value;
        std::shared_mutex m_mutex;
 
        unsigned int position_buffer_fillctr = 0;
        unsigned int position_buffer_index = 0;
        std::array<std::tuple<double, double, double, double, double>, 6> position_buffer; //x, y, yaw, axisX, axisY
        std::vector<Eigen::Vector2f> line_matches; // points of line segments
 
 
        unsigned long m_input_index = 0;
    protected:
 
    private:
        float normalizeYaw(float yaw) const;
        float getAngleBetweenVectors(const Eigen::Vector2f&, const Eigen::Vector2f&) const;
        float getXAxisAngle(const Eigen::Vector2f&) const;
 
    };
}