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