PointCloudRecorder.cpp

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
 */
 
#include "PointCloudRecorder.h"
 
 
#include <pcl/filters/filter.h>
#include <Eigen/Core>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/libraries/core/Pose.h>
 
using namespace armarx;
 
 
 
armarx::PropertyDefinitionsPtr PointCloudRecorder::createPropertyDefinitions()
{
    ARMARX_TRACE;
    return armarx::PropertyDefinitionsPtr(new PointCloudRecorderPropertyDefinitions(
            getConfigIdentifier()));
}
 
 
void armarx::PointCloudRecorder::onInitPointCloudProcessor()
{
    ARMARX_TRACE;
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    cycleKeeper.reset(new CycleUtil(1000.0f / getProperty<float>("UpdateRate").getValue()));
 
    numClouds = getProperty<int>("NumPointClouds").getValue();
    currentCloud = 0;
 
    writeBinary = getProperty<bool>("WriteBinary").getValue();
    removeNaNs = getProperty<bool>("RemoveNaNs").getValue();
}
 
void armarx::PointCloudRecorder::onConnectPointCloudProcessor()
{
    ARMARX_TRACE;
    debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
    debugDrawer = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
 
    enableResultPointClouds<PointT>();
}
 
 
void armarx::PointCloudRecorder::onDisconnectPointCloudProcessor()
{
}
 
void armarx::PointCloudRecorder::onExitPointCloudProcessor()
{
}
 
void armarx::PointCloudRecorder::process()
{
    ARMARX_TRACE;
    pcl::PointCloud<PointT>::Ptr inputCloud(new pcl::PointCloud<PointT>());
    ARMARX_TRACE;
    if (!waitForPointClouds())
    {
        ARMARX_TRACE;
        ARMARX_INFO << "Timeout or error while waiting for point cloud data";
        return;
    }
    else
    {
        ARMARX_TRACE;
        getPointClouds(inputCloud);
    }
    // StringVariantBaseMap debugValues;
    // debugValues["debug_value"] = new Variant(inputCloud->header.timestamp);
    // debugObserver->setDebugChannel(getName(), debugValues);
 
    currentCloud++;
 
    ARMARX_TRACE;
    std::string fileName = getProperty<std::string>("Folder").getValue() + "/cloud_" + std::to_string(inputCloud->header.stamp) + ".pcd";
 
    const auto isNan = [](const PointT& p)->bool{
      return std::isnan(p.z);
    };
 
    if(removeNaNs) // remove all invalid points
    {
        inputCloud->points.erase(std::remove_if(inputCloud->points.begin(), inputCloud->points.end(), isNan), inputCloud->points.end());
 
        inputCloud->width = inputCloud->points.size();
        inputCloud->height = 1;
        inputCloud->is_dense = true;
    }
   
    if(writeBinary)
    {
        pcl::io::savePCDFileBinary(fileName, *inputCloud);
    }
    else
    {
        pcl::io::savePCDFileASCII(fileName, *inputCloud);
    }
 
    ARMARX_LOG << "writing to file: " << fileName;
 
    provideResultPointClouds(inputCloud);
 
 
    if (numClouds && currentCloud >= numClouds)
    {
        ARMARX_TRACE;
        this->getArmarXManager()->asyncShutdown();
        PointCloudProcessor::onDisconnectComponent();
    }
 
 
    cycleKeeper->waitForCycleDuration();
}