robot_joint.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, 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 "rviz/robot/robot_joint.h"
00031 #include "rviz/robot/robot_link.h"
00032 #include "rviz/robot/robot.h"
00033 
00034 #include <OgreSceneNode.h>
00035 
00036 #include "rviz/properties/vector_property.h"
00037 #include "rviz/properties/quaternion_property.h"
00038 #include "rviz/ogre_helpers/axes.h"
00039 #include "rviz/load_resource.h"
00040 
00041 #include <urdf_model/model.h>
00042 #include <urdf_model/link.h>
00043 #include <urdf_model/joint.h>
00044 
00045 
00046 namespace rviz
00047 {
00048 
00049 RobotJoint::RobotJoint( Robot* robot, const boost::shared_ptr<const urdf::Joint>& joint )
00050   : robot_( robot )
00051   , name_( joint->name )
00052   , child_link_name_( joint->child_link_name )
00053   , parent_link_name_( joint->parent_link_name )
00054   , axes_( NULL )
00055   , has_decendent_links_with_geometry_( true )
00056   , doing_set_checkbox_( false )
00057 {
00058   joint_property_ = new Property(
00059                               name_.c_str(),
00060                               true,
00061                               "",
00062                               NULL,
00063                               SLOT( updateChildVisibility() ),
00064                               this);
00065   joint_property_->setIcon( rviz::loadPixmap( "package://rviz/icons/classes/RobotJoint.png" ) );
00066 
00067   details_ = new Property( "Details", QVariant(), "", NULL);
00068 
00069   axes_property_ = new Property(
00070                               "Show Axes",
00071                               false,
00072                               "Enable/disable showing the axes of this joint.",
00073                               joint_property_,
00074                               SLOT( updateAxes() ),
00075                               this );
00076 
00077   position_property_ = new VectorProperty(
00078                               "Position",
00079                               Ogre::Vector3::ZERO,
00080                               "Position of this joint, in the current Fixed Frame.  (Not editable)",
00081                               joint_property_ );
00082   position_property_->setReadOnly( true );
00083 
00084   orientation_property_ = new QuaternionProperty(
00085                               "Orientation",
00086                               Ogre::Quaternion::IDENTITY,
00087                               "Orientation of this joint, in the current Fixed Frame.  (Not editable)",
00088                               joint_property_ );
00089   orientation_property_->setReadOnly( true );
00090 
00091   joint_property_->collapse();
00092 
00093   const urdf::Vector3& pos = joint->parent_to_joint_origin_transform.position;
00094   const urdf::Rotation& rot = joint->parent_to_joint_origin_transform.rotation;
00095   joint_origin_pos_ = Ogre::Vector3(pos.x, pos.y, pos.z);
00096   joint_origin_rot_ = Ogre::Quaternion(rot.w, rot.x, rot.y, rot.z);
00097 }
00098 
00099 RobotJoint::~RobotJoint()
00100 {
00101   delete axes_;
00102   delete details_;
00103   delete joint_property_;
00104 }
00105 
00106 void RobotJoint::setJointPropertyDescription()
00107 {
00108   int links_with_geom;
00109   int links_with_geom_checked;
00110   int links_with_geom_unchecked;
00111   getChildLinkState(links_with_geom, links_with_geom_checked, links_with_geom_unchecked, true);
00112 
00113   std::stringstream desc;
00114   desc
00115     << "Joint <b>" << name_
00116     << "</b> with parent link <b>" << parent_link_name_
00117     << "</b> and child link <b>" << child_link_name_
00118     << "</b>.";
00119 
00120   if (links_with_geom == 0)
00121   {
00122     desc << "  This joint's descendents have NO geometry.";
00123     setJointCheckbox(QVariant());
00124     has_decendent_links_with_geometry_ = false;
00125   }
00126   else if (styleIsTree())
00127   {
00128     desc << "  Check/uncheck to show/hide all links descended from this joint.";
00129     setJointCheckbox(links_with_geom_unchecked == 0);
00130     has_decendent_links_with_geometry_ = true;
00131   }
00132   else
00133   {
00134     getChildLinkState(links_with_geom, links_with_geom_checked, links_with_geom_unchecked, false);
00135     if (links_with_geom == 0)
00136     {
00137       desc << "  This joint's child link has NO geometry.";
00138       setJointCheckbox(QVariant());
00139       has_decendent_links_with_geometry_ = false;
00140     }
00141     else
00142     {
00143       desc << "  Check/uncheck to show/hide this joint's child link.";
00144       setJointCheckbox(links_with_geom_unchecked == 0);
00145       has_decendent_links_with_geometry_ = true;
00146     }
00147   }
00148 
00149   joint_property_->setDescription(desc.str().c_str());
00150 }
00151 
00152 void RobotJoint::setJointCheckbox(QVariant val)
00153 {
00154   // setting doing_set_checkbox_ to true prevents updateChildVisibility() from
00155   // updating child link enables.
00156   doing_set_checkbox_ = true;
00157   joint_property_->setValue(val);
00158   doing_set_checkbox_ = false;
00159 }
00160 
00161 RobotJoint* RobotJoint::getParentJoint()
00162 {
00163   RobotLink* parent_link = robot_->getLink(parent_link_name_);
00164   if (!parent_link)
00165     return NULL;
00166 
00167   const std::string& parent_joint_name = parent_link->getParentJointName();
00168   if (parent_joint_name.empty())
00169     return NULL;
00170 
00171   return robot_->getJoint(parent_joint_name);
00172 }
00173 
00174 void RobotJoint::calculateJointCheckboxesRecursive(
00175       int& links_with_geom,
00176       int& links_with_geom_checked,
00177       int& links_with_geom_unchecked)
00178 {
00179   links_with_geom_checked = 0;
00180   links_with_geom_unchecked = 0;
00181 
00182   RobotLink *link = robot_->getLink(child_link_name_);
00183   if (link && link->hasGeometry())
00184   {
00185     bool checked = link->getLinkProperty()->getValue().toBool();
00186     links_with_geom_checked += checked ? 1 : 0;
00187     links_with_geom_unchecked += checked ? 0 : 1;
00188   }
00189   links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
00190 
00191   if (!styleIsTree())
00192   {
00193     if (!links_with_geom)
00194     {
00195       setJointCheckbox(QVariant());
00196     }
00197     else
00198     {
00199       setJointCheckbox(links_with_geom_unchecked == 0);
00200     }
00201   }
00202 
00203   std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
00204   std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
00205   for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
00206   {
00207     RobotJoint* child_joint = robot_->getJoint( *child_joint_it );
00208     if (child_joint)
00209     {
00210       int child_links_with_geom;
00211       int child_links_with_geom_checked;
00212       int child_links_with_geom_unchecked;
00213       child_joint->calculateJointCheckboxesRecursive(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked);
00214       links_with_geom_checked += child_links_with_geom_checked;
00215       links_with_geom_unchecked += child_links_with_geom_unchecked;
00216     }
00217   }
00218   links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
00219 
00220   if (styleIsTree())
00221   {
00222     if (!links_with_geom)
00223     {
00224       setJointCheckbox(QVariant());
00225     }
00226     else
00227     {
00228       setJointCheckbox(links_with_geom_unchecked == 0);
00229     }
00230   }
00231 }
00232 
00233 
00234 void RobotJoint::getChildLinkState(
00235       int& links_with_geom,
00236       int& links_with_geom_checked,
00237       int& links_with_geom_unchecked,
00238       bool recursive) const
00239 {
00240   links_with_geom_checked = 0;
00241   links_with_geom_unchecked = 0;
00242 
00243   RobotLink *link = robot_->getLink(child_link_name_);
00244   if (link && link->hasGeometry())
00245   {
00246     bool checked = link->getLinkProperty()->getValue().toBool();
00247     links_with_geom_checked += checked ? 1 : 0;
00248     links_with_geom_unchecked += checked ? 0 : 1;
00249   }
00250 
00251   if (recursive)
00252   {
00253     std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
00254     std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
00255     for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
00256     {
00257       RobotJoint* child_joint = robot_->getJoint( *child_joint_it );
00258       if (child_joint)
00259       {
00260         int child_links_with_geom;
00261         int child_links_with_geom_checked;
00262         int child_links_with_geom_unchecked;
00263         child_joint->getChildLinkState(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked, recursive);
00264         links_with_geom_checked += child_links_with_geom_checked;
00265         links_with_geom_unchecked += child_links_with_geom_unchecked;
00266       }
00267     }
00268   }
00269 
00270   links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
00271 }
00272 
00273 
00274 bool RobotJoint::getEnabled() const
00275 {
00276   if (!hasDescendentLinksWithGeometry())
00277     return true;
00278   return joint_property_->getValue().toBool();
00279 }
00280 
00281 bool RobotJoint::styleIsTree() const
00282 {
00283   return details_->getParent() != NULL;
00284 }
00285 
00286 void RobotJoint::updateChildVisibility()
00287 {
00288   if (doing_set_checkbox_)
00289     return;
00290 
00291   if (!hasDescendentLinksWithGeometry())
00292     return;
00293 
00294   bool visible = getEnabled();
00295 
00296   RobotLink *link = robot_->getLink(child_link_name_);
00297   if (link)
00298   {
00299     if (link->hasGeometry())
00300     {
00301       link->getLinkProperty()->setValue(visible);
00302     }
00303     
00304     if (styleIsTree())
00305     {
00306       std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
00307       std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
00308       for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
00309       {
00310         RobotJoint* child_joint = robot_->getJoint( *child_joint_it );
00311         if (child_joint)
00312         {
00313           child_joint->getJointProperty()->setValue(visible);
00314         }
00315       }
00316     }
00317   }
00318 }
00319 
00320 void RobotJoint::updateAxes()
00321 {
00322   if( axes_property_->getValue().toBool() )
00323   {
00324     if( !axes_ )
00325     {
00326       static int count = 0;
00327       std::stringstream ss;
00328       ss << "Axes for joint " << name_ << count++;
00329       axes_ = new Axes( robot_->getSceneManager(), robot_->getOtherNode(), 0.1, 0.01 );
00330       axes_->getSceneNode()->setVisible( getEnabled() );
00331 
00332       axes_->setPosition( position_property_->getVector() );
00333       axes_->setOrientation( orientation_property_->getQuaternion() );
00334     }
00335   }
00336   else
00337   {
00338     if( axes_ )
00339     {
00340       delete axes_;
00341       axes_ = NULL;
00342     }
00343   }
00344 }
00345 
00346 void RobotJoint::setTransforms( const Ogre::Vector3& parent_link_position,
00347                                 const Ogre::Quaternion& parent_link_orientation )
00348 {
00349   Ogre::Vector3 position = parent_link_position + parent_link_orientation * joint_origin_pos_;
00350   Ogre::Quaternion orientation = parent_link_orientation * joint_origin_rot_;
00351 
00352   position_property_->setVector( position );
00353   orientation_property_->setQuaternion( orientation );
00354 
00355   if ( axes_ )
00356   {
00357     axes_->setPosition( position );
00358     axes_->setOrientation( orientation );
00359   }
00360 }
00361 
00362 void RobotJoint::hideSubProperties(bool hide)
00363 {
00364   position_property_->setHidden(hide);
00365   orientation_property_->setHidden(hide);
00366   axes_property_->setHidden(hide);
00367 }
00368 
00369 Ogre::Vector3 RobotJoint::getPosition()
00370 {
00371   return position_property_->getVector();
00372 }
00373 
00374 Ogre::Quaternion RobotJoint::getOrientation()
00375 {
00376   return orientation_property_->getQuaternion();
00377 }
00378 
00379 void RobotJoint::setParentProperty(Property* new_parent)
00380 {
00381   Property* old_parent = joint_property_->getParent();
00382   if (old_parent)
00383     old_parent->takeChild(joint_property_);
00384 
00385   if (new_parent)
00386     new_parent->addChild(joint_property_);
00387 }
00388 
00389 // if use_detail:
00390 //    - all sub properties become children of details_ property.
00391 //    - details_ property becomes a child of joint_property_
00392 // else (!use_detail)
00393 //    - all sub properties become children of joint_property_.
00394 //    details_ property does not have a parent.
00395 void RobotJoint::useDetailProperty(bool use_detail)
00396 {
00397   Property* old_parent = details_->getParent();
00398   if (old_parent)
00399     old_parent->takeChild(details_);
00400 
00401   if (use_detail)
00402   {
00403     while (joint_property_->numChildren() > 0)
00404     {
00405       Property* child = joint_property_->childAt(0);
00406       joint_property_->takeChild(child);
00407       details_->addChild(child);
00408     }
00409 
00410     joint_property_->addChild(details_);
00411   }
00412   else
00413   {
00414     while (details_->numChildren() > 0)
00415     {
00416       Property* child = details_->childAt(0);
00417       details_->takeChild(child);
00418       joint_property_->addChild(child);
00419     }
00420   }
00421 }
00422 
00423 void RobotJoint::expandDetails(bool expand)
00424 {
00425   Property *parent = details_->getParent() ? details_ : joint_property_;
00426   if (expand)
00427   {
00428     parent->expand();
00429   }
00430   else
00431   {
00432     parent->collapse();
00433   }
00434 }
00435 
00436 } // namespace rviz
00437 


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