PointCloudToArMem.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    VisionX::ArmarXObjects::PointCloudRecorder
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
// pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
 
 
// base classes
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
 
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/components/pointcloud_processor/PointCloudToArMem/PointCloudToArMemInterface.h>
 
// aron
#include <VisionX/libraries/armem/vision/pointcloud/core/aron/PointCloudXYZ.aron.generated.h>
#include <VisionX/libraries/armem/vision/pointcloud/core/aron/PointCloudXYZL.aron.generated.h>
#include <VisionX/libraries/armem/vision/pointcloud/core/aron/PointCloudXYZRGBA.aron.generated.h>
#include <VisionX/libraries/armem/vision/pointcloud/core/aron/PointCloudXYZRGBL.aron.generated.h>
 
// memory
#include <RobotAPI/libraries/armem/client/Writer.h>
 
// VisionX PC stuff
#include <VisionX/libraries/PointCloudTools/tools.h>
 
// pc
#include <VisionX/libraries/armem/vision/pointcloud/client/constants.h>
#include <VisionX/libraries/armem/vision/pointcloud/core/constants.h>
 
namespace visionx
{
 
    class PointCloudToArMem :
        public visionx::PointCloudToArMemInterface,
        public visionx::PointCloudProcessor,
        public armarx::armem::client::PluginUser
    {
 
    public:
        std::string
        getDefaultName() const override
        {
            return "PointCloudToArMem";
        }
 
 
    protected:
        void onInitPointCloudProcessor() override;
        void onConnectPointCloudProcessor() override;
        void onDisconnectPointCloudProcessor() override;
        void onExitPointCloudProcessor() override;
 
        void process() override;
 
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
    private:
        template <class PointT, class AronT>
        void
        getDataAndSendToMemory()
        {
            const armarx::Duration timeout = armarx::Duration::MilliSeconds(1000);
            armarx::Duration timeGetPointClouds;
 
            typename pcl::PointCloud<PointT>::Ptr inputCloud(new pcl::PointCloud<PointT>());
            if (waitForPointClouds(static_cast<int>(timeout.toMilliSeconds())))
            {
                TIMING_START(GetPointClouds);
                getPointClouds(inputCloud);
                TIMING_END_STREAM(GetPointClouds, ARMARX_VERBOSE);
                timeGetPointClouds =
                    armarx::Duration::MicroSeconds(GetPointClouds.toMicroSeconds());
 
                ARMARX_CHECK(inputCloud);
                currentCloud++;
 
                if (p.numClouds <= 0 || currentCloud <= p.numClouds)
                {
                    if (p.removeNaNs) // remove all invalid points
                    {
                        visionx::filterNaNs(*inputCloud);
                    }
 
                    // Also provide PC in legacy mode
                    provideResultPointClouds(inputCloud);
 
                    // Send PC to armem
                    {
                        AronT arondto;
                        arondto.pointcloud = *inputCloud;
 
                        armarx::armem::Commit c;
                        armarx::armem::EntityUpdate& eu = c.add();
                        eu.confidence = 1.0;
 
                        eu.entityID = armarx::armem::MemoryID(
                            p.targetMemoryName,
                            visionx::armem::pointcloud::GetCoreSegmentNameFor<PointT>(),
                            getDefaultName(),
                            visionx::armem::pointcloud::ENTITY_NAME);
 
                        eu.instancesData = {arondto.toAron()};
 
                        eu.referencedTime =
                            armarx::core::time::DateTime::Now(); // Get creation time of PC
                        eu.sentTime = armarx::core::time::DateTime::Now();
                        auto result = memoryWriter.commit(c);
 
                        if (not result.allSuccess())
                        {
                            ARMARX_ERROR
                                << "An error occured when sending pointclouds to the memory. "
                                   "Try to continue with next PC. The message was: "
                                << result.allErrorMessages();
                        }
                    }
 
                    ARMARX_DEBUG << "Wait for next point cloud...";
                }
            }
            else
            {
                ARMARX_INFO << "Timeout or error while waiting for point cloud data";
                return;
            }
        }
 
    private:
        int currentCloud;
 
        armarx::armem::client::Writer memoryWriter;
 
        struct Properties
        {
            int numClouds = 0;
            bool removeNaNs = true;
 
            std::string pointtype = visionx::armem::pointcloud::POINT_XYZRGBA_SLUG;
 
            std::string targetMemoryName = "Vision";
 
        } p;
 
        std::atomic_bool processing = false;
    };
} // namespace visionx