PoseQualityMeasurement::PoseQualityMeasurement(VirtualRobot::RobotNodeSetPtr rns)
:rns(rns)
{
THROW_VR_EXCEPTION_IF( (!rns || !rns->getTCP()), "NULL data");
name = "PoseQualityMeasurement";
considerObstacle = false;
verbose = false;
}
bool SimoxRobotViewer::showPath( const std::string &pathName, VirtualRobot::TrajectoryPtr path, VirtualRobot::VisualizationFactory::Color c )
{
removePath(pathName);
if (!path)
return false;
lock();
VirtualRobot::RobotNodeSetPtr rns = path->getRobotNodeSet();
if (!rns)
{
VR_ERROR << "No rns" << endl;
unlock();
return false;
}
RobotNodePtr tcp = rns->getTCP();
if (!tcp)
{
VR_ERROR << "No tcp" << endl;
unlock();
return false;
}
SoSeparator *sep = new SoSeparator;
sep->addChild(VirtualRobot::CoinVisualizationFactory::getCoinVisualization(path));
//Saba::CoinRrtWorkspaceVisualization cv(robot,cspace,tcp->getName());
//cv.setCustomColor(c.r,c.g,c.b);
//cv.addCSpacePath(path,Saba::CoinRrtWorkspaceVisualization::eCustom);
//SoSeparator *sep = cv.getCoinVisualization();
if (!sep)
{
unlock();
return false;
}
pathData pd;
pd.trajectory = path;
pd.visu = sep;
paths[pathName] = pd;
// add visu
sceneSep->addChild(sep);
unlock();
return true;
}