robot.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 "robot.h"
31 #include "robot_link.h"
32 #include "robot_joint.h"
33 #include "properties/property.h"
36 #include "display_context.h"
37 
38 #include "ogre_helpers/object.h"
39 #include "ogre_helpers/shape.h"
40 #include "ogre_helpers/axes.h"
41 
42 #include <urdf_model/model.h>
43 
44 #include <OgreSceneNode.h>
45 #include <OgreSceneManager.h>
46 #include <OgreEntity.h>
47 #include <OgreMaterialManager.h>
48 #include <OgreMaterial.h>
49 #include <OgreResourceGroupManager.h>
50 
51 #include <ros/console.h>
52 #include <ros/assert.h>
53 
54 namespace rviz
55 {
56 
57 Robot::Robot( Ogre::SceneNode* root_node, DisplayContext* context, const std::string& name, Property* parent_property )
58  : scene_manager_( context->getSceneManager() )
59  , visible_( true )
60  , visual_visible_( true )
61  , collision_visible_( false )
62  , context_( context )
63  , name_( name )
64  , doing_set_checkbox_( false )
65  , robot_loaded_( false )
66  , inChangedEnableAllLinks( false )
67 {
68  root_visual_node_ = root_node->createChildSceneNode();
69  root_collision_node_ = root_node->createChildSceneNode();
70  root_other_node_ = root_node->createChildSceneNode();
71 
72  link_factory_ = new LinkFactory();
73 
76  setAlpha(1.0f);
77 
78  link_tree_ = new Property( "Links", QVariant(), "", parent_property );
79  link_tree_->hide(); // hide until loaded
80 
82  "Link Tree Style",
83  "",
84  "How the list of links is displayed",
85  link_tree_,
86  SLOT( changedLinkTreeStyle() ),
87  this );
90  "Expand Tree",
91  false,
92  "Expand or collapse link tree",
93  link_tree_,
94  SLOT( changedExpandTree() ),
95  this );
97  "Expand Link Details",
98  false,
99  "Expand link details (sub properties) to see all info for all links.",
100  link_tree_,
101  SLOT( changedExpandLinkDetails() ),
102  this );
104  "Expand Joint Details",
105  false,
106  "Expand joint details (sub properties) to see all info for all joints.",
107  link_tree_,
108  SLOT( changedExpandJointDetails() ),
109  this );
111  "All Links Enabled",
112  true,
113  "Turn all links on or off.",
114  link_tree_,
115  SLOT( changedEnableAllLinks() ),
116  this );
117 }
118 
120 {
121  clear();
122 
123  scene_manager_->destroySceneNode( root_visual_node_->getName() );
124  scene_manager_->destroySceneNode( root_collision_node_->getName() );
125  scene_manager_->destroySceneNode( root_other_node_->getName() );
126  delete link_factory_;
127 }
128 
130 {
131  if (link_factory)
132  {
133  delete link_factory_;
134  link_factory_ = link_factory;
135  }
136 }
137 
138 void Robot::setVisible( bool visible )
139 {
140  visible_ = visible;
141  if ( visible )
142  {
143  root_visual_node_->setVisible( visual_visible_ );
146  }
147  else
148  {
149  root_visual_node_->setVisible( false );
150  root_collision_node_->setVisible( false );
152  }
153 }
154 
155 void Robot::setVisualVisible( bool visible )
156 {
157  visual_visible_ = visible;
159 }
160 
161 void Robot::setCollisionVisible( bool visible )
162 {
163  collision_visible_ = visible;
165 }
166 
168 {
169  M_NameToLink::iterator it = links_.begin();
170  M_NameToLink::iterator end = links_.end();
171  for ( ; it != end; ++it )
172  {
173  RobotLink* link = it->second;
174  link->updateVisibility();
175  }
176 }
177 
179 {
180  return visible_;
181 }
182 
184 {
185  return visual_visible_;
186 }
187 
189 {
190  return collision_visible_;
191 }
192 
193 void Robot::setAlpha(float a)
194 {
195  alpha_ = a;
196 
197  M_NameToLink::iterator it = links_.begin();
198  M_NameToLink::iterator end = links_.end();
199  for ( ; it != end; ++it )
200  {
201  RobotLink* link = it->second;
202 
203  link->setRobotAlpha(alpha_);
204  }
205 }
206 
208 {
209  // unparent all link and joint properties so they can be deleted in arbitrary
210  // order without being delete by their parent propeties (which vary based on
211  // style)
213 
214  M_NameToLink::iterator link_it = links_.begin();
215  M_NameToLink::iterator link_end = links_.end();
216  for ( ; link_it != link_end; ++link_it )
217  {
218  RobotLink* link = link_it->second;
219  delete link;
220  }
221 
222  M_NameToJoint::iterator joint_it = joints_.begin();
223  M_NameToJoint::iterator joint_end = joints_.end();
224  for ( ; joint_it != joint_end; ++joint_it )
225  {
226  RobotJoint* joint = joint_it->second;
227  delete joint;
228  }
229 
230  links_.clear();
231  joints_.clear();
232  root_visual_node_->removeAndDestroyAllChildren();
233  root_collision_node_->removeAndDestroyAllChildren();
234  root_other_node_->removeAndDestroyAllChildren();
235 }
236 
238  Robot* robot,
239  const urdf::LinkConstSharedPtr& link,
240  const std::string& parent_joint_name,
241  bool visual,
242  bool collision)
243 {
244  return new RobotLink(robot, link, parent_joint_name, visual, collision);
245 }
246 
248  Robot* robot,
249  const urdf::JointConstSharedPtr& joint)
250 {
251  return new RobotJoint(robot, joint);
252 }
253 
254 void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
255 {
256  link_tree_->hide(); // hide until loaded
257  robot_loaded_ = false;
258 
259  // clear out any data (properties, shapes, etc) from a previously loaded robot.
260  clear();
261 
262  // the root link is discovered below. Set to NULL until found.
263  root_link_ = NULL;
264 
265  // Create properties for each link.
266  // Properties are not added to display until changedLinkTreeStyle() is called (below).
267  {
268  typedef std::map<std::string, urdf::LinkSharedPtr > M_NameToUrdfLink;
269  M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
270  M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
271  for( ; link_it != link_end; ++link_it )
272  {
273  const urdf::LinkConstSharedPtr& urdf_link = link_it->second;
274  std::string parent_joint_name;
275 
276  if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
277  {
278  parent_joint_name = urdf_link->parent_joint->name;
279  }
280 
281  RobotLink* link = link_factory_->createLink( this,
282  urdf_link,
283  parent_joint_name,
284  visual,
285  collision );
286 
287  if (urdf_link == urdf.getRoot())
288  {
289  root_link_ = link;
290  }
291 
292  links_[urdf_link->name] = link;
293 
294  link->setRobotAlpha( alpha_ );
295  }
296  }
297 
298  // Create properties for each joint.
299  // Properties are not added to display until changedLinkTreeStyle() is called (below).
300  {
301  typedef std::map<std::string, urdf::JointSharedPtr > M_NameToUrdfJoint;
302  M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
303  M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
304  for( ; joint_it != joint_end; ++joint_it )
305  {
306  const urdf::JointConstSharedPtr& urdf_joint = joint_it->second;
307  RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );
308 
309  joints_[urdf_joint->name] = joint;
310 
311  joint->setRobotAlpha( alpha_ );
312  }
313  }
314 
315  // robot is now loaded
316  robot_loaded_ = true;
317  link_tree_->show();
318 
319  // set the link tree style and add link/joint properties to rviz pane.
322 
323  // at startup the link tree is collapsed since it is large and not often needed.
324  link_tree_->collapse();
325 
328 }
329 
331 {
332  // remove link properties from their parents
333  M_NameToLink::iterator link_it = links_.begin();
334  M_NameToLink::iterator link_end = links_.end();
335  for ( ; link_it != link_end ; ++link_it )
336  {
337  link_it->second->setParentProperty(NULL);
338  }
339 
340  // remove joint properties from their parents
341  M_NameToJoint::iterator joint_it = joints_.begin();
342  M_NameToJoint::iterator joint_end = joints_.end();
343  for ( ; joint_it != joint_end ; ++joint_it )
344  {
345  joint_it->second->setParentProperty(NULL);
346  }
347 }
348 
349 void Robot::useDetailProperty(bool use_detail)
350 {
351  // remove sub properties and add them to detail
352  M_NameToLink::iterator link_it = links_.begin();
353  M_NameToLink::iterator link_end = links_.end();
354  for ( ; link_it != link_end ; ++link_it )
355  {
356  link_it->second->useDetailProperty(use_detail);
357  }
358 
359  // remove joint properties from their parents
360  M_NameToJoint::iterator joint_it = joints_.begin();
361  M_NameToJoint::iterator joint_end = joints_.end();
362  for ( ; joint_it != joint_end ; ++joint_it )
363  {
364  joint_it->second->useDetailProperty(use_detail);
365  }
366 }
367 
369 {
370  bool expand = expand_tree_->getBool();
371 
372  M_NameToLink::iterator link_it = links_.begin();
373  M_NameToLink::iterator link_end = links_.end();
374  for ( ; link_it != link_end ; ++link_it )
375  {
376  if (expand)
377  link_it->second->getLinkProperty()->expand();
378  else
379  link_it->second->getLinkProperty()->collapse();
380  }
381 
382  M_NameToJoint::iterator joint_it = joints_.begin();
383  M_NameToJoint::iterator joint_end = joints_.end();
384  for ( ; joint_it != joint_end ; ++joint_it )
385  {
386  if (expand)
387  joint_it->second->getJointProperty()->expand();
388  else
389  joint_it->second->getJointProperty()->collapse();
390  }
391 }
392 
394 {
395  bool hide = /* !show_details_->getBool(); */ false;
396 
397  M_NameToLink::iterator link_it = links_.begin();
398  M_NameToLink::iterator link_end = links_.end();
399  for ( ; link_it != link_end ; ++link_it )
400  {
401  link_it->second->hideSubProperties(hide);
402  }
403 
404  M_NameToJoint::iterator joint_it = joints_.begin();
405  M_NameToJoint::iterator joint_end = joints_.end();
406  for ( ; joint_it != joint_end ; ++joint_it )
407  {
408  joint_it->second->hideSubProperties(hide);
409  }
410 }
411 
413 {
414  bool expand = expand_link_details_->getBool();
415 
416  M_NameToLink::iterator link_it = links_.begin();
417  M_NameToLink::iterator link_end = links_.end();
418  for ( ; link_it != link_end ; ++link_it )
419  {
420  link_it->second->expandDetails(expand);
421  }
422 }
423 
425 {
426  bool expand = expand_joint_details_->getBool();
427 
428  M_NameToJoint::iterator joint_it = joints_.begin();
429  M_NameToJoint::iterator joint_end = joints_.end();
430  for ( ; joint_it != joint_end ; ++joint_it )
431  {
432  joint_it->second->expandDetails(expand);
433  }
434 }
435 
437 {
439  return;
440 
441  bool enable = enable_all_links_->getBool();
442 
444 
445  M_NameToLink::iterator link_it = links_.begin();
446  M_NameToLink::iterator link_end = links_.end();
447  for ( ; link_it != link_end ; ++link_it )
448  {
449  if (link_it->second->hasGeometry())
450  {
451  link_it->second->getLinkProperty()->setValue(enable);
452  }
453  }
454 
455  M_NameToJoint::iterator joint_it = joints_.begin();
456  M_NameToJoint::iterator joint_end = joints_.end();
457  for ( ; joint_it != joint_end ; ++joint_it )
458  {
459  if (joint_it->second->hasDescendentLinksWithGeometry())
460  {
461  joint_it->second->getJointProperty()->setValue(enable);
462  }
463  }
464 
465  inChangedEnableAllLinks = false;
466 }
467 
469 {
470  // doing_set_checkbox_ prevents changedEnableAllLinks from turning all
471  // links off when we modify the enable_all_links_ property.
472  doing_set_checkbox_ = true;
474  doing_set_checkbox_ = false;
475 }
476 
478 {
479  style_name_map_.clear();
480  style_name_map_[STYLE_LINK_LIST] = "Links in Alphabetic Order";
481  style_name_map_[STYLE_JOINT_LIST] = "Joints in Alphabetic Order";
482  style_name_map_[STYLE_LINK_TREE] = "Tree of links";
483  style_name_map_[STYLE_JOINT_LINK_TREE] = "Tree of links and joints";
484 
486  std::map<LinkTreeStyle, std::string>::const_iterator style_it = style_name_map_.begin();
487  std::map<LinkTreeStyle, std::string>::const_iterator style_end = style_name_map_.end();
488  for ( ; style_it != style_end ; ++style_it )
489  {
490  link_tree_style_->addOptionStd( style_it->second, style_it->first );
491  }
492 }
493 
495 {
496  return
497  style == STYLE_LINK_LIST ||
498  style == STYLE_LINK_TREE ||
499  style == STYLE_JOINT_LINK_TREE;
500 }
501 
503 {
504  return
505  style == STYLE_JOINT_LIST ||
506  style == STYLE_JOINT_LINK_TREE;
507 }
508 
510 {
511  return
512  style == STYLE_LINK_TREE ||
513  style == STYLE_JOINT_LINK_TREE;
514 }
515 
517 {
518  std::map<LinkTreeStyle, std::string>::const_iterator style_it = style_name_map_.find(style);
519  if (style_it == style_name_map_.end())
521  else
522  link_tree_style_->setValue(style_it->second.c_str());
523 }
524 
525 // insert properties into link_tree_ according to style
527 {
528  if (!robot_loaded_)
529  return;
530 
532 
534 
535  //expand_tree_->setValue(false);
536 
537  switch (style)
538  {
539  case STYLE_LINK_TREE:
541  useDetailProperty(true);
542  if (root_link_)
543  {
545  }
546  break;
547 
548  case STYLE_JOINT_LIST:
549  {
550  useDetailProperty(false);
551  M_NameToJoint::iterator joint_it = joints_.begin();
552  M_NameToJoint::iterator joint_end = joints_.end();
553  for ( ; joint_it != joint_end ; ++joint_it )
554  {
555  joint_it->second->setParentProperty(link_tree_);
556  joint_it->second->setJointPropertyDescription();
557  }
558  break;
559  }
560 
561  case STYLE_LINK_LIST:
562  default:
563  useDetailProperty(false);
564  M_NameToLink::iterator link_it = links_.begin();
565  M_NameToLink::iterator link_end = links_.end();
566  for ( ; link_it != link_end ; ++link_it )
567  {
568  link_it->second->setParentProperty(link_tree_);
569  }
570  break;
571  }
572 
573  switch (style)
574  {
575  case STYLE_LINK_TREE:
576  link_tree_->setName("Link Tree");
577  link_tree_->setDescription("A tree of all links in the robot. Uncheck a link to hide its geometry.");
578  expand_tree_->show();
581  break;
583  link_tree_->setName("Link/Joint Tree");
584  link_tree_->setDescription("A tree of all joints and links in the robot. Uncheck a link to hide its geometry.");
585  expand_tree_->show();
588  break;
589  case STYLE_JOINT_LIST:
590  link_tree_->setName("Joints");
591  link_tree_->setDescription("All joints in the robot in alphabetic order.");
592  expand_tree_->hide();
595  break;
596  case STYLE_LINK_LIST:
597  default:
598  link_tree_->setName("Links");
599  link_tree_->setDescription("All links in the robot in alphabetic order. Uncheck a link to hide its geometry.");
600  expand_tree_->hide();
603  break;
604  }
605 
608  expand_tree_->setValue(false);
610 }
611 
612 
613 // recursive helper for setLinkTreeStyle() when style is *_TREE
615 {
616  if (styleShowLink(style))
617  {
618  link->setParentProperty(parent);
619  parent = link->getLinkProperty();
620  }
621 
622  std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
623  std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
624  for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
625  {
626  RobotJoint* child_joint = getJoint( *child_joint_it );
627  if (child_joint)
628  {
629  addJointToLinkTree(style, parent, child_joint);
630  }
631  }
632 }
633 
634 // recursive helper for setLinkTreeStyle() when style is *_TREE
636 {
637  if (styleShowJoint(style))
638  {
639  joint->setParentProperty(parent);
640  parent = joint->getJointProperty();
642  }
643 
644  RobotLink *link = getLink( joint->getChildLinkName() );
645  if (link)
646  {
647  addLinkToLinkTree(style, parent, link);
648  }
649 }
650 
651 RobotLink* Robot::getLink( const std::string& name )
652 {
653  M_NameToLink::iterator it = links_.find( name );
654  if ( it == links_.end() )
655  {
656  ROS_WARN( "Link [%s] does not exist", name.c_str() );
657  return NULL;
658  }
659 
660  return it->second;
661 }
662 
663 RobotJoint* Robot::getJoint( const std::string& name )
664 {
665  M_NameToJoint::iterator it = joints_.find( name );
666  if ( it == joints_.end() )
667  {
668  ROS_WARN( "Joint [%s] does not exist", name.c_str() );
669  return NULL;
670  }
671 
672  return it->second;
673 }
674 
676 {
678  return;
679 
680  int links_with_geom_checked = 0;
681  int links_with_geom_unchecked = 0;
682 
683  // check root link
684  RobotLink *link = root_link_;
685 
686  if(!link)
687  {
688  setEnableAllLinksCheckbox(QVariant());
689  return;
690  }
691 
692  if (link->hasGeometry())
693  {
694  bool checked = link->getLinkProperty()->getValue().toBool();
695  links_with_geom_checked += checked ? 1 : 0;
696  links_with_geom_unchecked += checked ? 0 : 1;
697  }
698  int links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
699 
700  // check all child links and joints recursively
701  std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
702  std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
703  for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
704  {
705  RobotJoint* child_joint = getJoint( *child_joint_it );
706  if (child_joint)
707  {
708  int child_links_with_geom;
709  int child_links_with_geom_checked;
710  int child_links_with_geom_unchecked;
711  child_joint->calculateJointCheckboxesRecursive(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked);
712  links_with_geom_checked += child_links_with_geom_checked;
713  links_with_geom_unchecked += child_links_with_geom_unchecked;
714  }
715  }
716  links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
717 
718  if (!links_with_geom)
719  {
720  setEnableAllLinksCheckbox(QVariant());
721  }
722  else
723  {
724  setEnableAllLinksCheckbox(links_with_geom_unchecked == 0);
725  }
726 }
727 
728 void Robot::update(const LinkUpdater& updater)
729 {
730  M_NameToLink::iterator link_it = links_.begin();
731  M_NameToLink::iterator link_end = links_.end();
732  for ( ; link_it != link_end; ++link_it )
733  {
734  RobotLink* link = link_it->second;
735 
736  link->setToNormalMaterial();
737 
738  Ogre::Vector3 visual_position, collision_position;
739  Ogre::Quaternion visual_orientation, collision_orientation;
740  if( updater.getLinkTransforms( link->getName(),
741  visual_position, visual_orientation,
742  collision_position, collision_orientation
743  ))
744  {
745  // Check if visual_orientation, visual_position, collision_orientation, and collision_position are NaN.
746  if (visual_orientation.isNaN())
747  {
749  1.0,
750  "visual orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
751  link->getName().c_str()
752  );
753  continue;
754  }
755  if (visual_position.isNaN())
756  {
758  1.0,
759  "visual position of %s contains NaNs. Skipping render as long as the position is invalid.",
760  link->getName().c_str()
761  );
762  continue;
763  }
764  if (collision_orientation.isNaN())
765  {
767  1.0,
768  "collision orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
769  link->getName().c_str()
770  );
771  continue;
772  }
773  if (collision_position.isNaN())
774  {
776  1.0,
777  "collision position of %s contains NaNs. Skipping render as long as the position is invalid.",
778  link->getName().c_str()
779  );
780  continue;
781  }
782  link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation );
783 
784  std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin();
785  std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end();
786  for ( ; joint_it != joint_end ; ++joint_it )
787  {
788  RobotJoint *joint = getJoint(*joint_it);
789  if (joint)
790  {
791  joint->setTransforms(visual_position, visual_orientation);
792  }
793  }
794  }
795  else
796  {
797  link->setToErrorMaterial();
798  }
799  }
800 }
801 
802 void Robot::setPosition( const Ogre::Vector3& position )
803 {
804  root_visual_node_->setPosition( position );
805  root_collision_node_->setPosition( position );
806 }
807 
808 void Robot::setOrientation( const Ogre::Quaternion& orientation )
809 {
810  root_visual_node_->setOrientation( orientation );
811  root_collision_node_->setOrientation( orientation );
812 }
813 
814 void Robot::setScale( const Ogre::Vector3& scale )
815 {
816  root_visual_node_->setScale( scale );
817  root_collision_node_->setScale( scale );
818 }
819 
820 const Ogre::Vector3& Robot::getPosition()
821 {
822  return root_visual_node_->getPosition();
823 }
824 
825 const Ogre::Quaternion& Robot::getOrientation()
826 {
827  return root_visual_node_->getOrientation();
828 }
829 
830 } // namespace rviz
virtual void update(const LinkUpdater &updater)
Definition: robot.cpp:728
void setParentProperty(Property *new_parent)
Robot(Ogre::SceneNode *root_node, DisplayContext *context, const std::string &name, Property *parent_property)
Definition: robot.cpp:57
void setEnableAllLinksCheckbox(QVariant val)
Definition: robot.cpp:468
#define NULL
Definition: global.h:37
float alpha_
Definition: robot.h:276
void addLinkToLinkTree(LinkTreeStyle style, Property *parent, RobotLink *link)
Definition: robot.cpp:614
void setJointPropertyDescription()
M_NameToJoint joints_
Map of name to joint info, stores all loaded joints.
Definition: robot.h:246
virtual void clear()
Clears all data loaded from a URDF.
Definition: robot.cpp:207
void setAlpha(float a)
Definition: robot.cpp:193
f
LinkTreeStyle
Definition: robot.h:191
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
void useDetailProperty(bool use_detail)
Definition: robot.cpp:349
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
void calculateJointCheckboxesRecursive(int &links_with_geom, int &links_with_geom_checked, int &links_with_geom_unchecked)
virtual void clearOptions()
Clear the list of options.
virtual void setName(const QString &name)
Set the name.
Definition: property.cpp:150
void changedLinkTreeStyle()
Definition: robot.cpp:526
BoolProperty * expand_joint_details_
Definition: robot.h:264
LinkFactory * link_factory_
factory for generating links and joints
Definition: robot.h:249
virtual void setPosition(const Ogre::Vector3 &position)
Definition: robot.cpp:802
virtual RobotLink * createLink(Robot *robot, const urdf::LinkConstSharedPtr &link, const std::string &parent_joint_name, bool visual, bool collision)
Definition: robot.cpp:237
void setTransforms(const Ogre::Vector3 &parent_link_position, const Ogre::Quaternion &parent_link_orientation)
virtual const Ogre::Vector3 & getPosition()
Definition: robot.cpp:820
#define ROS_WARN(...)
virtual void setDescription(const QString &description)
Set the description.
Definition: property.cpp:164
RobotLink * getLink(const std::string &name)
Definition: robot.cpp:651
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
std::map< LinkTreeStyle, std::string > style_name_map_
Definition: robot.h:266
static bool styleShowJoint(LinkTreeStyle style)
Definition: robot.cpp:502
virtual void load(const urdf::ModelInterface &urdf, bool visual=true, bool collision=true)
Loads meshes/primitives from a robot description. Calls clear() before loading.
Definition: robot.cpp:254
virtual bool getBool() const
virtual void collapse()
Collapse (hide the children of) this Property.
Definition: property.cpp:550
bool visual_visible_
Should we show the visual representation?
Definition: robot.h:256
#define ROS_ERROR_THROTTLE(rate,...)
virtual void setScale(const Ogre::Vector3 &scale)
Definition: robot.cpp:814
void setLinkFactory(LinkFactory *link_factory)
Definition: robot.cpp:129
void changedExpandLinkDetails()
Definition: robot.cpp:412
Pure-virtual base class for objects which give Display subclasses context in which to work...
RobotLink * root_link_
Definition: robot.h:247
Ogre::SceneNode * root_visual_node_
Node all our visual nodes are children of.
Definition: robot.h:251
void setCollisionVisible(bool visible)
Set whether the collision meshes/primitives of the robot should be visible.
Definition: robot.cpp:161
void setLinkTreeStyle(LinkTreeStyle style)
Definition: robot.cpp:516
void show()
Show this Property in any PropertyTreeWidgets.
Definition: property.h:377
bool isVisible()
Returns whether anything is visible.
Definition: robot.cpp:178
void updateLinkVisibilities()
Call RobotLink::updateVisibility() on each link.
Definition: robot.cpp:167
void changedExpandJointDetails()
Definition: robot.cpp:424
BoolProperty * enable_all_links_
Definition: robot.h:265
BoolProperty * expand_tree_
Definition: robot.h:262
virtual bool getLinkTransforms(const std::string &link_name, Ogre::Vector3 &visual_position, Ogre::Quaternion &visual_orientation, Ogre::Vector3 &collision_position, Ogre::Quaternion &collision_orientation) const =0
void addJointToLinkTree(LinkTreeStyle style, Property *parent, RobotJoint *joint)
Definition: robot.cpp:635
void addOptionStd(const std::string &option, int value=0)
Definition: enum_property.h:63
Contains any data we need from a joint in the robot.
Definition: robot_joint.h:84
virtual void setOrientation(const Ogre::Quaternion &orientation)
Definition: robot.cpp:808
void setRobotAlpha(float a)
Definition: robot_joint.h:109
Ogre::SceneManager * scene_manager_
Definition: robot.h:243
bool visible_
Should we show anything at all? (affects visual, collision, axes, and trails)
Definition: robot.h:255
bool inChangedEnableAllLinks
Definition: robot.h:273
BoolProperty * expand_link_details_
Definition: robot.h:263
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
const Property * getJointProperty() const
Definition: robot_joint.h:98
static bool styleShowLink(LinkTreeStyle style)
Definition: robot.cpp:494
bool robot_loaded_
Definition: robot.h:269
void setVisualVisible(bool visible)
Set whether the visual meshes of the robot should be visible.
Definition: robot.cpp:155
void changedHideSubProperties()
Definition: robot.cpp:393
bool doing_set_checkbox_
Definition: robot.h:268
EnumProperty * link_tree_style_
Definition: robot.h:261
virtual ~Robot()
Definition: robot.cpp:119
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
Ogre::SceneNode * root_other_node_
Definition: robot.h:253
virtual RobotJoint * createJoint(Robot *robot, const urdf::JointConstSharedPtr &joint)
Definition: robot.cpp:247
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:371
Property * link_tree_
Definition: robot.h:260
const std::string & getChildLinkName() const
Definition: robot_joint.h:97
bool collision_visible_
Should we show the collision representation?
Definition: robot.h:257
Ogre::SceneNode * root_collision_node_
Node all our collision nodes are children of.
Definition: robot.h:252
virtual void setVisible(bool visible)
Set the robot as a whole to be visible or not.
Definition: robot.cpp:138
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
virtual const Ogre::Quaternion & getOrientation()
Definition: robot.cpp:825
M_NameToLink links_
Map of name to link info, stores all loaded links.
Definition: robot.h:245
Enum property.
Definition: enum_property.h:47
bool isVisualVisible()
Returns whether or not the visual representation is set to be visible To be visible this and isVisibl...
Definition: robot.cpp:183
void changedEnableAllLinks()
Definition: robot.cpp:436
static bool styleIsTree(LinkTreeStyle style)
Definition: robot.cpp:509
void unparentLinkProperties()
Definition: robot.cpp:330
void changedExpandTree()
Definition: robot.cpp:368
void initLinkTreeStyle()
Definition: robot.cpp:477
RobotJoint * getJoint(const std::string &name)
Definition: robot.cpp:663


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51