KinematicUnitGuiPlugin.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    ArmarX::Component::KinematicUnitGuiPlugin
* @author     Christian Boege <boege at kit dot edu>
* @copyright  2011 Christian Böge
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
 
*/
 
#pragma once
 
/* ArmarX headers */
#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_kinematicunitguiplugin.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
 
#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include <ArmarXCore/core/Component.h>
 
/* Qt headers */
#include <QMainWindow>
#include <QToolBar>
 
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
 
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#include <Inventor/Qt/SoQt.h>
#include <QStyledItemDelegate>
#include <ArmarXCore/core/util/IceReportSkipper.h>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <mutex>
#include <boost/circular_buffer.hpp>
 
#include <ArmarXCore/core/time.h>
 
namespace armarx
{
    class KinematicUnitConfigDialog;
 
    /*!
     \class KinematicUnitGuiPlugin
     \brief This plugin provides a generic widget showing position and velocity of all joints. Optionally a 3d robot model can be visualized.
     \see KinematicUnitWidget
    */
    class KinematicUnitGuiPlugin :
        public ArmarXGuiPlugin
    {
        Q_OBJECT
        Q_INTERFACES(ArmarXGuiInterface)
        Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
    public:
        KinematicUnitGuiPlugin();
 
        QString getPluginName() override
        {
            return "KinematicUnitGuiPlugin";
        }
    };
 
    class RangeValueDelegate :
        public QStyledItemDelegate
    {
        void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
    };
 
