robot.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "robot.h"
00031 #include "robot_link.h"
00032 #include "robot_joint.h"
00033 #include "properties/property.h"
00034 #include "properties/enum_property.h"
00035 #include "properties/bool_property.h"
00036 #include "display_context.h"
00037 
00038 #include "ogre_helpers/object.h"
00039 #include "ogre_helpers/shape.h"
00040 #include "ogre_helpers/axes.h"
00041 
00042 #include <urdf_model/model.h>
00043 
00044 #include <OgreSceneNode.h>
00045 #include <OgreSceneManager.h>
00046 #include <OgreEntity.h>
00047 #include <OgreMaterialManager.h>
00048 #include <OgreMaterial.h>
00049 #include <OgreResourceGroupManager.h>
00050 
00051 #include <ros/console.h>
00052 #include <ros/assert.h>
00053 
00054 namespace rviz
00055 {
00056 
00057 Robot::Robot( Ogre::SceneNode* root_node, DisplayContext* context, const std::string& name, Property* parent_property )
00058   : scene_manager_( context->getSceneManager() )
00059   , visible_( true )
00060   , visual_visible_( true )
00061   , collision_visible_( false )
00062   , context_( context )
00063   , name_( name )
00064   , doing_set_checkbox_( false )
00065   , robot_loaded_( false )
00066   , inChangedEnableAllLinks( false )
00067 {
00068   root_visual_node_ = root_node->createChildSceneNode();
00069   root_collision_node_ = root_node->createChildSceneNode();
00070   root_other_node_ = root_node->createChildSceneNode();
00071 
00072   link_factory_ = new LinkFactory();
00073 
00074   setVisualVisible( visual_visible_ );
00075   setCollisionVisible( collision_visible_ );
00076   setAlpha(1.0f);
00077 
00078   link_tree_ = new Property( "Links", QVariant(), "", parent_property );
00079   link_tree_->hide(); // hide until loaded
00080 
00081   link_tree_style_ = new EnumProperty(
00082                             "Link Tree Style",
00083                             "",
00084                             "How the list of links is displayed",
00085                             link_tree_,
00086                             SLOT( changedLinkTreeStyle() ),
00087                             this );
00088   initLinkTreeStyle();
00089   expand_tree_= new BoolProperty(
00090                             "Expand Tree",
00091                             false,
00092                             "Expand or collapse link tree",
00093                             link_tree_,
00094                             SLOT( changedExpandTree() ),
00095                             this );
00096   expand_link_details_ = new BoolProperty(
00097                             "Expand Link Details",
00098                             false,
00099                             "Expand link details (sub properties) to see all info for all links.",
00100                             link_tree_,
00101                             SLOT( changedExpandLinkDetails() ),
00102                             this );
00103   expand_joint_details_ = new BoolProperty(
00104                             "Expand Joint Details",
00105                             false,
00106                             "Expand joint details (sub properties) to see all info for all joints.",
00107                             link_tree_,
00108                             SLOT( changedExpandJointDetails() ),
00109                             this );
00110   enable_all_links_ = new BoolProperty(
00111                             "All Links Enabled",
00112                             true,
00113                             "Turn all links on or off.",
00114                             link_tree_,
00115                             SLOT( changedEnableAllLinks() ),
00116                             this );
00117 }
00118 
00119 Robot::~Robot()
00120 {
00121   clear();
00122 
00123   scene_manager_->destroySceneNode( root_visual_node_->getName() );
00124   scene_manager_->destroySceneNode( root_collision_node_->getName() );
00125   scene_manager_->destroySceneNode( root_other_node_->getName() );
00126   delete link_factory_;
00127 }
00128 
00129 void Robot::setLinkFactory(LinkFactory *link_factory)
00130 {
00131   if (link_factory)
00132   {
00133     delete link_factory_;
00134     link_factory_ = link_factory;
00135   }
00136 }
00137 
00138 void Robot::setVisible( bool visible )
00139 {
00140   visible_ = visible;
00141   if ( visible )
00142   {
00143     root_visual_node_->setVisible( visual_visible_ );
00144     root_collision_node_->setVisible( collision_visible_ );
00145     updateLinkVisibilities();
00146   }
00147   else
00148   {
00149     root_visual_node_->setVisible( false );
00150     root_collision_node_->setVisible( false );
00151     updateLinkVisibilities();
00152   }
00153 }
00154 
00155 void Robot::setVisualVisible( bool visible )
00156 {
00157   visual_visible_ = visible;
00158   updateLinkVisibilities();
00159 }
00160 
00161 void Robot::setCollisionVisible( bool visible )
00162 {
00163   collision_visible_ = visible;
00164   updateLinkVisibilities();
00165 }
00166 
00167 void Robot::updateLinkVisibilities()
00168 {
00169   M_NameToLink::iterator it = links_.begin();
00170   M_NameToLink::iterator end = links_.end();
00171   for ( ; it != end; ++it )
00172   {
00173     RobotLink* link = it->second;
00174     link->updateVisibility();
00175   }
00176 }
00177 
00178 bool Robot::isVisible()
00179 {
00180   return visible_;
00181 }
00182 
00183 bool Robot::isVisualVisible()
00184 {
00185   return visual_visible_;
00186 }
00187 
00188 bool Robot::isCollisionVisible()
00189 {
00190   return collision_visible_;
00191 }
00192 
00193 void Robot::setAlpha(float a)
00194 {
00195   alpha_ = a;
00196 
00197   M_NameToLink::iterator it = links_.begin();
00198   M_NameToLink::iterator end = links_.end();
00199   for ( ; it != end; ++it )
00200   {
00201     RobotLink* link = it->second;
00202 
00203     link->setRobotAlpha(alpha_);
00204   }
00205 }
00206 
00207 void Robot::clear()
00208 {
00209   // unparent all link and joint properties so they can be deleted in arbitrary
00210   // order without being delete by their parent propeties (which vary based on
00211   // style)
00212   unparentLinkProperties();
00213 
00214   M_NameToLink::iterator link_it = links_.begin();
00215   M_NameToLink::iterator link_end = links_.end();
00216   for ( ; link_it != link_end; ++link_it )
00217   {
00218     RobotLink* link = link_it->second;
00219     delete link;
00220   }
00221 
00222   M_NameToJoint::iterator joint_it = joints_.begin();
00223   M_NameToJoint::iterator joint_end = joints_.end();
00224   for ( ; joint_it != joint_end; ++joint_it )
00225   {
00226     RobotJoint* joint = joint_it->second;
00227     delete joint;
00228   }
00229 
00230   links_.clear();
00231   joints_.clear();
00232   root_visual_node_->removeAndDestroyAllChildren();
00233   root_collision_node_->removeAndDestroyAllChildren();
00234   root_other_node_->removeAndDestroyAllChildren();
00235 }
00236 
00237 RobotLink* Robot::LinkFactory::createLink(
00238     Robot* robot,
00239     const boost::shared_ptr<const urdf::Link>& link,
00240     const std::string& parent_joint_name,
00241     bool visual,
00242     bool collision)
00243 {
00244   return new RobotLink(robot, link, parent_joint_name, visual, collision);
00245 }
00246 
00247 RobotJoint* Robot::LinkFactory::createJoint(
00248     Robot* robot,
00249     const boost::shared_ptr<const urdf::Joint>& joint)
00250 {
00251   return new RobotJoint(robot, joint);
00252 }
00253 
00254 void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
00255 {
00256   link_tree_->hide(); // hide until loaded
00257   robot_loaded_ = false;
00258 
00259   // clear out any data (properties, shapes, etc) from a previously loaded robot.
00260   clear();
00261 
00262   // the root link is discovered below.  Set to NULL until found.
00263   root_link_ = NULL;
00264 
00265   // Create properties for each link.
00266   // Properties are not added to display until changedLinkTreeStyle() is called (below).
00267   {
00268     typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
00269     M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
00270     M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
00271     for( ; link_it != link_end; ++link_it )
00272     {
00273       const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
00274       std::string parent_joint_name;
00275 
00276       if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
00277       {
00278         parent_joint_name = urdf_link->parent_joint->name;
00279       }
00280       
00281       RobotLink* link = link_factory_->createLink( this,
00282                                                    urdf_link,
00283                                                    parent_joint_name,
00284                                                    visual,
00285                                                    collision );
00286 
00287       if (urdf_link == urdf.getRoot())
00288       {
00289         root_link_ = link;
00290       }
00291 
00292       links_[urdf_link->name] = link;
00293 
00294       link->setRobotAlpha( alpha_ );
00295     }
00296   }
00297 
00298   // Create properties for each joint.
00299   // Properties are not added to display until changedLinkTreeStyle() is called (below).
00300   {
00301     typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
00302     M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
00303     M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
00304     for( ; joint_it != joint_end; ++joint_it )
00305     {
00306       const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
00307       RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );
00308 
00309       joints_[urdf_joint->name] = joint;
00310 
00311       joint->setRobotAlpha( alpha_ );
00312     }
00313   }
00314 
00315   // robot is now loaded
00316   robot_loaded_ = true;
00317   link_tree_->show();
00318 
00319   // set the link tree style and add link/joint properties to rviz pane.
00320   setLinkTreeStyle(LinkTreeStyle(link_tree_style_->getOptionInt()));
00321   changedLinkTreeStyle();
00322 
00323   // at startup the link tree is collapsed since it is large and not often needed.
00324   link_tree_->collapse();
00325 
00326   setVisualVisible( isVisualVisible() );
00327   setCollisionVisible( isCollisionVisible() );
00328 }
00329 
00330 void Robot::unparentLinkProperties()
00331 {
00332   // remove link properties from their parents
00333   M_NameToLink::iterator link_it = links_.begin();
00334   M_NameToLink::iterator link_end = links_.end();
00335   for ( ; link_it != link_end ; ++link_it )
00336   {
00337     link_it->second->setParentProperty(NULL);
00338   }
00339 
00340   // remove joint properties from their parents
00341   M_NameToJoint::iterator joint_it = joints_.begin();
00342   M_NameToJoint::iterator joint_end = joints_.end();
00343   for ( ; joint_it != joint_end ; ++joint_it )
00344   {
00345     joint_it->second->setParentProperty(NULL);
00346   }
00347 }
00348 
00349 void Robot::useDetailProperty(bool use_detail)
00350 {
00351   // remove sub properties and add them to detail
00352   M_NameToLink::iterator link_it = links_.begin();
00353   M_NameToLink::iterator link_end = links_.end();
00354   for ( ; link_it != link_end ; ++link_it )
00355   {
00356     link_it->second->useDetailProperty(use_detail);
00357   }
00358 
00359   // remove joint properties from their parents
00360   M_NameToJoint::iterator joint_it = joints_.begin();
00361   M_NameToJoint::iterator joint_end = joints_.end();
00362   for ( ; joint_it != joint_end ; ++joint_it )
00363   {
00364     joint_it->second->useDetailProperty(use_detail);
00365   }
00366 }
00367 
00368 void Robot::changedExpandTree()
00369 {
00370   bool expand = expand_tree_->getBool();
00371 
00372   M_NameToLink::iterator link_it = links_.begin();
00373   M_NameToLink::iterator link_end = links_.end();
00374   for ( ; link_it != link_end ; ++link_it )
00375   {
00376     if (expand)
00377       link_it->second->getLinkProperty()->expand();
00378     else
00379       link_it->second->getLinkProperty()->collapse();
00380   }
00381 
00382   M_NameToJoint::iterator joint_it = joints_.begin();
00383   M_NameToJoint::iterator joint_end = joints_.end();
00384   for ( ; joint_it != joint_end ; ++joint_it )
00385   {
00386     if (expand)
00387       joint_it->second->getJointProperty()->expand();
00388     else
00389       joint_it->second->getJointProperty()->collapse();
00390   }
00391 }
00392 
00393 void Robot::changedHideSubProperties()
00394 {
00395   bool hide = /* !show_details_->getBool(); */ false;
00396 
00397   M_NameToLink::iterator link_it = links_.begin();
00398   M_NameToLink::iterator link_end = links_.end();
00399   for ( ; link_it != link_end ; ++link_it )
00400   {
00401     link_it->second->hideSubProperties(hide);
00402   }
00403 
00404   M_NameToJoint::iterator joint_it = joints_.begin();
00405   M_NameToJoint::iterator joint_end = joints_.end();
00406   for ( ; joint_it != joint_end ; ++joint_it )
00407   {
00408     joint_it->second->hideSubProperties(hide);
00409   }
00410 }
00411 
00412 void Robot::changedExpandLinkDetails()
00413 {
00414   bool expand = expand_link_details_->getBool();
00415 
00416   M_NameToLink::iterator link_it = links_.begin();
00417   M_NameToLink::iterator link_end = links_.end();
00418   for ( ; link_it != link_end ; ++link_it )
00419   {
00420     link_it->second->expandDetails(expand);
00421   }
00422 }
00423 
00424 void Robot::changedExpandJointDetails()
00425 {
00426   bool expand = expand_joint_details_->getBool();
00427 
00428   M_NameToJoint::iterator joint_it = joints_.begin();
00429   M_NameToJoint::iterator joint_end = joints_.end();
00430   for ( ; joint_it != joint_end ; ++joint_it )
00431   {
00432     joint_it->second->expandDetails(expand);
00433   }
00434 }
00435 
00436 void Robot::changedEnableAllLinks()
00437 {
00438   if (doing_set_checkbox_)
00439     return;
00440 
00441   bool enable = enable_all_links_->getBool();
00442 
00443   inChangedEnableAllLinks = true;
00444 
00445   M_NameToLink::iterator link_it = links_.begin();
00446   M_NameToLink::iterator link_end = links_.end();
00447   for ( ; link_it != link_end ; ++link_it )
00448   {
00449     if (link_it->second->hasGeometry())
00450     {
00451       link_it->second->getLinkProperty()->setValue(enable);
00452     }
00453   }
00454 
00455   M_NameToJoint::iterator joint_it = joints_.begin();
00456   M_NameToJoint::iterator joint_end = joints_.end();
00457   for ( ; joint_it != joint_end ; ++joint_it )
00458   {
00459     if (joint_it->second->hasDescendentLinksWithGeometry())
00460     {
00461       joint_it->second->getJointProperty()->setValue(enable);
00462     }
00463   }
00464 
00465   inChangedEnableAllLinks = false;
00466 }
00467 
00468 void Robot::setEnableAllLinksCheckbox(QVariant val)
00469 {
00470   // doing_set_checkbox_ prevents changedEnableAllLinks from turning all
00471   // links off when we modify the enable_all_links_ property.
00472   doing_set_checkbox_ = true;
00473   enable_all_links_->setValue(val);
00474   doing_set_checkbox_ = false;
00475 }
00476 
00477 void Robot::initLinkTreeStyle()
00478 {
00479   style_name_map_.clear();
00480         style_name_map_[STYLE_LINK_LIST] = "Links in Alphabetic Order";
00481         style_name_map_[STYLE_JOINT_LIST] = "Joints in Alphabetic Order";
00482         style_name_map_[STYLE_LINK_TREE] = "Tree of links";
00483         style_name_map_[STYLE_JOINT_LINK_TREE] = "Tree of links and joints";
00484 
00485   link_tree_style_->clearOptions();
00486   std::map<LinkTreeStyle, std::string>::const_iterator style_it = style_name_map_.begin();
00487   std::map<LinkTreeStyle, std::string>::const_iterator style_end = style_name_map_.end();
00488   for ( ; style_it != style_end ; ++style_it )
00489   {
00490     link_tree_style_->addOptionStd( style_it->second, style_it->first );
00491   }
00492 }
00493 
00494 bool Robot::styleShowLink(LinkTreeStyle style)
00495 {
00496   return 
00497     style == STYLE_LINK_LIST ||
00498     style == STYLE_LINK_TREE ||
00499     style == STYLE_JOINT_LINK_TREE;
00500 }
00501 
00502 bool Robot::styleShowJoint(LinkTreeStyle style)
00503 {
00504   return 
00505     style == STYLE_JOINT_LIST ||
00506     style == STYLE_JOINT_LINK_TREE;
00507 }
00508 
00509 bool Robot::styleIsTree(LinkTreeStyle style)
00510 {
00511   return 
00512     style == STYLE_LINK_TREE ||
00513     style == STYLE_JOINT_LINK_TREE;
00514 }
00515 
00516 void Robot::setLinkTreeStyle(LinkTreeStyle style)
00517 {
00518   std::map<LinkTreeStyle, std::string>::const_iterator style_it = style_name_map_.find(style);
00519   if (style_it == style_name_map_.end())
00520     link_tree_style_->setValue(style_name_map_[STYLE_DEFAULT].c_str());
00521   else
00522     link_tree_style_->setValue(style_it->second.c_str());
00523 }
00524 
00525 // insert properties into link_tree_ according to style
00526 void Robot::changedLinkTreeStyle()
00527 {
00528   if (!robot_loaded_)
00529     return;
00530 
00531   LinkTreeStyle style = LinkTreeStyle(link_tree_style_->getOptionInt());
00532 
00533   unparentLinkProperties();
00534 
00535   //expand_tree_->setValue(false);
00536 
00537   switch (style)
00538   {
00539   case STYLE_LINK_TREE:
00540   case STYLE_JOINT_LINK_TREE:
00541     useDetailProperty(true);
00542     if (root_link_)
00543     {
00544       addLinkToLinkTree(style, link_tree_, root_link_);
00545     }
00546     break;
00547 
00548   case STYLE_JOINT_LIST:
00549   {
00550     useDetailProperty(false);
00551     M_NameToJoint::iterator joint_it = joints_.begin();
00552     M_NameToJoint::iterator joint_end = joints_.end();
00553     for ( ; joint_it != joint_end ; ++joint_it )
00554     {
00555       joint_it->second->setParentProperty(link_tree_);
00556       joint_it->second->setJointPropertyDescription();
00557     }
00558     break;
00559   }
00560 
00561   case STYLE_LINK_LIST:
00562   default:
00563     useDetailProperty(false);
00564     M_NameToLink::iterator link_it = links_.begin();
00565     M_NameToLink::iterator link_end = links_.end();
00566     for ( ; link_it != link_end ; ++link_it )
00567     {
00568       link_it->second->setParentProperty(link_tree_);
00569     }
00570     break;
00571   }
00572 
00573   switch (style)
00574   {
00575   case STYLE_LINK_TREE:
00576     link_tree_->setName("Link Tree");
00577     link_tree_->setDescription("A tree of all links in the robot.  Uncheck a link to hide its geometry.");
00578     expand_tree_->show();
00579     expand_link_details_->show();
00580     expand_joint_details_->hide();
00581     break;
00582   case STYLE_JOINT_LINK_TREE:
00583     link_tree_->setName("Link/Joint Tree");
00584     link_tree_->setDescription("A tree of all joints and links in the robot.  Uncheck a link to hide its geometry.");
00585     expand_tree_->show();
00586     expand_link_details_->show();
00587     expand_joint_details_->show();
00588     break;
00589   case STYLE_JOINT_LIST:
00590     link_tree_->setName("Joints");
00591     link_tree_->setDescription("All joints in the robot in alphabetic order.");
00592     expand_tree_->hide();
00593     expand_link_details_->hide();
00594     expand_joint_details_->show();
00595     break;
00596   case STYLE_LINK_LIST:
00597   default:
00598     link_tree_->setName("Links");
00599     link_tree_->setDescription("All links in the robot in alphabetic order.  Uncheck a link to hide its geometry.");
00600     expand_tree_->hide();
00601     expand_link_details_->show();
00602     expand_joint_details_->hide();
00603     break;
00604   }
00605 
00606   expand_link_details_->setValue(false);
00607   expand_joint_details_->setValue(false);
00608   expand_tree_->setValue(false);
00609   calculateJointCheckboxes();
00610 }
00611 
00612 
00613 // recursive helper for setLinkTreeStyle() when style is *_TREE
00614 void Robot::addLinkToLinkTree(LinkTreeStyle style, Property *parent, RobotLink *link)
00615 {
00616   if (styleShowLink(style))
00617   {
00618     link->setParentProperty(parent);
00619     parent = link->getLinkProperty();
00620   }
00621 
00622   std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
00623   std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
00624   for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
00625   {
00626     RobotJoint* child_joint = getJoint( *child_joint_it );
00627     if (child_joint)
00628     {
00629       addJointToLinkTree(style, parent, child_joint);
00630     }
00631   }
00632 }
00633 
00634 // recursive helper for setLinkTreeStyle() when style is *_TREE
00635 void Robot::addJointToLinkTree(LinkTreeStyle style, Property *parent, RobotJoint *joint)
00636 {
00637   if (styleShowJoint(style))
00638   {
00639     joint->setParentProperty(parent);
00640     parent = joint->getJointProperty();
00641     joint->setJointPropertyDescription();
00642   }
00643 
00644   RobotLink *link = getLink( joint->getChildLinkName() );
00645   if (link)
00646   {
00647     addLinkToLinkTree(style, parent, link);
00648   }
00649 }
00650 
00651 RobotLink* Robot::getLink( const std::string& name )
00652 {
00653   M_NameToLink::iterator it = links_.find( name );
00654   if ( it == links_.end() )
00655   {
00656     ROS_WARN( "Link [%s] does not exist", name.c_str() );
00657     return NULL;
00658   }
00659 
00660   return it->second;
00661 }
00662 
00663 RobotJoint* Robot::getJoint( const std::string& name )
00664 {
00665   M_NameToJoint::iterator it = joints_.find( name );
00666   if ( it == joints_.end() )
00667   {
00668     ROS_WARN( "Joint [%s] does not exist", name.c_str() );
00669     return NULL;
00670   }
00671 
00672   return it->second;
00673 }
00674 
00675 void Robot::calculateJointCheckboxes()
00676 {
00677   if (inChangedEnableAllLinks || !robot_loaded_)
00678     return;
00679 
00680   int links_with_geom_checked = 0;
00681   int links_with_geom_unchecked = 0;
00682 
00683   // check root link
00684   RobotLink *link = root_link_;
00685   if (link && link->hasGeometry())
00686   {
00687     bool checked = link->getLinkProperty()->getValue().toBool();
00688     links_with_geom_checked += checked ? 1 : 0;
00689     links_with_geom_unchecked += checked ? 0 : 1;
00690   }
00691   int links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
00692 
00693   // check all child links and joints recursively
00694   std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
00695   std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
00696   for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
00697   {
00698     RobotJoint* child_joint = getJoint( *child_joint_it );
00699     if (child_joint)
00700     {
00701       int child_links_with_geom;
00702       int child_links_with_geom_checked;
00703       int child_links_with_geom_unchecked;
00704       child_joint->calculateJointCheckboxesRecursive(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked);
00705       links_with_geom_checked += child_links_with_geom_checked;
00706       links_with_geom_unchecked += child_links_with_geom_unchecked;
00707     }
00708   }
00709   links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
00710 
00711   if (!links_with_geom)
00712   {
00713     setEnableAllLinksCheckbox(QVariant());
00714   }
00715   else
00716   {
00717     setEnableAllLinksCheckbox(links_with_geom_unchecked == 0);
00718   }
00719 }
00720 
00721 void Robot::update(const LinkUpdater& updater)
00722 {
00723   M_NameToLink::iterator link_it = links_.begin();
00724   M_NameToLink::iterator link_end = links_.end();
00725   for ( ; link_it != link_end; ++link_it )
00726   {
00727     RobotLink* link = link_it->second;
00728 
00729     link->setToNormalMaterial();
00730 
00731     Ogre::Vector3 visual_position, collision_position;
00732     Ogre::Quaternion visual_orientation, collision_orientation;
00733     if( updater.getLinkTransforms( link->getName(),
00734                                    visual_position, visual_orientation,
00735                                    collision_position, collision_orientation
00736                                    ))
00737     {
00738       // Check if visual_orientation, visual_position, collision_orientation, and collision_position are NaN.
00739       if (visual_orientation.isNaN())
00740       {
00741         ROS_ERROR_THROTTLE(
00742           1.0,
00743           "visual orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
00744           link->getName().c_str()
00745         );
00746         continue;
00747       }
00748       if (visual_position.isNaN())
00749       {
00750         ROS_ERROR_THROTTLE(
00751           1.0,
00752           "visual position of %s contains NaNs. Skipping render as long as the position is invalid.",
00753           link->getName().c_str()
00754         );
00755         continue;
00756       }
00757       if (collision_orientation.isNaN())
00758       {
00759         ROS_ERROR_THROTTLE(
00760           1.0,
00761           "collision orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
00762           link->getName().c_str()
00763         );
00764         continue;
00765       }
00766       if (collision_position.isNaN())
00767       {
00768         ROS_ERROR_THROTTLE(
00769           1.0,
00770           "collision position of %s contains NaNs. Skipping render as long as the position is invalid.",
00771           link->getName().c_str()
00772         );
00773         continue;
00774       }
00775       link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation );
00776 
00777       std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin();
00778       std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end();
00779       for ( ; joint_it != joint_end ; ++joint_it )
00780       {
00781         RobotJoint *joint = getJoint(*joint_it);
00782         if (joint)
00783         {
00784           joint->setTransforms(visual_position, visual_orientation);
00785         }
00786       }
00787     }
00788     else
00789     {
00790       link->setToErrorMaterial();
00791     }
00792   }
00793 }
00794 
00795 void Robot::setPosition( const Ogre::Vector3& position )
00796 {
00797   root_visual_node_->setPosition( position );
00798   root_collision_node_->setPosition( position );
00799 }
00800 
00801 void Robot::setOrientation( const Ogre::Quaternion& orientation )
00802 {
00803   root_visual_node_->setOrientation( orientation );
00804   root_collision_node_->setOrientation( orientation );
00805 }
00806 
00807 void Robot::setScale( const Ogre::Vector3& scale )
00808 {
00809   root_visual_node_->setScale( scale );
00810   root_collision_node_->setScale( scale );
00811 }
00812 
00813 const Ogre::Vector3& Robot::getPosition()
00814 {
00815   return root_visual_node_->getPosition();
00816 }
00817 
00818 const Ogre::Quaternion& Robot::getOrientation()
00819 {
00820   return root_visual_node_->getOrientation();
00821 }
00822 
00823 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28