WaypointController.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    RobotTrajectoryDesigner::gui-plugins::Controller::WaypointController
* \author     Max Beddies
* \date       2018
* \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include "WaypointController.h"
#include "VirtualRobot/IK/IKSolver.h"
 
namespace armarx
{
    void WaypointController::onInitComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on init";
 
        // Fill cartesian selection combo box with items
        initCSComboBox();
        // Add validator to line edits
        initValidator(-10000.000, 10000.000, 3);
 
        // Set single selection and clear waypoint list
        this->guiWaypointTab->getWaypointTab()->waypointList->
        setSelectionMode(QAbstractItemView::SingleSelection);
        this->guiWaypointTab->getWaypointTab()->waypointList->clear();
    }
 
    void WaypointController::onConnectComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on connect";
 
        // Waypoint: update selected waypoint
        QObject::connect(guiWaypointTab->getWaypointTab()->waypointList,
                         SIGNAL(itemClicked(QListWidgetItem*)),
                         this, SLOT(updateSelectedWaypoint(QListWidgetItem*)));
 
        // Waypoint: change x-coordinate
        QObject::connect(guiWaypointTab->getWaypointTab()->xPositionLineEdit,
                         SIGNAL(editingFinished()),
                         this, SLOT(setXCoordinate()));
 
        // Waypoint: change y-coordinate
        QObject::connect(guiWaypointTab->getWaypointTab()->yPositionLineEdit,
                         SIGNAL(editingFinished()),
                         this, SLOT(setYCoordinate()));
 
        // Waypoint: change x-coordinate
        QObject::connect(guiWaypointTab->getWaypointTab()->zPositionLineEdit,
                         SIGNAL(editingFinished()),
                         this, SLOT(setZCoordinate()));
 
        // Waypoint: change euler angle roll
        QObject::connect(guiWaypointTab->getWaypointTab()->eulerRLineEdit,
                         SIGNAL(editingFinished()),
                         this, SLOT(setEulerAngleR()));
 
        // Waypoint: change euler angle pitch
        QObject::connect(guiWaypointTab->getWaypointTab()->eulerPLineEdit,
                         SIGNAL(editingFinished()),
                         this, SLOT(setEulerAngleP()));
 
        // Waypoint: change euler angle yaw
        QObject::connect(guiWaypointTab->getWaypointTab()->eulerYLineEdit,
                         SIGNAL(editingFinished()),
                         this, SLOT(setEulerAngleY()));
 
        // Waypoint: change cartesian selection
        QObject::connect(guiWaypointTab->getWaypointTab()->ikSelectionComboBox,
                         SIGNAL(activated(int)),
                         this, SLOT(setCartesianSelection(int)));
 
        // Waypoint: is breakpoint
        QObject::connect(guiWaypointTab->getWaypointTab()->isBreakpointCheckBox,
                         SIGNAL(clicked(bool)),
                         this, SLOT(setBreakpoint(bool)));
 
        // Waypoint: add waypoint
        QObject::connect(guiWaypointTab->getWaypointTab()->insertButton,
                         SIGNAL(clicked()),
                         this, SLOT(addWaypoint()));
 
        // Waypoint: remove waypoint
        QObject::connect(guiWaypointTab->getWaypointTab()->deleteButton,
                         SIGNAL(clicked()),
                         this, SLOT(removeWaypoint()));
    }
 
    void WaypointController::onDisconnectComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on disconnect";
    }
 
    void WaypointController::onExitComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on exit";
    }
 
    WaypointController::WaypointController(WaypointTabPtr guiWaypointTab) :
        guiWaypointTab(guiWaypointTab)
    {
        onInitComponent();
        onConnectComponent();
    }
 
    WaypointTabPtr WaypointController::getGuiWaypointTab()
    {
        return this->guiWaypointTab;
    }
 
    void WaypointController::setGuiWaypointTab(WaypointTabPtr guiWaypointTab)
    {
        if (guiWaypointTab != NULL)
        {
            this->guiWaypointTab = guiWaypointTab;
        }
    }
 
    void WaypointController::addWaypoint(
        int index,
        std::vector<double> values,
        int cartesianSelection,
        bool isBreakpoint)
    {
        if (values.size() == 6)
        {
            // Create struct representing waypoint
            struct GuiWaypoint waypoint;
            waypoint.values = values;
            waypoint.cartesianSelection = cartesianSelection;
            waypoint.isBreakpoint = isBreakpoint;
 
            // create qvariant from struct
            QVariant data;
            data.setValue(waypoint);
 
            // add data to item and insert
            QListWidgetItem* item = new QListWidgetItem(QString::fromStdString("Waypoint: " + std::to_string(index)));
            item->setData(Qt::UserRole, data);
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
 
            waypoints->insertItem(index, item);
            updateSelectedWaypoint(index);
 
            // Check if waypoint list contains exactly one waypoint
            if (waypoints->count() == 1)
            {
                emit enableIKSolutionButton(false);
            }
        }
        changeTextListWidgetItems();
    }
 
    void WaypointController::removeWaypoint(int index)
    {
        QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
 
        if (waypoints->count() > index)
        {
            delete waypoints->takeItem(index);
 
            // Check if waypoint list contains no item
            if (waypoints->count() == 0)
            {
                // Enables the button for a new ik solution
                emit enableIKSolutionButton(true);
            }
        }
        changeTextListWidgetItems();
    }
 
    void WaypointController::updateSelectedWaypoint(QListWidgetItem* item)
    {
        GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
 
        // Update all gui elements
        updateWaypointElements(waypoint);
 
        // Set current row to row of selected item
        int row = guiWaypointTab->getWaypointTab()->waypointList->row(item);
        guiWaypointTab->getWaypointTab()->waypointList->setCurrentRow(row);
 
        emit setCurrentIndex(row);
        emit setCurrentIndexRobotVisualization(row);
    }
 
    void WaypointController::updateSelectedWaypoint(int index)
    {
        QListWidgetItem* item = guiWaypointTab->getWaypointTab()->waypointList->item(index);
        GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
 
        // Update all gui elements
        updateWaypointElements(waypoint);
 
        // Set current row to row of selected item
        guiWaypointTab->getWaypointTab()->waypointList->setCurrentRow(index);
 
        emit setCurrentIndex(index);
    }
 
    void WaypointController::setXCoordinate()
    {
        // Check whether input is valid
        if (guiWaypointTab->getWaypointTab()->xPositionLineEdit->hasAcceptableInput())
        {
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->currentItem();
 
            // Check if list widget has any item
            if (item != NULL)
            {
                GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
                double x = guiWaypointTab->getWaypointTab()->
                           xPositionLineEdit->text().toDouble();
 
                // Check if x-coordinate of waypoint has changed
                if ((waypoint.values.size() == 6)
                    && (x != waypoint.values[0]))
                {
                    std::vector<double> values = waypoint.values;
                    values[0] = x;
                    emit changedWaypoint(waypoints->currentRow(), values);
                }
            }
        }
        else
        {
            (new QErrorMessage(0))->showMessage("Invalid input.");
        }
    }
 
    void WaypointController::setYCoordinate()
    {
        // Check whether input is valid
        if (guiWaypointTab->getWaypointTab()->yPositionLineEdit->hasAcceptableInput())
        {
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->currentItem();
 
            // Check if list widget has any item
            if (item != NULL)
            {
                GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
                double y = guiWaypointTab->getWaypointTab()->
                           yPositionLineEdit->text().toDouble();
 
                // Check if x-coordinate of waypoint has changed
                if ((waypoint.values.size() == 6)
                    && (y != waypoint.values[1]))
                {
                    std::vector<double> values = waypoint.values;
                    values[1] = y;
                    emit changedWaypoint(waypoints->currentRow(), values);
                }
            }
        }
        else
        {
            (new QErrorMessage(0))->showMessage("Invalid input.");
        }
    }
 
    void WaypointController::setZCoordinate()
    {
        // Check whether input is valid
        if (guiWaypointTab->getWaypointTab()->zPositionLineEdit->hasAcceptableInput())
        {
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->currentItem();
 
            // Check if list widget has any item
            if (item != NULL)
            {
                GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
                double z = guiWaypointTab->getWaypointTab()->
                           xPositionLineEdit->text().toDouble();
 
                // Check if x-coordinate of waypoint has changed
                if ((waypoint.values.size() == 6)
                    && (z != waypoint.values[2]))
                {
                    std::vector<double> values = waypoint.values;
                    values[2] = z;
                    emit changedWaypoint(waypoints->currentRow(), values);
                }
            }
        }
        else
        {
            (new QErrorMessage(0))->showMessage("Invalid Input.");
        }
    }
 
    void WaypointController::setEulerAngleR()
    {
        // Check whether input is valid
        if (guiWaypointTab->getWaypointTab()->eulerRLineEdit->hasAcceptableInput())
        {
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->currentItem();
 
            // Check if list widget has any item
            if (item != NULL)
            {
                GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
                double r = guiWaypointTab->getWaypointTab()->
                           eulerRLineEdit->text().toDouble();
 
                // Check if x-coordinate of waypoint has changed
                if ((waypoint.values.size() == 6)
                    && (r != waypoint.values[3]))
                {
                    std::vector<double> values = waypoint.values;
                    values[3] = r;
                    emit changedWaypoint(waypoints->currentRow(), values);
                }
            }
        }
        else
        {
            (new QErrorMessage(0))->showMessage("Invalid Input.");
        }
    }
 
    void WaypointController::setEulerAngleP()
    {
        // Check whether input is valid
        if (guiWaypointTab->getWaypointTab()->eulerPLineEdit->hasAcceptableInput())
        {
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->currentItem();
 
            // Check if list widget has any item
            if (item != NULL)
            {
                GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
                double p = guiWaypointTab->getWaypointTab()->
                           eulerPLineEdit->text().toDouble();
 
                // Check if x-coordinate of waypoint has changed
                if ((waypoint.values.size() == 6)
                    && (p != waypoint.values[4]))
                {
                    std::vector<double> values = waypoint.values;
                    values[4] = p;
                    emit changedWaypoint(waypoints->currentRow(), values);
                }
            }
        }
        else
        {
            (new QErrorMessage(0))->showMessage("Invalid Input.");
        }
    }
 
    void WaypointController::setEulerAngleY()
    {
        // Check whether input is valid
        if (guiWaypointTab->getWaypointTab()->eulerYLineEdit->hasAcceptableInput())
        {
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->currentItem();
 
            // Check if list widget has any item
            if (item != NULL)
            {
                GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
                double y = guiWaypointTab->getWaypointTab()->
                           eulerYLineEdit->text().toDouble();
 
                // Check if x-coordinate of waypoint has changed
                if ((waypoint.values.size() == 6)
                    && (y != waypoint.values[5]))
                {
                    std::vector<double> values = waypoint.values;
                    values[5] = y;
                    emit changedWaypoint(waypoints->currentRow(), values);
                }
            }
        }
        else
        {
            (new QErrorMessage(0))->showMessage("Invalid Input.");
        }
    }
 
    void WaypointController::setCartesianSelection(int cs)
    {
        QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
        QListWidgetItem* item = waypoints->currentItem();
 
        // Check if list widget has any item
        if (item != NULL)
        {
            GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
 
            // Check if x-coordinate of waypoint has changed
            int cartesianSelection = guiWaypointTab->getWaypointTab()->
                                     ikSelectionComboBox->itemData(cs, Qt::UserRole).toInt();
            if (cartesianSelection != waypoint.cartesianSelection)
            {
                emit changedWaypoint(waypoints->currentRow(), cartesianSelection);
            }
        }
    }
 
    void WaypointController::setBreakpoint(bool isBreakpoint)
    {
        QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
        QListWidgetItem* item = waypoints->currentItem();
 
        // Check if list widget has any item
        if (item != NULL)
        {
            GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
 
            // Check if x-coordinate of waypoint has changed
            if (isBreakpoint != waypoint.isBreakpoint)
            {
                emit changedWaypoint(waypoints->currentRow(), isBreakpoint);
            }
        }
    }
 
    void WaypointController::setWaypointData(
        int index,
        std::vector<double> values,
        int cartesianSelection,
        bool isBreakpoint)
    {
        // Create struct holding relevant data
        struct GuiWaypoint waypoint;
        waypoint.values = values;
        waypoint.cartesianSelection = cartesianSelection;
        waypoint.isBreakpoint = isBreakpoint;
 
        if (waypoint.values.size() == 6)
        {
            // Create QVariant from struct
            QVariant data;
            data.setValue(waypoint);
 
            QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
            QListWidgetItem* item = waypoints->item(index);
 
            // Update data
            item->setData(Qt::UserRole, data);
            // Display updated data
            if (waypoints->currentRow() == index)
            {
                updateWaypointElements(waypoint);
            }
        }
 
    }
 
    void WaypointController::addWaypoint()
    {
        if (guiWaypointTab->getWaypointTab()->waypointList->count() == 0)
        {
            emit addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->count(),
                               guiWaypointTab->getWaypointTab()->
                               insertAfterButton->isChecked());
        }
        else if (guiWaypointTab->getWaypointTab()->waypointList->currentRow() == -1)
        {
            emit addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->count() - 1,
                               guiWaypointTab->getWaypointTab()->
                               insertAfterButton->isChecked());
        }
        else
        {
            emit addedWaypoint(guiWaypointTab->getWaypointTab()->
                               waypointList->currentRow(),
                               guiWaypointTab->getWaypointTab()->
                               insertAfterButton->isChecked());
        }
    }
 
    void WaypointController::removeWaypoint()
    {
        QListWidget* waypoints = this->guiWaypointTab->getWaypointTab()->waypointList;
        if (waypoints->count() > 0)
        {
            int index = waypoints->currentRow();
            emit deletedWaypoint(index);
        }
    }
 
    void WaypointController::retranslateGui()
    {
        ARMARX_INFO << "not yet implemented";
    }
 
    void WaypointController::clearWaypointList()
    {
        // Clear waypoint list
        this->guiWaypointTab->getWaypointTab()->waypointList->clear();
        // Clear line edits
        this->guiWaypointTab->getWaypointTab()->xPositionLineEdit->clear();
        this->guiWaypointTab->getWaypointTab()->yPositionLineEdit->clear();
        this->guiWaypointTab->getWaypointTab()->zPositionLineEdit->clear();
        this->guiWaypointTab->getWaypointTab()->eulerRLineEdit->clear();
        this->guiWaypointTab->getWaypointTab()->eulerPLineEdit->clear();
        this->guiWaypointTab->getWaypointTab()->eulerYLineEdit->clear();
        // Reset cartesian selection to "all"
        this->guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setCurrentIndex(5);
        // Uncheck breakpoint check box
        this->guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setChecked(false);
        // Set insert after button as checked
        this->guiWaypointTab->getWaypointTab()->insertAfterButton->setChecked(true);
        this->guiWaypointTab->getWaypointTab()->insertBeforeButton->setChecked(false);
        // Reset current waypoint index
        emit setCurrentIndex(0);
    }
 
    void WaypointController::enableDeleteButton(bool enable)
    {
        guiWaypointTab->getWaypointTab()->deleteButton->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setEnabled(enable);
    }
 
    void WaypointController::enableAddButton(bool enable)
    {
        guiWaypointTab->getWaypointTab()->insertButton->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->insertAfterButton->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->insertBeforeButton->setEnabled(enable);
    }
 
    void WaypointController::enableWaypointListLineEdit(bool enable)
    {
        guiWaypointTab->getWaypointTab()->eulerPLineEdit->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->eulerRLineEdit->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->eulerYLineEdit->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->waypointList->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->xPositionLineEdit->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->yPositionLineEdit->setEnabled(enable);
        guiWaypointTab->getWaypointTab()->zPositionLineEdit->setEnabled(enable);
    }
 
    /************************************************************************************/
    /* Private Functions                                                                */
    /************************************************************************************/
    void WaypointController::initCSComboBox()
    {
        QComboBox* cs = guiWaypointTab->getWaypointTab()->ikSelectionComboBox;
 
        // Set strong focus, add wheel event filter
        cs->setFocusPolicy(Qt::StrongFocus);
        cs->installEventFilter(new WheelEventFilter(this));
 
        // Clear combo box and insert items
        cs->clear();
        QVariant xpos(1), ypos(2), zpos(4), position(7), orientation(8), all(15);
        cs->addItem(QString::fromStdString("X Position"), xpos);
        cs->addItem(QString::fromStdString("Y Position"), ypos);
        cs->addItem(QString::fromStdString("Z Position"), zpos);
        cs->addItem(QString::fromStdString("Position"), position);
        cs->addItem(QString::fromStdString("Orientation"), orientation);
        cs->addItem(QString::fromStdString("Position and Orientation"), all);
        cs->setEnabled(true);
        cs->setCurrentIndex(5);
    }
 
    void WaypointController::initValidator(double bottom, double top, int decimals)
    {
        QDoubleValidator* validator = new QDoubleValidator(bottom,
                top,
                decimals,
                guiWaypointTab.get());
        validator->setNotation(QDoubleValidator::StandardNotation);
 
        this->guiWaypointTab->getWaypointTab()->xPositionLineEdit->
        setValidator(validator);
        this->guiWaypointTab->getWaypointTab()->yPositionLineEdit->
        setValidator(validator);
        this->guiWaypointTab->getWaypointTab()->zPositionLineEdit->
        setValidator(validator);
        this->guiWaypointTab->getWaypointTab()->eulerRLineEdit->setValidator(validator);
        this->guiWaypointTab->getWaypointTab()->eulerPLineEdit->setValidator(validator);
        this->guiWaypointTab->getWaypointTab()->eulerYLineEdit->setValidator(validator);
    }
 
    void WaypointController::updateWaypointElements(GuiWaypoint waypoint)
    {
        if (waypoint.values.size() == 6)
        {
            // x-coordinate line edit
            guiWaypointTab->getWaypointTab()->xPositionLineEdit->
            setText(QString::number(waypoint.values[0]));
            // y-coordinate line edit
            guiWaypointTab->getWaypointTab()->yPositionLineEdit->
            setText(QString::number(waypoint.values[1]));
            // z-coordinate line edit
            guiWaypointTab->getWaypointTab()->zPositionLineEdit->
            setText(QString::number(waypoint.values[2]));
            // roll euler angle line edit
            guiWaypointTab->getWaypointTab()->eulerRLineEdit->
            setText(QString::number(waypoint.values[3]));
            // pitch euler angle line edit
            guiWaypointTab->getWaypointTab()->eulerPLineEdit->
            setText(QString::number(waypoint.values[4]));
            // yaw euler angle line edit
            guiWaypointTab->getWaypointTab()->eulerYLineEdit->
            setText(QString::number(waypoint.values[5]));
            // cartesian selection combo box
            guiWaypointTab->getWaypointTab()->ikSelectionComboBox->
            setCurrentIndex(guiWaypointTab->getWaypointTab()->ikSelectionComboBox->
                            findData(QVariant(waypoint.cartesianSelection)));
            // is breakpoint check box
            guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->
            setChecked(waypoint.isBreakpoint);
        }
    }
 
    void WaypointController::changeTextListWidgetItems()
    {
        QListWidget* waypoints = this->guiWaypointTab->getWaypointTab()->waypointList;
        for (int i = 0; i < waypoints->count(); i++)
        {
            waypoints->item(i)->setText(QString::fromStdString(("Waypoint: " + std::to_string(i))));
        }
    }
}