robot_link.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <boost/filesystem.hpp>
31 
32 #include <OgreEntity.h>
33 #include <OgreMaterial.h>
34 #include <OgreMaterialManager.h>
35 #include <OgreRibbonTrail.h>
36 #include <OgreSceneManager.h>
37 #include <OgreSceneNode.h>
38 #include <OgreSubEntity.h>
39 #include <OgreTextureManager.h>
40 #include <OgreSharedPtr.h>
41 #include <OgreTechnique.h>
42 
43 #include <ros/console.h>
44 
46 #include <urdf_model/model.h>
47 #include <urdf_model/link.h>
48 
49 #include "rviz/mesh_loader.h"
50 #include "rviz/ogre_helpers/axes.h"
58 #include "rviz/robot/robot.h"
61 #include "rviz/load_resource.h"
62 
63 #include "rviz/robot/robot_link.h"
64 #include "rviz/robot/robot_joint.h"
65 
66 namespace fs=boost::filesystem;
67 
68 #ifndef ROS_PACKAGE_NAME
69 # define ROS_PACKAGE_NAME "rviz"
70 #endif
71 
72 namespace rviz
73 {
74 
76 {
77 public:
80 
81  virtual void createProperties( const Picked& obj, Property* parent_property );
82  virtual void updateProperties();
83 
84  virtual void preRenderPass(uint32_t pass);
85  virtual void postRenderPass(uint32_t pass);
86 
87 private:
91 };
92 
94  : SelectionHandler( context )
95  , link_( link )
96 {
97 }
98 
100 {
101 }
102 
103 void RobotLinkSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
104 {
105  Property* group = new Property( "Link " + QString::fromStdString( link_->getName() ), QVariant(), "", parent_property );
106  properties_.push_back( group );
107 
108  position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, "", group );
110 
111  orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, "", group );
113 
114  group->expand();
115 }
116 
118 {
121 }
122 
123 
125 {
126  if (!link_->is_selectable_)
127  {
128  if( link_->visual_node_ )
129  {
130  link_->visual_node_->setVisible( false );
131  }
132  if( link_->collision_node_ )
133  {
134  link_->collision_node_->setVisible( false );
135  }
136  if( link_->trail_ )
137  {
138  link_->trail_->setVisible( false );
139  }
140  if( link_->axes_ )
141  {
142  link_->axes_->getSceneNode()->setVisible( false );
143  }
144  }
145 }
146 
148 {
149  if (!link_->is_selectable_)
150  {
152  }
153 }
154 
155 
157  const urdf::LinkConstSharedPtr& link,
158  const std::string& parent_joint_name,
159  bool visual,
160  bool collision)
161 : robot_( robot )
162 , scene_manager_( robot->getDisplayContext()->getSceneManager() )
163 , context_( robot->getDisplayContext() )
164 , name_( link->name )
165 , parent_joint_name_( parent_joint_name )
166 , visual_node_( NULL )
167 , collision_node_( NULL )
168 , trail_( NULL )
169 , axes_( NULL )
170 , material_alpha_( 1.0 )
171 , robot_alpha_(1.0)
172 , only_render_depth_(false)
173 , using_color_( false )
174 , is_selectable_( true )
175 {
176  link_property_ = new Property( link->name.c_str(), true, "", NULL, SLOT( updateVisibility() ), this );
177  link_property_->setIcon( rviz::loadPixmap( "package://rviz/icons/classes/RobotLink.png" ) );
178 
179  details_ = new Property( "Details", QVariant(), "", NULL);
180 
181  alpha_property_ = new FloatProperty( "Alpha", 1,
182  "Amount of transparency to apply to this link.",
183  link_property_, SLOT( updateAlpha() ), this );
184 
185  trail_property_ = new Property( "Show Trail", false,
186  "Enable/disable a 2 meter \"ribbon\" which follows this link.",
187  link_property_, SLOT( updateTrail() ), this );
188 
189  axes_property_ = new Property( "Show Axes", false,
190  "Enable/disable showing the axes of this link.",
191  link_property_, SLOT( updateAxes() ), this );
192 
193  position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
194  "Position of this link, in the current Fixed Frame. (Not editable)",
195  link_property_ );
197 
198  orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
199  "Orientation of this link, in the current Fixed Frame. (Not editable)",
200  link_property_ );
202 
204 
205  visual_node_ = robot_->getVisualNode()->createChildSceneNode();
206  collision_node_ = robot_->getCollisionNode()->createChildSceneNode();
207 
208  // create material for coloring links
209  std::stringstream ss;
210  static int count = 1;
211  ss << "robot link color material " << count++;
212  color_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), ROS_PACKAGE_NAME );
213  color_material_->setReceiveShadows(false);
214  color_material_->getTechnique(0)->setLightingEnabled(true);
215 
216  // create the ogre objects to display
217 
218  if ( visual )
219  {
220  createVisual( link );
221  }
222 
223  if ( collision )
224  {
225  createCollision( link );
226  }
227 
228  if (collision || visual)
229  {
230  createSelection();
231  }
232 
233  // create description and fill in child_joint_names_ vector
234  std::stringstream desc;
235  if (parent_joint_name_.empty())
236  {
237  desc << "Root Link <b>" << name_ << "</b>";
238  }
239  else
240  {
241  desc << "Link <b>" << name_ << "</b>";
242  desc << " with parent joint <b>" << parent_joint_name_ << "</b>";
243  }
244 
245  if (link->child_joints.empty())
246  {
247  desc << " has no children.";
248  }
249  else
250  {
251  desc
252  << " has "
253  << link->child_joints.size();
254 
255  if (link->child_joints.size() > 1)
256  {
257  desc << " child joints: ";
258  }
259  else
260  {
261  desc << " child joint: ";
262  }
263 
264  std::vector<urdf::JointSharedPtr >::const_iterator child_it = link->child_joints.begin();
265  std::vector<urdf::JointSharedPtr >::const_iterator child_end = link->child_joints.end();
266  for ( ; child_it != child_end ; ++child_it )
267  {
268  urdf::Joint *child_joint = child_it->get();
269  if (child_joint && !child_joint->name.empty())
270  {
271  child_joint_names_.push_back(child_joint->name);
272  desc << "<b>" << child_joint->name << "</b>" << ((child_it+1 == child_end) ? "." : ", ");
273  }
274  }
275  }
276  if (hasGeometry())
277  {
278  desc << " Check/uncheck to show/hide this link in the display.";
279  if (visual_meshes_.empty())
280  {
281  desc << " This link has collision geometry but no visible geometry.";
282  }
283  else if (collision_meshes_.empty())
284  {
285  desc << " This link has visible geometry but no collision geometry.";
286  }
287  }
288  else
289  {
290  desc << " This link has NO geometry.";
291  }
292 
293  link_property_->setDescription(desc.str().c_str());
294 
295  if (!hasGeometry())
296  {
297  link_property_->setIcon( rviz::loadPixmap( "package://rviz/icons/classes/RobotLinkNoGeom.png" ) );
299  link_property_->setValue(QVariant());
300  }
301 }
302 
304 {
305  for( size_t i = 0; i < visual_meshes_.size(); i++ )
306  {
307  scene_manager_->destroyEntity( visual_meshes_[ i ]);
308  }
309 
310  for( size_t i = 0; i < collision_meshes_.size(); i++ )
311  {
312  scene_manager_->destroyEntity( collision_meshes_[ i ]);
313  }
314 
315  scene_manager_->destroySceneNode( visual_node_ );
316  scene_manager_->destroySceneNode( collision_node_ );
317 
318  if ( trail_ )
319  {
320  scene_manager_->destroyRibbonTrail( trail_ );
321  }
322 
323  delete axes_;
324  delete details_;
325  delete link_property_;
326 }
327 
329 {
330  return visual_meshes_.size() + collision_meshes_.size() > 0;
331 }
332 
334 {
335  if (!hasGeometry())
336  return true;
337  return link_property_->getValue().toBool();
338 }
339 
341 {
342  robot_alpha_ = a;
343  updateAlpha();
344 }
345 
346 void RobotLink::setRenderQueueGroup( Ogre::uint8 group )
347 {
348  Ogre::SceneNode::ChildNodeIterator child_it = visual_node_->getChildIterator();
349  while( child_it.hasMoreElements() )
350  {
351  Ogre::SceneNode* child = dynamic_cast<Ogre::SceneNode*>( child_it.getNext() );
352  if( child )
353  {
354  Ogre::SceneNode::ObjectIterator object_it = child->getAttachedObjectIterator();
355  while( object_it.hasMoreElements() )
356  {
357  Ogre::MovableObject* obj = object_it.getNext();
358  obj->setRenderQueueGroup(group);
359  }
360  }
361  }
362 }
363 
364 void RobotLink::setOnlyRenderDepth(bool onlyRenderDepth)
365 {
366  setRenderQueueGroup( onlyRenderDepth ? Ogre::RENDER_QUEUE_BACKGROUND : Ogre::RENDER_QUEUE_MAIN );
367  only_render_depth_ = onlyRenderDepth;
368  updateAlpha();
369 }
370 
372 {
373  float link_alpha = alpha_property_->getFloat();
374  M_SubEntityToMaterial::iterator it = materials_.begin();
375  M_SubEntityToMaterial::iterator end = materials_.end();
376  for (; it != end; ++it)
377  {
378  const Ogre::MaterialPtr& material = it->second;
379 
380  if ( only_render_depth_ )
381  {
382  material->setColourWriteEnabled( false );
383  material->setDepthWriteEnabled( true );
384  }
385  else
386  {
387  Ogre::ColourValue color = material->getTechnique(0)->getPass(0)->getDiffuse();
388  color.a = robot_alpha_ * material_alpha_ * link_alpha;
389  material->setDiffuse( color );
390 
391  if ( color.a < 0.9998 )
392  {
393  material->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
394  material->setDepthWriteEnabled( false );
395  }
396  else
397  {
398  material->setSceneBlending( Ogre::SBT_REPLACE );
399  material->setDepthWriteEnabled( true );
400  }
401  }
402  }
403 
404  Ogre::ColourValue color = color_material_->getTechnique(0)->getPass(0)->getDiffuse();
405  color.a = robot_alpha_ * link_alpha;
406  color_material_->setDiffuse( color );
407 
408  if ( color.a < 0.9998 )
409  {
410  color_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
411  color_material_->setDepthWriteEnabled( false );
412  }
413  else
414  {
415  color_material_->setSceneBlending( Ogre::SBT_REPLACE );
416  color_material_->setDepthWriteEnabled( true );
417  }
418 }
419 
421 {
422  bool enabled = getEnabled();
423 
425 
426  if( visual_node_ )
427  {
428  visual_node_->setVisible( enabled && robot_->isVisible() && robot_->isVisualVisible() );
429  }
430  if( collision_node_ )
431  {
432  collision_node_->setVisible( enabled && robot_->isVisible() && robot_->isCollisionVisible() );
433  }
434  if( trail_ )
435  {
436  trail_->setVisible( enabled && robot_->isVisible() );
437  }
438  if( axes_ )
439  {
440  axes_->getSceneNode()->setVisible( enabled && robot_->isVisible() );
441  }
442 }
443 
444 Ogre::MaterialPtr RobotLink::getMaterialForLink( const urdf::LinkConstSharedPtr& link, const std::string material_name)
445 {
446  if (!link->visual || !link->visual->material)
447  {
448  return Ogre::MaterialManager::getSingleton().getByName("RVIZ/ShadedRed");
449  }
450 
451  static int count = 0;
452  std::stringstream ss;
453  ss << "Robot Link Material" << count++;
454 
455  Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME);
456  mat->getTechnique(0)->setLightingEnabled(true);
457 
458  urdf::VisualSharedPtr visual = link->visual;
459  std::vector<urdf::VisualSharedPtr>::const_iterator vi;
460  for( vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++ )
461  {
462  if( (*vi) && material_name != "" && (*vi)->material_name == material_name) {
463  visual = *vi;
464  break;
465  }
466  }
467  if ( vi == link->visual_array.end() ) {
468  visual = link->visual; // if link does not have material, use default oneee
469  }
470 
471  if (visual->material->texture_filename.empty())
472  {
473  const urdf::Color& col = visual->material->color;
474  mat->getTechnique(0)->setAmbient(col.r * 0.5, col.g * 0.5, col.b * 0.5);
475  mat->getTechnique(0)->setDiffuse(col.r, col.g, col.b, col.a);
476 
477  material_alpha_ = col.a;
478  }
479  else
480  {
481  std::string filename = visual->material->texture_filename;
482  if (!Ogre::TextureManager::getSingleton().resourceExists(filename))
483  {
486  try
487  {
488  res = retriever.get(filename);
489  }
491  {
492  ROS_ERROR("%s", e.what());
493  }
494 
495  if (res.size != 0)
496  {
497  Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
498  Ogre::Image image;
499  std::string extension = fs::extension(fs::path(filename));
500 
501  if (extension[0] == '.')
502  {
503  extension = extension.substr(1, extension.size() - 1);
504  }
505 
506  try
507  {
508  image.load(stream, extension);
509  Ogre::TextureManager::getSingleton().loadImage(filename, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
510  }
511  catch (Ogre::Exception& e)
512  {
513  ROS_ERROR("Could not load texture [%s]: %s", filename.c_str(), e.what());
514  }
515  }
516  }
517 
518  Ogre::Pass* pass = mat->getTechnique(0)->getPass(0);
519  Ogre::TextureUnitState* tex_unit = pass->createTextureUnitState();;
520  tex_unit->setTextureName(filename);
521  }
522 
523  return mat;
524 }
525 
526 void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link, const urdf::Geometry& geom, const urdf::Pose& origin, const std::string material_name, Ogre::SceneNode* scene_node, Ogre::Entity*& entity)
527 {
528  entity = NULL; // default in case nothing works.
529  Ogre::SceneNode* offset_node = scene_node->createChildSceneNode();
530 
531  static int count = 0;
532  std::stringstream ss;
533  ss << "Robot Link" << count++;
534  std::string entity_name = ss.str();
535 
536  Ogre::Vector3 scale(Ogre::Vector3::UNIT_SCALE);
537 
538  Ogre::Vector3 offset_position(Ogre::Vector3::ZERO);
539  Ogre::Quaternion offset_orientation(Ogre::Quaternion::IDENTITY);
540 
541 
542  {
543  Ogre::Vector3 position( origin.position.x, origin.position.y, origin.position.z );
544  Ogre::Quaternion orientation( Ogre::Quaternion::IDENTITY );
545  orientation = orientation * Ogre::Quaternion( origin.rotation.w, origin.rotation.x, origin.rotation.y, origin.rotation.z );
546 
547  offset_position = position;
548  offset_orientation = orientation;
549  }
550 
551  switch (geom.type)
552  {
553  case urdf::Geometry::SPHERE:
554  {
555  const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
556  entity = Shape::createEntity(entity_name, Shape::Sphere, scene_manager_);
557 
558  scale = Ogre::Vector3( sphere.radius*2, sphere.radius*2, sphere.radius*2 );
559  break;
560  }
561  case urdf::Geometry::BOX:
562  {
563  const urdf::Box& box = static_cast<const urdf::Box&>(geom);
564  entity = Shape::createEntity(entity_name, Shape::Cube, scene_manager_);
565 
566  scale = Ogre::Vector3( box.dim.x, box.dim.y, box.dim.z );
567 
568  break;
569  }
570  case urdf::Geometry::CYLINDER:
571  {
572  const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
573 
574  Ogre::Quaternion rotX;
575  rotX.FromAngleAxis( Ogre::Degree(90), Ogre::Vector3::UNIT_X );
576  offset_orientation = offset_orientation * rotX;
577 
578  entity = Shape::createEntity(entity_name, Shape::Cylinder, scene_manager_);
579  scale = Ogre::Vector3( cylinder.radius*2, cylinder.length, cylinder.radius*2 );
580  break;
581  }
582  case urdf::Geometry::MESH:
583  {
584  const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
585 
586  if ( mesh.filename.empty() )
587  return;
588 
589 
590 
591 
592  scale = Ogre::Vector3(mesh.scale.x, mesh.scale.y, mesh.scale.z);
593 
594  std::string model_name = mesh.filename;
595 
596  try
597  {
598  loadMeshFromResource(model_name);
599  entity = scene_manager_->createEntity( ss.str(), model_name );
600  }
601  catch( Ogre::InvalidParametersException& e )
602  {
603  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() );
604  }
605  catch( Ogre::Exception& e )
606  {
607  ROS_ERROR( "Could not load model '%s' for link '%s': %s", model_name.c_str(), link->name.c_str(), e.what() );
608  }
609  break;
610  }
611  default:
612  ROS_WARN("Unsupported geometry type for element: %d", geom.type);
613  break;
614  }
615 
616  if ( entity )
617  {
618  offset_node->attachObject(entity);
619  offset_node->setScale(scale);
620  offset_node->setPosition(offset_position);
621  offset_node->setOrientation(offset_orientation);
622 
623  static int count = 0;
624  if (default_material_name_.empty())
625  {
627 
628  std::stringstream ss;
629  ss << default_material_->getName() << count++ << "Robot";
630  std::string cloned_name = ss.str();
631 
632  default_material_ = default_material_->clone(cloned_name);
634  }
635 
636  for (uint32_t i = 0; i < entity->getNumSubEntities(); ++i)
637  {
638  default_material_ = getMaterialForLink(link, material_name);
639  std::stringstream ss;
640  ss << default_material_->getName() << count++ << "Robot";
641  std::string cloned_name = ss.str();
642 
643  default_material_ = default_material_->clone(cloned_name);
645 
646  // Assign materials only if the submesh does not have one already
647 
648  Ogre::SubEntity* sub = entity->getSubEntity(i);
649  const std::string& material_name = sub->getMaterialName();
650 
651  if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting")
652  {
653  sub->setMaterialName(default_material_name_);
654  }
655  else
656  {
657  // Need to clone here due to how selection works. Once selection id is done per object and not per material,
658  // this can go away
659  std::stringstream ss;
660  ss << material_name << count++ << "Robot";
661  std::string cloned_name = ss.str();
662  sub->getMaterial()->clone(cloned_name);
663  sub->setMaterialName(cloned_name);
664  }
665 
666  materials_[sub] = sub->getMaterial();
667  }
668  }
669 }
670 
671 void RobotLink::createCollision(const urdf::LinkConstSharedPtr& link)
672 {
673  bool valid_collision_found = false;
674 #if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
675  std::map<std::string, boost::shared_ptr<std::vector<urdf::CollisionSharedPtr > > >::const_iterator mi;
676  for( mi = link->collision_groups.begin(); mi != link->collision_groups.end(); mi++ )
677  {
678  if( mi->second )
679  {
680  std::vector<urdf::CollisionSharedPtr >::const_iterator vi;
681  for( vi = mi->second->begin(); vi != mi->second->end(); vi++ )
682  {
683  urdf::CollisionSharedPtr collision = *vi;
684  if( collision && collision->geometry )
685  {
686  Ogre::Entity* collision_mesh = NULL;
687  createEntityForGeometryElement( link, *collision->geometry, collision->origin, collision_node_, collision_mesh );
688  if( collision_mesh )
689  {
690  collision_meshes_.push_back( collision_mesh );
691  valid_collision_found = true;
692  }
693  }
694  }
695  }
696  }
697 #else
698  std::vector<urdf::CollisionSharedPtr >::const_iterator vi;
699  for( vi = link->collision_array.begin(); vi != link->collision_array.end(); vi++ )
700  {
701  urdf::CollisionSharedPtr collision = *vi;
702  if( collision && collision->geometry )
703  {
704  Ogre::Entity* collision_mesh = NULL;
705  createEntityForGeometryElement( link, *collision->geometry, collision->origin, "", collision_node_, collision_mesh );
706  if( collision_mesh )
707  {
708  collision_meshes_.push_back( collision_mesh );
709  valid_collision_found = true;
710  }
711  }
712  }
713 #endif
714 
715  if( !valid_collision_found && link->collision && link->collision->geometry )
716  {
717  Ogre::Entity* collision_mesh = NULL;
718  createEntityForGeometryElement( link, *link->collision->geometry, link->collision->origin, "", collision_node_, collision_mesh );
719  if( collision_mesh )
720  {
721  collision_meshes_.push_back( collision_mesh );
722  }
723  }
724 
725  collision_node_->setVisible( getEnabled() );
726 }
727 
728 void RobotLink::createVisual(const urdf::LinkConstSharedPtr& link )
729 {
730  bool valid_visual_found = false;
731 #if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
732  std::map<std::string, boost::shared_ptr<std::vector<urdf::VisualSharedPtr > > >::const_iterator mi;
733  for( mi = link->visual_groups.begin(); mi != link->visual_groups.end(); mi++ )
734  {
735  if( mi->second )
736  {
737  std::vector<urdf::VisualSharedPtr >::const_iterator vi;
738  for( vi = mi->second->begin(); vi != mi->second->end(); vi++ )
739  {
740  urdf::VisualSharedPtr visual = *vi;
741  if( visual && visual->geometry )
742  {
743  Ogre::Entity* visual_mesh = NULL;
744  createEntityForGeometryElement( link, *visual->geometry, visual->origin, visual_node_, visual_mesh );
745  if( visual_mesh )
746  {
747  visual_meshes_.push_back( visual_mesh );
748  valid_visual_found = true;
749  }
750  }
751  }
752  }
753  }
754 #else
755  std::vector<urdf::VisualSharedPtr >::const_iterator vi;
756  for( vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++ )
757  {
758  urdf::VisualSharedPtr visual = *vi;
759  if( visual && visual->geometry )
760  {
761  Ogre::Entity* visual_mesh = NULL;
762  createEntityForGeometryElement( link, *visual->geometry, visual->origin, visual->material_name, visual_node_, visual_mesh );
763  if( visual_mesh )
764  {
765  visual_meshes_.push_back( visual_mesh );
766  valid_visual_found = true;
767  }
768  }
769  }
770 #endif
771 
772  if( !valid_visual_found && link->visual && link->visual->geometry )
773  {
774  Ogre::Entity* visual_mesh = NULL;
775  createEntityForGeometryElement( link, *link->visual->geometry, link->visual->origin, link->visual->material_name, visual_node_, visual_mesh );
776  if( visual_mesh )
777  {
778  visual_meshes_.push_back( visual_mesh );
779  }
780  }
781 
782  visual_node_->setVisible( getEnabled() );
783 }
784 
786 {
788  for( size_t i = 0; i < visual_meshes_.size(); i++ )
789  {
790  selection_handler_->addTrackedObject( visual_meshes_[ i ]);
791  }
792  for( size_t i = 0; i < collision_meshes_.size(); i++ )
793  {
794  selection_handler_->addTrackedObject( collision_meshes_[ i ]);
795  }
796 }
797 
799 {
800  if( trail_property_->getValue().toBool() )
801  {
802  if( !trail_ )
803  {
804  if( visual_node_ )
805  {
806  static int count = 0;
807  std::stringstream ss;
808  ss << "Trail for link " << name_ << count++;
809  trail_ = scene_manager_->createRibbonTrail( ss.str() );
810  trail_->setMaxChainElements( 100 );
811  trail_->setInitialWidth( 0, 0.01f );
812  trail_->setInitialColour( 0, 0.0f, 0.5f, 0.5f );
813  trail_->addNode( visual_node_ );
814  trail_->setTrailLength( 2.0f );
815  trail_->setVisible( getEnabled() );
816  robot_->getOtherNode()->attachObject( trail_ );
817  }
818  else
819  {
820  ROS_WARN( "No visual node for link %s, cannot create a trail", name_.c_str() );
821  }
822  }
823  }
824  else
825  {
826  if( trail_ )
827  {
828  scene_manager_->destroyRibbonTrail( trail_ );
829  trail_ = NULL;
830  }
831  }
832 }
833 
835 {
836  if( axes_property_->getValue().toBool() )
837  {
838  if( !axes_ )
839  {
840  static int count = 0;
841  std::stringstream ss;
842  ss << "Axes for link " << name_ << count++;
843  axes_ = new Axes( scene_manager_, robot_->getOtherNode(), 0.1, 0.01 );
844  axes_->getSceneNode()->setVisible( getEnabled() );
845 
848  }
849  }
850  else
851  {
852  if( axes_ )
853  {
854  delete axes_;
855  axes_ = NULL;
856  }
857  }
858 }
859 
860 void RobotLink::setTransforms( const Ogre::Vector3& visual_position, const Ogre::Quaternion& visual_orientation,
861  const Ogre::Vector3& collision_position, const Ogre::Quaternion& collision_orientation )
862 {
863  if ( visual_node_ )
864  {
865  visual_node_->setPosition( visual_position );
866  visual_node_->setOrientation( visual_orientation );
867  }
868 
869  if ( collision_node_ )
870  {
871  collision_node_->setPosition( collision_position );
872  collision_node_->setOrientation( collision_orientation );
873  }
874 
875  position_property_->setVector( visual_position );
876  orientation_property_->setQuaternion( visual_orientation );
877 
878  if ( axes_ )
879  {
880  axes_->setPosition( visual_position );
881  axes_->setOrientation( visual_orientation );
882  }
883 }
884 
886 {
887  for( size_t i = 0; i < visual_meshes_.size(); i++ )
888  {
889  visual_meshes_[ i ]->setMaterialName("BaseWhiteNoLighting");
890  }
891  for( size_t i = 0; i < collision_meshes_.size(); i++ )
892  {
893  collision_meshes_[ i ]->setMaterialName("BaseWhiteNoLighting");
894  }
895 }
896 
898 {
899  if( using_color_ )
900  {
901  for( size_t i = 0; i < visual_meshes_.size(); i++ )
902  {
903  visual_meshes_[ i ]->setMaterial( color_material_ );
904  }
905  for( size_t i = 0; i < collision_meshes_.size(); i++ )
906  {
907  collision_meshes_[ i ]->setMaterial( color_material_ );
908  }
909  }
910  else
911  {
912  M_SubEntityToMaterial::iterator it = materials_.begin();
913  M_SubEntityToMaterial::iterator end = materials_.end();
914  for (; it != end; ++it)
915  {
916  it->first->setMaterial(it->second);
917  }
918  }
919 }
920 
921 void RobotLink::setColor( float red, float green, float blue )
922 {
923  Ogre::ColourValue color = color_material_->getTechnique(0)->getPass(0)->getDiffuse();
924  color.r = red;
925  color.g = green;
926  color.b = blue;
927  color_material_->getTechnique(0)->setAmbient( 0.5 * color );
928  color_material_->getTechnique(0)->setDiffuse( color );
929 
930  using_color_ = true;
932 }
933 
935 {
936  using_color_ = false;
938 }
939 
940 bool RobotLink::setSelectable( bool selectable )
941 {
942  bool old = is_selectable_;
943  is_selectable_ = selectable;
944  return old;
945 }
946 
948 {
949  return is_selectable_;
950 }
951 
953 {
956  trail_property_->setHidden(hide);
957  axes_property_->setHidden(hide);
958  alpha_property_->setHidden(hide);
959 }
960 
961 Ogre::Vector3 RobotLink::getPosition()
962 {
963  return position_property_->getVector();
964 }
965 
966 Ogre::Quaternion RobotLink::getOrientation()
967 {
969 }
970 
972 {
973  Property* old_parent = link_property_->getParent();
974  if (old_parent)
975  old_parent->takeChild(link_property_);
976 
977  if (new_parent)
978  new_parent->addChild(link_property_);
979 }
980 
981 // if use_detail:
982 // - all sub properties become children of details_ property.
983 // - details_ property becomes a child of link_property_
984 // else (!use_detail)
985 // - all sub properties become children of link_property_.
986 // details_ property does not have a parent.
987 void RobotLink::useDetailProperty(bool use_detail)
988 {
989  Property* old_parent = details_->getParent();
990  if (old_parent)
991  old_parent->takeChild(details_);
992 
993  if (use_detail)
994  {
995  while (link_property_->numChildren() > 0)
996  {
997  Property* child = link_property_->childAt(0);
998  link_property_->takeChild(child);
999  details_->addChild(child);
1000  }
1001 
1003  }
1004  else
1005  {
1006  while (details_->numChildren() > 0)
1007  {
1008  Property* child = details_->childAt(0);
1009  details_->takeChild(child);
1010  link_property_->addChild(child);
1011  }
1012  }
1013 }
1014 
1015 void RobotLink::expandDetails(bool expand)
1016 {
1018  if (expand)
1019  {
1020  parent->expand();
1021  }
1022  else
1023  {
1024  parent->collapse();
1025  }
1026 }
1027 
1028 
1029 } // namespace rviz
1030 
virtual Ogre::Quaternion getQuaternion() const
#define NULL
Definition: global.h:37
filename
VectorProperty * position_property_
Definition: robot_link.cpp:89
virtual void createProperties(const Picked &obj, Property *parent_property)
Override to create properties of the given picked object(s).
Definition: robot_link.cpp:103
virtual void postRenderPass(uint32_t pass)
Definition: robot_link.cpp:147
Property * getParent() const
Return the parent Property.
Definition: property.cpp:226
virtual void expand()
Expand (show the children of) this Property.
Definition: property.cpp:542
static Ogre::Entity * createEntity(const std::string &name, Type shape_type, Ogre::SceneManager *scene_manager)
Definition: shape.cpp:46
virtual bool setVector(const Ogre::Vector3 &vector)
f
Ogre::SceneNode * getOtherNode()
Definition: robot.h:156
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
Definition: property.cpp:130
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
virtual float getFloat() const
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
Ogre::MeshPtr loadMeshFromResource(const std::string &resource_path)
Property specialized to enforce floating point max/min.
#define ROS_WARN(...)
virtual void setDescription(const QString &description)
Set the description.
Definition: property.cpp:164
bool isCollisionVisible()
Returns whether or not the collision representation is set to be visible To be visible this and isVis...
Definition: robot.cpp:188
void calculateJointCheckboxes()
Definition: robot.cpp:675
virtual int numChildren() const
Return the number of child objects (Property or otherwise).
Definition: property.h:215
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: axes.cpp:84
Property * takeChild(Property *child)
Remove a given child object and return a pointer to it.
Definition: property.cpp:316
virtual void collapse()
Collapse (hide the children of) this Property.
Definition: property.cpp:550
Ogre::SceneNode * getCollisionNode()
Definition: robot.h:155
virtual void setIcon(const QIcon &icon)
Set the icon to be displayed next to the property.
Definition: property.h:187
Pure-virtual base class for objects which give Display subclasses context in which to work...
Property * childAt(int index) const
Return the child Property with the given index, or NULL if the index is out of bounds or if the child...
Definition: property.cpp:197
boost::shared_array< uint8_t > data
virtual void updateProperties()
Override to update property values.
Definition: robot_link.cpp:117
DisplayContext * context_
bool isVisible()
Returns whether anything is visible.
Definition: robot.cpp:178
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:58
Ogre::SceneNode * getVisualNode()
Definition: robot.h:154
MemoryResource get(const std::string &url)
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:530
virtual Ogre::Vector3 getVector() const
virtual void addChild(Property *child, int index=-1)
Add a child property.
Definition: property.cpp:350
QList< Property * > properties_
virtual QVariant getValue() const
Return the value of this Property as a QVariant. If the value has never been set, an invalid QVariant...
Definition: property.cpp:145
RobotLinkSelectionHandler(RobotLink *link, DisplayContext *context)
Definition: robot_link.cpp:93
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: axes.cpp:89
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:371
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:90
#define ROS_ERROR(...)
QPixmap loadPixmap(QString url, bool fill_cache)
bool isVisualVisible()
Returns whether or not the visual representation is set to be visible To be visible this and isVisibl...
Definition: robot.cpp:183
QuaternionProperty * orientation_property_
Definition: robot_link.cpp:90
virtual void preRenderPass(uint32_t pass)
Definition: robot_link.cpp:124


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:42