ImageProviderGodotSimulation.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    ArmarXSimulation::components::ImageProviderGodotSimulation
 * @author     Timo Birr
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImageProviderGodotSimulation.h"
 
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
namespace ag = arviz_interactive_project::components::arviz_interactive;
 
namespace armarx
{
    armarx::PropertyDefinitionsPtr
    ImageProviderGodotSimulation::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
 
        defs->defineOptionalProperty<float>("FrameRate", 30.0f, "Frames per second")
            .setMin(0.0f)
            .setMax(60.0f);
        defs->defineOptionalProperty<visionx::ImageDimension>(
                "ImageSize", visionx::ImageDimension(640, 480), "Resolution of the rendered image.")
            .setCaseInsensitive(true)
            .map("320x240", visionx::ImageDimension(320, 240))
            .map("640x480", visionx::ImageDimension(640, 480))
            .map("800x600", visionx::ImageDimension(800, 600))
            .map("1024x768", visionx::ImageDimension(1024, 768))
            .map("1280x960", visionx::ImageDimension(1280, 960));
        defs->defineOptionalProperty<std::string>(
            "RobotNodeCamera",
            "EyeLeftCameraSim",
            "Robot node whose pose is used as the virtual camera.");
        defs->defineOptionalProperty<std::string>(
            "ReferenceFrame",
            "EyeLeftCameraSim",
            "Reference frame reported for the provided images.");
        defs->defineOptionalProperty<float>(
            "focalLength", 600.0f, "Focal length in pixels (used to derive the field of view).");
        defs->defineOptionalProperty<float>(
            "NearClip", 0.01f, "Near clipping plane distance in meters.");
        defs->defineOptionalProperty<float>(
            "FarClip", 100.0f, "Far clipping plane distance in meters.");
        defs->defineOptionalProperty<std::string>(
            "ArVizGodotName", "arviz_godot", "Name of the arviz_godot component to render with.");
        defs->defineOptionalProperty<std::string>(
            "RobotStateComponentName",
            "RobotStateComponent",
            "Name of the RobotStateComponent providing the robot pose.");
        defs->defineOptionalProperty<float>(
            "CameraAlignRollDeg",
            180.0f,
            "Roll of the camera-node -> OpenGL-camera alignment (rotation about local X), in "
            "degrees. Defaults to 180 (camera node +Z optical axis -> Godot -Z forward).");
        defs->defineOptionalProperty<float>(
            "CameraAlignPitchDeg", 0.0f, "Pitch of the camera-node alignment (about local Y).");
        defs->defineOptionalProperty<float>(
            "CameraAlignYawDeg", 0.0f, "Yaw of the camera-node alignment (about local Z).");
        defs->defineOptionalProperty<int>(
            "RenderTimeoutMs", 2000, "Timeout in ms for a single render request to arviz_godot.");
        defs->defineOptionalProperty<std::string>(
            "RenderLayers",
            "",
            "Comma-separated list of ArViz layers to render. Each entry is either a component "
            "name (renders all of its layers, e.g. 'ObjectMemory') or a fully-qualified "
            "'component::layer'. Leave empty to render the whole scene.");
 
