PointCloudToArMem.cpp

Go to the documentation of this file.

#include "PointCloudToArMem.h"
 
// PCL
#include <pcl/filters/filter.h>
 
// EIGEN
#include <Eigen/Core>
 
// Simox
#include <SimoxUtility/algorithm/string.h>
 
// Core types
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
// Memory + aron
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
 
#include <VisionX/libraries/armem/vision/camera/core/aron_conversions.h>
#include <VisionX/libraries/armem/vision/pointcloud/core/aron_conversions.h>
 
// VisionX PC stuff
#include <VisionX/libraries/PointCloudTools/tools.h>
 
using namespace visionx;
 
armarx::PropertyDefinitionsPtr
PointCloudToArMem::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr def =
        new visionx::PointCloudProcessorPropertyDefinitions(getConfigIdentifier());
 
    // PC stuff
    def->optional(
        p.numClouds,
        "numClouds",
        "Number of point clouds before exiting. 0 or negative value means 'do not exit'.");
    def->optional(p.removeNaNs,
                  "removeNaNs",
                  "If enabled, invalid points will be removed from the point cloud.");
 
    def->optional(p.pointtype, "pc.PointType", "The Type of the point cloud.");
 
    // Memory stuff
    def->optional(p.targetMemoryName,
                  "mem.memoryName",
                  "The Memory Name of the Memory that should receive the PCs");
    return def;
}
 
void
PointCloudToArMem::onInitPointCloudProcessor()
{
    // Memory stuff
    memoryNameSystem().setComponent(this);
}
 
void
PointCloudToArMem::onConnectPointCloudProcessor()
{
    switch (visionx::armem::pointcloud::String2PointType.at(simox::alg::to_lower(p.pointtype)))
    {
        case visionx::armem::pointcloud::PointType::PointXYZ:
            enableResultPointClouds<pcl::PointXYZ>();
            break;
        case visionx::armem::pointcloud::PointType::PointXYZL:
            enableResultPointClouds<pcl::PointXYZL>();
            break;
        case visionx::armem::pointcloud::PointType::PointXYZRGBA:
            enableResultPointClouds<pcl::PointXYZRGBA>();
            break;
        case visionx::armem::pointcloud::PointType::PointXYZRGBL:
            enableResultPointClouds<pcl::PointXYZRGBL>();
            break;
    }
 
    // Memory stuff
    memoryWriter = memoryNameSystem().getWriter(armarx::armem::MemoryID(p.targetMemoryName, ""));
}
 
void
PointCloudToArMem::onDisconnectPointCloudProcessor()
{
}
 
void
PointCloudToArMem::onExitPointCloudProcessor()
{
}
 
void
PointCloudToArMem::process()
{
    if (not processing.exchange(true))
    {
        switch (visionx::armem::pointcloud::String2PointType.at(simox::alg::to_lower(p.pointtype)))
        {
            case visionx::armem::pointcloud::PointType::PointXYZ:
                getDataAndSendToMemory<pcl::PointXYZ, arondto::PointCloudXYZ>();
                break;
            case visionx::armem::pointcloud::PointType::PointXYZL:
                getDataAndSendToMemory<pcl::PointXYZL, arondto::PointCloudXYZL>();
                break;
            case visionx::armem::pointcloud::PointType::PointXYZRGBA:
                getDataAndSendToMemory<pcl::PointXYZRGBA, arondto::PointCloudXYZRGBA>();
                break;
            case visionx::armem::pointcloud::PointType::PointXYZRGBL:
                getDataAndSendToMemory<pcl::PointXYZRGBL, arondto::PointCloudXYZRGBL>();
                break;
        }
 
        processing = false;
    }
    else
    {
        ARMARX_INFO << "Already processing a point cloud...";
    }
}