    /*!
      \page RobotAPI-GuiPlugins-KinematicUnitPlugin KinematicUnitPlugin
 
      \brief This widget shows position and velocity of all joints. Optionally a 3d robot model can be visualized.
 
      \image html KinematicUnitGUI.png
      When you add the widget to the Gui, you need to specify the following parameters:
 
      Parameter Name   | Example Value     | Required?     | Description
      :----------------  | :-------------:   | :-------------- |:--------------------
      Robot model filepath     | $ArmarX_Core/Armar3/data/Armar3/robotmodel/ArmarIII.xml  | Yes | The robot model to use. This needs to be the same model the kinematic unit is using.
      Robot nodeset name     | Robot          | Yes | ?
      Kinematic unit name - Proxy | Armar3KinematicUnit | Yes | The kinematic unit you wish to observe/control.
      RobotState Topic Name | RobotState | ? | ?
 
      This widget allows you to both observe and control a kinematic unit. All joints are listed in
      the table in the center of the widget. The 3D viewer shows the current state of the robot.
 
      On the top you can select the joint you wish to control and the control mode. You can control
      a joint with the slider below.
     */
    class KinematicUnitWidgetController :
        public ArmarXComponentWidgetControllerTemplate<KinematicUnitWidgetController>
        // public KinematicUnitListener
    {
        Q_OBJECT
    public:
        /**
         * @brief Holds the column index for the joint tabel.
         * Used to avoid magic numbers.
         */
        enum JointTabelColumnIndex : int
        {
            eTabelColumnName = 0,
            eTabelColumnControlMode,
            eTabelColumnAngleProgressbar,
            eTabelColumnVelocity,
            eTabelColumnTorque,
            eTabelColumnCurrent,
            eTabelColumnTemperature,
            eTabelColumnOperation,
            eTabelColumnError,
            eTabelColumnEnabled,
            eTabelColumnEmergencyStop,
            eTabelColumnCount
        };
 
        enum Roles
        {
            eJointAngleRole = Qt::UserRole + 1,
            eJointHiRole,
            eJointLoRole
        };
 
        KinematicUnitWidgetController();
        ~KinematicUnitWidgetController() override;
 
        // inherited from Component
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
        // inherited of ArmarXWidget
        static QString GetWidgetName()
        {
            return "RobotControl.KinematicUnitGUI";
        }
        static QIcon GetWidgetIcon()
        {
            return QIcon("://icons/kinematic_icon.svg");
        }
 
        QPointer<QDialog> getConfigDialog(QWidget* parent = 0) override;
        void loadSettings(QSettings* settings) override;
        void saveSettings(QSettings* settings) override;
        void configured() override;
 
        SoNode* getScene() override;
 
        bool kinematicUnitSetupViewer();
 
        void modelUpdateCB();
 
        void updateGuiElements();
 
        // overwrite setMutex, so that we can inform the debugdrawer
        void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
 
        QPointer<QWidget> getCustomTitlebarWidget(QWidget* parent) override;
 
    signals:
 
        void jointAnglesReported();
        void jointVelocitiesReported();
        void jointControlModesReported();
        void jointTorquesReported();
        void jointCurrentsReported();
        void jointStatusesReported();
        void jointMotorTemperaturesReported();
 
        void onDebugInfoReceived(const DebugInfo& debugInfo);
 
 
    public slots:
 
        // KinematicUnit
        void kinematicUnitZeroVelocity();
 
        void setControlModePosition();
        void setControlModeVelocity();
        void setControlModeTorque();
 
        void selectJoint(int i);
        void selectJointFromTableWidget(int row, int column);
        void sliderValueChanged(int i);
 
        /**
         * @brief Sets the Slider ui.horizontalSliderKinematicUnitPos to 0 if
         * this->selectedControlMode is eVelocityControl.
         */
        void resetSlider();
        void resetSliderToZeroPosition();
 
        void updateJointAnglesTable(const NameValueMap& reportedJointAngles);
        void updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities);
        void updateJointTorquesTable(const NameValueMap& reportedJointTorques);
        void updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses);
        void updateMotorTemperaturesTable(const NameValueMap& reportedMotorTemperatures);
        void updateJointStatusesTable(const NameStatusMap& reportedJointStatuses);
        void updateControlModesTable(const NameControlModeMap& reportedJointControlModes);
 
        void updateKinematicUnitListInDialog();
 
        void fetchData();
 
 
    protected:
        void connectSlots();
        void initializeUi(const DebugInfo& debugInfo);
 
        QString translateStatus(OperationStatus status);
        QString translateStatus(ErrorStatus status);
 
        Ui::KinematicUnitGuiPlugin ui;
 
        // ice proxies
        KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
 
        bool verbose;
 
        std::string kinematicUnitFile;
        std::string kinematicUnitFileDefault;
        std::string kinematicUnitName;
        //std::string robotStateComponentName;
        std::string topicName;
        std::string robotNodeSetName;
 
        VirtualRobot::RobotPtr robot;
        VirtualRobot::RobotNodeSetPtr robotNodeSet;
        VirtualRobot::RobotNodePtr currentNode;
 
        VirtualRobot::CoinVisualizationPtr kinematicUnitVisualization;
        SoNode* kinematicUnitNode;
        SoSeparator* rootVisu;
        SoSeparator* robotVisu;
 
        SoSeparator* debugLayerVisu;
        armarx::DebugDrawerComponentPtr debugDrawer;
 
        void updateModel(const NameValueMap& jointAngles);
 
        void highlightCriticalValues(const NameStatusMap& reportedJointStatuses);
 
    protected slots:
 
        void showVisuLayers(bool show);
        void copyToClipboard();
        void on_pushButtonFromJson_clicked();
 
        void synchronizeRobotJointAngles();
        void debugInfoReceived(const DebugInfo& debugInfo);
 
 
    private:
 
        std::recursive_mutex mutexNodeSet;
        // init stuff
        VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
        VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
        VirtualRobot::RobotNodeSetPtr getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName);
        bool initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet);
        bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet);
 
        bool enableValueValidator;
        bool viewerEnabled = true;
        Ice::Long historyTime;
        // DatafieldFilterBasePtr jointAnglesUpdateFrequency;
        armarx::DateTime lastJointAngleUpdateTimestamp;
        float currentValueMax;
 
        boost::circular_buffer<NameValueMap> jointCurrentHistory;
 
        QPointer<QWidget> __widget;
        QPointer<KinematicUnitConfigDialog> dialog;
 
        RangeValueDelegate delegate;
        /**
         * @brief The zero position of the slider
         */
        static const int SLIDER_ZERO_POSITION = 0;
 
        /**
         * @brief Returns values in
         * (-ui.jitterThresholdSpinBox->value(),ui.jitterThresholdSpinBox->value())
         * as 0. (Other values remain unchanged)
         * @param value The value with jitter.
         * @return The value without jitter.
         */
        float cutJitter(float value);
 
        QPointer<QToolBar> customToolbar;
 
    protected:
        void runUpdate();
 
        armarx::RunningTask<KinematicUnitWidgetController>::pointer_type updateTask;
 
        ControlMode getSelectedControlMode() const;
        void setControlModeRadioButtonGroup(const ControlMode& controlMode);
        
    };
    using FloatVector = ::std::vector< ::Ice::Float>;
    using KinematicUnitGuiPluginPtr = std::shared_ptr<KinematicUnitWidgetController>;
 
 
}