Now visualizes node sets which have no end effector set

source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp

@@ -10,6 +10,8 @@
 #include <Inventor/actions/SoSearchAction.h>
 #include <Inventor/SbViewportRegion.h>
 
+#include <iostream>
+
 ManipulatorVisualization::ManipulatorVisualization() : isVisualizing(false)
 {
     //Im gonna live forever :)
@@ -29,10 +31,24 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, Vi
     this->removeVisualization();
 
     //First of all add the end effector
-    VirtualRobot::RobotPtr endEffector = robot->getEndEffector(nodeSet->getTCP()->getName())->createEefRobot("", "");
-    CoinVisualizationPtr endEffectorVisualization = endEffector->getVisualization<VirtualRobot::CoinVisualization>();
-    this->addChild(endEffectorVisualization->getCoinVisualization());
+    VirtualRobot::EndEffectorPtr endEffector = robot->getEndEffector(nodeSet->getTCP()->getName());
+
+    if(endEffector)
+    {
+        VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
+        CoinVisualizationPtr endEffectorVisualization = endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
+        this->addChild(endEffectorVisualization->getCoinVisualization());
+    }
+    else
+    {
+        for(VirtualRobot::RobotNodePtr node : nodeSet->getAllRobotNodes())
+        {
+            CoinVisualizationPtr endEffectorVisualization = node->getVisualization<VirtualRobot::CoinVisualization>();
+            this->addChild(endEffectorVisualization->getCoinVisualization());
 
+            std::cout << "Adding: " << node->getName() << std::endl;
+        }
+    }
     //And now wrap this bad boy with our manipulator
     this->manip.reset(new SoTransformerManip());
 
@@ -52,14 +68,17 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, Vi
     manip->getDragger()->setPart("scale7", nullSep);
     manip->getDragger()->setPart("scale8", nullSep);
 
-    //Move it to the tcp
-    Eigen::Matrix4f mat = nodeSet->getTCP()->getGlobalPose();
-    mat(0, 3) /= 1000;
-    mat(1, 3) /= 1000;
-    mat(2, 3) /= 1000;
-
-    //Apply to current and desired visualizer
-    manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat));
+    if(endEffector)
+    {
+        //Move it to the tcp
+        Eigen::Matrix4f mat = nodeSet->getTCP()->getGlobalPose();
+        mat(0, 3) /= 1000;
+        mat(1, 3) /= 1000;
+        mat(2, 3) /= 1000;
+
+        //Apply to current and desired visualizer
+        manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat));
+    }
 
     this->insertChild(manip.get(), 0);