00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00155
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
00390
00391
00392
00393
00394
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 }
00437