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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28