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 <mutex>
 
#include <pcl/octree/octree.h>
 
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/RobotPool.h>
 
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/interface/components/VoxelGridProviderInterface.h>
 
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;
    };
} // namespace visionx