From c257b72bb12eaa2864fb392a3caa49847d965124 Mon Sep 17 00:00:00 2001
From: Philipp Schmidt <ufedv@student.kit.edu>
Date: Fri, 14 Aug 2015 21:35:26 +0200
Subject: [PATCH] Now visualizes node sets which have no end effector set
---
.../ManipulatorVisualization.cpp | 41 ++++++++++++++-----
1 file changed, 30 insertions(+), 11 deletions(-)
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp b/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp
index 22f2f9c6..323bcfa4 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp
+++ b/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);
--
GitLab