void RobotVisualization::setSegmentNamesEnabled(bool value)
{
    segmentNamesEnabled_ = value;
    for (size_t i=0; i<segmentNames_.size(); i++){
        OSGSegment* seg = getSegment(segmentNames_[i]);
        if(value)
            seg->attachTextLabel();
        else
            seg->removeTextLabel();
    }
}
void RobotVisualization::showSegmentText(QString link_name, QString text){
    OSGSegment* seg = getSegment(link_name.toStdString());
    seg->attachTextLabel(text.toStdString());
}