CoinRobotViewerAdapter.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    ArmarXGuiPlugins::RobotTrajectoryDesigner::Visualization
 * @author     Timo Birr
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "../Util/OrientationConversion.h"
#include "CoinRobotViewerAdapter.h"
 
#include "Inventor/draggers/SoDragger.h"
 
#include "Inventor/nodes/SoSeparator.h"
 
#include <Inventor/sensors/SoTimerSensor.h>
 
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h"
 
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h"
 
#include "VirtualRobot/Scene.h"
 
#include "ArmarXCore/core/logging/Logging.h"
 
#include "RobotAPI/libraries/core/Pose.h"
 
#include <Inventor/SoInteraction.h>
 
#include <Inventor/actions/SoLineHighlightRenderAction.h>
 
#include <Inventor/Qt/SoQt.h>
 
#include <Inventor/SoFullPath.h>
 
#include <Inventor/nodes/SoPickStyle.h>
 
#include <Inventor/nodes/SoCube.h>
 
#include <Inventor/nodes/SoTranslation.h>
 
#include <Inventor/misc/SoContextHandler.h>
 
#define ROBOT_UPDATE_TIMER_MS 333
 
#define AUTO_FOLLOW_UPDATE 50
 
 
 
using namespace VirtualRobot;
using namespace armarx;
using namespace Qt;
 
CoinRobotViewerAdapter::CoinRobotViewerAdapter(QWidget* widget): RobotVisualization()
{
 
 
    robotUpdateSensor = NULL;
    this->selectedWaypoint = 0;
    this->wayPointCounter = 0;
    this->viewer = std::shared_ptr<RobotViewer>(new RobotViewer(widget));
    camera = viewer->getCamera();
    createAdvancedVisualizationFactory();
    manipulator = new ManipulatorVisualization;
    SoInteraction::init();
    viewer->setGLRenderAction(new SoLineHighlightRenderAction);
    viewer->getRootNode()->addChild((SoNode*)manipulator);
    manipulatorMoved = false;
    startUpCameraPositioningFlag = true;
    selected = new SoSelection();
    //selected->addSelectionCallback(made_selection, viewer.get());
    //selected->addcctionCallback(unmade_selection, viewer.get());
    //selected->setPickFilterCallback(pickFilterCB, viewer.get(), true);
    //selected->policy = SoSelection::SINGLE;
    viewer->getRootNode()->addSelectionCallback(made_selection, viewer.get());
    viewer->getRootNode()->addDeselectionCallback(unmade_selection, viewer.get());
    viewer->getRootNode()->setPickFilterCallback(pickFilterCB, viewer.get(), true);
    viewer->getRootNode()->policy = SoSelection::SINGLE;
    manipulator->addManipFinishCallback(manipFinishCallback, this);
    manipulator->addManipMovedCallback(manipMovedCallback, this);
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    autoFollowSensor = new SoTimerSensor(autoFollowSensorTimerCB, this);
    autoFollowSensor->setInterval(SbTime(AUTO_FOLLOW_UPDATE / 1000.0f));
    sensor_mgr->insertTimerSensor(autoFollowSensor);
    viewer->getRootNode()->addChild((SoNode*)manipulator);
    manipFinishCallback(this, NULL);
 
    //selected->ref();
    factory = AdvancedCoinVisualizationFactoryPtr(new AdvancedCoinVisualizationFactory());
}
 
