robot_joint.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, 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 "rviz/robot/robot_joint.h"
31 #include "rviz/robot/robot_link.h"
32 #include "rviz/robot/robot.h"
33 
34 #include <OgreSceneNode.h>
35 
41 #include "rviz/ogre_helpers/axes.h"
42 #include "rviz/load_resource.h"
43 
44 namespace rviz
45 {
46 
47 RobotJoint::RobotJoint( Robot* robot, const urdf::JointConstSharedPtr& joint )
48  : robot_( robot )
49  , name_( joint->name )
50  , child_link_name_( joint->child_link_name )
51  , parent_link_name_( joint->parent_link_name )
52  , axes_( NULL )
53  , axis_( NULL )
54  , has_decendent_links_with_geometry_( true )
55  , doing_set_checkbox_( false )
56 {
58  name_.c_str(),
59  true,
60  "",
61  NULL,
62  SLOT( updateChildVisibility() ),
63  this);
64  joint_property_->setIcon( rviz::loadPixmap( "package://rviz/icons/classes/RobotJoint.png" ) );
65 
66  details_ = new Property( "Details", QVariant(), "", NULL);
67 
69  "Show Axes",
70  false,
71  "Enable/disable showing the axes of this joint.",
73  SLOT( updateAxes() ),
74  this );
75 
77  "Position",
78  Ogre::Vector3::ZERO,
79  "Position of this joint, in the current Fixed Frame. (Not editable)",
82 
84  "Orientation",
85  Ogre::Quaternion::IDENTITY,
86  "Orientation of this joint, in the current Fixed Frame. (Not editable)",
89 
90  std::string type = "";
91  if (joint->type == urdf::Joint::UNKNOWN)
92  type = "unknown";
93  else if (joint->type == urdf::Joint::REVOLUTE)
94  type = "revolute";
95  else if (joint->type == urdf::Joint::CONTINUOUS)
96  type = "continuous";
97  else if (joint->type == urdf::Joint::PRISMATIC)
98  type = "prismatic";
99  else if (joint->type == urdf::Joint::FLOATING)
100  type = "floating";
101  else if (joint->type == urdf::Joint::PLANAR)
102  type = "planar";
103  else if (joint->type == urdf::Joint::FIXED)
104  type = "fixed";
105 
107  "Type",
108  QString::fromStdString(type),
109  "Type of this joint. (Not editable)",
110  joint_property_ );
111  type_property_->setReadOnly( true );
112 
113  if (joint->limits)
114  {
115  // continuous joints have lower limit and upper limits of zero,
116  // which means this isn't very useful but show it anyhow.
118  "Lower Limit",
119  joint->limits->lower,
120  "Lower limit of this joint. (Not editable)",
121  joint_property_ );
123 
125  "Upper Limit",
126  joint->limits->upper,
127  "Upper limit of this joint. (Not editable)",
128  joint_property_ );
130  }
131 
132  if ((type == "continuous") || (type == "revolute") ||
133  (type == "prismatic") || (type == "planar"))
134  {
136  "Show Joint Axis",
137  false,
138  "Enable/disable showing the axis of this joint.",
140  SLOT( updateAxis() ),
141  this );
142 
144  "Joint Axis",
145  Ogre::Vector3(joint->axis.x, joint->axis.y, joint->axis.z),
146  "Axis of this joint. (Not editable)",
147  joint_property_ );
148  axis_property_->setReadOnly( true );
149  }
150 
152 
153  const urdf::Vector3& pos = joint->parent_to_joint_origin_transform.position;
154  const urdf::Rotation& rot = joint->parent_to_joint_origin_transform.rotation;
155  joint_origin_pos_ = Ogre::Vector3(pos.x, pos.y, pos.z);
156  joint_origin_rot_ = Ogre::Quaternion(rot.w, rot.x, rot.y, rot.z);
157 }
158 
160 {
161  delete axes_;
162  if (axis_)
163  delete axis_;
164  delete details_;
165  delete joint_property_;
166 }
167 
169 {
170  int links_with_geom;
171  int links_with_geom_checked;
172  int links_with_geom_unchecked;
173  getChildLinkState(links_with_geom, links_with_geom_checked, links_with_geom_unchecked, true);
174 
175  std::stringstream desc;
176  desc
177  << "Joint <b>" << name_
178  << "</b> with parent link <b>" << parent_link_name_
179  << "</b> and child link <b>" << child_link_name_
180  << "</b>.";
181 
182  if (links_with_geom == 0)
183  {
184  desc << " This joint's descendents have NO geometry.";
185  setJointCheckbox(QVariant());
187  }
188  else if (styleIsTree())
189  {
190  desc << " Check/uncheck to show/hide all links descended from this joint.";
191  setJointCheckbox(links_with_geom_unchecked == 0);
193  }
194  else
195  {
196  getChildLinkState(links_with_geom, links_with_geom_checked, links_with_geom_unchecked, false);
197  if (links_with_geom == 0)
198  {
199  desc << " This joint's child link has NO geometry.";
200  setJointCheckbox(QVariant());
202  }
203  else
204  {
205  desc << " Check/uncheck to show/hide this joint's child link.";
206  setJointCheckbox(links_with_geom_unchecked == 0);
208  }
209  }
210 
211  joint_property_->setDescription(desc.str().c_str());
212 }
213 
215 {
216  // setting doing_set_checkbox_ to true prevents updateChildVisibility() from
217  // updating child link enables.
218  doing_set_checkbox_ = true;
220  doing_set_checkbox_ = false;
221 }
222 
224 {
225  RobotLink* parent_link = robot_->getLink(parent_link_name_);
226  if (!parent_link)
227  return NULL;
228 
229  const std::string& parent_joint_name = parent_link->getParentJointName();
230  if (parent_joint_name.empty())
231  return NULL;
232 
233  return robot_->getJoint(parent_joint_name);
234 }
235 
237  int& links_with_geom,
238  int& links_with_geom_checked,
239  int& links_with_geom_unchecked)
240 {
241  links_with_geom_checked = 0;
242  links_with_geom_unchecked = 0;
243 
245  if (link && link->hasGeometry())
246  {
247  bool checked = link->getLinkProperty()->getValue().toBool();
248  links_with_geom_checked += checked ? 1 : 0;
249  links_with_geom_unchecked += checked ? 0 : 1;
250  }
251  links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
252 
253  if (!styleIsTree())
254  {
255  if (!links_with_geom)
256  {
257  setJointCheckbox(QVariant());
258  }
259  else
260  {
261  setJointCheckbox(links_with_geom_unchecked == 0);
262  }
263  }
264 
265  std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
266  std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
267  for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
268  {
269  RobotJoint* child_joint = robot_->getJoint( *child_joint_it );
270  if (child_joint)
271  {
272  int child_links_with_geom;
273  int child_links_with_geom_checked;
274  int child_links_with_geom_unchecked;
275  child_joint->calculateJointCheckboxesRecursive(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked);
276  links_with_geom_checked += child_links_with_geom_checked;
277  links_with_geom_unchecked += child_links_with_geom_unchecked;
278  }
279  }
280  links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
281 
282  if (styleIsTree())
283  {
284  if (!links_with_geom)
285  {
286  setJointCheckbox(QVariant());
287  }
288  else
289  {
290  setJointCheckbox(links_with_geom_unchecked == 0);
291  }
292  }
293 }
294 
295 
297  int& links_with_geom,
298  int& links_with_geom_checked,
299  int& links_with_geom_unchecked,
300  bool recursive) const
301 {
302  links_with_geom_checked = 0;
303  links_with_geom_unchecked = 0;
304 
306  if (link && link->hasGeometry())
307  {
308  bool checked = link->getLinkProperty()->getValue().toBool();
309  links_with_geom_checked += checked ? 1 : 0;
310  links_with_geom_unchecked += checked ? 0 : 1;
311  }
312 
313  if (recursive)
314  {
315  std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
316  std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
317  for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
318  {
319  RobotJoint* child_joint = robot_->getJoint( *child_joint_it );
320  if (child_joint)
321  {
322  int child_links_with_geom;
323  int child_links_with_geom_checked;
324  int child_links_with_geom_unchecked;
325  child_joint->getChildLinkState(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked, recursive);
326  links_with_geom_checked += child_links_with_geom_checked;
327  links_with_geom_unchecked += child_links_with_geom_unchecked;
328  }
329  }
330  }
331 
332  links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
333 }
334 
335 
337 {
339  return true;
340  return joint_property_->getValue().toBool();
341 }
342 
344 {
345  return details_->getParent() != NULL;
346 }
347 
349 {
351  return;
352 
354  return;
355 
356  bool visible = getEnabled();
357 
359  if (link)
360  {
361  if (link->hasGeometry())
362  {
363  link->getLinkProperty()->setValue(visible);
364  }
365 
366  if (styleIsTree())
367  {
368  std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin();
369  std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end();
370  for ( ; child_joint_it != child_joint_end ; ++child_joint_it )
371  {
372  RobotJoint* child_joint = robot_->getJoint( *child_joint_it );
373  if (child_joint)
374  {
375  child_joint->getJointProperty()->setValue(visible);
376  }
377  }
378  }
379  }
380 }
381 
383 {
384  if( axes_property_->getValue().toBool() )
385  {
386  if( !axes_ )
387  {
388  static int count = 0;
389  std::stringstream ss;
390  ss << "Axes for joint " << name_ << count++;
391  axes_ = new Axes( robot_->getSceneManager(), robot_->getOtherNode(), 0.1, 0.01 );
392  axes_->getSceneNode()->setVisible( getEnabled() );
393 
396  }
397  }
398  else
399  {
400  if( axes_ )
401  {
402  delete axes_;
403  axes_ = NULL;
404  }
405  }
406 }
407 
409 {
410  if( show_axis_property_->getValue().toBool() )
411  {
412  if( !axis_ )
413  {
414  static int count = 0;
415  std::stringstream ss;
416  ss << "Axis for joint " << name_ << count++;
417  axis_ = new Arrow( robot_->getSceneManager(), robot_->getOtherNode(), 0.15, 0.05, 0.05, 0.08 );
418  axis_->getSceneNode()->setVisible( getEnabled() );
419 
422 
423  // TODO(lucasw) store an Ogre::ColorValue and set it according to
424  // joint type.
425  axis_->setColor(0.0, 0.8, 0.0, 1.0);
426  }
427  }
428  else
429  {
430  if( axis_ )
431  {
432  delete axis_;
433  axis_ = NULL;
434  }
435  }
436 }
437 
438 void RobotJoint::setTransforms( const Ogre::Vector3& parent_link_position,
439  const Ogre::Quaternion& parent_link_orientation )
440 {
441  Ogre::Vector3 position = parent_link_position + parent_link_orientation * joint_origin_pos_;
442  Ogre::Quaternion orientation = parent_link_orientation * joint_origin_rot_;
443 
444  position_property_->setVector( position );
445  orientation_property_->setQuaternion( orientation );
446 
447  if ( axes_ )
448  {
449  axes_->setPosition( position );
450  axes_->setOrientation( orientation );
451  }
452  if ( axis_ )
453  {
454  axis_->setPosition( position );
455  axis_->setOrientation( orientation );
456  axis_->setDirection( parent_link_orientation * axis_property_->getVector() );
457  }
458 }
459 
461 {
464  axes_property_->setHidden(hide);
466  axis_property_->setHidden(hide);
467 }
468 
469 Ogre::Vector3 RobotJoint::getPosition()
470 {
471  return position_property_->getVector();
472 }
473 
474 Ogre::Quaternion RobotJoint::getOrientation()
475 {
477 }
478 
480 {
481  Property* old_parent = joint_property_->getParent();
482  if (old_parent)
483  old_parent->takeChild(joint_property_);
484 
485  if (new_parent)
486  new_parent->addChild(joint_property_);
487 }
488 
489 // if use_detail:
490 // - all sub properties become children of details_ property.
491 // - details_ property becomes a child of joint_property_
492 // else (!use_detail)
493 // - all sub properties become children of joint_property_.
494 // details_ property does not have a parent.
495 void RobotJoint::useDetailProperty(bool use_detail)
496 {
497  Property* old_parent = details_->getParent();
498  if (old_parent)
499  old_parent->takeChild(details_);
500 
501  if (use_detail)
502  {
503  while (joint_property_->numChildren() > 0)
504  {
505  Property* child = joint_property_->childAt(0);
506  joint_property_->takeChild(child);
507  details_->addChild(child);
508  }
509 
511  }
512  else
513  {
514  while (details_->numChildren() > 0)
515  {
516  Property* child = details_->childAt(0);
517  details_->takeChild(child);
518  joint_property_->addChild(child);
519  }
520  }
521 }
522 
523 void RobotJoint::expandDetails(bool expand)
524 {
526  if (expand)
527  {
528  parent->expand();
529  }
530  else
531  {
532  parent->collapse();
533  }
534 }
535 
536 } // namespace rviz
537 
virtual Ogre::Quaternion getQuaternion() const
void setParentProperty(Property *new_parent)
#define NULL
Definition: global.h:37
VectorProperty * axis_property_
Definition: robot_joint.h:168
StringProperty * type_property_
Definition: robot_joint.h:170
void setJointPropertyDescription()
Ogre::Quaternion joint_origin_rot_
Definition: robot_joint.h:176
Property * getParent() const
Return the parent Property.
Definition: property.cpp:226
void updateChildVisibility()
virtual ~RobotJoint()
Property * details_
Definition: robot_joint.h:163
virtual void expand()
Expand (show the children of) this Property.
Definition: property.cpp:542
virtual bool setVector(const Ogre::Vector3 &vector)
void useDetailProperty(bool use_detail)
bool getEnabled() const
void hideSubProperties(bool hide)
QuaternionProperty * orientation_property_
Definition: robot_joint.h:165
Ogre::SceneNode * getOtherNode()
Definition: robot.h:156
void setDirection(const Ogre::Vector3 &direction)
Set the direction of the arrow.
Definition: arrow.cpp:123
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
std::string parent_link_name_
Definition: robot_joint.h:158
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
Ogre::SceneManager * getSceneManager()
Definition: robot.h:157
void calculateJointCheckboxesRecursive(int &links_with_geom, int &links_with_geom_checked, int &links_with_geom_unchecked)
Property * joint_property_
Definition: robot_joint.h:162
Ogre::Vector3 joint_origin_pos_
Definition: robot_joint.h:175
void setTransforms(const Ogre::Vector3 &parent_link_position, const Ogre::Quaternion &parent_link_orientation)
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
Property specialized to enforce floating point max/min.
VectorProperty * position_property_
Definition: robot_joint.h:164
virtual void setDescription(const QString &description)
Set the description.
Definition: property.cpp:164
RobotLink * getLink(const std::string &name)
Definition: robot.cpp:651
virtual int numChildren() const
Return the number of child objects (Property or otherwise).
Definition: property.h:215
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this arrow.
Definition: arrow.h:143
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of this object.
Definition: axes.cpp:84
Property * takeChild(Property *child)
Remove a given child object and return a pointer to it.
Definition: property.cpp:316
Property * show_axis_property_
Definition: robot_joint.h:169
virtual void collapse()
Collapse (hide the children of) this Property.
Definition: property.cpp:550
virtual void setIcon(const QIcon &icon)
Set the icon to be displayed next to the property.
Definition: property.h:187
Property * axes_property_
Definition: robot_joint.h:166
virtual void setPosition(const Ogre::Vector3 &position)
Set the position of the base of the arrow.
Definition: arrow.cpp:111
RobotJoint(Robot *robot, const urdf::JointConstSharedPtr &joint)
Definition: robot_joint.cpp:47
Property * childAt(int index) const
Return the child Property with the given index, or NULL if the index is out of bounds or if the child...
Definition: property.cpp:197
bool doing_set_checkbox_
Definition: robot_joint.h:179
bool hasDescendentLinksWithGeometry() const
Definition: robot_joint.h:111
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
virtual void setColor(float r, float g, float b, float a)
Set the color of this arrow. Sets both the head and shaft color to the same value. Values are in the range [0, 1].
Definition: arrow.cpp:86
FloatProperty * upper_limit_property_
Definition: robot_joint.h:172
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation.
Definition: arrow.cpp:116
Ogre::Vector3 getPosition()
void setJointCheckbox(QVariant val)
std::string name_
Name of this joint.
Definition: robot_joint.h:157
Contains any data we need from a joint in the robot.
Definition: robot_joint.h:84
void expandDetails(bool expand)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:58
bool styleIsTree() const
FloatProperty * lower_limit_property_
Definition: robot_joint.h:171
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:530
Property specialized for string values.
virtual Ogre::Vector3 getVector() const
std::string child_link_name_
Definition: robot_joint.h:159
const Property * getJointProperty() const
Definition: robot_joint.h:98
virtual void addChild(Property *child, int index=-1)
Add a child property.
Definition: property.cpp:350
RobotJoint * getParentJoint()
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
void getChildLinkState(int &links_with_geom, int &links_with_geom_checked, int &links_with_geom_unchecked, bool recursive) const
An arrow consisting of a cylinder and a cone.
Definition: arrow.h:59
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: axes.cpp:89
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:90
Ogre::Quaternion getOrientation()
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:397
QPixmap loadPixmap(QString url, bool fill_cache)
bool has_decendent_links_with_geometry_
Definition: robot_joint.h:177
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