PointCloudToArViz.h

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
 */
 
#pragma once
 
#include <map>
#include <mutex>
#include <optional>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 
// The has_member macro from
// > simox/SimoxUtility/meta/has_member_macros/has_member.hpp
// breaks compilation here if boost thread is not already included
#ifdef has_member
#undef has_member
#endif
 
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/interface/components/PointCloudToArViz.h>
 
namespace visionx
{
 
    /**
     * @class PointCloudToArVizPropertyDefinitions
     * @brief Property definitions of `PointCloudToArViz`.
     */
    class PointCloudToArVizPropertyDefinitions :
        public visionx::PointCloudProcessorPropertyDefinitions
    {
    public:
        PointCloudToArVizPropertyDefinitions(std::string prefix);
    };
 
    /**
     * @defgroup Component-PointCloudToArViz PointCloudToArViz
     * @ingroup VisionX-Components
     *
     * This point cloud processor draws the specified components to ArViz.
     *
     * @class PointCloudToArViz
     * @ingroup Component-PointCloudToArViz
     * @brief Brief description of class PointCloudToArViz.
     *
     * Detailed description of class PointCloudToArViz.
     */
    class PointCloudToArViz :
        virtual public visionx::PointCloudToArVizInterface,
        virtual public visionx::PointCloudProcessor,
        virtual public armarx::ArVizComponentPluginUser,
        virtual public armarx::LightweightRemoteGuiComponentPluginUser
    {
    public:
        PointCloudToArViz();
 
        /// @see armarx::ManagedIceObject::getDefaultName()
        std::string getDefaultName() const override;
 
        virtual void setPointSizeInPixels(float pointSizeInPixels,
                                          const Ice::Current& = Ice::emptyCurrent) override;
        virtual void setCustomTransform(const armarx::PoseBasePtr& transform,
                                        const Ice::Current& = Ice::emptyCurrent) override;
 
 
    protected:
        /// @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
        void onInitPointCloudProcessor() override;
 
        /// @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
        void onConnectPointCloudProcessor() override;
 
        /// @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
        void onDisconnectPointCloudProcessor() override;
 
        /// @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
        void onExitPointCloudProcessor() override;
 
 
        /// @see visionx::PointCloudProcessor::process()
        void process() override;
 
 
        /// @see PropertyUser::createPropertyDefinitions()
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
 
        // LightweightRemoteGuiComponentPluginUser interface
    public:
        void createRemoteGuiTab();
        void RemoteGui_update() override;
 
 
    private:
        struct Parameters
        {
            /// Time waiting for each provider before timeout.
            IceUtil::Time providerWaitTime = IceUtil::Time::milliSeconds(100);
 
            /// Whether to check whether points are finite.
            bool checkFinite = false;
 
            /// The point size in pixels.
            float pointSizeInPixels = 1;
 
            /// If true and point cloud is labeled, draw colors according to labels.
            bool labeled = false;
 
            /// An optional custom transform to apply to the point cloud.
            // std::optional<Eigen::Matrix4f> customTransform;
 
            /// Show coordinate system of node in ArViz
            bool alsoVisualizeNode = false;
 
            /// The robot name to use
            std::string robotName = "Armar6";
 
            // If the point cloud header does not provide a frame id, this info is used.
            std::string pointCloudNodeName;
        };
 
        std::mutex paramMutex;
        Parameters params;
 
        struct OriginInfo
        {
            std::string name;
            Eigen::Matrix4f pose;
        };
 
        template <class PointT>
        std::optional<OriginInfo> applyCustomTransform(pcl::PointCloud<PointT>& pointCloud);
 
 
        /// Cache of constructed and allocated `viz::PointCloud` instances.
        std::map<std::string, armarx::viz::PointCloud> cache;
 
        struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
        {
            armarx::RemoteGui::Client::CheckBox checkFinite;
            armarx::RemoteGui::Client::FloatSpinBox pointSizeInPixels;
            armarx::RemoteGui::Client::CheckBox labeled;
 
            armarx::RemoteGui::Client::ComboBox localTransformationNode;
            std::vector<armarx::RemoteGui::Client::FloatSpinBox> transfSpinBoxes;
        };
 
        RemoteGuiTab tab;
 
 
        armarx::armem::client::plugins::ReaderWriterPlugin<
            armarx::armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
 
        VirtualRobot::RobotPtr robot;
 
        std::string localTransformationNodeName;
        std::mutex robotMutex;
    };
} // namespace visionx