LaserScannerObstacleDetection.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    Armar6Skills::ArmarXObjects::LaserObstacleDetection
 * @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
 */
 
 
#include "LaserScannerObstacleDetection.h"
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
 
using namespace armarx;
 
 
const std::string
LaserScannerObstacleDetection::default_name = "LaserScannerObstacleDetection";
 
 
LaserScannerObstacleDetection::LaserScannerObstacleDetection() noexcept :
    m_enabled(true),
    m_only_submit_first_n_results(-1),
    m_max_distance_to_put_together(100),
    m_max_yaw_difference(M_PI / 4),
    m_periodic_task_interval(500),
    m_accept_lines_after(100)
{
 
}
 
void
LaserScannerObstacleDetection::onInitComponent()
{
    m_last_accepted_lines = IceUtil::Time::now();
}
 
 
void
LaserScannerObstacleDetection::onConnectComponent()
{
    m_eval_obstacles = new PeriodicTask<LaserScannerObstacleDetection>(this,
            &LaserScannerObstacleDetection::eval_obstacles, m_periodic_task_interval);
    m_eval_obstacles->start();
}
 
 
void
LaserScannerObstacleDetection::onDisconnectComponent()
{
    m_eval_obstacles->stop();
}
 
 
void
LaserScannerObstacleDetection::onExitComponent()
{
}
 
float
LaserScannerObstacleDetection::get_distance_from_point_to_line_segment(const  Eigen::Vector2f& start, const Eigen::Vector2f& end, const Eigen::Vector2f& p) const
{
    // Return minimum distance between line segment l (start = v, end = w) and point p
    const Eigen::Vector2f& v = start;
    const Eigen::Vector2f& w = end;
    const Eigen::Vector2f vp = p - v;
    const Eigen::Vector2f vw = w - v;
    const float line_length_sq = vw(0) * vw(0) + vw(1) * vw(1);
 
 
    if (line_length_sq == 0.0)
    {
        ARMARX_ERROR << "Got invalid line segment!";
        return 0;
    }
 
    // Consider the line extending the segment, parameterized as v + t (w - v).
    // We find projection of point p onto the line.
    // It falls where t = [(p-v) . (w-v)] / |w-v|^2
    // We clamp t from [0,1] to handle points outside the segment vw.
 
    const float pv_dot_wv = vp(0) * vw(0) + vp(1) * vw(1);
    const float t = std::max(0.0f, std::min(1.0f, pv_dot_wv / line_length_sq));
    const Eigen::Vector2f projection = v + (t * vw);
    const float distance = (projection - p).norm();
    return distance;
}
 
 
float
LaserScannerObstacleDetection::get_angle_between_vectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2) const
{
    // Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
    const Eigen::Vector2f v1_vec_normed = v1.normalized();
    const Eigen::Vector2f v2_vec_normed = v2.normalized();
    const float dot_product_vec = v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
    float yaw = acos(dot_product_vec);
    if (yaw >= M_PI)
    {
        yaw -= M_PI;
    }
    return yaw;
}
 
 
 
