LaserScannerObstacleDetection.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    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
 */
 
 
#pragma once
 
 
// STD/STL
#include <mutex>
#include <string>
 
// Eigen
#include <Eigen/Core>
 
// Ice
#include <Ice/Current.h>
 
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
// Interface
#include <RobotComponents/interface/components/ObstacleDetection/LaserScannerObstacleDetectionInterface.h>
 
// ObstacleAvoidance
#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
 
namespace armarx
{
 
    class LaserScannerObstacleDetection :
        virtual public Component,
        virtual public LaserScannerObstacleDetectionInterface
    {
 
    public:
        static const std::string default_name;
 
    public:
        LaserScannerObstacleDetection() noexcept;
 
        std::string getDefaultName() const override;
        void setEnabled(bool enable, const Ice::Current& = Ice::emptyCurrent) override;
        void enable(const Ice::Current& = Ice::emptyCurrent) override;
        void disable(const Ice::Current& = Ice::emptyCurrent) override;
 
        //        void reportCorrectedPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current& = Ice::Current()) override { /* nop */ };
        //        void reportPoseUncertainty(Ice::Float, Ice::Float, Ice::Float, const Ice::Current& = Ice::Current()) override { /* nop */ };
        //        void reportLaserScanPoints(const Vector2fSeq&, const Ice::Current& = Ice::Current()) override { /* nop */ };
        //        void reportExtractedEdges(const LineSegment2DSeq&, const Ice::Current& = Ice::Current()) override;
 
        void reportExtractedLineSegments(
            const laser_scanner_feature_extraction::LineSegment2DChainSeq& globalLineSegmentChains,
            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 eval_obstacles();
        float get_distance_from_point_to_line_segment(const Eigen::Vector2f& start,
                                                      const Eigen::Vector2f& end,
                                                      const Eigen::Vector2f& p) const;
        float get_angle_between_vectors(const Eigen::Vector2f&, const Eigen::Vector2f&) const;
 
        bool m_enabled;
        int m_only_submit_first_n_results;
        float m_max_distance_to_put_together;
        float m_max_yaw_difference;
 
        unsigned int m_periodic_task_interval;
        unsigned int m_accept_lines_after;
 
        PeriodicTask<LaserScannerObstacleDetection>::pointer_type m_eval_obstacles;
        laser_scanner_feature_extraction::LineSegment2DChainSeq m_lines_buffer;
        IceUtil::Time m_last_accepted_lines;
 
        std::mutex m_lines_buffer_mutex;
        std::mutex m_enabled_mutex;
 
        DynamicObstacleManagerInterface::ProxyType m_obstacle_manager;
    };
 
} // namespace armarx