DynamicObstacleManager.cpp

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
 */
 
#include "DynamicObstacleManager.h"
 
#include <VirtualRobot/MathTools.h>
 
// STD/STL
#include <cmath>
#include <limits>
#include <string>
#include <vector>
 
// Ice
#include <Ice/Current.h>
using namespace Ice;
 
// Eigen
#include <Eigen/Core>
using namespace Eigen;
 
// Simox
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/RobotIO.h>
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/util/time.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
 
using namespace armarx;
 
 
const std::string armarx::DynamicObstacleManager::default_name = "DynamicObstacleManager";
 
namespace armarx
{
 
    DynamicObstacleManager::DynamicObstacleManager() noexcept :
        m_obstacle_index(0),
        m_decay_after_ms(5000),
        m_periodic_task_interval(500),
        m_min_value_for_accepting(1000),
        m_min_coverage_of_obstacles(0.7f),
        m_min_coverage_of_line_samples_in_obstacle(0.9f),
        m_min_size_of_obstacles(100),
        m_min_length_of_lines(50),
        m_max_size_of_obstacles(600),
        m_max_length_of_lines(10000),
        m_thickness_of_lines(200),
        m_security_margin_for_obstacles(500),
        m_security_margin_for_lines(400),
        m_remove_enabled(false),
        m_only_visualize(false),
        m_allow_spwan_inside(false)
    {
    }
 
    std::string
    DynamicObstacleManager::getDefaultName() const
    {
        return DynamicObstacleManager::default_name;
    }
 
    void
    DynamicObstacleManager::onInitComponent()
    {
    }
 
    void
    DynamicObstacleManager::onConnectComponent()
    {
        m_decay_factor = m_periodic_task_interval;
        m_access_bonus = m_periodic_task_interval;
 
        m_update_obstacles = new PeriodicTask<DynamicObstacleManager>(
            this, &DynamicObstacleManager::update_decayable_obstacles, m_periodic_task_interval);
        m_update_obstacles->start();
    }
 
    void
    DynamicObstacleManager::onDisconnectComponent()
    {
        m_update_obstacles->stop();
    }
 
    void
    DynamicObstacleManager::onExitComponent()
    {
    }
 
    void
    DynamicObstacleManager::add_decayable_obstacle(const Eigen::Vector2f& e_origin,
                                                   float e_rx,
                                                   float e_ry,
                                                   float e_yaw,
                                                   const Ice::Current&)
    {
        // TODO
    }
 