void
LaserScannerObstacleDetection::eval_obstacles()
{
    laser_scanner_feature_extraction::LineSegment2DChainSeq lines;
 
    {
        std::unique_lock l{m_lines_buffer_mutex};
        lines = m_lines_buffer;
        m_lines_buffer.clear();
    }
 
    if (lines.empty() || m_only_submit_first_n_results == 0)
    {
        return;
    }
 
    bool update_found = false;
    int lines_before = lines.size();
    IceUtil::Time started = IceUtil::Time::now();
    do
    {
        update_found = false;
        if (lines.size() < 2)
        {
            break;
        }
 
        // search for lines to put together
        unsigned int remove1 = 0;
        unsigned int remove2 = 0;
 
        // TODO(fabian..)
        //        for (unsigned int i = 0; i < lines.size() - 1; ++i)
        //        {
        //            remove1 = i;
        //            const LineSegment2D& line1 = lines[i];
        //            const Eigen::Vector2f origin_start(line1.start.e0, line1.start.e1);
        //            const Eigen::Vector2f origin_end(line1.end.e0, line1.end.e1);
        //            for (unsigned int j = i + 1; j < lines.size(); ++j)
        //            {
        //                remove2 = j;
        //                const LineSegment2D& line2 = lines[j];
        //                Eigen::Vector2f start(line2.start.e0, line2.start.e1);
        //                Eigen::Vector2f end(line2.end.e0, line2.end.e1);
 
        //                // check yaws
        //                float yaw = get_angle_between_vectors((origin_end - origin_start), (end - start));
 
 
        //                // check points and distances
        //                if (yaw < m_max_yaw_difference && (get_distance_from_point_to_line_segment(line1, start) < m_max_distance_to_put_together || get_distance_from_point_to_line_segment(line1, end) < m_max_distance_to_put_together))
        //                {
        //                    // Group those lines. Only take longets one
        //                    float length_os_s = (origin_start - start).norm();
        //                    float length_os_e = (origin_start - end).norm();
        //                    float length_oe_s = (origin_end - start).norm();
        //                    float length_oe_e = (origin_end - end).norm();
 
        //                    Eigen::Vector2f new_line_s;
        //                    Eigen::Vector2f new_line_e;
        //                    if (length_os_s > length_os_e && length_os_s > length_oe_s && length_os_s > length_oe_e)
        //                    {
        //                        // length_os_s is maximum
        //                        new_line_s = origin_start;
        //                        new_line_e = start;
        //                    }
        //                    else if (length_os_e > length_oe_s && length_os_e > length_oe_e)
        //                    {
        //                        // length_os_e is maximum
        //                        new_line_s = origin_start;
        //                        new_line_e = end;
        //                    }
        //                    else if (length_oe_s > length_oe_e)
        //                    {
        //                        // length_oe_s is maximum
        //                        new_line_s = origin_end;
        //                        new_line_e = start;
        //                    }
        //                    else
        //                    {
        //                        // length_oe_s is maximum
        //                        new_line_s = origin_end;
        //                        new_line_e = end;
        //                    }
 
        //                    LineSegment2D new_line;
        //                    new_line.start.e0 = new_line_s(0);
        //                    new_line.start.e1 = new_line_s(1);
        //                    new_line.end.e0 = new_line_e(0);
        //                    new_line.end.e1 = new_line_e(1);
 
        //                    lines.push_back(new_line);
        //                    update_found = true;
        //                    break;
        //                }
 
        //            }
        //            if (update_found)
        //            {
        //                break;
        //            }
        //        }
 
        // remove lines from update
        if (update_found)
        {
            ARMARX_CHECK(remove2 > remove1);
            lines.erase(lines.begin() + remove2);
            lines.erase(lines.begin() + remove1);
        }
    }
    while (update_found);
 
    ARMARX_DEBUG << "Finished updating the " << lines_before << " lines. Now we have " << lines.size() << " lines. The operation took " << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
 
    // Try to explain line segments by the obstacles from the obstacle avoidance environment.
    std::vector<dynamicobstaclemanager::LineSegment> converted_lines;
    for (const auto& chain : lines)
    {
 
        if (chain.size() < 2)
        {
            continue;
        }
 
        for (auto it = chain.begin(); it != chain.end() - 1; it++)
        {
            converted_lines.emplace_back(dynamicobstaclemanager::LineSegment{*it, *(it + 1)});
        }
    }
    m_obstacle_manager->add_decayable_line_segments(converted_lines);
 
 
    // Debug
    if (m_only_submit_first_n_results > 0)
    {
        m_only_submit_first_n_results--;
    }
    ARMARX_DEBUG << "Finished update";
}
 
 
//void
//LaserScannerObstacleDetection::reportExtractedEdges(
//    const std::vector<LineSegment2D>& lines,
//    const Ice::Current&)
//{
//    {
//        std::lock_guard l{m_enabled_mutex};
//        if (! m_enabled or lines.empty())
//        {
//            return;
//        }
//    }
 
//    if ((IceUtil::Time::now() - m_last_accepted_lines).toMilliSeconds() > m_accept_lines_after)
//    {
//        std::unique_lock l{m_lines_buffer_mutex};
//        ARMARX_DEBUG << "Got " << lines.size() << " new extracted edges to buffer.";
//        m_lines_buffer.insert(m_lines_buffer.end(), lines.begin(), lines.end());
//        m_last_accepted_lines = IceUtil::Time::now();
//    }
 
//}
 
void
LaserScannerObstacleDetection::setEnabled(bool enable, const Ice::Current&)
{
    std::lock_guard l{m_enabled_mutex};
    m_enabled = enable;
}
 
 
void
LaserScannerObstacleDetection::enable(const Ice::Current&)
{
    std::lock_guard l{m_enabled_mutex};
    m_enabled = true;
}
 
 
void
LaserScannerObstacleDetection::disable(const Ice::Current&)
{
    std::lock_guard l{m_enabled_mutex};
    m_enabled = false;
}
 
 
std::string
LaserScannerObstacleDetection::getDefaultName() const
{
    return default_name;
}
 
 
PropertyDefinitionsPtr
LaserScannerObstacleDetection::createPropertyDefinitions()
{
    PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
 
    defs->component(m_obstacle_manager, "DynamicObstacleManager", "ObstacleManager", "The name of the obstacle manager proxy");
    defs->topic<LaserScannerFeaturesListener>();
 
    defs->optional(m_only_submit_first_n_results, "SubmitOnlyFirstNResults", "Only send the first laser scanner result. -1 for all.");
    defs->optional(m_max_distance_to_put_together, "MaxDistanceOfLinesToGroup", "Max distance in mm of two lines to put them together");
    defs->optional(m_max_yaw_difference, "MaxAngleDifferenceOfLinesToGroup", "Max angle difference in yaw of two lines to put them together");
    defs->optional(m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
    defs->optional(m_accept_lines_after, "AcceptLinesAfter", "Accept new Lines for buffer every x ms");
    defs->optional(m_enabled, "ActivateOnStartup", "Activate the component on startup.");
 
    return defs;
}
 
void LaserScannerObstacleDetection::reportExtractedLineSegments(const laser_scanner_feature_extraction::LineSegment2DChainSeq& globalLineSegmentChains, const Ice::Current&)
{
    {
        std::lock_guard l{m_enabled_mutex};
        if (! m_enabled or globalLineSegmentChains.empty())
        {
            return;
        }
    }
 
    if ((IceUtil::Time::now() - m_last_accepted_lines).toMilliSeconds() > m_accept_lines_after)
    {
        std::unique_lock l{m_lines_buffer_mutex};
 
 
        ARMARX_DEBUG << "Got " << globalLineSegmentChains.size() << " new extracted edges to buffer.";
 
        m_lines_buffer.insert(m_lines_buffer.end(), globalLineSegmentChains.begin(), globalLineSegmentChains.end());
        m_last_accepted_lines = IceUtil::Time::now();
    }
}