From 90933d5757fb7e0308ba16776b9ae1cc033bf448 Mon Sep 17 00:00:00 2001 From: Stefan Haase Date: Wed, 25 Nov 2015 16:28:09 +0100 Subject: [PATCH 1/7] Just formatting and tab-removing. --- viz/RigidBodyStateVisualization.cpp | 33 +++++++++++++++++++++-------- viz/RigidBodyStateVisualization.hpp | 18 ++++++++-------- 2 files changed, 33 insertions(+), 18 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index bf082a49..99a0b351 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -51,15 +51,22 @@ void RigidBodyStateVisualization::setColor(const Vec4d& color, Geode* geode) bool RigidBodyStateVisualization::isPositionDisplayForced() const { return forcePositionDisplay; } + void RigidBodyStateVisualization::setPositionDisplayForceFlag(bool flag) { forcePositionDisplay = flag; emit propertyChanged("forcePositionDisplay"); } + bool RigidBodyStateVisualization::isOrientationDisplayForced() const { return forceOrientationDisplay; } + void RigidBodyStateVisualization::setOrientationDisplayForceFlag(bool flag) -{ forceOrientationDisplay = flag; emit propertyChanged("forceOrientationDisplay"); } +{ + forceOrientationDisplay = flag; + emit propertyChanged("forceOrientationDisplay"); +} void RigidBodyStateVisualization::setTexture(QString const& path) { return setTexture(path.toStdString()); } + void RigidBodyStateVisualization::setTexture(std::string const& path) { if (path.empty()) @@ -78,8 +85,11 @@ void RigidBodyStateVisualization::clearTexture() void RigidBodyStateVisualization::addBumpMapping( QString const& diffuse_color_map_path, QString const& normal_map_path) -{ return addBumpMapping(diffuse_color_map_path.toStdString(), - normal_map_path.toStdString()); } +{ + return addBumpMapping(diffuse_color_map_path.toStdString(), + normal_map_path.toStdString()); +} + void RigidBodyStateVisualization::addBumpMapping( std::string const& diffuse_color_map_path, std::string const& normal_map_path) @@ -175,7 +185,7 @@ ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) ref_ptr spd = new ShapeDrawable(sp); spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); geode->addDrawable(spd); - if(text_size>0.0) + if(text_size > 0.0) { double actual_size = text_size * size; ref_ptr text= new osgText::Text; @@ -332,10 +342,12 @@ void RigidBodyStateVisualization::loadModel(std::string const& path) setDirty(); emit propertyChanged("modelPath"); } + QVector3D RigidBodyStateVisualization::getTranslation() const { return QVector3D(translation.x(), translation.y(), translation.z()); } + void RigidBodyStateVisualization::setTranslation(QVector3D const& v) { translation = osg::Vec3(v.x(), v.y(), v.z()); @@ -350,6 +362,7 @@ void RigidBodyStateVisualization::setRotation(QQuaternion const& q) void RigidBodyStateVisualization::displayCovariance(bool enable) { covariance = enable; emit propertyChanged("displayCovariance"); } + bool RigidBodyStateVisualization::isCovarianceDisplayed() const { return covariance; } @@ -357,15 +370,18 @@ void RigidBodyStateVisualization::setColor(base::Vector3d const& color) { this->color = color; } void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable) -{ covariance_with_samples = enable; emit propertyChanged("displayCovarianceWithSamples"); } +{ + covariance_with_samples = enable; + emit propertyChanged("displayCovarianceWithSamples"); +} + bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const { return covariance_with_samples; } ref_ptr RigidBodyStateVisualization::createMainNode() { Group* group = new Group; - PositionAttitudeTransform* body_pose = - new PositionAttitudeTransform(); + PositionAttitudeTransform* body_pose = new PositionAttitudeTransform(); if (!body_model) resetModel(total_size); body_pose->addChild(body_model); @@ -430,8 +446,7 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) if (forcePositionDisplay || state.hasValidPosition()) { - osg::Vec3d pos( - state.position.x(), state.position.y(), state.position.z()); + osg::Vec3d pos(state.position.x(), state.position.y(), state.position.z()); body_pose->setPosition(pos + translation); } diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp index 11da90b0..27258f37 100644 --- a/viz/RigidBodyStateVisualization.hpp +++ b/viz/RigidBodyStateVisualization.hpp @@ -29,9 +29,9 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin::updateData(state); } @@ -40,8 +40,8 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin createMainNode(); - virtual void updateMainNode(osg::Node* node); - void updateDataIntern( const base::samples::RigidBodyState& state ); + virtual void updateMainNode(osg::Node* node); + void updateDataIntern( const base::samples::RigidBodyState& state ); base::samples::RigidBodyState state; public slots: @@ -54,7 +54,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin body_model; + osg::ref_ptr body_model; osg::ref_ptr createSimpleBody(double size); - osg::ref_ptr createSimpleSphere(double size); + osg::ref_ptr createSimpleSphere(double size); osg::ref_ptr image; osg::ref_ptr texture; From 404d9f6e67809b61d044b27cbc857a3ebe5255a4 Mon Sep 17 00:00:00 2001 From: Stefan Haase Date: Thu, 26 Nov 2015 12:06:22 +0100 Subject: [PATCH 2/7] First version to visualize a vector of RGBs, not done yet. --- viz/RigidBodyStateVisualization.cpp | 120 ++++++++++++++-------------- viz/RigidBodyStateVisualization.hpp | 8 +- viz/vizkit_plugin.rb | 1 + 3 files changed, 68 insertions(+), 61 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index 99a0b351..e077144b 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -14,6 +14,7 @@ namespace vizkit3d RigidBodyStateVisualization::RigidBodyStateVisualization(QObject* parent) : Vizkit3DPlugin(parent) + , states() , covariance(false) , covariance_with_samples(false) , color(1, 1, 1) @@ -27,10 +28,8 @@ RigidBodyStateVisualization::RigidBodyStateVisualization(QObject* parent) , bump_mapping_dirty(false) , forcePositionDisplay(false) , forceOrientationDisplay(false) + , model_path() { - state = base::samples::RigidBodyState::invalid(); - state.position = base::Vector3d::Zero(); - state.orientation = base::Quaterniond::Identity(); } RigidBodyStateVisualization::~RigidBodyStateVisualization() @@ -185,6 +184,7 @@ ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) ref_ptr spd = new ShapeDrawable(sp); spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); geode->addDrawable(spd); + /* if(text_size > 0.0) { double actual_size = text_size * size; @@ -194,6 +194,7 @@ ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0)); geode->addDrawable(text); } + */ group->addChild(geode); @@ -381,11 +382,6 @@ bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const ref_ptr RigidBodyStateVisualization::createMainNode() { Group* group = new Group; - PositionAttitudeTransform* body_pose = new PositionAttitudeTransform(); - if (!body_model) - resetModel(total_size); - body_pose->addChild(body_model); - group->addChild(body_pose); texture = new osg::Texture2D; texture->setWrap(Texture::WRAP_S, Texture::REPEAT); @@ -409,26 +405,54 @@ ref_ptr RigidBodyStateVisualization::createMainNode() void RigidBodyStateVisualization::updateMainNode(Node* node) { Group* group = node->asGroup(); - PositionAttitudeTransform* body_pose = - dynamic_cast(group->getChild(0)); - - // Check if we need an uncertainty representation node, and manage the - // uncertainty child accordingly - bool needs_uncertainty = covariance && state.hasValidPositionCovariance(); - Uncertainty* uncertainty = 0; - if (group->getNumChildren() > 1) - { - if (needs_uncertainty) - uncertainty = dynamic_cast(group->getChild(1)); - else - group->removeChild(1); - } - else if (needs_uncertainty) - { - uncertainty = new Uncertainty; - group->addChild(uncertainty); + + group->removeChildren(0, group->getNumChildren()); + + if (!body_model) + resetModel(total_size); + if (texture_dirty) + updateTexture(); + // Bump mapping not added yet, seems not to work anyway. + //if (bump_mapping_dirty) + // updateBumpMapping(); + + std::vector::iterator it; + for(it = states.begin(); it != states.end(); it++) { + PositionAttitudeTransform* body_pose = new PositionAttitudeTransform(); + + body_pose->addChild(body_model); + group->addChild(body_pose); + + if (forcePositionDisplay || it->hasValidPosition()) { + osg::Vec3d pos(it->position.x(), it->position.y(), it->position.z()); + body_pose->setPosition(pos + translation); + } + + if (forceOrientationDisplay || it->hasValidOrientation()) { + osg::Quat orientation(it->orientation.x(), + it->orientation.y(), + it->orientation.z(), + it->orientation.w()); + body_pose->setAttitude(rotation * orientation); + } + + // Add uncertainties if required. + bool needs_uncertainty = covariance && it->hasValidPositionCovariance(); + if(needs_uncertainty) { + Uncertainty* uncertainty = new Uncertainty; + uncertainty->setMean(static_cast(it->position)); + uncertainty->setCovariance(static_cast(it->cov_position)); + group->addChild(uncertainty); + + if (covariance_with_samples) { + uncertainty->showSamples(); + } else { + uncertainty->hideSamples(); + } + } } - + + /* // Reset the body model if needed Node* body_node = body_pose->getChild(0); if (bump_mapping && body_node != bump_mapping) @@ -437,42 +461,20 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) body_pose->setChild(0, bump_mapping); } else if (body_node != body_model) - body_pose->setChild(0, body_model); - - if (texture_dirty) - updateTexture(); - if (bump_mapping_dirty) - updateBumpMapping(); - - if (forcePositionDisplay || state.hasValidPosition()) - { - osg::Vec3d pos(state.position.x(), state.position.y(), state.position.z()); - - body_pose->setPosition(pos + translation); - } - if (needs_uncertainty) - { - if (covariance_with_samples) - uncertainty->showSamples(); - else - uncertainty->hideSamples(); - - uncertainty->setMean(static_cast(state.position)); - uncertainty->setCovariance(static_cast(state.cov_position)); - } - if (forceOrientationDisplay || state.hasValidOrientation()) - { - osg::Quat orientation(state.orientation.x(), - state.orientation.y(), - state.orientation.z(), - state.orientation.w()); - body_pose->setAttitude(rotation * orientation); - } + body_pose->setChild(0, body_model); + */ } void RigidBodyStateVisualization::updateDataIntern( const base::samples::RigidBodyState& state ) { - this->state = state; + states.clear(); + states.resize(1); + states[0] = state; +} + +void RigidBodyStateVisualization::updateDataIntern( const std::vector& states ) +{ + this->states = states; } } diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp index 27258f37..e51e4cb0 100644 --- a/viz/RigidBodyStateVisualization.hpp +++ b/viz/RigidBodyStateVisualization.hpp @@ -16,7 +16,8 @@ namespace osgFX namespace vizkit3d { -class RigidBodyStateVisualization : public Vizkit3DPlugin +class RigidBodyStateVisualization : public Vizkit3DPlugin, + public VizPluginAddType > { Q_OBJECT Q_PROPERTY(double size READ getSize WRITE setSize) @@ -37,12 +38,15 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin::updateData(state); } Q_INVOKABLE void updateRigidBodyState( const base::samples::RigidBodyState& state ) { return updateData(state); } + Q_INVOKABLE void updateData( const std::vector& states ) + { return Vizkit3DPlugin::updateData(states); } protected: virtual osg::ref_ptr createMainNode(); virtual void updateMainNode(osg::Node* node); void updateDataIntern( const base::samples::RigidBodyState& state ); - base::samples::RigidBodyState state; + void updateDataIntern( const std::vector& states ); + std::vector states; public slots: bool isPositionDisplayForced() const; diff --git a/viz/vizkit_plugin.rb b/viz/vizkit_plugin.rb index 5317dd31..3387aaac 100644 --- a/viz/vizkit_plugin.rb +++ b/viz/vizkit_plugin.rb @@ -11,6 +11,7 @@ Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateVisualization', "/base/samples/RigidBodyState", :updateRigidBodyState) Vizkit::UiLoader.register_3d_plugin('BodyStateVisualization',"base", 'BodyStateVisualization') Vizkit::UiLoader.register_3d_plugin_for('BodyStateVisualization', "/base/samples/BodyState", :updateBodyState) +Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateVisualization', "/std/vector", :updateData) Vizkit::UiLoader.register_3d_plugin('LaserScanVisualization',"base", 'LaserScanVisualization') Vizkit::UiLoader.register_3d_plugin_for('LaserScanVisualization', "/base/samples/LaserScan", :updateLaserScan) Vizkit::UiLoader.register_3d_plugin('MotionCommandVisualization',"base", 'MotionCommandVisualization') From f890e1201ca51a9daaa61b914c424cae7d1382cd Mon Sep 17 00:00:00 2001 From: Stefan Haase Date: Fri, 27 Nov 2015 16:44:44 +0100 Subject: [PATCH 3/7] Prevents crash if model could not be loaded and checks the passed path. Textures and bump mapping still not added. --- viz/RigidBodyStateVisualization.cpp | 54 +++++++++++++++++++---------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index e077144b..4f187259 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -7,6 +7,7 @@ #include #include #include +#include using namespace osg; namespace vizkit3d @@ -92,10 +93,17 @@ void RigidBodyStateVisualization::addBumpMapping( void RigidBodyStateVisualization::addBumpMapping( std::string const& diffuse_color_map_path, std::string const& normal_map_path) -{ - if (!body_model->asGeode()) - { - std::cerr << "model is not a geometry, cannot use bump mapping" << std::endl; +{ + if(body_model == NULL) { + return; + } + + if (!body_model->asGeode()) { + std::cerr << "model is not a geode, cannot use bump mapping" << std::endl; + return; + } else if ( !body_model->asGeode()->getDrawable(0) || + !body_model->asGeode()->getDrawable(0)->asGeometry()) { + std::cerr << "model does not contain a mesh, cannot use bump mapping" << std::endl; return; } @@ -310,18 +318,25 @@ void RigidBodyStateVisualization::loadModel(std::string const& path) return; } - ref_ptr model = osgDB::readNodeFile(path); + // Creates an absolute file path. + QString qt_path(path.c_str()); + if (qt_path.startsWith ("~/")) { + qt_path.replace (0, 1, QDir::homePath()); + } + QDir dir(qt_path); + qt_path = dir.absolutePath(); + + // If the model cannot be opened, NULL will be returned. + ref_ptr model = osgDB::readNodeFile(qt_path.toStdString()); + if(model == NULL) { + return; + } + body_type = BODY_CUSTOM_MODEL; body_model = model; - if (!body_model->asGeode()) - std::cerr << "model is not a geode, using bump mapping will not be possible" << std::endl; - else if (!body_model->asGeode()->getDrawable(0)) - std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl; - else if (!body_model->asGeode()->getDrawable(0)->asGeometry()) - std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl; - - model_path = QString::fromStdString(path); - //set plugin name + model_path = qt_path; + + // Set plugin name. if(vizkit3d_plugin_name.isEmpty()) { size_t found; @@ -331,12 +346,14 @@ void RigidBodyStateVisualization::loadModel(std::string const& path) str = path; else str = path.substr(found+1); + found = str.find_last_of("."); if(found != std::string::npos) { str = str.substr(0,found); - if(!str.empty()) + if(!str.empty()) { setPluginName(QString::fromStdString(str)); + } } } @@ -408,10 +425,11 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) group->removeChildren(0, group->getNumChildren()); - if (!body_model) + if (!body_model) { resetModel(total_size); - if (texture_dirty) - updateTexture(); + } + //if (texture_dirty) + // updateTexture(); // Bump mapping not added yet, seems not to work anyway. //if (bump_mapping_dirty) // updateBumpMapping(); From 13d157fcbd4bd529e3b5e110ce20da8861232883 Mon Sep 17 00:00:00 2001 From: Stefan Haase Date: Fri, 27 Nov 2015 17:37:57 +0100 Subject: [PATCH 4/7] Readds showing source frame as text. --- viz/RigidBodyStateVisualization.cpp | 30 ++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index 4f187259..4daa0989 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -192,18 +192,6 @@ ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) ref_ptr spd = new ShapeDrawable(sp); spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); geode->addDrawable(spd); - /* - if(text_size > 0.0) - { - double actual_size = text_size * size; - ref_ptr text= new osgText::Text; - text->setText(state.sourceFrame); - text->setCharacterSize(actual_size); - text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0)); - geode->addDrawable(text); - } - */ - group->addChild(geode); //up @@ -428,8 +416,11 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) if (!body_model) { resetModel(total_size); } - //if (texture_dirty) + + //if (texture_dirty) { // updateTexture(); + //} + // Bump mapping not added yet, seems not to work anyway. //if (bump_mapping_dirty) // updateBumpMapping(); @@ -438,6 +429,19 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) for(it = states.begin(); it != states.end(); it++) { PositionAttitudeTransform* body_pose = new PositionAttitudeTransform(); + // Add texture. + if(text_size > 0.0 && !it->sourceFrame.empty()) + { + ref_ptr geode = new Geode(); + double actual_size = text_size * total_size; + ref_ptr text= new osgText::Text; + text->setText(it->sourceFrame); + text->setCharacterSize(actual_size); + text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0)); + geode->addDrawable(text); + body_pose->addChild(geode); + } + body_pose->addChild(body_model); group->addChild(body_pose); From 1c9ac50936c5522fa5aed4c5a68e199e16ba72ac Mon Sep 17 00:00:00 2001 From: Stefan Haase Date: Fri, 27 Nov 2015 18:43:22 +0100 Subject: [PATCH 5/7] Re-adds the possibility to load textures. Bump mapping still not added.. really required? --- viz/RigidBodyStateVisualization.cpp | 39 +++++++++++++++++++---------- viz/RigidBodyStateVisualization.hpp | 7 +++++- 2 files changed, 32 insertions(+), 14 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index 4daa0989..c214d8ce 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -71,9 +71,19 @@ void RigidBodyStateVisualization::setTexture(std::string const& path) { if (path.empty()) return clearTexture(); - - image = osgDB::readImageFile(path); + + QString qt_path = createAbsolutePath(path); + + image = osgDB::readImageFile(qt_path.toStdString()); + if(image == NULL) { + return; + } texture_dirty = true; + texture_path = path; +} + +QString RigidBodyStateVisualization::getTexture() const { + return QString(texture_path.c_str()); } void RigidBodyStateVisualization::clearTexture() @@ -149,7 +159,6 @@ void RigidBodyStateVisualization::updateBumpMapping() } else { - ref_ptr geometry = body_model->asGeode()->getDrawable(0)->asGeometry(); ref_ptr tex_coord = geometry->getTexCoordArray(0); geometry->setTexCoordArray(1, tex_coord); @@ -306,13 +315,7 @@ void RigidBodyStateVisualization::loadModel(std::string const& path) return; } - // Creates an absolute file path. - QString qt_path(path.c_str()); - if (qt_path.startsWith ("~/")) { - qt_path.replace (0, 1, QDir::homePath()); - } - QDir dir(qt_path); - qt_path = dir.absolutePath(); + QString qt_path = createAbsolutePath(path); // If the model cannot be opened, NULL will be returned. ref_ptr model = osgDB::readNodeFile(qt_path.toStdString()); @@ -417,9 +420,9 @@ void RigidBodyStateVisualization::updateMainNode(Node* node) resetModel(total_size); } - //if (texture_dirty) { - // updateTexture(); - //} + if (texture_dirty) { + updateTexture(); + } // Bump mapping not added yet, seems not to work anyway. //if (bump_mapping_dirty) @@ -499,4 +502,14 @@ void RigidBodyStateVisualization::updateDataIntern( const std::vectorstates = states; } +QString RigidBodyStateVisualization::createAbsolutePath(std::string const& path){ + QString qt_path(path.c_str()); + if (qt_path.startsWith ("~/")) { + qt_path.replace (0, 1, QDir::homePath()); + } + QDir dir(qt_path); + qt_path = dir.absolutePath(); + return qt_path; } + +} // end namespace vizkit3d \ No newline at end of file diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp index e51e4cb0..29e57877 100644 --- a/viz/RigidBodyStateVisualization.hpp +++ b/viz/RigidBodyStateVisualization.hpp @@ -28,6 +28,7 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin Date: Fri, 27 Nov 2015 19:08:58 +0100 Subject: [PATCH 6/7] Aligns the order of the methods within header and source file. --- viz/RigidBodyStateVisualization.cpp | 684 ++++++++++++++-------------- viz/RigidBodyStateVisualization.hpp | 19 +- 2 files changed, 353 insertions(+), 350 deletions(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index c214d8ce..9609571e 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -13,6 +13,7 @@ using namespace osg; namespace vizkit3d { +// PUBLIC RigidBodyStateVisualization::RigidBodyStateVisualization(QObject* parent) : Vizkit3DPlugin(parent) , states() @@ -37,18 +38,123 @@ RigidBodyStateVisualization::~RigidBodyStateVisualization() { } -void RigidBodyStateVisualization::setColor(const Vec4d& color, Geode* geode) +// PROTECTED +ref_ptr RigidBodyStateVisualization::createMainNode() { - Material *material = new Material(); - material->setDiffuse(Material::FRONT, Vec4(0.1, 0.1, 0.1, 1.0)); - material->setSpecular(Material::FRONT, Vec4(0.6, 0.6, 0.6, 1.0)); - material->setAmbient(Material::FRONT, Vec4(0.1, 0.1, 0.1, 1.0)); - material->setEmission(Material::FRONT, color); - material->setShininess(Material::FRONT, 10.0); + Group* group = new Group; - geode->getOrCreateStateSet()->setAttribute(material); + texture = new osg::Texture2D; + texture->setWrap(Texture::WRAP_S, Texture::REPEAT); + texture->setWrap(Texture::WRAP_T, Texture::REPEAT); + texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); + texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + diffuse_texture = new osg::Texture2D; + diffuse_texture->setWrap(Texture::WRAP_S, Texture::REPEAT); + diffuse_texture->setWrap(Texture::WRAP_T, Texture::REPEAT); + diffuse_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); + diffuse_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + normal_texture = new osg::Texture2D; + normal_texture->setWrap(Texture::WRAP_S, Texture::REPEAT); + normal_texture->setWrap(Texture::WRAP_T, Texture::REPEAT); + normal_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); + normal_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + + return group; +} + +void RigidBodyStateVisualization::updateMainNode(Node* node) +{ + Group* group = node->asGroup(); + + group->removeChildren(0, group->getNumChildren()); + + if (!body_model) { + resetModel(total_size); + } + + if (texture_dirty) { + updateTexture(); + } + + // Bump mapping not added yet, seems not to work anyway. + //if (bump_mapping_dirty) + // updateBumpMapping(); + + std::vector::iterator it; + for(it = states.begin(); it != states.end(); it++) { + PositionAttitudeTransform* body_pose = new PositionAttitudeTransform(); + + // Add texture. + if(text_size > 0.0 && !it->sourceFrame.empty()) + { + ref_ptr geode = new Geode(); + double actual_size = text_size * total_size; + ref_ptr text= new osgText::Text; + text->setText(it->sourceFrame); + text->setCharacterSize(actual_size); + text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0)); + geode->addDrawable(text); + body_pose->addChild(geode); + } + + body_pose->addChild(body_model); + group->addChild(body_pose); + + if (forcePositionDisplay || it->hasValidPosition()) { + osg::Vec3d pos(it->position.x(), it->position.y(), it->position.z()); + body_pose->setPosition(pos + translation); + } + + if (forceOrientationDisplay || it->hasValidOrientation()) { + osg::Quat orientation(it->orientation.x(), + it->orientation.y(), + it->orientation.z(), + it->orientation.w()); + body_pose->setAttitude(rotation * orientation); + } + + // Add uncertainties if required. + bool needs_uncertainty = covariance && it->hasValidPositionCovariance(); + if(needs_uncertainty) { + Uncertainty* uncertainty = new Uncertainty; + uncertainty->setMean(static_cast(it->position)); + uncertainty->setCovariance(static_cast(it->cov_position)); + group->addChild(uncertainty); + + if (covariance_with_samples) { + uncertainty->showSamples(); + } else { + uncertainty->hideSamples(); + } + } + } + + /* + // Reset the body model if needed + Node* body_node = body_pose->getChild(0); + if (bump_mapping && body_node != bump_mapping) + { + bump_mapping->addChild(body_model); + body_pose->setChild(0, bump_mapping); + } + else if (body_node != body_model) + body_pose->setChild(0, body_model); + */ +} + +void RigidBodyStateVisualization::updateDataIntern( const base::samples::RigidBodyState& state ) +{ + states.clear(); + states.resize(1); + states[0] = state; +} + +void RigidBodyStateVisualization::updateDataIntern( const std::vector& states ) +{ + this->states = states; } +// PUBLIC SLOTS bool RigidBodyStateVisualization::isPositionDisplayForced() const { return forcePositionDisplay; } @@ -64,177 +170,83 @@ void RigidBodyStateVisualization::setOrientationDisplayForceFlag(bool flag) emit propertyChanged("forceOrientationDisplay"); } -void RigidBodyStateVisualization::setTexture(QString const& path) -{ return setTexture(path.toStdString()); } - -void RigidBodyStateVisualization::setTexture(std::string const& path) +void RigidBodyStateVisualization::resetModel(double size) { - if (path.empty()) - return clearTexture(); - - QString qt_path = createAbsolutePath(path); - - image = osgDB::readImageFile(qt_path.toStdString()); - if(image == NULL) { - return; - } - texture_dirty = true; - texture_path = path; -} - -QString RigidBodyStateVisualization::getTexture() const { - return QString(texture_path.c_str()); + body_type = BODY_SIMPLE; + body_model = createSimpleBody(size); + setDirty(); } -void RigidBodyStateVisualization::clearTexture() +void RigidBodyStateVisualization::resetModelSphere(double size) { - image.release(); - texture_dirty = true; + body_type = BODY_SPHERE; + body_model = createSimpleSphere(size); + setDirty(); } -void RigidBodyStateVisualization::addBumpMapping( - QString const& diffuse_color_map_path, - QString const& normal_map_path) +QString RigidBodyStateVisualization::getModelPath() const { - return addBumpMapping(diffuse_color_map_path.toStdString(), - normal_map_path.toStdString()); + if (body_type == BODY_SPHERE) + return "sphere"; + else if (body_type == BODY_SIMPLE) + return "simple"; + else + return model_path; } - -void RigidBodyStateVisualization::addBumpMapping( - std::string const& diffuse_color_map_path, - std::string const& normal_map_path) -{ - if(body_model == NULL) { - return; - } - - if (!body_model->asGeode()) { - std::cerr << "model is not a geode, cannot use bump mapping" << std::endl; - return; - } else if ( !body_model->asGeode()->getDrawable(0) || - !body_model->asGeode()->getDrawable(0)->asGeometry()) { - std::cerr << "model does not contain a mesh, cannot use bump mapping" << std::endl; - return; - } - - // Setup the textures - diffuse_image = osgDB::readImageFile(diffuse_color_map_path); - normal_image = osgDB::readImageFile(normal_map_path); - // And add bump mapping - osgFX::BumpMapping* bump_mapping = new osgFX::BumpMapping(); - bump_mapping->setLightNumber(0); - bump_mapping->setNormalMapTextureUnit(1); - bump_mapping->setDiffuseTextureUnit(2); - this->bump_mapping = bump_mapping; - bump_mapping_dirty = true; +void RigidBodyStateVisualization::loadModel(QString const& path) +{ + return loadModel(path.toStdString()); } -void RigidBodyStateVisualization::updateTexture() +void RigidBodyStateVisualization::loadModel(std::string const& path) { - ref_ptr state = body_model->getOrCreateStateSet(); - if (!image) + if (path == "sphere") { - state->setTextureMode(0, GL_TEXTURE_2D, StateAttribute::OFF); + resetModelSphere(total_size); return; } - else + else if (path == "simple") { - texture->setImage(image.get()); - state->setTextureAttributeAndModes(0, texture, StateAttribute::ON); + resetModel(total_size); + return; } -} - -void RigidBodyStateVisualization::updateBumpMapping() -{ - ref_ptr state = body_model->getOrCreateStateSet(); - if (!bump_mapping) - { - diffuse_image.release(); - normal_image.release(); - state->setTextureMode(1, GL_TEXTURE_2D, StateAttribute::OFF); - state->setTextureMode(2, GL_TEXTURE_2D, StateAttribute::OFF); + QString qt_path = createAbsolutePath(path); + + // If the model cannot be opened, NULL will be returned. + ref_ptr model = osgDB::readNodeFile(qt_path.toStdString()); + if(model == NULL) { return; } - else + + body_type = BODY_CUSTOM_MODEL; + body_model = model; + model_path = qt_path; + + // Set plugin name. + if(vizkit3d_plugin_name.isEmpty()) { - ref_ptr geometry = body_model->asGeode()->getDrawable(0)->asGeometry(); - ref_ptr tex_coord = geometry->getTexCoordArray(0); - geometry->setTexCoordArray(1, tex_coord); - geometry->setTexCoordArray(2, tex_coord); - - diffuse_texture->setImage(diffuse_image.get()); - normal_texture->setImage(normal_image.get()); - state->setTextureAttributeAndModes(1, normal_texture, StateAttribute::ON); - state->setTextureAttributeAndModes(2, diffuse_texture, StateAttribute::ON); - bump_mapping->prepareChildren(); + size_t found; + std::string str; + found = path.find_last_of("/\\"); + if(found == std::string::npos) + str = path; + else + str = path.substr(found+1); + + found = str.find_last_of("."); + if(found != std::string::npos) + { + str = str.substr(0,found); + if(!str.empty()) { + setPluginName(QString::fromStdString(str)); + } + } } -} -void RigidBodyStateVisualization::removeBumpMapping() -{ - bump_mapping.release(); - bump_mapping_dirty = true; -} - -ref_ptr RigidBodyStateVisualization::createSimpleSphere(double size) -{ - ref_ptr group = new Group(); - - ref_ptr geode = new Geode(); - ref_ptr sp = new Sphere(Vec3f(0,0,0), main_size * size); - ref_ptr spd = new ShapeDrawable(sp); - spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); - geode->addDrawable(spd); - group->addChild(geode); - - return group; -} - -ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) -{ - ref_ptr group = new Group(); - - ref_ptr geode = new Geode(); - ref_ptr sp = new Sphere(Vec3f(0,0,0), main_size * size); - ref_ptr spd = new ShapeDrawable(sp); - spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); - geode->addDrawable(spd); - group->addChild(geode); - - //up - ref_ptr c1g = new Geode(); - ref_ptr c1 = new Cylinder(Vec3f(0, 0, size / 2), size / 40, size); - ref_ptr c1d = new ShapeDrawable(c1); - c1g->addDrawable(c1d); - setColor(Vec4f(0, 0, 1.0, 1.0), c1g); - group->addChild(c1g); - - //north direction - ref_ptr c2g = new Geode(); - ref_ptr c2 = new Cylinder(Vec3f(0, size / 2, 0), size / 40, size); - c2->setRotation(Quat(M_PI/2.0, Vec3d(1,0,0))); - ref_ptr c2d = new ShapeDrawable(c2); - c2g->addDrawable(c2d); - setColor(Vec4f(0.0, 1.0, 0, 1.0), c2g); - group->addChild(c2g); - - //east - ref_ptr c3g = new Geode(); - ref_ptr c3 = new Cylinder(Vec3f(size / 2, 0, 0), size / 40, size); - c3->setRotation(Quat(M_PI/2.0, Vec3d(0,1,0))); - ref_ptr c3d = new ShapeDrawable(c3); - c3g->addDrawable(c3d); - setColor(Vec4f(1.0, 0.0, 0, 1.0), c3g); - group->addChild(c3g); - - return group; -} - -double RigidBodyStateVisualization::getMainSphereSize() const -{ - return main_size; + setDirty(); + emit propertyChanged("modelPath"); } void RigidBodyStateVisualization::setMainSphereSize(double size) @@ -245,17 +257,9 @@ void RigidBodyStateVisualization::setMainSphereSize(double size) setSize(total_size); } -double RigidBodyStateVisualization::getTextSize() const -{ - return text_size; -} - -void RigidBodyStateVisualization::setTextSize(double size) +double RigidBodyStateVisualization::getMainSphereSize() const { - text_size = size; - emit propertyChanged("textSize"); - // This triggers an update of the model if we don't have a custom model - setSize(total_size); + return main_size; } void RigidBodyStateVisualization::setSize(double size) @@ -273,83 +277,119 @@ double RigidBodyStateVisualization::getSize() const return total_size; } -void RigidBodyStateVisualization::resetModel(double size) +void RigidBodyStateVisualization::setTextSize(double size) { - body_type = BODY_SIMPLE; - body_model = createSimpleBody(size); - setDirty(); + text_size = size; + emit propertyChanged("textSize"); + // This triggers an update of the model if we don't have a custom model + setSize(total_size); } -void RigidBodyStateVisualization::resetModelSphere(double size) +double RigidBodyStateVisualization::getTextSize() const { - body_type = BODY_SPHERE; - body_model = createSimpleSphere(size); - setDirty(); + return text_size; } -QString RigidBodyStateVisualization::getModelPath() const +void RigidBodyStateVisualization::displayCovariance(bool enable) +{ covariance = enable; emit propertyChanged("displayCovariance"); } + +bool RigidBodyStateVisualization::isCovarianceDisplayed() const +{ return covariance; } + +void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable) { - if (body_type == BODY_SPHERE) - return "sphere"; - else if (body_type == BODY_SIMPLE) - return "simple"; - else - return model_path; + covariance_with_samples = enable; + emit propertyChanged("displayCovarianceWithSamples"); } -void RigidBodyStateVisualization::loadModel(QString const& path) +bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const +{ return covariance_with_samples; } + +void RigidBodyStateVisualization::setColor(base::Vector3d const& color) +{ this->color = color; } + +void RigidBodyStateVisualization::setColor(const Vec4d& color, Geode* geode) { - return loadModel(path.toStdString()); + Material *material = new Material(); + material->setDiffuse(Material::FRONT, Vec4(0.1, 0.1, 0.1, 1.0)); + material->setSpecular(Material::FRONT, Vec4(0.6, 0.6, 0.6, 1.0)); + material->setAmbient(Material::FRONT, Vec4(0.1, 0.1, 0.1, 1.0)); + material->setEmission(Material::FRONT, color); + material->setShininess(Material::FRONT, 10.0); + + geode->getOrCreateStateSet()->setAttribute(material); } -void RigidBodyStateVisualization::loadModel(std::string const& path) +void RigidBodyStateVisualization::setTexture(QString const& path) +{ return setTexture(path.toStdString()); } + +void RigidBodyStateVisualization::setTexture(std::string const& path) { - if (path == "sphere") - { - resetModelSphere(total_size); - return; - } - else if (path == "simple") - { - resetModel(total_size); + if (path.empty()) + return clearTexture(); + + QString qt_path = createAbsolutePath(path); + + image = osgDB::readImageFile(qt_path.toStdString()); + if(image == NULL) { return; } + texture_dirty = true; + texture_path = path; +} - QString qt_path = createAbsolutePath(path); - - // If the model cannot be opened, NULL will be returned. - ref_ptr model = osgDB::readNodeFile(qt_path.toStdString()); - if(model == NULL) { +QString RigidBodyStateVisualization::getTexture() const { + return QString(texture_path.c_str()); +} + +void RigidBodyStateVisualization::clearTexture() +{ + image.release(); + texture_dirty = true; +} + +void RigidBodyStateVisualization::addBumpMapping( + QString const& diffuse_color_map_path, + QString const& normal_map_path) +{ + return addBumpMapping(diffuse_color_map_path.toStdString(), + normal_map_path.toStdString()); +} + +void RigidBodyStateVisualization::addBumpMapping( + std::string const& diffuse_color_map_path, + std::string const& normal_map_path) +{ + if(body_model == NULL) { return; } - - body_type = BODY_CUSTOM_MODEL; - body_model = model; - model_path = qt_path; - - // Set plugin name. - if(vizkit3d_plugin_name.isEmpty()) - { - size_t found; - std::string str; - found = path.find_last_of("/\\"); - if(found == std::string::npos) - str = path; - else - str = path.substr(found+1); - - found = str.find_last_of("."); - if(found != std::string::npos) - { - str = str.substr(0,found); - if(!str.empty()) { - setPluginName(QString::fromStdString(str)); - } - } + + if (!body_model->asGeode()) { + std::cerr << "model is not a geode, cannot use bump mapping" << std::endl; + return; + } else if ( !body_model->asGeode()->getDrawable(0) || + !body_model->asGeode()->getDrawable(0)->asGeometry()) { + std::cerr << "model does not contain a mesh, cannot use bump mapping" << std::endl; + return; } - setDirty(); - emit propertyChanged("modelPath"); + // Setup the textures + diffuse_image = osgDB::readImageFile(diffuse_color_map_path); + normal_image = osgDB::readImageFile(normal_map_path); + + // And add bump mapping + osgFX::BumpMapping* bump_mapping = new osgFX::BumpMapping(); + bump_mapping->setLightNumber(0); + bump_mapping->setNormalMapTextureUnit(1); + bump_mapping->setDiffuseTextureUnit(2); + this->bump_mapping = bump_mapping; + bump_mapping_dirty = true; +} + +void RigidBodyStateVisualization::removeBumpMapping() +{ + bump_mapping.release(); + bump_mapping_dirty = true; } QVector3D RigidBodyStateVisualization::getTranslation() const @@ -369,137 +409,101 @@ void RigidBodyStateVisualization::setRotation(QQuaternion const& q) setDirty(); } -void RigidBodyStateVisualization::displayCovariance(bool enable) -{ covariance = enable; emit propertyChanged("displayCovariance"); } - -bool RigidBodyStateVisualization::isCovarianceDisplayed() const -{ return covariance; } - -void RigidBodyStateVisualization::setColor(base::Vector3d const& color) -{ this->color = color; } - -void RigidBodyStateVisualization::displayCovarianceWithSamples(bool enable) -{ - covariance_with_samples = enable; - emit propertyChanged("displayCovarianceWithSamples"); +// PRIVATE +ref_ptr RigidBodyStateVisualization::createSimpleSphere(double size) +{ + ref_ptr group = new Group(); + + ref_ptr geode = new Geode(); + ref_ptr sp = new Sphere(Vec3f(0,0,0), main_size * size); + ref_ptr spd = new ShapeDrawable(sp); + spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); + geode->addDrawable(spd); + group->addChild(geode); + + return group; } + +ref_ptr RigidBodyStateVisualization::createSimpleBody(double size) +{ + ref_ptr group = new Group(); + + ref_ptr geode = new Geode(); + ref_ptr sp = new Sphere(Vec3f(0,0,0), main_size * size); + ref_ptr spd = new ShapeDrawable(sp); + spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); + geode->addDrawable(spd); + group->addChild(geode); + + //up + ref_ptr c1g = new Geode(); + ref_ptr c1 = new Cylinder(Vec3f(0, 0, size / 2), size / 40, size); + ref_ptr c1d = new ShapeDrawable(c1); + c1g->addDrawable(c1d); + setColor(Vec4f(0, 0, 1.0, 1.0), c1g); + group->addChild(c1g); + + //north direction + ref_ptr c2g = new Geode(); + ref_ptr c2 = new Cylinder(Vec3f(0, size / 2, 0), size / 40, size); + c2->setRotation(Quat(M_PI/2.0, Vec3d(1,0,0))); + ref_ptr c2d = new ShapeDrawable(c2); + c2g->addDrawable(c2d); + setColor(Vec4f(0.0, 1.0, 0, 1.0), c2g); + group->addChild(c2g); -bool RigidBodyStateVisualization::isCovarianceDisplayedWithSamples() const -{ return covariance_with_samples; } - -ref_ptr RigidBodyStateVisualization::createMainNode() -{ - Group* group = new Group; - - texture = new osg::Texture2D; - texture->setWrap(Texture::WRAP_S, Texture::REPEAT); - texture->setWrap(Texture::WRAP_T, Texture::REPEAT); - texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); - texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); - diffuse_texture = new osg::Texture2D; - diffuse_texture->setWrap(Texture::WRAP_S, Texture::REPEAT); - diffuse_texture->setWrap(Texture::WRAP_T, Texture::REPEAT); - diffuse_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); - diffuse_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); - normal_texture = new osg::Texture2D; - normal_texture->setWrap(Texture::WRAP_S, Texture::REPEAT); - normal_texture->setWrap(Texture::WRAP_T, Texture::REPEAT); - normal_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); - normal_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + //east + ref_ptr c3g = new Geode(); + ref_ptr c3 = new Cylinder(Vec3f(size / 2, 0, 0), size / 40, size); + c3->setRotation(Quat(M_PI/2.0, Vec3d(0,1,0))); + ref_ptr c3d = new ShapeDrawable(c3); + c3g->addDrawable(c3d); + setColor(Vec4f(1.0, 0.0, 0, 1.0), c3g); + group->addChild(c3g); return group; } -void RigidBodyStateVisualization::updateMainNode(Node* node) +void RigidBodyStateVisualization::updateTexture() { - Group* group = node->asGroup(); - - group->removeChildren(0, group->getNumChildren()); - - if (!body_model) { - resetModel(total_size); - } - - if (texture_dirty) { - updateTexture(); - } - - // Bump mapping not added yet, seems not to work anyway. - //if (bump_mapping_dirty) - // updateBumpMapping(); - - std::vector::iterator it; - for(it = states.begin(); it != states.end(); it++) { - PositionAttitudeTransform* body_pose = new PositionAttitudeTransform(); - - // Add texture. - if(text_size > 0.0 && !it->sourceFrame.empty()) - { - ref_ptr geode = new Geode(); - double actual_size = text_size * total_size; - ref_ptr text= new osgText::Text; - text->setText(it->sourceFrame); - text->setCharacterSize(actual_size); - text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0)); - geode->addDrawable(text); - body_pose->addChild(geode); - } - - body_pose->addChild(body_model); - group->addChild(body_pose); - - if (forcePositionDisplay || it->hasValidPosition()) { - osg::Vec3d pos(it->position.x(), it->position.y(), it->position.z()); - body_pose->setPosition(pos + translation); - } - - if (forceOrientationDisplay || it->hasValidOrientation()) { - osg::Quat orientation(it->orientation.x(), - it->orientation.y(), - it->orientation.z(), - it->orientation.w()); - body_pose->setAttitude(rotation * orientation); - } - - // Add uncertainties if required. - bool needs_uncertainty = covariance && it->hasValidPositionCovariance(); - if(needs_uncertainty) { - Uncertainty* uncertainty = new Uncertainty; - uncertainty->setMean(static_cast(it->position)); - uncertainty->setCovariance(static_cast(it->cov_position)); - group->addChild(uncertainty); - - if (covariance_with_samples) { - uncertainty->showSamples(); - } else { - uncertainty->hideSamples(); - } - } + ref_ptr state = body_model->getOrCreateStateSet(); + if (!image) + { + state->setTextureMode(0, GL_TEXTURE_2D, StateAttribute::OFF); + return; } - - /* - // Reset the body model if needed - Node* body_node = body_pose->getChild(0); - if (bump_mapping && body_node != bump_mapping) + else { - bump_mapping->addChild(body_model); - body_pose->setChild(0, bump_mapping); + texture->setImage(image.get()); + state->setTextureAttributeAndModes(0, texture, StateAttribute::ON); } - else if (body_node != body_model) - body_pose->setChild(0, body_model); - */ } -void RigidBodyStateVisualization::updateDataIntern( const base::samples::RigidBodyState& state ) +void RigidBodyStateVisualization::updateBumpMapping() { - states.clear(); - states.resize(1); - states[0] = state; -} + ref_ptr state = body_model->getOrCreateStateSet(); -void RigidBodyStateVisualization::updateDataIntern( const std::vector& states ) -{ - this->states = states; + if (!bump_mapping) + { + diffuse_image.release(); + normal_image.release(); + state->setTextureMode(1, GL_TEXTURE_2D, StateAttribute::OFF); + state->setTextureMode(2, GL_TEXTURE_2D, StateAttribute::OFF); + return; + } + else + { + ref_ptr geometry = body_model->asGeode()->getDrawable(0)->asGeometry(); + ref_ptr tex_coord = geometry->getTexCoordArray(0); + geometry->setTexCoordArray(1, tex_coord); + geometry->setTexCoordArray(2, tex_coord); + + diffuse_texture->setImage(diffuse_image.get()); + normal_texture->setImage(normal_image.get()); + state->setTextureAttributeAndModes(1, normal_texture, StateAttribute::ON); + state->setTextureAttributeAndModes(2, diffuse_texture, StateAttribute::ON); + bump_mapping->prepareChildren(); + } } QString RigidBodyStateVisualization::createAbsolutePath(std::string const& path){ diff --git a/viz/RigidBodyStateVisualization.hpp b/viz/RigidBodyStateVisualization.hpp index 29e57877..69f6c2e7 100644 --- a/viz/RigidBodyStateVisualization.hpp +++ b/viz/RigidBodyStateVisualization.hpp @@ -55,9 +55,6 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin body_model; - osg::ref_ptr createSimpleBody(double size); - osg::ref_ptr createSimpleSphere(double size); osg::ref_ptr image; osg::ref_ptr texture; bool texture_dirty; - void updateTexture(); osg::ref_ptr diffuse_image; osg::ref_ptr normal_image; @@ -149,7 +145,6 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin normal_texture; osg::ref_ptr bump_mapping; bool bump_mapping_dirty; - void updateBumpMapping(); bool forcePositionDisplay; bool forceOrientationDisplay; @@ -157,6 +152,10 @@ class RigidBodyStateVisualization : public Vizkit3DPlugin createSimpleBody(double size); + osg::ref_ptr createSimpleSphere(double size); + void updateTexture(); + void updateBumpMapping(); QString createAbsolutePath(std::string const& path); }; From f3bf7c8c26da72726c5b96c3bcc76c524dd326b1 Mon Sep 17 00:00:00 2001 From: Stefan Haase Date: Fri, 27 Nov 2015 19:14:49 +0100 Subject: [PATCH 7/7] Resets model to simple if an empty path is passed. --- viz/RigidBodyStateVisualization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/viz/RigidBodyStateVisualization.cpp b/viz/RigidBodyStateVisualization.cpp index 9609571e..e7b7684c 100644 --- a/viz/RigidBodyStateVisualization.cpp +++ b/viz/RigidBodyStateVisualization.cpp @@ -206,7 +206,7 @@ void RigidBodyStateVisualization::loadModel(std::string const& path) resetModelSphere(total_size); return; } - else if (path == "simple") + else if (path == "simple" || path.empty()) { resetModel(total_size); return;