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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31