PointCloudToArViz.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::PointCloudToArViz
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudToArViz.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <pcl/common/transforms.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/point_types.h>
 
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Clock.h"
 
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/libraries/core/Pose.h>
 
namespace visionx
{
 
    PointCloudToArVizPropertyDefinitions::PointCloudToArVizPropertyDefinitions(std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        defineOptionalProperty<int>(
            "ProviderWaitMs", 100, "Time to wait for each point cloud provider [ms].");
    }
 
    armarx::PropertyDefinitionsPtr
    PointCloudToArViz::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new PointCloudToArVizPropertyDefinitions(getConfigIdentifier()));
 
        defs->optional(
            params.pointSizeInPixels, "viz.pointSizeInPixels", "The point size in pixels.");
        defs->optional(params.checkFinite,
                       "viz.checkFinite",
                       "Enable to check points for being finite. "
                       "\nEnable this option if the drawn point cloud causes the screen to become "
                       "purely white.");
        defs->optional(params.labeled,
                       "viz.labeled",
                       "If true and point cloud is labeled, draw colors according to labels.");
        defs->optional(params.alsoVisualizeNode,
                       "viz.showNode",
                       "If true also the node's coordinate systems is shown.");
 
        defs->optional(params.robotName, "robotName", "");
        defs->optional(
            params.pointCloudNodeName,
            "pointCloudNodeName",
            " If the point cloud header does not provide a frame id, this info is used.");
 
        return defs;
    }
 
    PointCloudToArViz::PointCloudToArViz()
    {
        addPlugin(virtualRobotReaderPlugin);
    }
 
    std::string
    PointCloudToArViz::GetDefaultName()
    {
        return "PointCloudToArViz";
    }
 
    std::string
    PointCloudToArViz::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    void
    PointCloudToArViz::onInitPointCloudProcessor()
    {
        params.providerWaitTime =
            IceUtil::Time::milliSeconds(getProperty<int>("ProviderWaitMs").getValue());
    }
 
    void
    PointCloudToArViz::onConnectPointCloudProcessor()
    {
        if (virtualRobotReaderPlugin->isAvailable())
        {
            robot = virtualRobotReaderPlugin->get().getRobot(params.robotName);
            // No need to synchronize the robot here. We synchronize it later anyway.
        }
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
    }
 
    void
    PointCloudToArViz::onDisconnectPointCloudProcessor()
    {
    }
 
    void
    PointCloudToArViz::onExitPointCloudProcessor()
    {
    }
 
    void
    PointCloudToArViz::process()
    {
        // Fetch input clouds.
        for (const std::string& providerName : getPointCloudProviderNames())
        {
            if (waitForPointClouds(providerName,
                                   static_cast<int>(params.providerWaitTime.toMilliSeconds())))
            {
                auto it = cache.find(providerName);
                if (it == cache.end())
                {
                    it = cache.emplace(providerName, armarx::viz::PointCloud(providerName)).first;
                }
                armarx::viz::PointCloud& vizCloud = it->second;
 
                Parameters params;
                {
                    std::scoped_lock lock(paramMutex);
                    params = this->params;
                }
 
                vizCloud.checkFinite(params.checkFinite);
                vizCloud.pointSizeInPixels(params.pointSizeInPixels);
 
                const PointContentType type = getPointCloudFormat(providerName)->type;
                std::optional<PointCloudToArViz::OriginInfo> originInfo;
 
                if (visionx::tools::isLabeled(type))
                {
                    pcl::PointCloud<pcl::PointXYZRGBL>::Ptr inputCloud(
                        new pcl::PointCloud<pcl::PointXYZRGBL>);
                    getPointClouds(providerName, inputCloud);
                    originInfo = applyCustomTransform(*inputCloud);
                    vizCloud.pointCloud(*inputCloud, params.labeled);
                }
                else
                {
                    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr inputCloud(
                        new pcl::PointCloud<pcl::PointXYZRGBA>);
                    getPointClouds(providerName, inputCloud);
                    originInfo = applyCustomTransform(*inputCloud);
                    vizCloud.pointCloud(*inputCloud);
                }
 
                armarx::viz::Layer layer = arviz.layer(providerName);
                if (params.alsoVisualizeNode && originInfo.has_value())
                {
                    auto originPose = armarx::viz::Pose(originInfo->name);
                    originPose.pose(originInfo->pose);
                    originPose.scale(3);
                    layer.add(originPose);
                }
 
                layer.add(vizCloud);
                arviz.commit({layer});
            }
            else
            {
                ARMARX_VERBOSE << "Timeout waiting for point cloud provider " << providerName
                               << ".";
            }
        }
    }
 
    void
    PointCloudToArViz::setPointSizeInPixels(float pointSizeInPixels, const Ice::Current&)
    {
        std::scoped_lock lock(paramMutex);
        params.pointSizeInPixels = pointSizeInPixels;
    }
 
    void
    PointCloudToArViz::setCustomTransform(const armarx::PoseBasePtr& transformPtr,
                                          const Ice::Current&)
    {
        return;
 
        // std::scoped_lock lock(paramMutex);
        // params.customTransform = armarx::PosePtr::dynamicCast(transformPtr)->toEigen();
        // if (params.customTransform->isIdentity())
        // {
        //     params.customTransform = std::nullopt;
        // }
    }
 
    template <class PointT>
    std::optional<PointCloudToArViz::OriginInfo>
    PointCloudToArViz::applyCustomTransform(pcl::PointCloud<PointT>& pointCloud)
    {
        ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
        if (not virtualRobotReaderPlugin->isAvailable())
        {
            ARMARX_INFO << deactivateSpam(100)
                        << "The robot reader plugin is deactivated. Won't transform point cloud";
            return std::nullopt;
        }
 
        ARMARX_CHECK_NOT_NULL(robot);
        {
            std::scoped_lock lock(robotMutex);
            ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(
                *robot, armarx::core::time::Duration::MicroSeconds(pointCloud.header.stamp)));
        }
 
        const auto pointCloudNodeName = [&]() -> std::string
        {
            // highest prio: use frame id of point cloud.
            if (not pointCloud.header.frame_id.empty())
            {
                return pointCloud.header.frame_id;
            }
 
            // check if fallback solution is available
            ARMARX_INFO
                << deactivateSpam(100)
                << "The point cloud header does not provide a frame_id. Using property instead";
 
            ARMARX_CHECK_NOT_EMPTY(params.pointCloudNodeName)
                << "The `pointCloudNodeName` property must be provided as the point cloud "
                   "header "
                   "frame id is not set!";
 
            return params.pointCloudNodeName;
        }();
 
        if (not robot)
        {
            ARMARX_INFO << deactivateSpam(100)
                        << "The robot is not available. Won't transform point cloud";
            return std::nullopt;
        }
 
        Eigen::Matrix4f globalSensorPose = Eigen::Matrix4f::Identity();
        if (pointCloudNodeName != armarx::GlobalFrame)
        {
            {
                std::scoped_lock lock(robotMutex);
                ARMARX_CHECK_NOT_NULL(robot);
                const auto node = robot->getRobotNode(pointCloudNodeName);
                ARMARX_CHECK_NOT_NULL(node)
                    << "No robot node with name `" << pointCloudNodeName << "`";
                globalSensorPose = node->getGlobalPose();
            }
 
            pcl::transformPointCloud(pointCloud, pointCloud, globalSensorPose);
        }
        else if (not localTransformationNodeName.empty())
        {
            ARMARX_WARNING << deactivateSpam(100)
                           << "Local transformation node cannot be set as point cloud is in global "
                              "frame already";
        }
 
        pointCloud.header.frame_id = armarx::GlobalFrame;
 
        return OriginInfo{.name = pointCloudNodeName, .pose = globalSensorPose};
    }
 
    void
    PointCloudToArViz::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        GridLayout grid;
        int row = 0;
 
        tab.checkFinite.setName("Check Finite");
        tab.checkFinite.setValue(params.checkFinite);
 
        tab.pointSizeInPixels.setName("Point Size [pixels]");
        tab.pointSizeInPixels.setDecimals(2);
        tab.pointSizeInPixels.setRange(0, 100);
        tab.pointSizeInPixels.setSteps(100);
        tab.pointSizeInPixels.setValue(params.pointSizeInPixels);
 
        tab.labeled.setName("Color by Labels");
        tab.labeled.setValue(params.labeled);
 
        grid.add(Label(tab.checkFinite.getName()), {row, 0}).add(tab.checkFinite, {row, 1});
        row++;
        grid.add(Label(tab.pointSizeInPixels.getName()), {row, 0})
            .add(tab.pointSizeInPixels, {row, 1});
        row++;
        grid.add(Label(tab.labeled.getName()), {row, 0}).add(tab.labeled, {row, 1});
        row++;
 
        tab.localTransformationNode = ComboBox();
        tab.localTransformationNode.addOption("");
        for (const std::string& nodeName : robot->getRobotNodeNames())
        {
            tab.localTransformationNode.addOption(nodeName);
        }
        grid.add(Label("Local node: "), {row, 0}).add(tab.localTransformationNode, {row, 1});
        row++;
 
        auto createFloatSpinBox = [&](float min, float max, float value)
        {
            auto spinBox = FloatSpinBox();
            spinBox.setRange(min, max);
            spinBox.setValue(value);
            if (max > 100)
            {
                spinBox.setSteps(max - min);
                spinBox.setDecimals(1);
            }
            else 
            {
                spinBox.setSteps((max - min) * 1000);
                spinBox.setDecimals(4);
            }
            tab.transfSpinBoxes.push_back(spinBox);
            return spinBox;
        };
 
        int col = 0;
        grid.add(Label("Transformation: "), {row, col++})
            .add(Label("x"), {row, col++})
            .add(createFloatSpinBox(-5000, 5000, 0), {row, col++})
            .add(Label("y"), {row, col++})
            .add(createFloatSpinBox(-5000, 5000, 0), {row, col++})
            .add(Label("z"), {row, col++})
            .add(createFloatSpinBox(-5000, 5000, 0), {row, col++})
            .add(Label("roll"), {row, col++})
            .add(createFloatSpinBox(-M_PI, M_PI, 0), {row, col++})
            .add(Label("pitch"), {row, col++})
            .add(createFloatSpinBox(-M_PI, M_PI, 0), {row, col++})
            .add(Label("yaw"), {row, col++})
            .add(createFloatSpinBox(-M_PI, M_PI, 0), {row, col++});
        row++;
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    PointCloudToArViz::RemoteGui_update()
    {
        std::scoped_lock lock(paramMutex);
        if (tab.checkFinite.hasValueChanged())
        {
            params.checkFinite = tab.checkFinite.getValue();
        }
        if (tab.pointSizeInPixels.hasValueChanged())
        {
            params.pointSizeInPixels = tab.pointSizeInPixels.getValue();
        }
        if (tab.labeled.hasValueChanged())
        {
            params.labeled = tab.labeled.getValue();
        }
 
        if (tab.localTransformationNode.hasValueChanged())
        {
            localTransformationNodeName = tab.localTransformationNode.getValue();
 
            if (not localTransformationNodeName.empty())
            {
                const auto localTransf =
                    robot->getRobotNode(localTransformationNodeName)->getLocalTransformation();
                const Eigen::Vector3f pos = simox::math::mat4f_to_pos(localTransf);
                const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(localTransf);
 
                for (unsigned int i = 0; i < 3; i++)
                {
                    tab.transfSpinBoxes.at(i).setValue(pos[i]);
                }
                for (unsigned int i = 0; i < 3; i++)
                {
                    tab.transfSpinBoxes.at(i + 3).setValue(rpy[i]);
                }
            }
        }
 
        bool update = false;
        for (auto& spinBox : tab.transfSpinBoxes)
        {
            if (spinBox.hasValueChanged())
            {
                update = true;
            }
        }
        if (update)
        {
            {
                std::scoped_lock lock(robotMutex);
 
                auto localTransformation =
                    simox::math::pos_rpy_to_mat4f(tab.transfSpinBoxes.at(0).getValue(),
                                                  tab.transfSpinBoxes.at(1).getValue(),
                                                  tab.transfSpinBoxes.at(2).getValue(),
                                                  tab.transfSpinBoxes.at(3).getValue(),
                                                  tab.transfSpinBoxes.at(4).getValue(),
                                                  tab.transfSpinBoxes.at(5).getValue());
 
                robot->getRobotNode(localTransformationNodeName)
                    ->setLocalTransformation(localTransformation);
                robot->updatePose();
 
                ARMARX_INFO << "Updating localTransformation of " +
                                   tab.localTransformationNode.getName() + " to "
                            << localTransformation.matrix();
            }
        }
    }
 
 
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::visionx::PointCloudToArViz, ::visionx::PointCloudToArViz::GetDefaultName());
} // namespace visionx