00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00435
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 }
00675