void RobotNode::showStructure( bool enable, const std::string &visualizationType)
{
ReadLockPtr lock = getRobot()->getReadLock();
if (!enable && !visualizationModel)
return; // nothing to do
if (!ensureVisualization(visualizationType))
return;
std::stringstream ss;
ss << getName() << "_RobotNodeStructurePre";
std::string attachName1 = ss.str();
std::string attachName2("RobotNodeStructureJoint");
std::string attachName3("RobotNodeStructurePost");
SceneObjectPtr par = getParent();
RobotNodePtr parRN = dynamic_pointer_cast<RobotNode>(par);
// need to add "pre" visualization to parent node!
if (parRN && parRN->getVisualization())
parRN->getVisualization()->detachVisualization(attachName1);
else
visualizationModel->detachVisualization(attachName1);
visualizationModel->detachVisualization(attachName2);
visualizationModel->detachVisualization(attachName3);
if (enable)
{
VisualizationFactoryPtr visualizationFactory;
if (visualizationType.empty())
visualizationFactory = VisualizationFactory::first(NULL);
else
visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
if (!visualizationFactory)
{
VR_WARNING << "No visualization factory for name " << visualizationType << endl;
return;
}
// create visu
Eigen::Matrix4f i = Eigen::Matrix4f::Identity();
if (!localTransformation.isIdentity())
{
VisualizationNodePtr visualizationNode1;
if (parRN && parRN->getVisualization())
{
// add to parent node (pre joint trafo moves with parent!)
//visualizationNode1 = visualizationFactory->createLine(parRN->postJointTransformation, parRN->postJointTransformation*localTransformation);
visualizationNode1 = visualizationFactory->createLine(Eigen::Matrix4f::Identity(), localTransformation);
if (visualizationNode1)
parRN->getVisualization()->attachVisualization(attachName1,visualizationNode1);
} else
{
visualizationNode1 = visualizationFactory->createLine(localTransformation.inverse(),i);
if (visualizationNode1)
visualizationModel->attachVisualization(attachName1,visualizationNode1);
}
}
VisualizationNodePtr visualizationNode2 = visualizationFactory->createSphere(5.0f);
if (visualizationNode2)
visualizationModel->attachVisualization(attachName2,visualizationNode2);
}
}