    void
    DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start,
                                                       const Eigen::Vector2f& line_end,
                                                       const Ice::Current&)
    {
        TIMING_START(add_decayable_line_segment);
        const Eigen::Vector2f difference_vec = line_end - line_start;
        const float length = difference_vec.norm();
        const Eigen::Vector2f center_vec = line_start + 0.5 * difference_vec;
        const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
        const float dot_product = difference_vec_normed(0);
        const float cross_product = difference_vec_normed(1);
        float yaw = acos(dot_product);
 
        if (cross_product < 0)
        {
            yaw = 2 * M_PI - yaw;
        }
 
        if (length < m_min_length_of_lines)
        {
            return;
        }
 
        {
            std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
 
            TIMING_START(add_decayable_line_segment_mutex);
            // First check own obstacles
            for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
            {
 
                std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
                float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
                    {managed_obstacle->m_obstacle.posX, managed_obstacle->m_obstacle.posY},
                    managed_obstacle->m_obstacle.axisLengthX,
                    managed_obstacle->m_obstacle.axisLengthY,
                    managed_obstacle->m_obstacle.yaw,
                    line_start,
                    line_end);
                //ARMARX_DEBUG << "The coverage of the new line to obstacle " << managed_obstacle.obstacle.name << "(" << ManagedObstacle::calculateObstacleArea(managed_obstacle.obstacle) << ") is " << coverage;
                if (coverage >= m_min_coverage_of_line_samples_in_obstacle)
                {
                    // Obstacle already in list. Increase counter and update current obstacle.
                    managed_obstacle->line_matches.push_back(line_start);
                    managed_obstacle->line_matches.push_back(line_end);
                    managed_obstacle->m_last_matched = IceUtil::Time::now();
                    TIMING_END(add_decayable_line_segment_mutex);
                    return;
                }
            }
            TIMING_END(add_decayable_line_segment_mutex);
        }
 
        // Second check the obstacles from the obstacle avoidance
        /*for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
        {
            float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
            {published_obstacle.posX, published_obstacle.posY}, published_obstacle.axisLengthX, published_obstacle.axisLengthY, published_obstacle.yaw,
            line_start, line_end);
            if (coverage >= m_min_coverage_of_line_samples_in_obstacle)
            {
                // If own managed obstacle we already updatet its value. If its a static obstacle from the obstacle avoidance, then we can't update the position anyway...
                ARMARX_DEBUG << "Found the obstacle in the static obstacle list! Matching name: " << published_obstacle.name;
                return;
            }
        }*/
 
        ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
        ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
        new_managed_obstacle->m_obstacle.name =
            "managed_obstacle_" + std::to_string(m_obstacle_index++);
        new_managed_obstacle->m_obstacle.posX = center_vec(0);
        new_managed_obstacle->m_obstacle.posY = center_vec(1);
        new_managed_obstacle->m_obstacle.refPosX = center_vec(0);
        new_managed_obstacle->m_obstacle.refPosY = center_vec(1);
        new_managed_obstacle->m_obstacle.yaw = yaw;
        new_managed_obstacle->m_obstacle.axisLengthX =
            std::clamp(length * 0.5f,
                       static_cast<float>(m_min_length_of_lines),
                       static_cast<float>(m_max_length_of_lines));
        new_managed_obstacle->m_obstacle.axisLengthY = m_thickness_of_lines;
        new_managed_obstacle->m_obstacle.safetyMarginX = m_security_margin_for_lines;
        new_managed_obstacle->m_obstacle.safetyMarginY = m_security_margin_for_lines;
        new_managed_obstacle->m_last_matched = IceUtil::Time::now();
        new_managed_obstacle->m_published = false;
        new_managed_obstacle->m_value = m_min_value_for_accepting * 0.5;
        new_managed_obstacle->position_buffer_index = 0;
        new_managed_obstacle->position_buffer_fillctr = 0;
        new_managed_obstacle->line_matches.push_back(line_start);
        new_managed_obstacle->line_matches.push_back(line_end);
 
        {
            std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
            m_managed_obstacles.push_back(new_managed_obstacle);
        }
        TIMING_END(add_decayable_line_segment);
    }
 
    void
    DynamicObstacleManager::add_decayable_line_segments(
        const dynamicobstaclemanager::LineSegments& lines,
        const Ice::Current& c)
    {
        for (const auto& line : lines)
        {
            add_decayable_line_segment(line.lineStart, line.lineEnd, c);
        }
    }
 
    void
    DynamicObstacleManager::remove_all_decayable_obstacles(const Ice::Current&)
    {
        std::lock_guard l(m_managed_obstacles_mutex);
 
        ARMARX_DEBUG << "Remove all obstacles from obstacle map by setting their value to -inf";
        for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
        {
            managed_obstacle->m_value = std::numeric_limits<double>::min();
            managed_obstacle->m_updated = true;
        }
    }
 
    void
    DynamicObstacleManager::remove_obstacle(const std::string& name, const Ice::Current&)
    {
        std::lock_guard l(m_managed_obstacles_mutex);
 
        ARMARX_DEBUG << "Remove Obstacle " << name
                     << " from obstacle map and from obstacledetection";
        for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
        {
            if (managed_obstacle->m_obstacle.name == name)
            {
                managed_obstacle->m_value = std::numeric_limits<double>::min();
                managed_obstacle->m_updated = true;
                managed_obstacle->m_published = false;
                break;
            }
        }
        m_obstacle_detection->removeObstacle(name);
        m_obstacle_detection->updateEnvironment();
    }
 
    void
    DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
    {
        ARMARX_INFO << "Waiting for obstacles to get ready";
        usleep(2.0 * m_min_value_for_accepting);
        ARMARX_INFO << "Finished waiting for obstacles";
    }
 
    float
    DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition,
                                               float safetyRadius,
                                               const Eigen::Vector2f& goal,
                                               const Ice::Current&)
    {
        std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
 
        const Eigen::Vector2f diff = goal - agentPosition;
        const Eigen::Vector2f orthogonal_normalized =
            Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2) * diff).normalized();
 
        const float sample_step = 5; // in [mm], sample step size towards goal.
        const float distance_to_goal = diff.norm() + safetyRadius;
 
        float current_distance = safetyRadius;
 
        while (current_distance < distance_to_goal)
        {
            for (const auto& man_obstacle : m_managed_obstacles)
            {
                Eigen::Vector2f sample =
                    agentPosition + ((goal - agentPosition).normalized() * current_distance);
                Eigen::Vector2f sample_left = sample + (orthogonal_normalized * 250);
                Eigen::Vector2f sample_right = sample - (orthogonal_normalized * 250);
 
                obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
                Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
 
                if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin,
                                                             obstacle.axisLengthX,
                                                             obstacle.axisLengthY,
                                                             obstacle.yaw,
                                                             sample))
                {
                    return current_distance - safetyRadius;
                }
 
                if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin,
                                                             obstacle.axisLengthX,
                                                             obstacle.axisLengthY,
                                                             obstacle.yaw,
                                                             sample_left))
                {
                    return current_distance - safetyRadius;
                }
 
                if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin,
                                                             obstacle.axisLengthX,
                                                             obstacle.axisLengthY,
                                                             obstacle.yaw,
                                                             sample_right))
                {
                    return current_distance - safetyRadius;
                }
            }
 
            current_distance += sample_step;
        }
 
        return std::numeric_limits<float>::infinity();
    }
 
    void
    DynamicObstacleManager::directly_update_obstacle(const std::string& name,
                                                     const Eigen::Vector2f& obs_pos,
                                                     float e_rx,
                                                     float e_ry,
                                                     float e_yaw,
                                                     const Ice::Current&)
    {
        obstacledetection::Obstacle new_unmanaged_obstacle;
        new_unmanaged_obstacle.name = name;
        new_unmanaged_obstacle.posX = obs_pos(0);
        new_unmanaged_obstacle.posY = obs_pos(1);
        new_unmanaged_obstacle.refPosX = obs_pos(0);
        new_unmanaged_obstacle.refPosY = obs_pos(1);
        new_unmanaged_obstacle.yaw = e_yaw;
        new_unmanaged_obstacle.axisLengthX =
            std::clamp(e_rx,
                       static_cast<float>(m_min_size_of_obstacles),
                       static_cast<float>(m_max_size_of_obstacles));
        new_unmanaged_obstacle.axisLengthY =
            std::clamp(e_ry,
                       static_cast<float>(m_min_size_of_obstacles),
                       static_cast<float>(m_max_size_of_obstacles));
        new_unmanaged_obstacle.safetyMarginX = m_security_margin_for_obstacles;
        new_unmanaged_obstacle.safetyMarginY = m_security_margin_for_obstacles;
        m_obstacle_detection->updateObstacle(new_unmanaged_obstacle);
        m_obstacle_detection->updateEnvironment();
    }
 
    void
    DynamicObstacleManager::update_decayable_obstacles()
    {
        ARMARX_DEBUG << "update obstacles";
        IceUtil::Time started = IceUtil::Time::now();
        std::vector<std::string> remove_obstacles;
 
        // Some debug definitions
        viz::Layer obstacleLayer = arviz.layer(m_obstacle_manager_layer_name);
 
        // Update positions
        {
            ARMARX_DEBUG << "update obstacle position";
            std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
            if (!m_managed_obstacles.size())
            {
                return;
            }
 
            for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
            {
                std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
                managed_obstacle->update_position(m_thickness_of_lines);
            }
        }
 
        // Update obstacle avoidance
        int checked_obstacles = 0;
        bool updated = false;
        int published_obstacles = 0;
        int updated_obstacles = 0;
        {
            ARMARX_DEBUG << "update obstacle values and publish";
            std::lock_guard l(m_managed_obstacles_mutex);
            std::sort(m_managed_obstacles.begin(),
                      m_managed_obstacles.end(),
                      ManagedObstacle::ComparatorDESCPrt);
 
            checked_obstacles = m_managed_obstacles.size();
 
            for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
            {
                std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
                if (!managed_obstacle->m_updated)
                {
                    managed_obstacle->m_value =
                        std::max(-1.0f, managed_obstacle->m_value - m_decay_factor);
                }
                else
                {
                    managed_obstacle->m_value = std::min(
                        1.0f * m_decay_after_ms, managed_obstacle->m_value + m_access_bonus);
                }
 
                if (managed_obstacle->m_published)
                {
                    if (managed_obstacle->m_value < m_min_value_for_accepting)
                    {
                        updated_obstacles++;
                        managed_obstacle->m_published = false;
                        if (!m_only_visualize)
                        {
                            m_obstacle_detection->removeObstacle(managed_obstacle->m_obstacle.name);
                            updated = true;
                        }
                    }
                    else
                    {
                        published_obstacles++;
                        if (managed_obstacle->m_updated)
                        {
                            updated_obstacles++;
                            if (!m_only_visualize)
                            {
                                m_obstacle_detection->updateObstacle(managed_obstacle->m_obstacle);
                                updated = true;
                            }
                            //visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 1, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
                        }
                        else
                        {
                            //visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 0, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
                        }
                    }
                }
                else // if (!managed_obstacle.published)
                {
                    if (managed_obstacle->m_value >= m_min_value_for_accepting)
                    {
                        published_obstacles++;
                        updated_obstacles++;
                        managed_obstacle->m_published = true;
                        if (!m_only_visualize)
                        {
                            m_obstacle_detection->updateObstacle(managed_obstacle->m_obstacle);
                            updated = true;
                        }
                        //visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 0, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
                    }
                    else if (managed_obstacle->m_value < 0 &&
                             m_remove_enabled) // Completely forget the obstacle
                    {
                        remove_obstacles.push_back(managed_obstacle->m_obstacle.name);
                    }
                    else if (managed_obstacle->m_updated)
                    {
                        visualize_obstacle(
                            obstacleLayer,
                            managed_obstacle,
                            armarx::DrawColor{
                                1,
                                1,
                                1,
                                std::min(0.9f,
                                         managed_obstacle->m_value / m_min_value_for_accepting)},
                            40,
                            true);
                    }
                    else
                    {
                        visualize_obstacle(
                            obstacleLayer,
                            managed_obstacle,
                            armarx::DrawColor{
                                1,
                                1,
                                1,
                                std::min(0.9f,
                                         managed_obstacle->m_value / m_min_value_for_accepting)},
                            40,
                            true);
                    }
                }
                managed_obstacle->m_updated = false;
            }
        }
 
        if (updated)
        {
            ARMARX_DEBUG << "update environment";
            m_obstacle_detection->updateEnvironment();
        }
        arviz.commit({obstacleLayer});
 
        if (!remove_obstacles.empty())
        {
            std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
            for (const auto& name : remove_obstacles)
            {
                // TODO schöner machen ohne loop. Iteratoren in main loop
                m_managed_obstacles.erase(std::remove_if(m_managed_obstacles.begin(),
                                                         m_managed_obstacles.end(),
                                                         [&](const ManagedObstaclePtr& oc)
                                                         { return oc->m_obstacle.name == name; }),
                                          m_managed_obstacles.end());
            }
        }
 
        ARMARX_DEBUG << "Finished updating the " << checked_obstacles
                     << " managed obstacles (removed: " << remove_obstacles.size()
                     << ", updated: " << updated_obstacles << "). " << published_obstacles
                     << " of them should be published. The whole operation took "
                     << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
    }
 
    void
    DynamicObstacleManager::visualize_obstacle(viz::Layer& layer,
                                               const ManagedObstaclePtr& o,
                                               const armarx::DrawColor& color,
                                               double pos_z,
                                               bool text)
    {
        // Visualize ellipses.m_obstacle_manager_layer_name
        Eigen::Vector3f dim(o->m_obstacle.axisLengthX * 2,
                            o->m_obstacle.axisLengthY * 2,
                            o->m_obstacle.axisLengthZ * 2);
 
        const std::string name = o->m_obstacle.name;
 
        layer.add(viz::Box(name)
                      .position(Eigen::Vector3f(o->m_obstacle.posX, o->m_obstacle.posY, pos_z))
                      .orientation(Eigen::AngleAxisf(o->m_obstacle.yaw, Eigen::Vector3f::UnitZ())
                                       .toRotationMatrix())
                      .size(dim)
                      .color(color.r, color.g, color.b, color.a));
    }
 
    armarx::PropertyDefinitionsPtr
    DynamicObstacleManager::createPropertyDefinitions()
    {
        PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
 
        defs->component(m_obstacle_detection,
                        "PlatformObstacleAvoidance",
                        "ObstacleAvoidanceName",
                        "The name of the used obstacle avoidance interface");
 
        defs->optional(m_min_coverage_of_obstacles,
                       "MinSampleRatioPerEllipsis",
                       "Minimum percentage of samples which have to be in an elllipsis to be "
                       "considered as known obsacle");
        defs->optional(m_min_coverage_of_line_samples_in_obstacle,
                       "MinSampleRatioPerLineSegment",
                       "Minimum percentage of samples which have to be in an elllipsis to be "
                       "considered as known obsacle");
        defs->optional(
            m_min_size_of_obstacles, "MinObstacleSize", "The minimal obstacle size in mm.");
        defs->optional(
            m_max_size_of_obstacles, "MaxObstacleSize", "The maximal obstacle size in mm.");
        defs->optional(m_min_length_of_lines, "MinLengthOfLines", "Minimum length of lines in mm");
        defs->optional(m_max_length_of_lines, "MaxLengthOfLines", "Maximum length of lines in mm");
        defs->optional(m_decay_after_ms, "MaxObstacleValue", "Maximum value for the obstacles");
        defs->optional(m_min_value_for_accepting,
                       "MinObstacleValueForAccepting",
                       "Minimum value for the obstacles to get accepted");
        defs->optional(
            m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
        defs->optional(m_thickness_of_lines, "LineThickness", "The thickness of line obstacles");
        defs->optional(m_security_margin_for_obstacles,
                       "ObstacleSecurityMargin",
                       "Security margin of ellipsis obstacles");
        defs->optional(
            m_security_margin_for_lines, "LineSecurityMargin", "Security margin of line obstacles");
        defs->optional(m_remove_enabled, "EnableRemove", "Delete Obstacles when value < 0");
        defs->optional(
            m_only_visualize, "OnlyVisualizeObstacles", "Connection to obstacle avoidance");
        defs->optional(m_allow_spwan_inside,
                       "AllowInRobotSpawning",
                       "Allow obstacles to spawn inside the robot");
 
        return defs;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(DynamicObstacleManager, "DynamicObstacleManager");
} // namespace armarx