HandEyeCalibrationWidgetController.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    RobotComponents::gui-plugins::HandEyeCalibrationWidgetController
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
 
#include <RobotAPI/interface/core/RobotState.h>
 
#include <RobotComponents/gui-plugins/HandEyeCalibration/ui_HandEyeCalibrationWidget.h>
 
#include <Inventor/sensors/SoTimerSensor.h>
 
//Point Cloud Processor
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
 
//VisionX
#include <VisionX/components/pointcloud_core/PointCloudProvider.h>
#include <VisionX/interface/core/PointCloudProviderInterface.h>
 
// Qt
#include <QColorDialog>
#include <QFile>
 
#include "ManipulatorVisualization.h"
#include "PointCloudVisualization.h"
 
namespace armarx
{
    /**
    \page RobotComponents-GuiPlugins-HandEyeCalibration HandEyeCalibration
    \brief The HandEyeCalibration allows visualizing ...
 
    \image html HandEyeCalibration.png
    The user can
 
    API Documentation \ref HandEyeCalibrationWidgetController
 
    \see HandEyeCalibrationGuiPlugin
    */
 
    /**
     * \class HandEyeCalibrationWidgetController
     * \brief HandEyeCalibrationWidgetController brief one line description
     *
     * Detailed description
     */
    class ARMARXCOMPONENT_IMPORT_EXPORT HandEyeCalibrationWidgetController :
        public armarx::ArmarXComponentWidgetControllerTemplate<HandEyeCalibrationWidgetController>,
        public visionx::PointCloudProcessor
    {
        Q_OBJECT
 
    public:
        /**
         * Controller Constructor
         */
        explicit HandEyeCalibrationWidgetController();
 
        /**
         * Controller destructor
         */
        virtual ~HandEyeCalibrationWidgetController();
 
        /**
         * @see ArmarXWidgetController::loadSettings()
         */
        void loadSettings(QSettings* settings) override;
 
        /**
         * @see ArmarXWidgetController::saveSettings()
         */
        void saveSettings(QSettings* settings) override;
 
        /**
         * Returns the Widget name displayed in the ArmarXGui to create an
         * instance of this class.
         */
        static QString
        GetWidgetName()
        {
            return "RobotControl.HandEyeCalibrationGUI";
        }
 
        QPointer<QDialog> getConfigDialog(QWidget* parent = 0) override;
        void configured() override;
 
        // PointCloudProcessor interface
        void onInitPointCloudProcessor() override;
        void onConnectPointCloudProcessor() override;
        void onDisconnectPointCloudProcessor() override;
        void onExitPointCloudProcessor() override;
        void process() override;
 
    private:
        void setupEndEffectorSelection(VirtualRobot::RobotPtr robot);
        void setupCameraNodeSlider(VirtualRobot::RobotPtr robot);
        VirtualRobot::RobotPtr loadRobotFromFile() const;
        std::string getProviderFrame() const;
        void cropOriginalPointCloud(const pcl::PointCloud<PointT>& cloud,
                                    const VirtualRobot::EndEffectorPtr eef) const;
        std::string formatTransformationMatrix(Eigen::Matrix4f mat,
                                               int decimalPlacesOrientation = 6,
                                               int decimalPlacesPosition = 2) const;
        void enableGuiElements();
        void disableGuiElements();
        void updateManipulatorVisualization();
        bool findStringIC(const std::string& strHaystack, const std::string& strNeedle) const;
 
    private slots:
        void processPointCloud();
        void activeEndEffectorChanged(QString endEffectorName);
 
        void slider_croppingRange_changed(int value);
        void cb_croppingActive_changed(int state);
        void cb_inverted_changed(bool checked);
        void btn_copyToClipboard_pressed();
        void btn_resetToModel_pressed();
        void btn_backgroundColor_pressed();
        void btn_pointCloudColor_pressed();
        void sB_pointSize_changed(int value);
 
        void cameraNodeTransformationChanged(double value);
 
        void connectQt();
        void disconnectQt();
        void initUI();
        void saveDatapoint();
        void selectFile();
 
    signals:
        void pointCloudUpdated();
 
    private:
        QPointer<QWidget> m_widget;
 
        /**
         * Widget Form
         */
        Ui::HandEyeCalibrationWidget widget;
        QPointer<SimpleConfigDialog> dialog;
 
        std::string providerName;
        visionx::PointCloudProviderInfo providerInfo;
        std::string referenceFrame;
        pcl::PointCloud<PointT>::Ptr providerBuffer;
 
        // PointCloudVisualization
        PointCloudVisualization* pointCloudVisu;
        bool showPointCloud;
        // ManipulatorVisualization
        ManipulatorVisualization* manipulatorVisu;
 
        std::string robotStateComponentName;
        RobotStateComponentInterfacePrx robotStateComponentPrx;
        VirtualRobot::RobotPtr localRobot;
        VirtualRobot::RobotPtr pointcloudRobot;
 
        VirtualRobot::EndEffectorPtr activeEndEffector;
        std::map<std::string, std::pair<VirtualRobot::RobotPtr, Eigen::Matrix4f>> robotEEFMap;
        VirtualRobot::RobotPtr getEEFRobot(VirtualRobot::EndEffectorPtr eef) const;
        Eigen::Matrix4f getOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef)
            const; // OffsetMatrix is stored from Model to Pointcloud
        void setOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef,
                                   const Eigen::Matrix4f& offset);
 
        bool croppingActive = true;
 
        PeriodicTask<HandEyeCalibrationWidgetController>::pointer_type taskEEFManipulation;
        void taskEEFManipulationCB();
        PeriodicTask<HandEyeCalibrationWidgetController>::pointer_type taskLocalRobotUpdate;
        void taskLocalRobotUpdateCB();
 
        // Color Dialog
        QColorDialog colorDialog;
 
        VirtualRobot::RobotNodePtr cameraNode;
    };
} // namespace armarx