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 <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
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
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
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;
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
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
00632
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
00921
00922
00923
00924
00925
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 }
00969