SimoxCSpaceVisualizerWidgetController.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author     Raphael Grimm (raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <deque>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>
 
#include <QCheckBox>
#include <QPointer>
#include <QTimer>
 
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
 
#include <ArmarXCore/core/system/ImportExportComponent.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXWidgetController.h>
 
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/util/CollisionCheckUtil.h>
#include <RobotComponents/components/MotionPlanning/util/Metrics.h>
 
#include "../QtWidgets/IndexedQCheckBox.h"
#include "../QtWidgets/MotionPlanningServerTaskList.h"
#include <Inventor/nodes/SoSeparator.h>
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include <RobotComponents/gui-plugins/MotionPlanning/ui_SimoxCSpaceVisualizerWidget.h>
#pragma GCC diagnostic pop
 
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
 
namespace armarx
{
 
    /**
     * @class SimoxCSpaceVisualizerWidgetController
     * @page RobotComponents-GuiPlugins-SimoxCSpaceVisualizer Collision Space Visualizer
     *
     * @brief The SimoxCSpaceVisualizer allows to visualize any \ref armarx::SimoxCSpace
     * used by a planning task on the configurated \ref armarx::MotionPlanningServer
     *
     * @see \ref RobotComponents-Tutorials-MotionPlanning
     * @see \ref RobotComponents-Tutorials-SimoxCSpace
     * @see \ref RobotComponents-GuiPlugins-SimoxCSpaceVisualizer
     *
     * @image html mplan_simox_cspace_gui-empty.png
     * Tasks to load are displayed in the top left corner. You can select one and load it.
     *
     * @image html mplan_simox_cspace_gui-loaded.png
     * Once loaded the bottom left shows all dimensions used for planning, their bounds and current value displayed in the 3D Viewer.
     * The 3D Viewer shows all objects, the agent and draws found paths.
     * The paths are visualized for the tcp of the kinematic chain used for planning.
     * The path consits of the tcp's poses for each node in the path and lines connecting them.
     *
     * @image html mplan_simox_cspace_gui-hide_obj.png
     * To hide an object or the agent, uncheck its box in the bottom mid list.
     *
     * @image html mplan_simox_cspace_gui-trace.png
     * To display the trace of the movement along the path (instead of lines connecting the nodes), check the
     * box "show joint traces instead of edges" in the top mid.
     *
     * @image html mplan_simox_cspace_gui-multi_traces.png
     * To display the path for other joints, simply select them from the list int the top mid.
     *
     * @image html mplan_simox_cspace_gui-move_along_path.png
     * To play back the motion, select the path in the bottom right and move the slider.
     * You can play it back by setting some value in the spin box "Animation speed".
     *
     * @image html mplan_simox_cspace_gui-set_pose.png
     * To set a pose, edit the joint values in the bottom left.
     */
    class ARMARXCOMPONENT_IMPORT_EXPORT SimoxCSpaceVisualizerWidgetController :
        public armarx::ArmarXComponentWidgetController
    {
        Q_OBJECT
    protected:
    public:
        struct PathData
        {
            VectorXfSeq path;
 
            float dcdStep = std::numeric_limits<float>::infinity();
 
            bool visible = true;
 
            /**
             * @return The path's length.
             */
            float
            getLength() const
            {
                return accPathLength.back();
            }
 
            /**
             * @brief The path's length up to the i-th node.
             */
            std::vector<float> accPathLength;
 
            static std::string
            getEdgeLayerName(std::size_t i)
            {
                return "PathEdges::" + to_string(i);
            }
        };
 
        /**
         * Controller Constructor
         */
        explicit SimoxCSpaceVisualizerWidgetController();
 
        /**
         * Controller destructor
         */
        ~SimoxCSpaceVisualizerWidgetController() override = default;
 
        /**
         * @see ArmarXWidgetController::loadSettings()
         */
        void loadSettings(QSettings* settings) override;
 
        /**
         * @see ArmarXWidgetController::saveSettings()
         */
        void saveSettings(QSettings* settings) override;
 
        /**
         * @return The Widget name displayed in the ArmarXGui to create an
         * instance of this class.
         */
        QString getWidgetName() const override;
 
        /**
         * @see armarx::Component::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @param parent The dialog's parent.
         * @return The plugin's config dialog.
         */
        QPointer<QDialog> getConfigDialog(QWidget* parent) override;
 
        /**
         * @brief Callback called when the config dialog is closed.
         */
        void
        configured() override
        {
            motionPlanningServerProxyName = dialog->getProxyName("MotionPlanningServer");
        }
 
        /**
         * @see armarx::Component::onConnectComponent()
         */
        void onConnectComponent() override;
 
        void
        onDisconnectComponent() override
        {
            motionPlanningServerTaskList->enableAutoUpdate(false);
            resetSimoxCSpace();
        }
 
        /**
         * @brief Performs cleanup on exit.
         */
        void
        onExitComponent() override
        {
            resetSimoxCSpace();
            soSeparatorCleanUpAndRemoveFromRoot(visuAgent);
            soSeparatorCleanUp(visuRoot);
        }
 
        /**
         * @return The bullet scene used to display the CSpace.
         */
        SoNode* getScene() override;
 
        /**
         * @brief Sets the mutex used to protect the bullet scene.
         * @param mutex3D The mutex to set.
         */
        void setMutex3D(RecursiveMutexPtr const& mutex3D) override;
 
        /**
         * @brief Connects all slots.
         */
        void connectSlots();
 
        /**
         * @return The current user selected solution path. (exception if there is none)
         */
        PathData&
        getCurrentPath()
        {
            return paths.at(widget.spinBoxPathNumber->value() - 1);
        }
 
    public slots:
        void loadTask();
 
        //set state
        /**
         * @brief Updates the current path according to the user selection.
         */
        void setCurrentPath();
        /**
         * @brief Updates the current position on the current path according to the user selection.
         */
        void setCurrentPathPosition();
        /**
         * @brief Updates the current displayed configuration .
         */
        void setCurrentConfig();
        void updateCollisionState();
 
        //draw
        void drawEdges(bool drawTrace);
 
        void
        drawEdges()
        {
            drawEdges(widget.checkBoxDrawTrace->isChecked());
        }
 
        //set visibility
        /**
         * @brief Sets the visibility of the given stationary object.
         * @param index The object's index.
         */
        void
        setVisibilityObjectStationary(int index, Qt::CheckState state)
        {
            soSeparatorRootChildVis(visuObjStat.at(index), state == Qt::Checked);
        }
 
        /**
         * @brief Sets the visibility of the given moveable object.
         * @param index The object's index.
         */
        void
        setVisibilityAgent(int state)
        {
            soSeparatorRootChildVis(visuAgent, state == Qt::Checked);
        }
 
        /**
         * @brief Sets the visibility of the given solution path.
         * @param index The path's index.
         * @param state The checkboxes state.
         */
        void setVisibilityPath(int index, Qt::CheckState state);
 
        /**
         * @brief Hides all solution paths.
         */
        void hideAllPaths();
 
        //do work
        void traceStepChanged();
        void toggleRobotNodePath(int index, Qt::CheckState);
        void updateEdgeVisibility();
        void highlightCollisionNodes(bool checked);
 
    protected:
        //helper
        void loadTaskFromServer();
 
        void loadTaskFromSend();
 
        /**
         * @brief Cleans up a soseparator and removes it from the root separator.
         * @param toRemove The separator to remove.
         */
        void soSeparatorCleanUpAndRemoveFromRoot(SoSeparator*& toRemove);
        /**
         * @brief Cleans up a soseparator. (removes children and the separator.
         * @param toClear The separator to clean up.
         */
        void soSeparatorCleanUp(SoSeparator*& toClear);
        /**
         * @brief Removes a soseparator from the root separator.
         * @param toRemove The separator to remove.
         */
        void soSeparatorRemoveFromRoot(SoSeparator* toRemove);
        /**
         * @brief Adds a soseparator to the root separator.
         * @param toAdd The separator to add.
         */
        void soSeparatorAddToRoot(SoSeparator* toAdd);
        /**
         * @brief Toggles a soseparator's visibility.
         * @param toToggle The separator to toggle.
         */
        void soSeparatorToggleRootChild(SoSeparator* toToggle);
 
        void soSeparatorRootChildVis(SoSeparator* child, bool visible = true);
 
        //set and init
        /**
         * @brief Sets a new simox CSpace (and cleans up the old one)
         * @param newCSpace The new CSpace.
         */
        void setSimoxCSpace(SimoxCSpacePtr newCSpace);
 
        void setPaths(std::vector<Path> newPaths);
 
        /**
         * @brief Sets a configuration and visualizes it.
         * @param cfg The configuration.
         */
        void setAndApplyConfig(const VectorXf& cfg);
 
        //reset
        /**
         * @brief Cleans up the current CSpace.(all visualization will be disabled)
         */
        void resetSimoxCSpace();
 
        void resetPaths();
 
        void
        resetCurrentPath()
        {
            widget.sliderPathPosition->setScale(0, 0);
        }
 
        /**
         * @brief Enables or disables the configuration selection and controles for object visibility
         * according to the parameter. (called when an CSpace is added)
         * @param active True = active, false = inactive.
         */
        void setEnabledSimoxCSpace(bool active = true);
 
        void setEnabledPaths(bool active = true);
 
        void timerEvent(QTimerEvent*) override;
        void setCurrentPathPosition(double p);
 
    private:
        //ui
        /**
         * @brief The plugin's ui.
         */
        Ui::SimoxCSpaceVisualizerWidget widget;
        /**
         * @brief The plugin's config dialog.
         */
        QPointer<SimpleConfigDialog> dialog;
 
        QPointer<MotionPlanningServerTaskList> motionPlanningServerTaskList;
 
        //components
        std::string motionPlanningServerProxyName;
        MotionPlanningServerInterfacePrx motionPlanningServerProxy;
 
        //basic visu
        /**
         * @brief The debug drawer used to draw edges and nodes.
         */
        armarx::DebugDrawerComponentPtr debugDrawer;
        /**
         * @brief The bullet animation's root separator.
         */
        SoSeparator* visuRoot;
 
        /**
         * @brief CSpace used to do collision checks and calculations. (using the visualization one would move the visualized agents)
         */
        SimoxCSpacePtr cspaceUtil;
        /**
         * @brief CSpace used for visualization. (to visualize configurations)
         */
        SimoxCSpacePtr cspaceVisu;
        /**
         * @brief Separators holding stationary object's models.
         */
        std::vector<SoSeparator*> visuObjStat;
        SoSeparator* visuAgent = nullptr;
        VirtualRobot::CoinVisualizationPtr robotVisu;
        //data
        std::vector<PathData> paths;
        /**
         * @brief The robot's nodes and whether they are visible (poses + traces are drawn for them)
         */
        std::vector<std::pair<VirtualRobot::RobotNodePtr, bool>> robotNodes;
        QTimer updateCollisionStateTimer;
        const long timerPeriod{30};
    };
} // namespace armarx