robot_link.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <boost/filesystem.hpp>
00031 
00032 #include <OgreEntity.h>
00033 #include <OgreMaterial.h>
00034 #include <OgreMaterialManager.h>
00035 #include <OgreRibbonTrail.h>
00036 #include <OgreSceneManager.h>
00037 #include <OgreSceneNode.h>
00038 #include <OgreSubEntity.h>
00039 #include <OgreTextureManager.h>
00040 #include <OgreSharedPtr.h>
00041 #include <OgreTechnique.h>
00042 
00043 #include <ros/console.h>
00044 
00045 #include <resource_retriever/retriever.h>
00046 
00047 #include <urdf_model/model.h>
00048 #include <urdf_model/link.h>
00049 
00050 #include "rviz/mesh_loader.h"
00051 #include "rviz/ogre_helpers/axes.h"
00052 #include "rviz/ogre_helpers/object.h"
00053 #include "rviz/ogre_helpers/shape.h"
00054 #include "rviz/properties/float_property.h"
00055 #include "rviz/properties/bool_property.h"
00056 #include "rviz/properties/property.h"
00057 #include "rviz/properties/quaternion_property.h"
00058 #include "rviz/properties/vector_property.h"
00059 #include "rviz/robot/robot.h"
00060 #include "rviz/selection/selection_manager.h"
00061 #include "rviz/visualization_manager.h"
00062 #include "rviz/load_resource.h"
00063 
00064 #include "rviz/robot/robot_link.h"
00065 #include "rviz/robot/robot_joint.h"
00066 
00067 namespace fs=boost::filesystem;
00068 
00069 #ifndef ROS_PACKAGE_NAME
00070 # define ROS_PACKAGE_NAME "rviz"
00071 #endif
00072 
00073 namespace rviz
00074 {
00075 
00076 class RobotLinkSelectionHandler : public SelectionHandler
00077 {
00078 public:
00079   RobotLinkSelectionHandler( RobotLink* link, DisplayContext* context );
00080   virtual ~RobotLinkSelectionHandler();
00081 
00082   virtual void createProperties( const Picked& obj, Property* parent_property );
00083   virtual void updateProperties();
00084 
00085   virtual void preRenderPass(uint32_t pass);
00086   virtual void postRenderPass(uint32_t pass);
00087 
00088 private:
00089   RobotLink* link_;
00090   VectorProperty* position_property_;
00091   QuaternionProperty* orientation_property_;
00092 };
00093 
00094 RobotLinkSelectionHandler::RobotLinkSelectionHandler( RobotLink* link, DisplayContext* context )
00095   : SelectionHandler( context )
00096   , link_( link )
00097 {
00098 }
00099 
00100 RobotLinkSelectionHandler::~RobotLinkSelectionHandler()
00101 {
00102 }
00103 
00104 void RobotLinkSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
00105 {
00106   Property* group = new Property( "Link " + QString::fromStdString( link_->getName() ), QVariant(), "", parent_property );
00107   properties_.push_back( group );
00108 
00109   position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", group );
00110   position_property_->setReadOnly( true );
00111 
00112   orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", group );
00113   orientation_property_->setReadOnly( true );
00114 
00115   group->expand();
00116 }
00117 
00118 void RobotLinkSelectionHandler::updateProperties()
00119 {
00120   position_property_->setVector( link_->getPosition() );
00121   orientation_property_->setQuaternion( link_->getOrientation() );
00122 }
00123 
00124 
00125 void RobotLinkSelectionHandler::preRenderPass(uint32_t pass)
00126 {
00127   if (!link_->is_selectable_)
00128   {
00129     if( link_->visual_node_ )
00130     {
00131       link_->visual_node_->setVisible( false );
00132     }
00133     if( link_->collision_node_ )
00134     {
00135       link_->collision_node_->setVisible( false );
00136     }
00137     if( link_->trail_ )
00138     {
00139       link_->trail_->setVisible( false );
00140     }
00141     if( link_->axes_ )
00142     {
00143       link_->axes_->getSceneNode()->setVisible( false );
00144     }
00145   }
00146 }
00147 
00148 void RobotLinkSelectionHandler::postRenderPass(uint32_t pass)
00149 {
00150   if (!link_->is_selectable_)
00151   {
00152     link_->updateVisibility();
00153   }
00154 }
00155 
00156 
00157 RobotLink::RobotLink( Robot* robot,
00158                       const urdf::LinkConstSharedPtr& link,
00159                       const std::string& parent_joint_name,
00160                       bool visual,
00161                       bool collision)
00162 : robot_( robot )
00163 , scene_manager_( robot->getDisplayContext()->getSceneManager() )
00164 , context_( robot->getDisplayContext() )
00165 , name_( link->name )
00166 , parent_joint_name_( parent_joint_name )
00167 , visual_node_( NULL )
00168 , collision_node_( NULL )
00169 , trail_( NULL )
00170 , axes_( NULL )
00171 , material_alpha_( 1.0 )
00172 , robot_alpha_(1.0)
00173 , only_render_depth_(false)
00174 , using_color_( false )
00175 , is_selectable_( true )
00176 {
00177   link_property_ = new Property( link->name.c_str(), true, "", NULL, SLOT( updateVisibility() ), this );
00178   link_property_->setIcon( rviz::loadPixmap( "package://rviz/icons/classes/RobotLink.png" ) );
00179 
00180   details_ = new Property( "Details", QVariant(), "", NULL);
00181 
00182   alpha_property_ = new FloatProperty( "Alpha", 1,
00183                                        "Amount of transparency to apply to this link.",
00184                                        link_property_, SLOT( updateAlpha() ), this );
00185 
00186   trail_property_ = new Property( "Show Trail", false,
00187                                   "Enable/disable a 2 meter \"ribbon\" which follows this link.",
00188                                   link_property_, SLOT( updateTrail() ), this );
00189 
00190   axes_property_ = new Property( "Show Axes", false,
00191                                  "Enable/disable showing the axes of this link.",
00192                                  link_property_, SLOT( updateAxes() ), this );
00193 
00194   position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
00195                                            "Position of this link, in the current Fixed Frame.  (Not editable)",
00196                                            link_property_ );
00197   position_property_->setReadOnly( true );
00198 
00199   orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
00200                                                   "Orientation of this link, in the current Fixed Frame.  (Not editable)",
00201                                                   link_property_ );
00202   orientation_property_->setReadOnly( true );
00203 
00204   link_property_->collapse();
00205 
00206   visual_node_ = robot_->getVisualNode()->createChildSceneNode();
00207   collision_node_ = robot_->getCollisionNode()->createChildSceneNode();
00208 
00209   // create material for coloring links
00210   std::stringstream ss;
00211   static int count = 1;
00212   ss << "robot link color material " << count++;
00213   color_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), ROS_PACKAGE_NAME );
00214   color_material_->setReceiveShadows(false);
00215   color_material_->getTechnique(0)->setLightingEnabled(true);
00216 
00217   // create the ogre objects to display
00218 
00219   if ( visual )
00220   {
00221     createVisual( link );
00222   }
00223 
00224   if ( collision )
00225   {
00226     createCollision( link );
00227   }
00228 
00229   if (collision || visual)
00230   {
00231     createSelection();
00232   }
00233 
00234   // create description and fill in child_joint_names_ vector
00235   std::stringstream desc;
00236   if (parent_joint_name_.empty())
00237   {
00238     desc << "Root Link <b>" << name_ << "</b>";
00239   }
00240   else
00241   {
00242     desc << "Link <b>" << name_ << "</b>";
00243     desc << " with parent joint <b>" << parent_joint_name_ << "</b>";
00244   }
00245 
00246   if (link->child_joints.empty())
00247   {
00248     desc << " has no children.";
00249   }
00250   else
00251   {
00252     desc
00253       << " has " 
00254       << link->child_joints.size();
00255 
00256     if (link->child_joints.size() > 1)
00257     {
00258       desc << " child joints: ";
00259     }
00260     else
00261     {
00262       desc << " child joint: ";
00263     }
00264 
00265     std::vector<urdf::JointSharedPtr >::const_iterator child_it = link->child_joints.begin();
00266     std::vector<urdf::JointSharedPtr >::const_iterator child_end = link->child_joints.end();
00267     for ( ; child_it != child_end ; ++child_it )
00268     {
00269       urdf::Joint *child_joint = child_it->get();
00270       if (child_joint && !child_joint->name.empty())
00271       {
00272         child_joint_names_.push_back(child_joint->name);
00273         desc << "<b>" << child_joint->name << "</b>" << ((child_it+1 == child_end) ? "." : ", ");
00274       }
00275     }
00276   }
00277   if (hasGeometry())
00278   {
00279     desc << "  Check/uncheck to show/hide this link in the display.";
00280     if (visual_meshes_.empty())
00281     {
00282       desc << "  This link has collision geometry but no visible geometry.";
00283     }
00284     else if (collision_meshes_.empty())
00285     {
00286       desc << "  This link has visible geometry but no collision geometry.";
00287     }
00288   }
00289   else
00290   {
00291     desc << "  This link has NO geometry.";
00292   }
00293 
00294   link_property_->setDescription(desc.str().c_str());
00295   
00296   if (!hasGeometry())
00297   {
00298     link_property_->setIcon( rviz::loadPixmap( "package://rviz/icons/classes/RobotLinkNoGeom.png" ) );
00299     alpha_property_->hide();
00300     link_property_->setValue(QVariant());
00301   }
00302 }
00303 
00304 RobotLink::~RobotLink()
00305 {
00306   for( size_t i = 0; i < visual_meshes_.size(); i++ )
00307   {
00308     scene_manager_->destroyEntity( visual_meshes_[ i ]);
00309   }
00310 
00311   for( size_t i = 0; i < collision_meshes_.size(); i++ )
00312   {
00313     scene_manager_->destroyEntity( collision_meshes_[ i ]);
00314   }
00315 
00316   scene_manager_->destroySceneNode( visual_node_ );
00317   scene_manager_->destroySceneNode( collision_node_ );
00318 
00319   if ( trail_ )
00320   {
00321     scene_manager_->destroyRibbonTrail( trail_ );
00322   }
00323 
00324   delete axes_;
00325   delete details_;
00326   delete link_property_;
00327 }
00328 
00329 bool RobotLink::hasGeometry() const
00330 {
00331   return visual_meshes_.size() + collision_meshes_.size() > 0;
00332 }
00333 
00334 bool RobotLink::getEnabled() const
00335 {
00336   if (!hasGeometry())
00337     return true;
00338   return link_property_->getValue().toBool();
00339 }
00340 
00341 void RobotLink::setRobotAlpha( float a )
00342 {
00343   robot_alpha_ = a;
00344   updateAlpha();
00345 }
00346 
00347 void RobotLink::setRenderQueueGroup( Ogre::uint8 group )
00348 {
00349   Ogre::SceneNode::ChildNodeIterator child_it = visual_node_->getChildIterator();
00350   while( child_it.hasMoreElements() )
00351   {
00352     Ogre::SceneNode* child = dynamic_cast<Ogre::SceneNode*>( child_it.getNext() );
00353     if( child )
00354     {
00355       Ogre::SceneNode::ObjectIterator object_it = child->getAttachedObjectIterator();
00356       while( object_it.hasMoreElements() )
00357       {
00358         Ogre::MovableObject* obj = object_it.getNext();
00359         obj->setRenderQueueGroup(group);
00360       }
00361     }
00362   }
00363 }
00364 
00365 void RobotLink::setOnlyRenderDepth(bool onlyRenderDepth)
00366 {
00367   setRenderQueueGroup( onlyRenderDepth ? Ogre::RENDER_QUEUE_BACKGROUND : Ogre::RENDER_QUEUE_MAIN );
00368   only_render_depth_ = onlyRenderDepth;
00369   updateAlpha();
00370 }
00371 
00372 void RobotLink::updateAlpha()
00373 {
00374   float link_alpha = alpha_property_->getFloat();
00375   M_SubEntityToMaterial::iterator it = materials_.begin();
00376   M_SubEntityToMaterial::iterator end = materials_.end();
00377   for (; it != end; ++it)
00378   {
00379     const Ogre::MaterialPtr& material = it->second;
00380 
00381     if ( only_render_depth_ )
00382     {
00383       material->setColourWriteEnabled( false );
00384       material->setDepthWriteEnabled( true );
00385     }
00386     else
00387     {
00388       Ogre::ColourValue color = material->getTechnique(0)->getPass(0)->getDiffuse();
00389       color.a = robot_alpha_ * material_alpha_ * link_alpha;
00390       material->setDiffuse( color );
00391 
00392       if ( color.a < 0.9998 )
00393       {
00394         material->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00395         material->setDepthWriteEnabled( false );
00396       }
00397       else
00398       {
00399         material->setSceneBlending( Ogre::SBT_REPLACE );
00400         material->setDepthWriteEnabled( true );
00401       }
00402     }
00403   }
00404 
00405   Ogre::ColourValue color = color_material_->getTechnique(0)->getPass(0)->getDiffuse();
00406   color.a = robot_alpha_ * link_alpha;
00407   color_material_->setDiffuse( color );
00408 
00409   if ( color.a < 0.9998 )
00410   {
00411     color_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00412     color_material_->setDepthWriteEnabled( false );
00413   }
00414   else
00415   {
00416     color_material_->setSceneBlending( Ogre::SBT_REPLACE );
00417     color_material_->setDepthWriteEnabled( true );
00418   }
00419 }
00420 
00421 void RobotLink::updateVisibility()
00422 {
00423   bool enabled = getEnabled();
00424 
00425   robot_->calculateJointCheckboxes();
00426 
00427   if( visual_node_ )
00428   {
00429     visual_node_->setVisible( enabled && robot_->isVisible() && robot_->isVisualVisible() );
00430   }
00431   if( collision_node_ )
00432   {
00433     collision_node_->setVisible( enabled && robot_->isVisible() && robot_->isCollisionVisible() );
00434   }
00435   if( trail_ )
00436   {
00437     trail_->setVisible( enabled && robot_->isVisible() );
00438   }
00439   if( axes_ )
00440   {
00441     axes_->getSceneNode()->setVisible( enabled && robot_->isVisible() );
00442   }
00443 }
00444 
00445 Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr& link, const std::string material_name)
00446 {
00447   if (!link->visual || !link->visual->material)
00448   {
00449     return Ogre::MaterialManager::getSingleton().getByName("RVIZ/ShadedRed");
00450   }
00451 
00452   static int count = 0;
00453   std::stringstream ss;
00454   ss << "Robot Link Material" << count++;
00455 
00456   Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME);
00457   mat->getTechnique(0)->setLightingEnabled(true);
00458 
00459   urdf::VisualSharedPtr visual = link->visual;
00460   std::vector<urdf::VisualSharedPtr>::const_iterator vi;
00461   for( vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++ )
00462   {
00463     if( (*vi) && material_name != "" && (*vi)->material_name  == material_name) {
00464       visual = *vi;
00465       break;
00466     }
00467   }
00468   if ( vi == link->visual_array.end() ) {
00469     visual = link->visual; // if link does not have material, use default oneee
00470   }
00471 
00472   if (visual->material->texture_filename.empty())
00473   {
00474     const urdf::Color& col = visual->material->color;
00475     mat->getTechnique(0)->setAmbient(col.r * 0.5, col.g * 0.5, col.b * 0.5);
00476     mat->getTechnique(0)->setDiffuse(col.r, col.g, col.b, col.a);
00477 
00478     material_alpha_ = col.a;
00479   }
00480   else
00481   {
00482     std::string filename = visual->material->texture_filename;
00483     if (!Ogre::TextureManager::getSingleton().resourceExists(filename))
00484     {
00485       resource_retriever::Retriever retriever;
00486       resource_retriever::MemoryResource res;
00487       try
00488       {
00489         res = retriever.get(filename);
00490       }
00491       catch (resource_retriever::Exception& e)
00492       {
00493         ROS_ERROR("%s", e.what());
00494       }
00495 
00496       if (res.size != 0)
00497       {
00498         Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
00499         Ogre::Image image;
00500         std::string extension = fs::extension(fs::path(filename));
00501 
00502         if (extension[0] == '.')
00503         {
00504           extension = extension.substr(1, extension.size() - 1);
00505         }
00506 
00507         try
00508         {
00509           image.load(stream, extension);
00510           Ogre::TextureManager::getSingleton().loadImage(filename, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00511         }
00512         catch (Ogre::Exception& e)
00513         {
00514           ROS_ERROR("Could not load texture [%s]: %s", filename.c_str(), e.what());
00515         }
00516       }
00517     }
00518 
00519     Ogre::Pass* pass = mat->getTechnique(0)->getPass(0);
00520     Ogre::TextureUnitState* tex_unit = pass->createTextureUnitState();;
00521     tex_unit->setTextureName(filename);
00522   }
00523 
00524   return mat;
00525 }
00526 
00527 void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, const std::string material_name, Ogre::SceneNode* scene_node, Ogre::Entity*& entity)
00528 {
00529   entity = NULL; // default in case nothing works.
00530   Ogre::SceneNode* offset_node = scene_node->createChildSceneNode();
00531 
00532   static int count = 0;
00533   std::stringstream ss;
00534   ss << "Robot Link" << count++;
00535   std::string entity_name = ss.str();
00536 
00537   Ogre::Vector3 scale(Ogre::Vector3::UNIT_SCALE);
00538 
00539   Ogre::Vector3 offset_position(Ogre::Vector3::ZERO);
00540   Ogre::Quaternion offset_orientation(Ogre::Quaternion::IDENTITY);
00541 
00542 
00543   {
00544     Ogre::Vector3 position( origin.position.x, origin.position.y, origin.position.z );
00545     Ogre::Quaternion orientation( Ogre::Quaternion::IDENTITY );
00546     orientation = orientation * Ogre::Quaternion( origin.rotation.w, origin.rotation.x, origin.rotation.y, origin.rotation.z  );
00547 
00548     offset_position = position;
00549     offset_orientation = orientation;
00550   }
00551 
00552   switch (geom.type)
00553   {
00554   case urdf::Geometry::SPHERE:
00555   {
00556     const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
00557     entity = Shape::createEntity(entity_name, Shape::Sphere, scene_manager_);
00558 
00559     scale = Ogre::Vector3( sphere.radius*2, sphere.radius*2, sphere.radius*2 );
00560     break;
00561   }
00562   case urdf::Geometry::BOX:
00563   {
00564     const urdf::Box& box = static_cast<const urdf::Box&>(geom);
00565     entity = Shape::createEntity(entity_name, Shape::Cube, scene_manager_);
00566 
00567     scale = Ogre::Vector3( box.dim.x, box.dim.y, box.dim.z );
00568 
00569     break;
00570   }
00571   case urdf::Geometry::CYLINDER:
00572   {
00573     const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
00574 
00575     Ogre::Quaternion rotX;
00576     rotX.FromAngleAxis( Ogre::Degree(90), Ogre::Vector3::UNIT_X );
00577     offset_orientation = offset_orientation * rotX;
00578 
00579     entity = Shape::createEntity(entity_name, Shape::Cylinder, scene_manager_);
00580     scale = Ogre::Vector3( cylinder.radius*2, cylinder.length, cylinder.radius*2 );
00581     break;
00582   }
00583   case urdf::Geometry::MESH:
00584   {
00585     const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
00586 
00587     if ( mesh.filename.empty() )
00588       return;
00589 
00590     
00591     
00592 
00593     scale = Ogre::Vector3(mesh.scale.x, mesh.scale.y, mesh.scale.z);
00594     
00595     std::string model_name = mesh.filename;
00596     
00597     try
00598     {
00599       loadMeshFromResource(model_name);
00600       entity = scene_manager_->createEntity( ss.str(), model_name );
00601     }
00602     catch( Ogre::InvalidParametersException& e )
00603     {
00604       ROS_ERROR( "Could not convert mesh resource '%s' for link '%s'. It might be an empty mesh: %s", model_name.c_str(), link->name.c_str(), e.what() );
00605     }
00606     catch( Ogre::Exception& e )
00607     {
00608       ROS_ERROR( "Could not load model '%s' for link '%s': %s", model_name.c_str(), link->name.c_str(), e.what() );
00609     }
00610     break;
00611   }
00612   default:
00613     ROS_WARN("Unsupported geometry type for element: %d", geom.type);
00614     break;
00615   }
00616 
00617   if ( entity )
00618   {
00619     offset_node->attachObject(entity);
00620     offset_node->setScale(scale);
00621     offset_node->setPosition(offset_position);
00622     offset_node->setOrientation(offset_orientation);
00623 
00624     if (default_material_name_.empty())
00625     {
00626       default_material_ = getMaterialForLink(link);
00627 
00628       static int count = 0;
00629       std::stringstream ss;
00630       ss << default_material_->getName() << count++ << "Robot";
00631       std::string cloned_name = ss.str();
00632 
00633       default_material_ = default_material_->clone(cloned_name);
00634       default_material_name_ = default_material_->getName();
00635     }
00636 
00637     for (uint32_t i = 0; i < entity->getNumSubEntities(); ++i)
00638     {
00639       default_material_ = getMaterialForLink(link, material_name);
00640       static int count = 0;
00641       std::stringstream ss;
00642       ss << default_material_->getName() << count++ << "Robot";
00643       std::string cloned_name = ss.str();
00644 
00645       default_material_ = default_material_->clone(cloned_name);
00646       default_material_name_ = default_material_->getName();
00647 
00648       // Assign materials only if the submesh does not have one already
00649 
00650       Ogre::SubEntity* sub = entity->getSubEntity(i);
00651       const std::string& material_name = sub->getMaterialName();
00652 
00653       if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting")
00654       {
00655         sub->setMaterialName(default_material_name_);
00656       }
00657       else
00658       {
00659         // Need to clone here due to how selection works.  Once selection id is done per object and not per material,
00660         // this can go away
00661         static int count = 0;
00662         std::stringstream ss;
00663         ss << material_name << count++ << "Robot";
00664         std::string cloned_name = ss.str();
00665         sub->getMaterial()->clone(cloned_name);
00666         sub->setMaterialName(cloned_name);
00667       }
00668 
00669       materials_[sub] = sub->getMaterial();
00670     }
00671   }
00672 }
00673 
00674 void RobotLink::createCollision(const urdf::LinkConstSharedPtr& link)
00675 {
00676   bool valid_collision_found = false;
00677 #if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
00678   std::map<std::string, boost::shared_ptr<std::vector<urdf::CollisionSharedPtr > > >::const_iterator mi;
00679   for( mi = link->collision_groups.begin(); mi != link->collision_groups.end(); mi++ )
00680   {
00681     if( mi->second )
00682     {
00683       std::vector<urdf::CollisionSharedPtr >::const_iterator vi;
00684       for( vi = mi->second->begin(); vi != mi->second->end(); vi++ )
00685       {
00686         urdf::CollisionSharedPtr collision = *vi;
00687         if( collision && collision->geometry )
00688         {
00689           Ogre::Entity* collision_mesh = NULL;
00690           createEntityForGeometryElement( link, *collision->geometry, collision->origin, collision_node_, collision_mesh );
00691           if( collision_mesh )
00692           {
00693             collision_meshes_.push_back( collision_mesh );
00694             valid_collision_found = true;
00695           }
00696         }
00697       }
00698     }
00699   }
00700 #else
00701   std::vector<urdf::CollisionSharedPtr >::const_iterator vi;
00702   for( vi = link->collision_array.begin(); vi != link->collision_array.end(); vi++ )
00703   {
00704     urdf::CollisionSharedPtr collision = *vi;
00705     if( collision && collision->geometry )
00706     {
00707       Ogre::Entity* collision_mesh = NULL;
00708       createEntityForGeometryElement( link, *collision->geometry, collision->origin, "", collision_node_, collision_mesh );
00709       if( collision_mesh )
00710       {
00711         collision_meshes_.push_back( collision_mesh );
00712         valid_collision_found = true;
00713       }
00714     }
00715   }
00716 #endif
00717 
00718   if( !valid_collision_found && link->collision && link->collision->geometry )
00719   {
00720     Ogre::Entity* collision_mesh = NULL;
00721     createEntityForGeometryElement( link, *link->collision->geometry, link->collision->origin, "", collision_node_, collision_mesh );
00722     if( collision_mesh )
00723     {
00724       collision_meshes_.push_back( collision_mesh );
00725     }
00726   }
00727 
00728   collision_node_->setVisible( getEnabled() );
00729 }
00730 
00731 void RobotLink::createVisual(const urdf::LinkConstSharedPtr& link )
00732 {
00733   bool valid_visual_found = false;
00734 #if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
00735   std::map<std::string, boost::shared_ptr<std::vector<urdf::VisualSharedPtr > > >::const_iterator mi;
00736   for( mi = link->visual_groups.begin(); mi != link->visual_groups.end(); mi++ )
00737   {
00738     if( mi->second )
00739     {
00740       std::vector<urdf::VisualSharedPtr >::const_iterator vi;
00741       for( vi = mi->second->begin(); vi != mi->second->end(); vi++ )
00742       {
00743         urdf::VisualSharedPtr visual = *vi;
00744         if( visual && visual->geometry )
00745         {
00746           Ogre::Entity* visual_mesh = NULL;
00747           createEntityForGeometryElement( link, *visual->geometry, visual->origin, visual_node_, visual_mesh );
00748           if( visual_mesh )
00749           {
00750             visual_meshes_.push_back( visual_mesh );
00751             valid_visual_found = true;
00752           }
00753         }
00754       }
00755     }
00756   }
00757 #else
00758   std::vector<urdf::VisualSharedPtr >::const_iterator vi;
00759   for( vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++ )
00760   {
00761     urdf::VisualSharedPtr visual = *vi;
00762     if( visual && visual->geometry )
00763     {
00764       Ogre::Entity* visual_mesh = NULL;
00765       createEntityForGeometryElement( link, *visual->geometry, visual->origin, visual->material_name, visual_node_, visual_mesh );
00766       if( visual_mesh )
00767       {
00768         visual_meshes_.push_back( visual_mesh );
00769         valid_visual_found = true;
00770       }
00771     }
00772   }
00773 #endif
00774 
00775   if( !valid_visual_found && link->visual && link->visual->geometry )
00776   {
00777     Ogre::Entity* visual_mesh = NULL;
00778     createEntityForGeometryElement( link, *link->visual->geometry, link->visual->origin, link->visual->material_name, visual_node_, visual_mesh );
00779     if( visual_mesh )
00780     {
00781       visual_meshes_.push_back( visual_mesh );
00782     }
00783   }
00784 
00785   visual_node_->setVisible( getEnabled() );
00786 }
00787 
00788 void RobotLink::createSelection()
00789 {
00790   selection_handler_.reset( new RobotLinkSelectionHandler( this, context_ ));
00791   for( size_t i = 0; i < visual_meshes_.size(); i++ )
00792   {
00793     selection_handler_->addTrackedObject( visual_meshes_[ i ]);
00794   }
00795   for( size_t i = 0; i < collision_meshes_.size(); i++ )
00796   {
00797     selection_handler_->addTrackedObject( collision_meshes_[ i ]);
00798   }
00799 }
00800 
00801 void RobotLink::updateTrail()
00802 {
00803   if( trail_property_->getValue().toBool() )
00804   {
00805     if( !trail_ )
00806     {
00807       if( visual_node_ )
00808       {
00809         static int count = 0;
00810         std::stringstream ss;
00811         ss << "Trail for link " << name_ << count++;
00812         trail_ = scene_manager_->createRibbonTrail( ss.str() );
00813         trail_->setMaxChainElements( 100 );
00814         trail_->setInitialWidth( 0, 0.01f );
00815         trail_->setInitialColour( 0, 0.0f, 0.5f, 0.5f );
00816         trail_->addNode( visual_node_ );
00817         trail_->setTrailLength( 2.0f );
00818         trail_->setVisible( getEnabled() );
00819         robot_->getOtherNode()->attachObject( trail_ );
00820       }
00821       else
00822       {
00823         ROS_WARN( "No visual node for link %s, cannot create a trail", name_.c_str() );
00824       }
00825     }
00826   }
00827   else
00828   {
00829     if( trail_ )
00830     {
00831       scene_manager_->destroyRibbonTrail( trail_ );
00832       trail_ = NULL;
00833     }
00834   }
00835 }
00836 
00837 void RobotLink::updateAxes()
00838 {
00839   if( axes_property_->getValue().toBool() )
00840   {
00841     if( !axes_ )
00842     {
00843       static int count = 0;
00844       std::stringstream ss;
00845       ss << "Axes for link " << name_ << count++;
00846       axes_ = new Axes( scene_manager_, robot_->getOtherNode(), 0.1, 0.01 );
00847       axes_->getSceneNode()->setVisible( getEnabled() );
00848 
00849       axes_->setPosition( position_property_->getVector() );
00850       axes_->setOrientation( orientation_property_->getQuaternion() );
00851     }
00852   }
00853   else
00854   {
00855     if( axes_ )
00856     {
00857       delete axes_;
00858       axes_ = NULL;
00859     }
00860   }
00861 }
00862 
00863 void RobotLink::setTransforms( const Ogre::Vector3& visual_position, const Ogre::Quaternion& visual_orientation,
00864                                const Ogre::Vector3& collision_position, const Ogre::Quaternion& collision_orientation )
00865 {
00866   if ( visual_node_ )
00867   {
00868     visual_node_->setPosition( visual_position );
00869     visual_node_->setOrientation( visual_orientation );
00870   }
00871 
00872   if ( collision_node_ )
00873   {
00874     collision_node_->setPosition( collision_position );
00875     collision_node_->setOrientation( collision_orientation );
00876   }
00877 
00878   position_property_->setVector( visual_position );
00879   orientation_property_->setQuaternion( visual_orientation );
00880 
00881   if ( axes_ )
00882   {
00883     axes_->setPosition( visual_position );
00884     axes_->setOrientation( visual_orientation );
00885   }
00886 }
00887 
00888 void RobotLink::setToErrorMaterial()
00889 {
00890   for( size_t i = 0; i < visual_meshes_.size(); i++ )
00891   {
00892     visual_meshes_[ i ]->setMaterialName("BaseWhiteNoLighting");
00893   }
00894   for( size_t i = 0; i < collision_meshes_.size(); i++ )
00895   {
00896     collision_meshes_[ i ]->setMaterialName("BaseWhiteNoLighting");
00897   }
00898 }
00899 
00900 void RobotLink::setToNormalMaterial()
00901 {
00902   if( using_color_ )
00903   {
00904     for( size_t i = 0; i < visual_meshes_.size(); i++ )
00905     {
00906       visual_meshes_[ i ]->setMaterial( color_material_ );
00907     }
00908     for( size_t i = 0; i < collision_meshes_.size(); i++ )
00909     {
00910       collision_meshes_[ i ]->setMaterial( color_material_ );
00911     }
00912   }
00913   else
00914   {
00915     M_SubEntityToMaterial::iterator it = materials_.begin();
00916     M_SubEntityToMaterial::iterator end = materials_.end();
00917     for (; it != end; ++it)
00918     {
00919       it->first->setMaterial(it->second);
00920     }
00921   }
00922 }
00923 
00924 void RobotLink::setColor( float red, float green, float blue )
00925 {
00926   Ogre::ColourValue color = color_material_->getTechnique(0)->getPass(0)->getDiffuse();
00927   color.r = red;
00928   color.g = green;
00929   color.b = blue;
00930   color_material_->getTechnique(0)->setAmbient( 0.5 * color );
00931   color_material_->getTechnique(0)->setDiffuse( color );
00932 
00933   using_color_ = true;
00934   setToNormalMaterial();
00935 }
00936 
00937 void RobotLink::unsetColor()
00938 {
00939   using_color_ = false;
00940   setToNormalMaterial();
00941 }
00942 
00943 bool RobotLink::setSelectable( bool selectable )
00944 {
00945   bool old = is_selectable_;
00946   is_selectable_ = selectable;
00947   return old;
00948 }
00949 
00950 bool RobotLink::getSelectable()
00951 {
00952   return is_selectable_;
00953 }
00954 
00955 void RobotLink::hideSubProperties(bool hide)
00956 {
00957   position_property_->setHidden(hide);
00958   orientation_property_->setHidden(hide);
00959   trail_property_->setHidden(hide);
00960   axes_property_->setHidden(hide);
00961   alpha_property_->setHidden(hide);
00962 }
00963 
00964 Ogre::Vector3 RobotLink::getPosition()
00965 {
00966   return position_property_->getVector();
00967 }
00968 
00969 Ogre::Quaternion RobotLink::getOrientation()
00970 {
00971   return orientation_property_->getQuaternion();
00972 }
00973 
00974 void RobotLink::setParentProperty(Property* new_parent)
00975 {
00976   Property* old_parent = link_property_->getParent();
00977   if (old_parent)
00978     old_parent->takeChild(link_property_);
00979 
00980   if (new_parent)
00981     new_parent->addChild(link_property_);
00982 }
00983 
00984 // if use_detail:
00985 //    - all sub properties become children of details_ property.
00986 //    - details_ property becomes a child of link_property_
00987 // else (!use_detail)
00988 //    - all sub properties become children of link_property_.
00989 //    details_ property does not have a parent.
00990 void RobotLink::useDetailProperty(bool use_detail)
00991 {
00992   Property* old_parent = details_->getParent();
00993   if (old_parent)
00994     old_parent->takeChild(details_);
00995 
00996   if (use_detail)
00997   {
00998     while (link_property_->numChildren() > 0)
00999     {
01000       Property* child = link_property_->childAt(0);
01001       link_property_->takeChild(child);
01002       details_->addChild(child);
01003     }
01004 
01005     link_property_->addChild(details_);
01006   }
01007   else
01008   {
01009     while (details_->numChildren() > 0)
01010     {
01011       Property* child = details_->childAt(0);
01012       details_->takeChild(child);
01013       link_property_->addChild(child);
01014     }
01015   }
01016 }
01017 
01018 void RobotLink::expandDetails(bool expand)
01019 {
01020   Property *parent = details_->getParent() ? details_ : link_property_;
01021   if (expand)
01022   {
01023     parent->expand();
01024   }
01025   else
01026   {
01027     parent->collapse();
01028   }
01029 }
01030 
01031 
01032 } // namespace rviz
01033 


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:16