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 "robot_link.h"
00031 #include "robot.h"
00032 #include "properties/property.h"
00033 #include "properties/property_manager.h"
00034 #include "visualization_manager.h"
00035 #include "selection/selection_manager.h"
00036 #include "mesh_loader.h"
00037 
00038 #include "ogre_helpers/object.h"
00039 #include "ogre_helpers/shape.h"
00040 #include "ogre_helpers/axes.h"
00041 
00042 #include <urdf/model.h>
00043 #include <urdf_interface/link.h>
00044 
00045 #include <OGRE/OgreSceneNode.h>
00046 #include <OGRE/OgreSceneManager.h>
00047 #include <OGRE/OgreRibbonTrail.h>
00048 #include <OGRE/OgreEntity.h>
00049 #include <OGRE/OgreSubEntity.h>
00050 #include <OGRE/OgreMaterialManager.h>
00051 #include <OGRE/OgreMaterial.h>
00052 #include <OGRE/OgreTextureManager.h>
00053 
00054 #include <ros/console.h>
00055 
00056 #include <resource_retriever/retriever.h>
00057 
00058 #include <boost/filesystem.hpp>
00059 
00060 namespace fs=boost::filesystem;
00061 
00062 namespace rviz
00063 {
00064 
00065 class RobotLinkSelectionHandler : public SelectionHandler
00066 {
00067 public:
00068   RobotLinkSelectionHandler(RobotLink* link);
00069   virtual ~RobotLinkSelectionHandler();
00070 
00071   virtual void createProperties(const Picked& obj, PropertyManager* property_manager);
00072 
00073 private:
00074   RobotLink* link_;
00075 };
00076 
00077 RobotLinkSelectionHandler::RobotLinkSelectionHandler(RobotLink* link)
00078 : link_(link)
00079 {
00080 }
00081 
00082 RobotLinkSelectionHandler::~RobotLinkSelectionHandler()
00083 {
00084 }
00085 
00086 void RobotLinkSelectionHandler::createProperties(const Picked& obj, PropertyManager* property_manager)
00087 {
00088   std::stringstream ss;
00089   ss << link_->getName() << " Link " << link_->getName();
00090 
00091   CategoryPropertyWPtr cat = property_manager->createCategory( "Link " + link_->getName(), ss.str(), CategoryPropertyWPtr(), (void*)obj.handle );
00092   properties_.push_back(cat);
00093 
00094   properties_.push_back(property_manager->createProperty<Vector3Property>( "Position", ss.str(), boost::bind( &RobotLink::getPositionInRobotFrame, link_ ),
00095                                                                                 Vector3Property::Setter(), cat, (void*)obj.handle ));
00096 
00097   properties_.push_back(property_manager->createProperty<QuaternionProperty>( "Orientation", ss.str(), boost::bind( &RobotLink::getOrientationInRobotFrame, link_ ),
00098                                                                                       QuaternionProperty::Setter(), cat, (void*)obj.handle ));
00099 }
00100 
00101 RobotLink::RobotLink(Robot* parent, VisualizationManager* manager)
00102 : parent_(parent)
00103 , scene_manager_(manager->getSceneManager())
00104 , property_manager_(0)
00105 , vis_manager_(manager)
00106 , enabled_( true )
00107 , visual_mesh_( NULL )
00108 , collision_mesh_( NULL )
00109 , visual_node_( NULL )
00110 , collision_node_( NULL )
00111 , position_(Ogre::Vector3::ZERO)
00112 , orientation_(Ogre::Quaternion::IDENTITY)
00113 , trail_( NULL )
00114 , axes_( NULL )
00115 , material_alpha_( 1.0 )
00116 , link_alpha_( 1.0 )
00117 , selection_object_(NULL)
00118 {
00119 }
00120 
00121 RobotLink::~RobotLink()
00122 {
00123   if ( visual_mesh_ )
00124   {
00125     scene_manager_->destroyEntity( visual_mesh_ );
00126   }
00127 
00128   if ( collision_mesh_ )
00129   {
00130     scene_manager_->destroyEntity( collision_mesh_ );
00131   }
00132 
00133   if ( trail_ )
00134   {
00135     scene_manager_->destroyRibbonTrail( trail_ );
00136   }
00137 
00138   delete axes_;
00139 
00140   if (selection_object_)
00141   {
00142     vis_manager_->getSelectionManager()->removeObject(selection_object_);
00143   }
00144 
00145   if (property_manager_)
00146   {
00147     property_manager_->deleteByUserData(this);
00148   }
00149 }
00150 
00151 bool RobotLink::isValid()
00152 {
00153   return visual_mesh_ || collision_mesh_;
00154 }
00155 
00156 void RobotLink::load(TiXmlElement* root_element, urdf::Model& descr, const urdf::LinkConstPtr& link, bool visual, bool collision)
00157 {
00158   name_ = link->name;
00159 
00160   if ( visual )
00161   {
00162     createVisual( root_element, link );
00163   }
00164 
00165   if ( collision )
00166   {
00167     createCollision( root_element, link );
00168   }
00169 
00170   if (collision || visual)
00171   {
00172     createSelection( descr, link );
00173   }
00174 
00175   if ( property_manager_ )
00176   {
00177     createProperties();
00178   }
00179 }
00180 
00181 void RobotLink::setAlpha(float a)
00182 {
00183   robot_alpha_ = a;
00184   updateAlpha();
00185 }
00186 
00187 void RobotLink::updateAlpha()
00188 {
00189   M_SubEntityToMaterial::iterator it = materials_.begin();
00190   M_SubEntityToMaterial::iterator end = materials_.end();
00191   for (; it != end; ++it)
00192   {
00193     const Ogre::MaterialPtr& material = it->second;
00194 
00195     Ogre::ColourValue color = material->getTechnique(0)->getPass(0)->getDiffuse();
00196     color.a = robot_alpha_ * material_alpha_ * link_alpha_;
00197     material->setDiffuse( color );
00198 
00199     if ( color.a < 0.9998 )
00200     {
00201       material->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00202       material->setDepthWriteEnabled( false );
00203     }
00204     else
00205     {
00206       material->setSceneBlending( Ogre::SBT_REPLACE );
00207       material->setDepthWriteEnabled( true );
00208     }
00209   }
00210 }
00211 
00212 void RobotLink::setLinkAlpha( float a )
00213 {
00214   link_alpha_ = a;
00215   propertyChanged( alpha_property_ );
00216   updateAlpha();
00217 }
00218 
00219 float RobotLink::getLinkAlpha()
00220 {
00221   return link_alpha_;
00222 }
00223 
00224 void RobotLink::setLinkEnabled( bool enabled )
00225 {
00226   enabled_ = enabled;
00227   propertyChanged( link_property_ );
00228   updateVisibility();
00229 }
00230 
00231 void RobotLink::updateVisibility()
00232 {
00233   if( visual_node_ )
00234   {
00235     visual_node_->setVisible( enabled_ && parent_->isVisualVisible() );
00236   }
00237   if( collision_node_ )
00238   {
00239     collision_node_->setVisible( enabled_ && parent_->isCollisionVisible() );
00240   }
00241   if( trail_ )
00242   {
00243     trail_->setVisible( enabled_ );
00244   }
00245   if( axes_ )
00246   {
00247     axes_->getSceneNode()->setVisible( enabled_ );
00248   }
00249 }
00250 
00251 Ogre::MaterialPtr RobotLink::getMaterialForLink(TiXmlElement* root_element, const urdf::LinkConstPtr& link)
00252 {
00253   if (!link->visual || !link->visual->material)
00254   {
00255     return Ogre::MaterialManager::getSingleton().getByName("RVIZ/Red");
00256   }
00257 
00258   static int count = 0;
00259   std::stringstream ss;
00260   ss << "Robot Link Material" << count;
00261 
00262   Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME);
00263   mat->getTechnique(0)->setLightingEnabled(true);
00264   if (link->visual->material->texture_filename.empty())
00265   {
00266     const urdf::Color& col = link->visual->material->color;
00267     mat->getTechnique(0)->setAmbient(col.r * 0.5, col.g * 0.5, col.b * 0.5);
00268     mat->getTechnique(0)->setDiffuse(col.r, col.g, col.b, col.a);
00269 
00270     material_alpha_ = col.a;
00271   }
00272   else
00273   {
00274     std::string filename = link->visual->material->texture_filename;
00275     if (!Ogre::TextureManager::getSingleton().resourceExists(filename))
00276     {
00277       resource_retriever::Retriever retriever;
00278       resource_retriever::MemoryResource res;
00279       try
00280       {
00281         res = retriever.get(filename);
00282       }
00283       catch (resource_retriever::Exception& e)
00284       {
00285         ROS_ERROR("%s", e.what());
00286       }
00287 
00288       if (res.size != 0)
00289       {
00290         Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
00291         Ogre::Image image;
00292         std::string extension = fs::extension(fs::path(filename));
00293 
00294         if (extension[0] == '.')
00295         {
00296           extension = extension.substr(1, extension.size() - 1);
00297         }
00298 
00299         try
00300         {
00301           image.load(stream, extension);
00302           Ogre::TextureManager::getSingleton().loadImage(filename, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00303         }
00304         catch (Ogre::Exception& e)
00305         {
00306           ROS_ERROR("Could not load texture [%s]: %s", filename.c_str(), e.what());
00307         }
00308       }
00309     }
00310 
00311     Ogre::Pass* pass = mat->getTechnique(0)->getPass(0);
00312     Ogre::TextureUnitState* tex_unit = pass->createTextureUnitState();;
00313     tex_unit->setTextureName(filename);
00314   }
00315 
00316   return mat;
00317 }
00318 
00319 void RobotLink::createEntityForGeometryElement(TiXmlElement* root_element, const urdf::LinkConstPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, Ogre::SceneNode* parent_node, Ogre::Entity*& entity, Ogre::SceneNode*& scene_node, Ogre::SceneNode*& offset_node)
00320 {
00321   scene_node = parent_node->createChildSceneNode();
00322   offset_node = scene_node->createChildSceneNode();
00323 
00324   static int count = 0;
00325   std::stringstream ss;
00326   ss << "Robot Link" << count++;
00327   std::string entity_name = ss.str();
00328 
00329   Ogre::Vector3 scale(Ogre::Vector3::UNIT_SCALE);
00330 
00331   Ogre::Vector3 offset_position(Ogre::Vector3::ZERO);
00332   Ogre::Quaternion offset_orientation(Ogre::Quaternion::IDENTITY);
00333 
00334 
00335   {
00336     Ogre::Vector3 position( origin.position.x, origin.position.y, origin.position.z );
00337     Ogre::Quaternion orientation( Ogre::Quaternion::IDENTITY );
00338     orientation = orientation * Ogre::Quaternion( origin.rotation.w, origin.rotation.x, origin.rotation.y, origin.rotation.z  );
00339 
00340     offset_position = position;
00341     offset_orientation = orientation;
00342   }
00343 
00344   switch (geom.type)
00345   {
00346   case urdf::Geometry::SPHERE:
00347   {
00348     const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
00349     entity = Shape::createEntity(entity_name, Shape::Sphere, scene_manager_);
00350 
00351     scale = Ogre::Vector3( sphere.radius*2, sphere.radius*2, sphere.radius*2 );
00352     break;
00353   }
00354   case urdf::Geometry::BOX:
00355   {
00356     const urdf::Box& box = static_cast<const urdf::Box&>(geom);
00357     entity = Shape::createEntity(entity_name, Shape::Cube, scene_manager_);
00358 
00359     scale = Ogre::Vector3( box.dim.x, box.dim.y, box.dim.z );
00360 
00361     break;
00362   }
00363   case urdf::Geometry::CYLINDER:
00364   {
00365     const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
00366 
00367     Ogre::Quaternion rotX;
00368     rotX.FromAngleAxis( Ogre::Degree(90), Ogre::Vector3::UNIT_X );
00369     offset_orientation = offset_orientation * rotX;
00370 
00371     entity = Shape::createEntity(entity_name, Shape::Cylinder, scene_manager_);
00372     scale = Ogre::Vector3( cylinder.radius*2, cylinder.length, cylinder.radius*2 );
00373     break;
00374   }
00375   case urdf::Geometry::MESH:
00376   {
00377     const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
00378 
00379     if ( mesh.filename.empty() )
00380       return;
00381 
00382     scale = Ogre::Vector3(mesh.scale.x, mesh.scale.y, mesh.scale.z);
00383 
00384     std::string model_name = mesh.filename;
00385     loadMeshFromResource(model_name);
00386 
00387     try
00388     {
00389       entity = scene_manager_->createEntity( ss.str(), model_name );
00390     }
00391     catch( Ogre::Exception& e )
00392     {
00393       ROS_ERROR( "Could not load model '%s' for link '%s': %s\n", model_name.c_str(), link->name.c_str(), e.what() );
00394     }
00395     break;
00396   }
00397   default:
00398     ROS_WARN("Unsupported geometry type for element: %d", geom.type);
00399     break;
00400   }
00401 
00402   if ( entity )
00403   {
00404     offset_node->attachObject(entity);
00405     offset_node->setScale(scale);
00406     offset_node->setPosition(offset_position);
00407     offset_node->setOrientation(offset_orientation);
00408 
00409     if (default_material_name_.empty())
00410     {
00411       default_material_ = getMaterialForLink(root_element, link);
00412 
00413       static int count = 0;
00414       std::stringstream ss;
00415       ss << default_material_->getName() << count++ << "Robot";
00416       std::string cloned_name = ss.str();
00417 
00418       default_material_ = default_material_->clone(cloned_name);
00419       default_material_name_ = default_material_->getName();
00420     }
00421 
00422     for (uint32_t i = 0; i < entity->getNumSubEntities(); ++i)
00423     {
00424       // Assign materials only if the submesh does not have one already
00425       Ogre::SubEntity* sub = entity->getSubEntity(i);
00426       const std::string& material_name = sub->getMaterialName();
00427 
00428       if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting")
00429       {
00430         sub->setMaterialName(default_material_name_);
00431       }
00432       else
00433       {
00434         // Need to clone here due to how selection works.  Once selection id is done per object and not per material,
00435         // this can go away
00436         static int count = 0;
00437         std::stringstream ss;
00438         ss << material_name << count++ << "Robot";
00439         std::string cloned_name = ss.str();
00440         sub->getMaterial()->clone(cloned_name);
00441         sub->setMaterialName(cloned_name);
00442       }
00443 
00444       materials_[sub] = sub->getMaterial();
00445     }
00446   }
00447 }
00448 
00449 void RobotLink::createCollision(TiXmlElement* root_element, const urdf::LinkConstPtr& link)
00450 {
00451   if (!link->collision || !link->collision->geometry)
00452     return;
00453 
00454   createEntityForGeometryElement(root_element, link, *link->collision->geometry, link->collision->origin, parent_->getCollisionNode(), collision_mesh_, collision_node_, collision_offset_node_);
00455   collision_node_->setVisible( enabled_ );
00456 }
00457 
00458 void RobotLink::createVisual(TiXmlElement* root_element, const urdf::LinkConstPtr& link )
00459 {
00460   if (!link->visual || !link->visual->geometry)
00461     return;
00462 
00463   createEntityForGeometryElement(root_element, link, *link->visual->geometry, link->visual->origin, parent_->getVisualNode(), visual_mesh_, visual_node_, visual_offset_node_);
00464   visual_node_->setVisible( enabled_ );
00465 }
00466 
00467 void RobotLink::createSelection(const urdf::Model& descr, const urdf::LinkConstPtr& link)
00468 {
00469   selection_handler_ = RobotLinkSelectionHandlerPtr(new RobotLinkSelectionHandler(this));
00470   SelectionManager* sel_man = vis_manager_->getSelectionManager();
00471   selection_object_ = sel_man->createHandle();
00472   sel_man->addObject(selection_object_, selection_handler_);
00473 
00474   M_SubEntityToMaterial::iterator it = materials_.begin();
00475   M_SubEntityToMaterial::iterator end = materials_.end();
00476   for (; it != end; ++it)
00477   {
00478     const Ogre::MaterialPtr& material = it->second;
00479 
00480     sel_man->addPickTechnique(selection_object_, material);
00481   }
00482 
00483   if (visual_mesh_)
00484   {
00485     selection_handler_->addTrackedObject(visual_mesh_);
00486   }
00487 
00488   if (collision_mesh_)
00489   {
00490     selection_handler_->addTrackedObject(collision_mesh_);
00491   }
00492 }
00493 
00494 void RobotLink::setPropertyManager(PropertyManager* property_manager)
00495 {
00496   property_manager_ = property_manager;
00497 }
00498 
00499 void RobotLink::createProperties()
00500 {
00501   ROS_ASSERT( property_manager_ );
00502 
00503   std::stringstream ss;
00504   ss << parent_->getName() << " Link " << name_;
00505 
00506   link_property_ = property_manager_->createCheckboxCategory( name_, "Enabled", ss.str(),
00507                                                               boost::bind( &RobotLink::getLinkEnabled, this ),
00508                                                               boost::bind( &RobotLink::setLinkEnabled, this, _1 ),
00509                                                               parent_->getLinksCategory(), this );
00510 
00511   alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", ss.str(),
00512                                                                       boost::bind( &RobotLink::getLinkAlpha, this ),
00513                                                                       boost::bind( &RobotLink::setLinkAlpha, this, _1 ),
00514                                                                       link_property_, this );
00515                                                                                    
00516   trail_property_ = property_manager_->createProperty<BoolProperty>( "Show Trail", ss.str(), boost::bind( &RobotLink::getShowTrail, this ),
00517                                                                           boost::bind( &RobotLink::setShowTrail, this, _1 ), link_property_, this );
00518   setPropertyHelpText(trail_property_, "Enable/disable a 2 meter \"ribbon\" which follows this link.");
00519 
00520   axes_property_ = property_manager_->createProperty<BoolProperty>( "Show Axes", ss.str(), boost::bind( &RobotLink::getShowAxes, this ),
00521                                                                           boost::bind( &RobotLink::setShowAxes, this, _1 ), link_property_, this );
00522   setPropertyHelpText(axes_property_, "Enable/disable showing the axes of this link.");
00523 
00524   position_property_ = property_manager_->createProperty<Vector3Property>( "Position", ss.str(), boost::bind( &RobotLink::getPositionInRobotFrame, this ),
00525                                                                                 Vector3Property::Setter(), link_property_, this );
00526   setPropertyHelpText(position_property_, "Position of this link, in the current Fixed Frame.  (Not editable)");
00527   orientation_property_ = property_manager_->createProperty<QuaternionProperty>( "Orientation", ss.str(), boost::bind( &RobotLink::getOrientationInRobotFrame, this ),
00528                                                                                       QuaternionProperty::Setter(), link_property_, this );
00529   setPropertyHelpText(orientation_property_, "Orientation of this link, in the current Fixed Frame.  (Not editable)");
00530 
00531   CategoryPropertyPtr cat_prop = link_property_.lock();
00532   cat_prop->collapse();
00533 }
00534 
00535 
00536 Ogre::Vector3 RobotLink::getPositionInRobotFrame()
00537 {
00538   return position_;
00539 }
00540 
00541 Ogre::Quaternion RobotLink::getOrientationInRobotFrame()
00542 {
00543   return orientation_;
00544 }
00545 
00546 void RobotLink::setShowTrail(bool show)
00547 {
00548   if ( show )
00549   {
00550     if ( !trail_ )
00551     {
00552       if ( visual_node_ )
00553       {
00554         static int count = 0;
00555         std::stringstream ss;
00556         ss << "Trail for link " << name_ << count++;
00557         trail_ = scene_manager_->createRibbonTrail( ss.str() );
00558         trail_->setMaxChainElements( 100 );
00559         trail_->setInitialWidth( 0, 0.01f );
00560         trail_->setInitialColour( 0, 0.0f, 0.5f, 0.5f );
00561         trail_->addNode( visual_node_ );
00562         trail_->setTrailLength( 2.0f );
00563         trail_->setVisible( enabled_ );
00564         parent_->getOtherNode()->attachObject( trail_ );
00565       }
00566       else
00567       {
00568         ROS_WARN( "No visual node for link %s, cannot create a trail", name_.c_str() );
00569       }
00570     }
00571   }
00572   else
00573   {
00574     if ( trail_ )
00575     {
00576       scene_manager_->destroyRibbonTrail( trail_ );
00577       trail_ = NULL;
00578     }
00579   }
00580 
00581   propertyChanged(trail_property_);
00582 }
00583 
00584 bool RobotLink::getShowTrail()
00585 {
00586   return trail_ != NULL;
00587 }
00588 
00589 void RobotLink::setShowAxes(bool show)
00590 {
00591   if ( show )
00592   {
00593     if ( !axes_ )
00594     {
00595       static int count = 0;
00596       std::stringstream ss;
00597       ss << "Axes for link " << name_ << count++;
00598       axes_ = new Axes( scene_manager_, parent_->getOtherNode(), 0.1, 0.01 );
00599       axes_->getSceneNode()->setVisible( enabled_ );
00600     }
00601   }
00602   else
00603   {
00604     if ( axes_ )
00605     {
00606       delete axes_;
00607       axes_ = NULL;
00608     }
00609   }
00610 
00611   propertyChanged(axes_property_);
00612 }
00613 
00614 bool RobotLink::getShowAxes()
00615 {
00616   return axes_ != NULL;
00617 }
00618 
00619 void RobotLink::setTransforms( const Ogre::Vector3& visual_position, const Ogre::Quaternion& visual_orientation,
00620                           const Ogre::Vector3& collision_position, const Ogre::Quaternion& collision_orientation, bool apply_offset_transforms )
00621 {
00622   position_ = visual_position;
00623   orientation_ = visual_orientation;
00624 
00625   if ( visual_node_ )
00626   {
00627     visual_node_->setPosition( visual_position );
00628     visual_node_->setOrientation( visual_orientation );
00629   }
00630 
00631   if ( collision_node_ )
00632   {
00633     collision_node_->setPosition( collision_position );
00634     collision_node_->setOrientation( collision_orientation );
00635   }
00636 
00637   if (property_manager_)
00638   {
00639     propertyChanged(position_property_);
00640     propertyChanged(orientation_property_);
00641   }
00642 
00643 
00644   if ( axes_ )
00645   {
00646     axes_->setPosition( position_ );
00647     axes_->setOrientation( orientation_ );
00648   }
00649 }
00650 
00651 void RobotLink::setToErrorMaterial()
00652 {
00653   if (visual_mesh_)
00654   {
00655     visual_mesh_->setMaterialName("BaseWhiteNoLighting");
00656   }
00657 
00658   if (collision_mesh_)
00659   {
00660     collision_mesh_->setMaterialName("BaseWhiteNoLighting");
00661   }
00662 }
00663 
00664 void RobotLink::setToNormalMaterial()
00665 {
00666   M_SubEntityToMaterial::iterator it = materials_.begin();
00667   M_SubEntityToMaterial::iterator end = materials_.end();
00668   for (; it != end; ++it)
00669   {
00670     it->first->setMaterial(it->second);
00671   }
00672 }
00673 
00674 } // namespace rviz
00675 


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32