SimpleEpisodicMemoryImageConnector.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::SimpleEpisodicMemoryImageConnector
 * @author     Fabian Peller ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SimpleEpisodicMemoryImageConnector.h"
 
 
// STD/STL
 
// IVT
#include <Image/ImageProcessor.h>
 
// ArmarX
 
using namespace visionx;
 
armarx::PropertyDefinitionsPtr
visionx::SimpleEpisodicMemoryImageConnector::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new SimpleEpisodicMemoryImageConnectorPropertyDefinitions(getConfigIdentifier()));
}
 
void
visionx::SimpleEpisodicMemoryImageConnector::onInitImageProcessor()
{
    ARMARX_DEBUG << "Initializing SimpleEpisodicMemoryImageConnector";
 
    m_image_provider_id = getProperty<std::string>("ImageProviderName").getValue();
    m_image_provider_channel = getProperty<unsigned int>("ImageProviderChannel");
    ARMARX_VERBOSE << "Using image provider with ID '" << m_image_provider_id << "'.";
 
    usingImageProvider(m_image_provider_id);
    usingProxy("SimpleEpisodicMemory");
 
    m_image_received = false;
 
    setFramerate(1);
}
 
void
visionx::SimpleEpisodicMemoryImageConnector::onConnectImageProcessor()
{
    // Get SimpleEpisodicMemory
    getProxy(m_simple_episodic_memory, m_simple_episodic_memory_proxy_name);
 
    // Connect to image provider.
    m_image_provider_info = getImageProvider(m_image_provider_id);
    m_image_provider = getProxy<visionx::ImageProviderInterfacePrx>(m_image_provider_id);
 
    // Init input image.
    num_of_received_images = static_cast<unsigned int>(m_image_provider_info.numberImages);
    m_input_image_buf = new ::CByteImage*[num_of_received_images];
    for (unsigned int i = 0; i < num_of_received_images; ++i)
    {
        m_input_image_buf[i] = visionx::tools::createByteImage(m_image_provider_info);
    }
    m_input_image = visionx::tools::createByteImage(m_image_provider_info);
 
    // Kick off running task
    m_periodic_task = new armarx::PeriodicTask<SimpleEpisodicMemoryImageConnector>(
        this, &SimpleEpisodicMemoryImageConnector::checkForNewImages, m_periodic_task_interval);
    m_periodic_task->start();
}
 
void
visionx::SimpleEpisodicMemoryImageConnector::onDisconnectImageProcessor()
{
    // Stop task.
    {
        m_periodic_task->stop();
    }
 
    // Clear input image buffer.
    {
        for (unsigned int i = 0; i < num_of_received_images; ++i)
        {
            delete m_input_image_buf[i];
        }
        delete[] m_input_image_buf;
    }
}
 
void
visionx::SimpleEpisodicMemoryImageConnector::onExitImageProcessor()
{
}
 
void
visionx::SimpleEpisodicMemoryImageConnector::process()
{
    const IceUtil::Time timeout = IceUtil::Time::milliSeconds(1000);
    if (!waitForImages(m_image_provider_id, static_cast<int>(timeout.toMilliSeconds())))
    {
        ARMARX_WARNING << "Timeout while waiting for camera images (>" << timeout << ")";
        return;
    }
 
    ARMARX_DEBUG << "Received a new image. Putting image to member variable";
    std::lock_guard<std::mutex> lock{m_input_image_mutex};
 
    int num_images = getImages(m_image_provider_id, m_input_image_buf, m_image_meta_info);
    m_timestamp_last_image = IceUtil::Time::microSeconds(m_image_meta_info->timeProvided);
 
    if (static_cast<unsigned int>(num_images) != num_of_received_images)
    {
        // ERROR
    }
    else
    {
        // Only consider channel image.
        ::ImageProcessor::CopyImage(m_input_image_buf[m_image_provider_channel], m_input_image);
        m_image_received = true;
    }
    ARMARX_DEBUG << "Wait for next image";
}
 
void
visionx::SimpleEpisodicMemoryImageConnector::checkForNewImages()
{
 
    if (!m_image_received)
    {
        return;
    }
 
    ARMARX_DEBUG
        << "Got a new image for processing. Send image to EpisodicMemory. Image information: Size: "
        << m_input_image->width << ", " << m_input_image->height << ", "
        << m_input_image->bytesPerPixel;
    std::lock_guard<std::mutex> lock{m_input_image_mutex};
    m_image_received = false;
 
    // TODO convert2Blob and enable png compression
 
    memoryx::ImageEvent memory_image_event;
    memory_image_event.providerName =
        m_image_provider_id + "__" + std::to_string(m_image_provider_channel);
    memory_image_event.receivedInMs = m_timestamp_last_image.toMilliSecondsDouble();
    memory_image_event.height = m_input_image->height;
    memory_image_event.width = m_input_image->width;
    memory_image_event.colourType =
        (m_input_image->bytesPerPixel == 3 ? memoryx::ColourSpace::RGB
                                           : memoryx::ColourSpace::GRAYSCALE);
    memory_image_event.data = std::vector<unsigned char>(
        m_input_image->pixels,
        m_input_image->pixels +
            (m_input_image->height * m_input_image->width * m_input_image->bytesPerPixel));
    m_simple_episodic_memory->registerImageEvent(memory_image_event);
 
    ARMARX_DEBUG << deactivateSpam(0.5) << "Processed a new image";
}