PointCloudProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::Core
 * @author     David Gonzalez Aguirre (david dot gonzalez at kit dot edu)
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudProvider.h"
 
#include <string>
 
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
using namespace armarx;
 
namespace visionx
{
    // ================================================================== //
    // == PointCloudProvider ice interface ============================== //
    // ================================================================== //
    armarx::Blob
    PointCloudProvider::getPointCloud(MetaPointCloudFormatPtr& info, const Ice::Current& c)
    {
        MetaInfoSizeBasePtr infoBase;
        armarx::Blob result = sharedMemoryProvider->getData(infoBase);
        // info is an output parameter and must be set explicitly
        info = MetaPointCloudFormatPtr::dynamicCast(infoBase);
        return result;
    }
 
    MetaPointCloudFormatPtr
    PointCloudProvider::getPointCloudFormat(const Ice::Current& c)
    {
        //    armarx::SharedMemoryScopedWriteLockPtr lock(sharedMemoryProvider->getScopedWriteLock());
 
        return sharedMemoryProvider->getMetaInfo();
    }
 
    // ================================================================== //
    // == Component implementation ====================================== //
    // ================================================================== //
    void
    PointCloudProvider::onInitComponent()
    {
        ARMARX_TRACE;
        // init members
        exiting = false;
 
        // call setup of derived poitcloud provider implementation to setup Point Clouds type and size
        onInitPointCloudProvider();
 
        MetaPointCloudFormatPtr info = getDefaultPointCloudFormat();
        intermediateBuffer.resize(info->capacity);
        sharedMemoryProvider = new IceSharedMemoryProvider<unsigned char, MetaPointCloudFormat>(
            this, info, "PointCloudProvider");
 
        // Offer topic for point cloud events
        offeringTopic(getName() + ".PointCloudListener");
    }
 
    void
    PointCloudProvider::onConnectComponent()
    {
        ARMARX_TRACE;
        pointCloudProcessorProxy =
            getTopic<PointCloudProcessorInterfacePrx>(getName() + ".PointCloudListener");
 
        if (!sharedMemoryProvider)
        {
            // Ensure that the shared memory provider is created
            ARMARX_INFO << "Shared memory provider is null: Recreating it";
            MetaPointCloudFormatPtr info = getDefaultPointCloudFormat();
            sharedMemoryProvider = new IceSharedMemoryProvider<unsigned char, MetaPointCloudFormat>(
                this, info, "PointCloudProvider");
        }
        sharedMemoryProvider->start();
 
        onConnectPointCloudProvider();
        fps.reset();
    }
 
    void
    PointCloudProvider::onDisconnectComponent()
    {
        ARMARX_TRACE;
        if (sharedMemoryProvider)
        {
            sharedMemoryProvider->stop();
            sharedMemoryProvider = nullptr;
        }
    }
 
    void
    PointCloudProvider::onExitComponent()
    {
        ARMARX_TRACE;
        if (sharedMemoryProvider)
        {
            sharedMemoryProvider->stop();
            sharedMemoryProvider = nullptr;
        }
        exiting = true;
        onExitPointCloudProvider();
    }
 
    void
    PointCloudProvider::updateComponentMetaInfo(const MetaPointCloudFormatPtr& info)
    {
        fps.update();
        setMetaInfo(
            "PointCloudTimestamp",
            new armarx::Variant(IceUtil::Time::microSeconds(info->timeProvided).toDateTime()));
        setMetaInfo("FPS", new armarx::Variant(fps.getFPS()));
        setMetaInfo("MaxCycleTimeMS", new armarx::Variant(fps.getMaxCycleTimeMS()));
        setMetaInfo("MeanCycleTimeMS", new armarx::Variant(fps.getMeanCycleTimeMS()));
        setMetaInfo("MinCycleTimeMS", new armarx::Variant(fps.getMinCycleTimeMS()));
        setMetaInfo("PointCloudSize", new armarx::Variant(info->size));
    }
 
    MetaPointCloudFormatPtr
    PointCloudProvider::getDefaultPointCloudFormat()
    {
        ARMARX_TRACE;
        MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
        //info->frameId = getProperty<std::string>("frameId").getValue();
        info->type = PointContentType::eColoredPoints;
        info->capacity = 640 * 480 * sizeof(ColoredPoint3D); // + info->frameId.size();
        info->size = info->capacity;
        return info;
    }
} // namespace visionx