RCImageProvider.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::RCImageProvider
 * @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 "RCImageProvider.h"
 
 
using namespace armarx;
 
 
#include <stdlib.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <VisionX/libraries/RoboceptionUtility/roboception_constants.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <rc_genicam_api/buffer.h>
#include <rc_genicam_api/config.h>
#include <rc_genicam_api/image.h>
#include <rc_genicam_api/interface.h>
#include <rc_genicam_api/pixel_formats.h>
#include <rc_genicam_api/system.h>
 
 
using namespace armarx;
 
RCImageProviderPropertyDefinitions::RCImageProviderPropertyDefinitions(std::string prefix) :
    armarx::ComponentPropertyDefinitions(prefix)
{
    defineOptionalProperty<std::string>("DeviceId", "00_14_2d_2c_6e_ed", "");
    defineOptionalProperty<float>("ScaleFactor", 1.0, "Image down scale factor");
    defineOptionalProperty<float>("FrameRate", 25.0f, "Frames per second")
        .setMatchRegex("\\d+(.\\d*)?")
        .setMin(0.0)
        .setMax(25.0);
    defineOptionalProperty<bool>("AutoExposure", false, "Enables auto exposure");
    defineOptionalProperty<float>(
        "ExposureTimeMs", 12.0f, "Exposure time in ms if auto exposure is disabled");
    defineOptionalProperty<float>("GainDb", 0.0f, "Gain in db");
 
    defineOptionalProperty<bool>("EnableColor", true, "Enables colored images");
 
    defineOptionalProperty<bool>("EnableAutoWhiteBalance", false, "Enable auto white balance");
    defineOptionalProperty<float>("WhiteBalanceRatioRed", 1.2f, "Red to green balance ratio.")
        .setMin(0.125)
        .setMax(8.0);
    defineOptionalProperty<float>("WhiteBalanceRatioBlue", 2.4f, "Blue to green balance ratio.")
        .setMin(0.125)
        .setMax(8.0);
 
    defineOptionalProperty<std::string>(
        "ReferenceFrameName", "Roboception_LeftCamera", "Optional reference frame name.");
 
    defineOptionalProperty<std::string>(
        "RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
}
 
armarx::PropertyDefinitionsPtr
RCImageProvider::createPropertyDefinitions()
{
    armarx::PropertyDefinitionsPtr defs(
        new RCImageProviderPropertyDefinitions(getConfigIdentifier()));
 
    // This option is disabled, as the depth image cannot be set to Full quality (like RGB image).
    // The RGB image could be scaled down to the depth image. If this is what you want, feel free to implement it.
    defs->optional(enableDepth,
                   "EnableDepth",
                   "Enables depth image, i.e. provide RGB (left) + Depth.\n"
                   "Note that the depth image is scaled up by 2 (nearest neighbour) to match the "
                   "resolution of the color image.");
 
    return defs;
}
 
RCImageProvider::RCImageProvider() : intensityBuffer(50), disparityBuffer(25)
{
}
 
void
RCImageProvider::updateCameraSettings()
{
    using namespace visionx::roboception;
 
    bool autoExposure = getProperty<bool>("AutoExposure");
    float exposureTimeMs = getProperty<float>("ExposureTimeMs");
    float gainDb = getProperty<float>("GainDb");
 
    bool enableColor = getProperty<bool>("EnableColor").getValue();
 
    std::shared_ptr<GenApi::CNodeMapRef> nodemap = dev->getRemoteNodeMap();
    rcg::setString(nodemap,
                   GC_COMPONENT_SELECTOR,
                   "IntensityCombined",
                   true); // selectz for the following actions
 
    ARMARX_INFO << "updating camera settings...";
 
    if (enableColor)
    {
        if (rcg::setEnum(nodemap, "PixelFormat", "YCbCr411_8", false))
        {
            ARMARX_INFO << "Enabling color mode";
        }
        else
        {
            ARMARX_WARNING << "Falling back to monochrome mode";
            enableColor = false;
        }
    }
 
    if (enableColor)
    {
        if (getProperty<bool>("EnableAutoWhiteBalance").getValue())
        {
            if (rcg::setEnum(nodemap, "BalanceWhiteAuto", "Continuous"))
            {
                ARMARX_INFO << "enabling auto white balance";
            }
            else
            {
                ARMARX_WARNING << "unable to enable auto white balance";
            }
        }
        else
        {
            if (rcg::setEnum(nodemap, "BalanceWhiteAuto", "Off"))
            {
                ARMARX_INFO << "disabling auto white balance";
 
                rcg::setEnum(nodemap, "BalanceRatioSelector", "Red");
                if (!rcg::setFloat(nodemap,
                                   "BalanceRatio",
                                   getProperty<float>("WhiteBalanceRatioRed").getValue()))
                {
                    ARMARX_WARNING << "unable to set white balance ratio for red-green";
                }
 
                rcg::setEnum(nodemap, "BalanceRatioSelector", "Blue");
                if (!rcg::setFloat(nodemap,
                                   "BalanceRatio",
                                   getProperty<float>("WhiteBalanceRatioBlue").getValue()))
                {
                    ARMARX_WARNING << "unable to set white balance ratio for green-blue";
                }
            }
            else
            {
                ARMARX_WARNING << "unable to disable auto white balance";
            }
        }
 
        rcg::setEnum(nodemap, "BalanceRatioSelector", "Red");
        double wbRatioRed = rcg::getFloat(nodemap, "BalanceRatio");
 
        rcg::setEnum(nodemap, "BalanceRatioSelector", "Blue");
        double wbRatioBlue = rcg::getFloat(nodemap, "BalanceRatio");
 
        ARMARX_INFO << "white balance ratio is red: " << wbRatioRed << " blue: " << wbRatioBlue;
    }
    else
    {
        if (rcg::setEnum(nodemap, "PixelFormat", "Mono8", false))
        {
            ARMARX_INFO << "Enabling monochrome mode";
        }
        else
        {
            ARMARX_ERROR << "Could not set monochrome mode";
        }
    }
 
    if (enableDepth)
    {
        try
        {
            // rcg::setEnum(nodemap, GC_DEPTH_QUALITY, GC_DEPTH_QUALITY_FULL, true);
            rcg::setEnum(nodemap, GC_DEPTH_QUALITY, GC_DEPTH_QUALITY_HIGH, true);
        }
        catch (std::invalid_argument& e)
        {
            ARMARX_INFO << e.what();
            ARMARX_WARNING << "Depth quality '" << GC_DEPTH_QUALITY_FULL
                           << "' is currently not available for this device. "
                           << "Falling back to '" << GC_DEPTH_QUALITY_HIGH << "' (640x480@3FPS).";
            rcg::setEnum(nodemap, GC_DEPTH_QUALITY, GC_DEPTH_QUALITY_HIGH, true);
        }
    }
 
    std::string oldAutoExposure = rcg::getEnum(nodemap, GC_EXPOSURE_AUTO, false);
    ARMARX_INFO << "Setting auto exposure from " << oldAutoExposure << " to " << autoExposure;
    if (!rcg::setEnum(nodemap, GC_EXPOSURE_AUTO, autoExposure ? "Continuous" : "Off", false))
    {
        ARMARX_WARNING << "Could not set auto exposure to: " << autoExposure;
    }
    double oldExposureTime = rcg::getFloat(nodemap, GC_EXPOSURE_TIME) / 1000.0;
    ARMARX_INFO << "Setting exposure time from " << oldExposureTime << " to " << exposureTimeMs;
    if (!rcg::setFloat(nodemap, GC_EXPOSURE_TIME, exposureTimeMs * 1000.0, false))
    {
        ARMARX_WARNING << "Could not set exposure time to: " << exposureTimeMs;
    }
 
    double oldGain = rcg::getFloat(nodemap, GC_GAIN);
    ARMARX_INFO << "Setting gain from " << oldGain << " to " << gainDb;
    if (!rcg::setFloat(nodemap, GC_GAIN, gainDb, false))
    {
        ARMARX_WARNING << "Could not set gain to: " << gainDb;
    }
 
    ARMARX_INFO << "updating camera settings...done!";
}
 
void
armarx::RCImageProvider::onInitCapturingImageProvider()
{
    //    offeringTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
 
    if (!initDevice(getProperty<std::string>("DeviceId")))
    {
        getArmarXManager()->asyncShutdown();
        return;
    }
 
    if (enableDepth)
    {
        enableIntensity(true);
        enableIntensityCombined(false);
        enableDisparity(true);
        enableConfidence(false);
        enableError(false);
    }
    else
    {
        enableIntensity(false);
        enableIntensityCombined(true);
        enableDisparity(false);
        enableConfidence(false);
        enableError(false);
    }
 
    rcg::setString(nodemap, visionx::roboception::GC_COMPONENT_SELECTOR, "IntensityCombined", true);
 
 
    updateCameraSettings();
 
    setupStreamAndCalibration(getProperty<float>("ScaleFactor"),
                              "/tmp/armar6_rc_color_calibration.txt");
 
    frameRate = getProperty<float>("FrameRate");
    rcg::setFloat(nodemap, "AcquisitionFrameRate", double(frameRate));
    ARMARX_INFO << VAROUT(frameRate);
 
    scan3dCoordinateScale = rcg::getFloat(nodemap, "Scan3dCoordinateScale", nullptr, nullptr, true);
 
    setNumberImages(int(numImages));
    setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
    setImageSyncMode(visionx::eCaptureSynchronization);
}
 
void
RCImageProvider::onStartCapturingImageProvider()
{
    //    CapturingImageProvider::onConnectImageProvider();
    // robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
}
 
void
armarx::RCImageProvider::onExitCapturingImageProvider()
{
    cleanupRC();
}
 
void
armarx::RCImageProvider::onStartCapture(float framesPerSecond)
{
    (void)framesPerSecond;
    startRC();
}
 
void
armarx::RCImageProvider::onStopCapture()
{
    stopRC();
}
 
bool
RCImageProvider::capture(void** ppImageBuffers)
{
    visionx::ImageFormatInfo imageFormat = getImageFormat();
 
    IceUtil::Time timeoutTime =
        IceUtil::Time::now(IceUtil::Time::Monotonic) + IceUtil::Time::secondsDouble(2);
    while (IceUtil::Time::now(IceUtil::Time::Monotonic) < timeoutTime)
    {
        const rcg::Buffer* buffer = stream->grab(
            (timeoutTime - IceUtil::Time::now(IceUtil::Time::Monotonic)).toMilliSeconds());
        if (!buffer)
        {
            ARMARX_WARNING << deactivateSpam(10)
                           << "buffer is NULL - Unable to get image - restarting streaming";
            try
            {
                stream->stopStreaming();
            }
            catch (std::exception const& ex)
            {
                ARMARX_WARNING << "stopStreaming failed: " << ex.what();
            }
            try
            {
                stream->close();
            }
            catch (std::exception const& ex)
            {
                ARMARX_WARNING << "close failed: " << ex.what();
            }
            ARMARX_WARNING << deactivateSpam(10) << "streaming stopped";
            stream = nullptr;
            setupStreamAndCalibration(getProperty<float>("ScaleFactor"),
                                      "/tmp/armar6_rc_color_calibration.txt");
 
            stream->open();
            stream->startStreaming();
            ARMARX_WARNING << deactivateSpam(10) << "streaming started again";
            return false;
        }
 
        if (buffer->getIsIncomplete())
        {
            continue; // try again
        }
        if (buffer->getNumberOfParts() > 1)
        {
            ARMARX_WARNING << deactivateSpam(10) << "Buffer contains more than one part!";
            return false;
        }
 
        const int part = 0;
        if (!buffer->getImagePresent(part))
        {
            ARMARX_WARNING << deactivateSpam(10) << "Buffer does not contain an image!";
            return false;
        }
 
        size_t px = buffer->getXPadding(part);
        uint64_t format = buffer->getPixelFormat(part);
 
        int width = imageFormat.dimension.width;
        int height = imageFormat.dimension.height;
        if (scaleFactor > 1.0f)
        {
            width *= scaleFactor;
            height *= scaleFactor;
        }
 
        const int imageSize =
            imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
 
        // ARMARX_LOG << "grabbing image " << imageFormat.dimension.width << "x" << imageFormat.dimension.height << " vs " <<  buffer->getWidth() << "x" << buffer->getHeight();
 
 
        if (!enableDepth)
        {
            const uint8_t* inputPixels = static_cast<const uint8_t*>(buffer->getBase(0));
 
            for (size_t n = 0; n < numImages; n++)
            {
                fillCameraImageRGB(cameraImages[n]->pixels,
                                   n,
                                   inputPixels,
                                   size_t(width),
                                   size_t(height),
                                   format,
                                   px);
            }
        }
        else
        {
            switch (format)
            {
                case PfncFormat::Mono8:
                case PfncFormat::Confidence8:
                case Error8:
                case PfncFormat::YCbCr411_8:
                    intensityBuffer.add(buffer, part);
                    break;
                case Coord3D_C16:
                    disparityBuffer.add(buffer, part);
                    break;
                default:
                    ARMARX_INFO << "Unexpected pixel format: " << int(format);
                    break;
            }
 
            uint64_t timestamp = buffer->getTimestampNS();
 
            std::shared_ptr<const rcg::Image> intensity = intensityBuffer.find(timestamp);
            std::shared_ptr<const rcg::Image> disparity = disparityBuffer.find(timestamp);
 
            if (!intensity || !disparity)
            {
                // Waiting for second synchronized image
                return false;
            }
            updateTimestamp(Ice::Long(buffer->getTimestamp()));
            ARMARX_DEBUG << "Received all synchronized images at timestamp "
                         << buffer->getTimestamp() << ".";
 
 
            // Build images
 
            size_t upscale = 1;
            if (intensity->getWidth() / disparity->getWidth() == 2 &&
                intensity->getHeight() / disparity->getHeight() == 2)
            {
                upscale = 2;
                static bool reported = false;
                if (!reported)
                {
                    ARMARX_INFO << "Scaling up depth image by a factor of " << upscale << ".";
                    reported = true;
                }
            }
 
            // Left eye image.
            fillCameraImageRGB(cameraImages[0]->pixels, 0, *intensity);
            // Depth image.
            fillCameraImageDepth(cameraImages[1]->pixels,
                                 focalLengthFactor,
                                 baseline,
                                 scan3dCoordinateScale,
                                 *disparity,
                                 upscale);
        }
 
 
        armarx::SharedMemoryScopedWriteLockPtr lock(sharedMemoryProvider->getScopedWriteLock());
        updateTimestamp(buffer->getTimestampNS() / 1000, false);
        if (scaleFactor > 1.0f)
        {
            ImageProcessor::Resize(cameraImages[0], smallImages[0]);
            ImageProcessor::Resize(cameraImages[1], smallImages[1]);
 
            for (size_t i = 0; i < numImages; i++)
            {
                memcpy(ppImageBuffers[i], smallImages[i]->pixels, imageSize);
            }
        }
        else
        {
 
            for (size_t i = 0; i < numImages; i++)
            {
                memcpy(ppImageBuffers[i], cameraImages[i]->pixels, imageSize);
            }
        }
 
        //        RobotHealthHeartbeatArgs rhha;
        //        rhha.maximumCycleTimeWarningMS = 500; // Robotception is really slow.
        //        rhha.maximumCycleTimeErrorMS = 1000;
        //        robotHealthTopic->heartbeat(getName(), rhha);
 
        return true;
    }
 
    ARMARX_INFO << deactivateSpam() << "failed to capture image";
    return false;
}