CoinRobotViewerAdapter::~CoinRobotViewerAdapter()
{
 
    this->clearTrajectory();
    while (transitions.getLength() != 0)
    {
        transitions.remove(0);
    }
    while (wayPoints.getLength() != 0)
    {
        wayPoints.remove(0);
    }
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    sensor_mgr->removeTimerSensor(robotUpdateSensor);
    sensor_mgr->removeTimerSensor(autoFollowSensor);
    this->viewer->getRootNode()->removeAllChildren();
 
    manipulator->addManipFinishCallback(NULL, NULL);
    manipulator->addManipMovedCallback(NULL, NULL);
    manipulator->removeVisualization();
    manipulator = NULL;
    viewer = NULL;
    robotUpdateSensor = NULL;
    autoFollowSensor = NULL;
    selected = NULL;
    dragger = NULL;
    camera = NULL;
    factory = NULL;
    ARMARX_INFO << "Destroyed CoinViewer";
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR VISUALIZATION SETUP
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void CoinRobotViewerAdapter::addRobotVisualization(RobotPtr robot, QString selectedChain = "")
{
    if (robot)
    {
        if ((robot) && (this->robot) && this->robot->getName() != robot->getName())
        {
            this->viewer->getRootNode()->removeAllChildren();
        }
        this->robot = robot;
        this->selectedWaypoint = 0;
        viewer->getRootNode()->deselectAll();
        manipulator->removeVisualization();
        manipulatorMoved = false;
        viewer->getRootNode()->addChild((SoNode*)manipulator);
        //Make Robot not pickable
        SoPickStyle* unpickable = new SoPickStyle();
        unpickable->style = SoPickStyle::UNPICKABLE;
        SoPickStyle* pickable = new SoPickStyle();
        pickable->style = SoPickStyle::SHAPE;
        viewer->getRootNode()->addChild(unpickable);
        CoinVisualizationPtr robotViewerVisualization = this->robot->getVisualization<VirtualRobot::CoinVisualization>();
        this->viewer->getRootNode()->addChild(robotViewerVisualization->getCoinVisualization());
        RobotNodeSetPtr nodeset = robot->getRobotNodeSet(selectedChain.toStdString());
        //manipulator->setVisualization(robot, nodeset);
        SoSensorManager* sensor_mgr = SoDB::getSensorManager();
        sensor_mgr->removeTimerSensor(robotUpdateSensor);
        robotUpdateSensor = new SoTimerSensor(robotUpdateTimerCB, this);
        robotUpdateSensor->setInterval(SbTime(ROBOT_UPDATE_TIMER_MS / 1000.0f));
        sensor_mgr->insertTimerSensor(robotUpdateSensor);
 
        viewer->getRootNode()->addChild(pickable);
        viewer->getRootNode()->addChild((SoNode*)manipulator);
        manipulator->addManipFinishCallback(manipFinishCallback, this);
        manipulator->addManipMovedCallback(manipMovedCallback, this);
        viewer->viewAll();
    }
}
 
void CoinRobotViewerAdapter::addSceneVisualization(ScenePtr scene)
{
    CoinVisualizationPtr sceneVisualization = scene->getVisualization<VirtualRobot::CoinVisualization>();
    this->viewer->getRootNode()->addChild(sceneVisualization->getCoinVisualization());
}
 
void CoinRobotViewerAdapter::setCamera(Eigen::VectorXf position, Eigen::VectorXf pointAtA, Eigen::VectorXf pointAtB)
{
    camera->position.setValue(position[0], position[1], position[2]);
    camera->pointAt(SbVec3f(pointAtA[0], pointAtA[1], pointAtA[2]), SbVec3f(pointAtB[0], pointAtB[1], pointAtB[2]));
    camera->viewAll(viewer->getRootNode(), SbViewportRegion());
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR TRANSITIONS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void CoinRobotViewerAdapter::addTransitionVisualization(int index, std::vector<PoseBasePtr> transition)
{
    VisualizationNodePtr node = this->factory->createCurve(transition, false);
    CoinVisualizationNodePtr visu = boost::dynamic_pointer_cast<CoinVisualizationNode>(node);
    SoNode* newTransition = visu->getCoinVisualization();
    viewer->getRootNode()->addChild(newTransition);
    transitions.insert(newTransition, index);
}
 
void CoinRobotViewerAdapter::removeTransitionVisualization(int index)
{
    SoNode* toDelete = transitions[index];
    viewer->getRootNode()->removeChild(toDelete);
    transitions.remove(index);
}
 
void CoinRobotViewerAdapter::highlightTransitionVisualization(int index, std::vector<PoseBasePtr> transition)
{
    removeTransitionVisualization(index);
    VisualizationNodePtr node = this->factory->createCurve(transition, true);
    CoinVisualizationNodePtr visu = boost::dynamic_pointer_cast<CoinVisualizationNode>(node);
    SoNode* newTransition = visu->getCoinVisualization();
    viewer->getRootNode()->addChild(newTransition);
    transitions.insert(newTransition, index);
    /* std::vector<Eigen::Vector3f> curve = std::vector<Eigen::Vector3f>();
 
     for (Vector3BasePtr position : transition)
     {
         curve.push_back(Eigen::Vector3f(position->x, position->y, position->z));
     }
     removeTransitionVisualization(index);
     transitions.insert(dynamic_cast<CoinVisualizationNode&>(visu).getCoinVisualization(), index);*/
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// INHERITED BY OBSERVER
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix4f CoinRobotViewerAdapter::getManipulatorPose()
{
    return manipulator->getUserDesiredPose();
}
 
int CoinRobotViewerAdapter::getSelectedWaypoint()
{
    //ARMARX_INFO << std::to_string(this->selectedWaypoint);
    return this->selectedWaypoint;
}
 
AdvancedVisualizationFactoryPtr CoinRobotViewerAdapter::createAdvancedVisualizationFactory()
{
    return std::make_shared<AdvancedCoinVisualizationFactory>(AdvancedCoinVisualizationFactory());
}
 
void CoinRobotViewerAdapter::addWaypointVisualization(int index, PoseBasePtr waypoint, EndEffectorPtr tcp)
{
 
    SoNode* newWaypoint;
    if (tcp)
    {
        VirtualRobot::RobotPtr endEffectorRobot = tcp->createEefRobot("", "");
        Pose p = Pose(waypoint->position, waypoint->orientation);
        EndEffectorPtr eef = endEffectorRobot->getEndEffectors()[0];
        endEffectorRobot->setGlobalPoseForRobotNode(eef->getTcp(), p.toEigen());
        eef->getTcp()->setGlobalPoseNoChecks(p.toEigen());
 
        newWaypoint = CoinVisualizationFactory::getCoinVisualization(endEffectorRobot, SceneObject::VisualizationType::Full, true);
    }
    else
    {
        SoCube* cube = new SoCube();
        cube->width = 0.09;
        cube->height = 0.09;
        cube->depth = 0.09;
        SoTranslation* cubePoint = new SoTranslation();
        cubePoint->translation.setValue(waypoint->position->x / 1000.0, waypoint->position->y / 1000.0, waypoint->position->z / 1000.0);
        SoSeparator* parent = new SoSeparator();
        parent->addChild(cubePoint);
        parent->addChild(cube);
        newWaypoint = parent;
    }
    const std::string s = "W" + std::to_string(index);
    const char* c = s.c_str();
    newWaypoint->setName(SbName(c));
    std::string st = std::string(newWaypoint->getName().getString());
    viewer->getRootNode()->addChild(newWaypoint);
    viewer->getRootNode()->select(newWaypoint);
    wayPoints.insert(newWaypoint, index);
    wayPointCounter++;
    viewer->getRootNode()->deselectAll();
}
 
void CoinRobotViewerAdapter::removeWaypointVisualization(int index)
{
    SoNode* toDelete = wayPoints[index];
    viewer->getRootNode()->removeChild(toDelete);
    wayPoints.remove(index);
    viewer->getRootNode()->deselectAll();
}
 
void CoinRobotViewerAdapter::removeAllWaypointVisualizations()
{
    while (wayPoints.getLength() != 0)
    {
        removeWaypointVisualization(0);
    }
    wayPointCounter = 0;//just to be sure
}
 
void CoinRobotViewerAdapter::setSelectedWaypoint(int index)
{
    this->selectedWaypoint = index;
    refreshSelectedPoint();
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// SETTING MANIPULATOR
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void CoinRobotViewerAdapter::setManipulator(VirtualRobot::RobotNodeSetPtr kc, std::vector<float>jointAngles)
{
    if (!kc)
    {
        manipulator->removeVisualization();
        return;
    }
    robot->getRobotNodeSet(kc->getName())->setJointValues(jointAngles);
    manipulator->setVisualization(robot, kc);
    manipulator->addManipFinishCallback(manipFinishCallback, this);
    manipulator->addManipMovedCallback(manipMovedCallback, this);
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// UPDATING OF VISUALIZATION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void CoinRobotViewerAdapter::updateRobotVisualization()
{
    viewer->render();
}
 
void CoinRobotViewerAdapter::enableVisualization()
{
    viewer->show();
}
 
void CoinRobotViewerAdapter::disableVisualization()
{
    viewer->hide();
}
 
void CoinRobotViewerAdapter::clearTrajectory()
{
    while (transitions.getLength() != 0)
    {
        this->removeTransitionVisualization(0);
    }
    while (wayPoints.getLength() != 0)
    {
        this->removeWaypointVisualization(0);
    }
 
}
 
RobotVisualizationPtr CoinRobotViewerAdapter::reproduce(QWidget* parent)
{
    std::shared_ptr<CoinRobotViewerAdapter> reproduction = std::make_shared<CoinRobotViewerAdapter>(parent);
    reproduction->viewer->getRootNode()->addChild(this->viewer->getRootNode());
    reproduction->camera = reproduction->viewer->getCamera();
    Eigen::Vector3f position;
    Eigen::Vector3f pointAtA = Eigen::Vector3f(0, 0, 0);
    Eigen::Vector3f pointAtB;
    position = Eigen::Vector3f(0, 2, 3.5);
    pointAtB = Eigen::Vector3f(0, 0, 5);
    reproduction->setCamera(position, pointAtA, pointAtB);
    reproduction->viewer->getCamera()->viewAll(this->viewer->getRootNode(), SbViewportRegion());
    return reproduction;
}
 
 
 
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///CALLBACK METHODS
/// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void CoinRobotViewerAdapter::manipFinishCallback(void* data, SoDragger* dragger)
{
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
    if (controller)
    {
        if (controller->observer.lock())
        {
            controller->observer.lock()->refresh();
        }
    }
}
 
void CoinRobotViewerAdapter::manipMovedCallback(void* data, SoDragger* dragger)
{
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
    if (controller)
    {
        controller->manipulatorMoved = true;
        if (controller->observer.lock())
        {
            controller->observer.lock()->refresh();
        }
    }
}
 
void CoinRobotViewerAdapter::autoFollowSensorTimerCB(void* data, SoSensor* sensor)
{
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
 
    if (controller && controller->manipulatorMoved)
    {
        if (controller->observer.lock())
        {
            controller->observer.lock()->refresh();
        }
    }
}
 
void CoinRobotViewerAdapter::made_selection(void* data, SoPath* path)
{
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
    if (controller)
    {
        //ARMARX_INFO << std::to_string(controller->selectedWaypoint);
        std::string s = path->getTail()->getName().getString();
        std::string number = s.substr(1);
        controller->selectedWaypoint = std::stoi(number);
        //ARMARX_INFO << std::to_string(controller->selectedWaypoint);
        if (controller->observer.lock())
        {
            controller->observer.lock()->refresh();
        }
    }
}
 
void CoinRobotViewerAdapter::unmade_selection(void* data, SoPath* path)
{
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
    if (!controller || !controller->robot)
    {
        return;
    }
}
 
SoPath* CoinRobotViewerAdapter::pickFilterCB(void* data, const SoPickedPoint* pick)
{
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
    if (!controller || !controller->robot)
    {
        return new SoPath();
    }
 
    SoFullPath* p = (SoFullPath*)pick->getPath();
    //Make sure we didn't select a manipulator
    for (int i = 0; i < p->getLength();)
    {
        SoNode* n = p->getNode(p->getLength() - 1);
        //ARMARX_INFO <<  std::to_string(controller->wayPointCounter);
        //TODO find out how to get counter
        for (int j = 0; j < 50; j++)
        {
            std::string currentName = std::string(n->getName().getString());
            //ARMARX_INFO << currentName << "XXXXXXXXXXXX";
            //ARMARX_INFO << "W" + std::to_string(j) << "XLOKEDFOR";
            if (currentName == "W" + std::to_string(j))
            {
                return p;
            }
        }
        p->truncate(p->getLength() - 1);
    }
    return new SoPath();
}
 
void CoinRobotViewerAdapter::refreshSelectedPoint()
{
    SoNode* selectedPoint = static_cast<SoNode*>(wayPoints.get(this->selectedWaypoint));
    this->viewer->getRootNode()->deselectAll();
    this->viewer->getRootNode()->select(selectedPoint);
}
 
void CoinRobotViewerAdapter::robotUpdateTimerCB(void* data, SoSensor* sensor)
{
    //static_cast<SoPerspectiveCamera*>(viewer->getCamera()->);
    CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
    if (!controller || !controller->robot)
    {
        return;
    }
    //UPDATE SELCETION
    if (controller->viewer->getRootNode()->getList()->getLength() != 0)
    {
        SoPath* p = (SoPath*)controller->viewer->getRootNode()->getList()->get(0);
        if (p)
        {
            std::string s = p->getTail()->getName().getString();
            if (s.length() != 0)
            {
                std::string number = s.substr(1);
                controller->selectedWaypoint = std::stoi(number);
                controller->refreshSelectedPoint();
                if (controller->observer.lock())
                {
                    controller->observer.lock()->refresh();
                }
 
            }
        }
    }
    controller->viewer->render();
 
 
    if (controller->startUpCameraPositioningFlag)
    {
        controller->viewer->cameraViewAll();
        controller->startUpCameraPositioningFlag = false;
    }
}