VoxelGridMappingProvider.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    RobotComponents::ArmarXObjects::VoxelGridMappingProvider
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/interface/components/VoxelGridProviderInterface.h>
#include <RobotAPI/libraries/core/RobotPool.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/Component.h>
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
 
#include <pcl/octree/octree.h>
 
#include <mutex>
 
namespace armarx
{
    class IceReportSkipper;
}
 
namespace visionx
{
    /**
     * @class VoxelGridMappingProviderPropertyDefinitions
     * @brief
     */
    class VoxelGridMappingProviderPropertyDefinitions:
        public armarx::ComponentPropertyDefinitions
    {
    public:
        VoxelGridMappingProviderPropertyDefinitions(std::string prefix):
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("PointCloudLoadFilepath", "", "If set, the accumulated point cloud is loaded from the file");
            defineOptionalProperty<std::string>("PointCloudStoreFilepath", "", "If set, the accumulated point cloud is stored to this file periodically");
            defineOptionalProperty<float>("PointCloudStorageFrequency", 0.1f, "Frequency at which the collected point cloud is stored.");
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>("providerName", "OpenNIPointCloudProvider", "name of the point cloud provider");
            defineOptionalProperty<std::string>("sourceFrameName", "", "The source frame name. If unspecified the value will be retrieved from the provider.");
            defineOptionalProperty<std::string>("BoundingBox", "0,6000;0,6000;100,2500", "Point cloud points outside of this bouning box will be cropped", armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("GridSize", 50, "Size in x,y,z dimension of each entry of the voxel grid", armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<int>("MinimumPointNumberPerVoxel", 10, "Minimum number of points in a voxel of the grid to be considered a filled voxel.", armarx::PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("UpdateRate", 2.0f, "Frequency how often the voxel grid is updated");
            defineOptionalProperty<bool>("ActivateOnStartUp", true, "If true, the component will start collection point cloud directly after start up. Otherwise use interface functions.");
 
            //defineRequiredProperty<std::string>("PropertyName", "Description");
            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
        }
    };
 
    /**
     * @defgroup Component-VoxelGridMappingProvider VoxelGridMappingProvider
     * @ingroup RobotComponents-Components
     * A description of the component VoxelGridMappingProvider.
     *
     * @class VoxelGridMappingProvider
     * @ingroup Component-VoxelGridMappingProvider
     * @brief Brief description of class VoxelGridMappingProvider.
     *
     * Detailed description of class VoxelGridMappingProvider.
     */
    class VoxelGridMappingProvider :
        virtual public visionx::PointCloudProcessor,
        virtual public VoxelGridProviderInterface
    {
    public:
        using PointType = pcl::PointXYZRGBA;
        using Cloud = pcl::PointCloud<PointType>;
        using CloudPtr = Cloud::Ptr;
 
        VoxelGridMappingProvider();
 
 
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override;
 
    protected:
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
        // PointCloudProcessor interface
    protected:
        void onInitPointCloudProcessor() override;
        void onConnectPointCloudProcessor() override;
        void onExitPointCloudProcessor() override;
        void process() override;
 
 
        std::pair<armarx::Vector3fSeq, std::vector<VirtualRobot::VisualizationFactory::Color>> removeRobotVoxels(const armarx::Vector3fSeq& gridPoints, std::vector<VirtualRobot::VisualizationFactory::Color>const& gridColors, const VirtualRobot::RobotPtr& robot, float distanceThreshold);
        CloudPtr removeRobotVoxels(const CloudPtr& cloud, const VirtualRobot::RobotPtr& robot, float distanceThreshold);
 
        std::string providerName;
 
        std::string pointCloudFormat;
 
        armarx::RobotPoolPtr  robotPool;
 
        armarx::RobotStateComponentInterfacePrx robotStateComponent;
 
        std::string sourceFrameName;
 
        armarx::DebugDrawerInterfacePrx dd;
 
        CloudPtr accumulatedPointCloud, lastPointCloud;
        pcl::octree::OctreePointCloud<PointType>::Ptr octtree;
        float gridLeafSize = 0.0f;
        VirtualRobot::TriMeshModelPtr gridMesh;
        armarx::Vector3fSeq gridPositions;
        mutable std::mutex dataMutex;
        bool collectingPointClouds = false;
 
        Eigen::Vector3f croppingMin, croppingMax;
        std::unique_ptr<armarx::IceReportSkipper> skipper;
        std::unique_ptr<armarx::CycleUtil> cycleKeeper;
 
 
 
        // VoxelGridMappingProviderInterface interface
        void updateBoundaries();
 
    public:
        armarx::Vector3fSeq getFilledGridPositions(const Ice::Current&) const override;
        ::Ice::Float getGridSize(const ::Ice::Current& = Ice::emptyCurrent) const override;
        void startCollectingPointClouds(const Ice::Current&) override;
        void stopCollectingPointClouds(const Ice::Current&) override;
        void reset(const Ice::Current&) override;
 
        // Component interface
    public:
        void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
    };
}