        return defs;
    }
 
    void
    ImageProviderGodotSimulation::onInitCapturingImageProvider()
    {
        setImageSyncMode(visionx::eFpsSynchronization);
        frameRate = getProperty<float>("FrameRate").getValue();
 
        const visionx::ImageDimension dimensions =
            getProperty<visionx::ImageDimension>("ImageSize").getValue();
        renderWidth_ = dimensions.width;
        renderHeight_ = dimensions.height;
 
        focalLength_ = getProperty<float>("focalLength").getValue();
        nearClipMeters_ = getProperty<float>("NearClip").getValue();
        farClipMeters_ = getProperty<float>("FarClip").getValue();
        cameraNodeName_ = getProperty<std::string>("RobotNodeCamera").getValue();
        referenceFrame_ = getProperty<std::string>("ReferenceFrame").getValue();
        arvizGodotName_ = getProperty<std::string>("ArVizGodotName").getValue();
        robotStateComponentName_ = getProperty<std::string>("RobotStateComponentName").getValue();
        renderTimeoutMs_ = getProperty<int>("RenderTimeoutMs").getValue();
 
        renderLayers_.clear();
        const std::string renderLayersStr = getProperty<std::string>("RenderLayers").getValue();
        for (std::size_t start = 0; start < renderLayersStr.size() || start == 0;)
        {
            const std::size_t comma = renderLayersStr.find(',', start);
            std::string token = renderLayersStr.substr(
                start, comma == std::string::npos ? std::string::npos : comma - start);
            // Trim surrounding whitespace.
            const std::size_t first = token.find_first_not_of(" \t");
            const std::size_t last = token.find_last_not_of(" \t");
            if (first != std::string::npos)
            {
                renderLayers_.push_back(token.substr(first, last - first + 1));
            }
            if (comma == std::string::npos)
            {
                break;
            }
            start = comma + 1;
        }
        if (!renderLayers_.empty())
        {
            ARMARX_INFO << "Restricting rendering to " << renderLayers_.size()
                        << " ArViz layer selector(s).";
        }
 
        const float deg2rad = static_cast<float>(M_PI) / 180.0f;
        cameraAlignment_ =
            Eigen::AngleAxisf(getProperty<float>("CameraAlignRollDeg").getValue() * deg2rad,
                              Eigen::Vector3f::UnitX()) *
            Eigen::AngleAxisf(getProperty<float>("CameraAlignPitchDeg").getValue() * deg2rad,
                              Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(getProperty<float>("CameraAlignYawDeg").getValue() * deg2rad,
                              Eigen::Vector3f::UnitZ());
 
        setImageFormat(visionx::ImageDimension(renderWidth_, renderHeight_), visionx::eRgb);
        setNumberImages(1);
 
        // arviz_godot must be running (it renders the scene); the RobotStateComponent provides
        // the camera pose. Declaring them here makes the component wait for them on connect.
        usingProxy(arvizGodotName_);
        usingProxy(robotStateComponentName_);
    }
 
    void
    ImageProviderGodotSimulation::onStartCapturingImageProvider()
    {
        ensureConnected();
    }
 
    bool
    ImageProviderGodotSimulation::ensureConnected()
    {
        try
        {
            if (!arvizGodot_)
            {
                arvizGodot_ = ag::ArVizInteractiveInterfacePrx::uncheckedCast(
                    getProxy<ag::ArVizInteractiveInterfacePrx>(arvizGodotName_)
                        ->ice_timeout(renderTimeoutMs_ + 1000));
            }
 
            if (!sharedRobot_ || !localRobot_)
            {
                auto robotStateComponent =
                    getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName_);
                sharedRobot_ = robotStateComponent->getSynchronizedRobot();
                localRobot_ = RemoteRobot::createLocalClone(sharedRobot_);
 
                if (localRobot_ && !localRobot_->hasRobotNode(cameraNodeName_))
                {
                    ARMARX_ERROR << "Configured camera node '" << cameraNodeName_
                                 << "' is not part of the robot model.";
                }
            }
        }
        catch (...)
        {
            handleExceptions();
        }
 
        return arvizGodot_ && localRobot_ && sharedRobot_;
    }
 
    void
    ImageProviderGodotSimulation::onStartCapture(float /*frameRate*/)
    {
        ARMARX_VERBOSE << "start capture";
        ensureConnected();
    }
 
    void
    ImageProviderGodotSimulation::onStopCapture()
    {
        ARMARX_VERBOSE << "stop capture";
    }
 
    void
    ImageProviderGodotSimulation::onExitCapturingImageProvider()
    {
        std::unique_lock<std::mutex> lock(captureMutex_);
        arvizGodot_ = nullptr;
        sharedRobot_ = nullptr;
        localRobot_.reset();
    }
 
    bool
    ImageProviderGodotSimulation::capture(void** ppImageBuffers)
    {
        std::unique_lock<std::mutex> lock(captureMutex_);
 
        if ((!arvizGodot_ || !localRobot_ || !sharedRobot_) && !ensureConnected())
        {
            return false;
        }
 
        try
        {
            // Update the local robot clone to the current robot state.
            RemoteRobot::synchronizeLocalClone(localRobot_, sharedRobot_);
 
            VirtualRobot::RobotNodePtr cameraNode = localRobot_->getRobotNode(cameraNodeName_);
            if (!cameraNode)
            {
                ARMARX_WARNING << deactivateSpam(5) << "Camera node '" << cameraNodeName_
                               << "' not found in robot model.";
                return false;
            }
 
            // Camera node global pose in ArmarX world coordinates (mm).
            const Eigen::Matrix4f globalPose = cameraNode->getGlobalPose();
            const Eigen::Matrix3f nodeRotation = globalPose.block<3, 3>(0, 0);
            const Eigen::Quaternionf cameraRotation =
                Eigen::Quaternionf(nodeRotation * cameraAlignment_.toRotationMatrix()).normalized();
 
            ag::CameraRenderPose pose;
            pose.px = globalPose(0, 3);
            pose.py = globalPose(1, 3);
            pose.pz = globalPose(2, 3);
            pose.qx = cameraRotation.x();
            pose.qy = cameraRotation.y();
            pose.qz = cameraRotation.z();
            pose.qw = cameraRotation.w();
 
            const float verticalFovRad = 2.0f * std::atan(renderHeight_ / (2.0f * focalLength_));
 
            const ag::StringSeq layerNames(renderLayers_.begin(), renderLayers_.end());
            const ag::RenderedFrame frame = arvizGodot_->renderCameraImage(pose,
                                                                           verticalFovRad,
                                                                           renderWidth_,
                                                                           renderHeight_,
                                                                           nearClipMeters_,
                                                                           farClipMeters_,
                                                                           layerNames);
 
            if (!frame.valid)
            {
                ARMARX_WARNING << deactivateSpam(5) << "arviz_godot returned an invalid frame.";
                return false;
            }
 
            if (frame.width != renderWidth_ || frame.height != renderHeight_)
            {
                ARMARX_WARNING << deactivateSpam(5) << "Rendered frame size " << frame.width << "x"
                               << frame.height << " differs from expected " << renderWidth_ << "x"
                               << renderHeight_ << ".";
                return false;
            }
 
            const std::size_t pixelCount =
                static_cast<std::size_t>(renderWidth_) * static_cast<std::size_t>(renderHeight_);
            if (frame.rgba.size() < pixelCount * 4)
            {
                ARMARX_WARNING << deactivateSpam(5) << "Rendered frame buffer too small.";
                return false;
            }
 
            if (!sharedMemoryProvider)
            {
                ARMARX_WARNING << "Shared memory provider is null (possibly shutting down).";
                return false;
            }
 
            armarx::SharedMemoryScopedWriteLockPtr writeLock = getScopedWriteLock();
            auto* dst = reinterpret_cast<std::uint8_t*>(ppImageBuffers[0]);
            const auto& src = frame.rgba;
            for (std::size_t i = 0; i < pixelCount; ++i)
            {
                dst[i * 3 + 0] = src[i * 4 + 0];
                dst[i * 3 + 1] = src[i * 4 + 1];
                dst[i * 3 + 2] = src[i * 4 + 2];
            }
        }
        catch (...)
        {
            handleExceptions();
            return false;
        }
 
        return true;
    }
 
    visionx::MonocularCalibration
    ImageProviderGodotSimulation::getCalibration(const Ice::Current&)
    {
        visionx::MonocularCalibration calibration;
        visionx::CameraParameters& params = calibration.cameraParam;
        params.width = renderWidth_;
        params.height = renderHeight_;
        params.principalPoint = {renderWidth_ / 2.0f, renderHeight_ / 2.0f};
        params.focalLength = {focalLength_, focalLength_};
        params.distortion = {0.0f, 0.0f, 0.0f, 0.0f};
        params.rotation = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}};
        params.translation = {0.0f, 0.0f, 0.0f};
        return calibration;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::ImageProviderGodotSimulation,
                                         ::armarx::ImageProviderGodotSimulation::GetDefaultName());
} // namespace armarx