robot_link.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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 <boost/filesystem.hpp>
31 
32 #include <OgreEntity.h>
33 #include <OgreMaterial.h>
34 #include <OgreMaterialManager.h>
35 #include <OgreRibbonTrail.h>
36 #include <OgreSceneManager.h>
37 #include <OgreSceneNode.h>
38 #include <OgreSubEntity.h>
39 #include <OgreTextureManager.h>
40 #include <OgreSharedPtr.h>
41 #include <OgreTechnique.h>
42 
43 #include <ros/console.h>
44 
46 #include <urdf_model/model.h>
47 #include <urdf_model/link.h>
48 
49 #include "rviz/mesh_loader.h"
50 #include "rviz/ogre_helpers/axes.h"
58 #include "rviz/robot/robot.h"
61 #include "rviz/load_resource.h"
62 
63 #include "rviz/robot/robot_link.h"
64 #include "rviz/robot/robot_joint.h"
65 
66 namespace fs = boost::filesystem;
67 
68 #ifndef ROS_PACKAGE_NAME
69 #define ROS_PACKAGE_NAME "rviz"
70 #endif
71 
72 namespace rviz
73 {
75 {
76 public:
78  ~RobotLinkSelectionHandler() override;
79 
80  void createProperties(const Picked& obj, Property* parent_property) override;
81  void updateProperties() override;
82 
83  void preRenderPass(uint32_t pass) override;
84  void postRenderPass(uint32_t pass) override;
85 
86 private:
90 };
91 
93  : SelectionHandler(context), link_(link)
94 {
95 }
96 
98 {
99 }
100 
101 void RobotLinkSelectionHandler::createProperties(const Picked& /*obj*/, Property* parent_property)
102 {
103  Property* group =
104  new Property("Link " + QString::fromStdString(link_->getName()), QVariant(), "", parent_property);
105  properties_.push_back(group);
106 
107  position_property_ = new VectorProperty("Position", Ogre::Vector3::ZERO, "", group);
109 
110  orientation_property_ = new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, "", group);
112 
113  group->expand();
114 }
115 
117 {
120 }
121 
122 
124 {
125  if (!link_->is_selectable_)
126  {
127  if (link_->visual_node_)
128  {
129  link_->visual_node_->setVisible(false);
130  }
131  if (link_->collision_node_)
132  {
133  link_->collision_node_->setVisible(false);
134  }
135  if (link_->trail_)
136  {
137  link_->trail_->setVisible(false);
138  }
139  if (link_->axes_)
140  {
141  link_->axes_->getSceneNode()->setVisible(false);
142  }
143  }
144 }
145 
147 {
148  if (!link_->is_selectable_)
149  {
151  }
152 }
153 
154 static std::map<const RobotLink*, std::string> errors;
155 
157  const urdf::LinkConstSharedPtr& link,
158  const std::string& parent_joint_name,
159  bool visual,
160  bool collision)
161  : robot_(robot)
162  , scene_manager_(robot->getDisplayContext()->getSceneManager())
163  , context_(robot->getDisplayContext())
164  , name_(link->name)
165  , parent_joint_name_(parent_joint_name)
166  , visual_node_(nullptr)
167  , collision_node_(nullptr)
168  , trail_(nullptr)
169  , axes_(nullptr)
170  , material_alpha_(1.0)
171  , robot_alpha_(1.0)
172  , only_render_depth_(false)
173  , is_selectable_(true)
174  , material_mode_flags_(ORIGINAL)
175 {
176  link_property_ = new Property(link->name.c_str(), true, "", nullptr, SLOT(updateVisibility()), this);
177  link_property_->setIcon(rviz::loadPixmap("package://rviz/icons/classes/RobotLink.png"));
178 
179  details_ = new Property("Details", QVariant(), "", nullptr);
180 
181  alpha_property_ = new FloatProperty("Alpha", 1, "Amount of transparency to apply to this link.",
182  link_property_, SLOT(updateAlpha()), this);
183 
185  new Property("Show Trail", false, "Enable/disable a 2 meter \"ribbon\" which follows this link.",
186  link_property_, SLOT(updateTrail()), this);
187 
188  axes_property_ = new Property("Show Axes", false, "Enable/disable showing the axes of this link.",
189  link_property_, SLOT(updateAxes()), this);
190 
192  new VectorProperty("Position", Ogre::Vector3::ZERO,
193  "Position of this link, in the current Fixed Frame. (Not editable)",
196 
198  new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY,
199  "Orientation of this link, in the current Fixed Frame. (Not editable)",
202 
204 
205  visual_node_ = robot_->getVisualNode()->createChildSceneNode();
206  collision_node_ = robot_->getCollisionNode()->createChildSceneNode();
207 
208  // create material for coloring links
210  Ogre::MaterialPtr(new Ogre::Material(nullptr, "robot link color material", 0, ROS_PACKAGE_NAME));
211  color_material_->setReceiveShadows(false);
212  color_material_->getTechnique(0)->setLightingEnabled(true);
213 
214  // create the ogre objects to display
215 
216  if (visual)
217  {
218  createVisual(link);
219  }
220 
221  if (collision)
222  {
223  createCollision(link);
224  }
225 
226  if (collision || visual)
227  {
228  createSelection();
229  }
230 
231  // create description and fill in child_joint_names_ vector
232  std::stringstream desc;
233  if (parent_joint_name_.empty())
234  {
235  desc << "Root Link <b>" << name_ << "</b>";
236  }
237  else
238  {
239  desc << "Link <b>" << name_ << "</b>";
240  desc << " with parent joint <b>" << parent_joint_name_ << "</b>";
241  }
242 
243  if (link->child_joints.empty())
244  {
245  desc << " has no children.";
246  }
247  else
248  {
249  desc << " has " << link->child_joints.size();
250 
251  if (link->child_joints.size() > 1)
252  {
253  desc << " child joints: ";
254  }
255  else
256  {
257  desc << " child joint: ";
258  }
259 
260  std::vector<urdf::JointSharedPtr>::const_iterator child_it = link->child_joints.begin();
261  std::vector<urdf::JointSharedPtr>::const_iterator child_end = link->child_joints.end();
262  for (; child_it != child_end; ++child_it)
263  {
264  urdf::Joint* child_joint = child_it->get();
265  if (child_joint && !child_joint->name.empty())
266  {
267  child_joint_names_.push_back(child_joint->name);
268  desc << "<b>" << child_joint->name << "</b>" << ((child_it + 1 == child_end) ? "." : ", ");
269  }
270  }
271  }
272  if (hasGeometry())
273  {
274  desc << " Check/uncheck to show/hide this link in the display.";
275  if (visual_meshes_.empty())
276  {
277  desc << " This link has collision geometry but no visible geometry.";
278  }
279  else if (collision_meshes_.empty())
280  {
281  desc << " This link has visible geometry but no collision geometry.";
282  }
283  }
284  else
285  {
286  desc << " This link has NO geometry.";
287  }
288 
289  link_property_->setDescription(desc.str().c_str());
290 
291  if (!hasGeometry())
292  {
293  link_property_->setIcon(rviz::loadPixmap("package://rviz/icons/classes/RobotLinkNoGeom.png"));
295  link_property_->setValue(QVariant());
296  }
297 }
298 
300 {
301  for (size_t i = 0; i < visual_meshes_.size(); i++)
302  {
303  scene_manager_->destroyEntity(visual_meshes_[i]);
304  }
305 
306  for (size_t i = 0; i < collision_meshes_.size(); i++)
307  {
308  scene_manager_->destroyEntity(collision_meshes_[i]);
309  }
310 
311  scene_manager_->destroySceneNode(visual_node_);
312  scene_manager_->destroySceneNode(collision_node_);
313 
314  if (trail_)
315  {
316  scene_manager_->destroyRibbonTrail(trail_);
317  }
318 
319  delete axes_;
320  delete details_;
321  delete link_property_;
322  errors.erase(this);
323 }
324 
325 void RobotLink::addError(const char* format, ...)
326 {
327  char buffer[256];
328  va_list args;
329  va_start(args, format);
330  vsnprintf(buffer, sizeof(buffer), format, args);
331  va_end(args);
332 
333  std::string& err = const_cast<std::string&>(getGeometryErrors());
334  if (!err.empty())
335  err.append("\n");
336  err.append(buffer);
337 }
338 
339 const std::string& RobotLink::getGeometryErrors() const
340 {
341  return errors[this];
342 }
343 
345 {
346  return visual_meshes_.size() + collision_meshes_.size() > 0;
347 }
348 
350 {
351  if (!hasGeometry())
352  return true;
353  return link_property_->getValue().toBool();
354 }
355 
357 {
358  robot_alpha_ = a;
359  updateAlpha();
360 }
361 
362 void RobotLink::setRenderQueueGroup(Ogre::uint8 group)
363 {
364  Ogre::SceneNode::ChildNodeIterator child_it = visual_node_->getChildIterator();
365  while (child_it.hasMoreElements())
366  {
367  Ogre::SceneNode* child = dynamic_cast<Ogre::SceneNode*>(child_it.getNext());
368  if (child)
369  {
370  Ogre::SceneNode::ObjectIterator object_it = child->getAttachedObjectIterator();
371  while (object_it.hasMoreElements())
372  {
373  Ogre::MovableObject* obj = object_it.getNext();
374  obj->setRenderQueueGroup(group);
375  }
376  }
377  }
378 }
379 
380 void RobotLink::setOnlyRenderDepth(bool onlyRenderDepth)
381 {
382  setRenderQueueGroup(onlyRenderDepth ? Ogre::RENDER_QUEUE_BACKGROUND : Ogre::RENDER_QUEUE_MAIN);
383  only_render_depth_ = onlyRenderDepth;
384  updateAlpha();
385 }
386 
388 {
389  float link_alpha = alpha_property_->getFloat();
390  M_SubEntityToMaterial::iterator it = materials_.begin();
391  M_SubEntityToMaterial::iterator end = materials_.end();
392  for (; it != end; ++it)
393  {
394  const Ogre::MaterialPtr& material = it->second;
395 
396  if (only_render_depth_)
397  {
398  material->setColourWriteEnabled(false);
399  material->setDepthWriteEnabled(true);
400  }
401  else
402  {
403  Ogre::ColourValue color = material->getTechnique(0)->getPass(0)->getDiffuse();
404  color.a = robot_alpha_ * material_alpha_ * link_alpha;
405  material->setDiffuse(color);
406 
407  if (color.a < 0.9998)
408  {
409  material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
410  material->setDepthWriteEnabled(false);
411  }
412  else
413  {
414  material->setSceneBlending(Ogre::SBT_REPLACE);
415  material->setDepthWriteEnabled(true);
416  }
417  }
418  }
419 
420  Ogre::ColourValue color = color_material_->getTechnique(0)->getPass(0)->getDiffuse();
421  color.a = robot_alpha_ * link_alpha;
422  color_material_->setDiffuse(color);
423 
424  if (color.a < 0.9998)
425  {
426  color_material_->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
427  color_material_->setDepthWriteEnabled(false);
428  }
429  else
430  {
431  color_material_->setSceneBlending(Ogre::SBT_REPLACE);
432  color_material_->setDepthWriteEnabled(true);
433  }
434 }
435 
437 {
438  bool enabled = getEnabled();
439 
441 
442  if (visual_node_)
443  {
444  visual_node_->setVisible(enabled && robot_->isVisible() && robot_->isVisualVisible());
445  }
446  if (collision_node_)
447  {
448  collision_node_->setVisible(enabled && robot_->isVisible() && robot_->isCollisionVisible());
449  }
450  if (trail_)
451  {
452  trail_->setVisible(enabled && robot_->isVisible());
453  }
454  if (axes_)
455  {
456  axes_->getSceneNode()->setVisible(enabled && robot_->isVisible());
457  }
458 }
459 
460 Ogre::MaterialPtr RobotLink::getMaterialForLink(const urdf::LinkConstSharedPtr& link,
461  urdf::MaterialConstSharedPtr material)
462 {
463  Ogre::MaterialPtr mat =
464  Ogre::MaterialPtr(new Ogre::Material(nullptr, "robot link material", 0, ROS_PACKAGE_NAME));
465 
466  // only the first visual's material actually comprises color values, all others only have the name
467  // hence search for the first visual with given material name (better fix the bug in urdf parser)
468  if (material && !material->name.empty())
469  {
470  for (const auto& visual : link->visual_array)
471  {
472  if (visual->material_name == material->name)
473  {
474  material = visual->material;
475  break;
476  }
477  }
478  }
479  if (!material && link->visual && link->visual->material)
480  material = link->visual->material; // fallback to visual's material
481 
482  if (!material)
483  {
484  // clone default material (for modification by link)
485  *mat = *Ogre::MaterialManager::getSingleton().getByName("RVIZ/ShadedRed");
486  return mat;
487  }
488 
489  mat->getTechnique(0)->setLightingEnabled(true);
490  if (material->texture_filename.empty())
491  {
492  const urdf::Color& col = material->color;
493  mat->getTechnique(0)->setAmbient(col.r * 0.5, col.g * 0.5, col.b * 0.5);
494  mat->getTechnique(0)->setDiffuse(col.r, col.g, col.b, col.a);
495 
496  material_alpha_ = col.a;
497  }
498  else
499  {
500  std::string filename = material->texture_filename;
501  if (!Ogre::TextureManager::getSingleton().resourceExists(filename))
502  {
505  try
506  {
507  res = retriever.get(filename);
508  }
510  {
511  ROS_ERROR("%s", e.what());
512  }
513 
514  if (res.size != 0)
515  {
516  Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
517  Ogre::Image image;
518  std::string extension = fs::extension(fs::path(filename));
519 
520  if (extension[0] == '.')
521  {
522  extension = extension.substr(1, extension.size() - 1);
523  }
524 
525  try
526  {
527  image.load(stream, extension);
528  Ogre::TextureManager::getSingleton().loadImage(
529  filename, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
530  }
531  catch (Ogre::Exception& e)
532  {
533  ROS_ERROR("Could not load texture [%s]: %s", filename.c_str(), e.what());
534  }
535  }
536  }
537 
538  Ogre::Pass* pass = mat->getTechnique(0)->getPass(0);
539  Ogre::TextureUnitState* tex_unit = pass->createTextureUnitState();
540  ;
541  tex_unit->setTextureName(filename);
542  }
543 
544  return mat;
545 }
546 
547 void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& link,
548  const urdf::Geometry& geom,
549  const urdf::MaterialSharedPtr& material,
550  const urdf::Pose& origin,
551  Ogre::SceneNode* scene_node,
552  Ogre::Entity*& entity)
553 {
554  entity = nullptr; // default in case nothing works.
555  Ogre::SceneNode* offset_node = scene_node->createChildSceneNode();
556 
557  static unsigned count = 0;
558  std::stringstream ss;
559  ss << "Robot Link" << ++count;
560  std::string entity_name = ss.str();
561 
562  Ogre::Vector3 scale(Ogre::Vector3::UNIT_SCALE);
563 
564  Ogre::Vector3 offset_position(Ogre::Vector3::ZERO);
565  Ogre::Quaternion offset_orientation(Ogre::Quaternion::IDENTITY);
566 
567 
568  {
569  Ogre::Vector3 position(origin.position.x, origin.position.y, origin.position.z);
570  Ogre::Quaternion orientation(Ogre::Quaternion::IDENTITY);
571  orientation = orientation * Ogre::Quaternion(origin.rotation.w, origin.rotation.x, origin.rotation.y,
572  origin.rotation.z);
573 
574  offset_position = position;
575  offset_orientation = orientation;
576  }
577 
578  switch (geom.type)
579  {
580  case urdf::Geometry::SPHERE:
581  {
582  const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
583  entity = Shape::createEntity(entity_name, Shape::Sphere, scene_manager_);
584 
585  scale = Ogre::Vector3(sphere.radius * 2, sphere.radius * 2, sphere.radius * 2);
586  break;
587  }
588  case urdf::Geometry::BOX:
589  {
590  const urdf::Box& box = static_cast<const urdf::Box&>(geom);
591  entity = Shape::createEntity(entity_name, Shape::Cube, scene_manager_);
592 
593  scale = Ogre::Vector3(box.dim.x, box.dim.y, box.dim.z);
594 
595  break;
596  }
597  case urdf::Geometry::CYLINDER:
598  {
599  const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
600 
601  Ogre::Quaternion rotX;
602  rotX.FromAngleAxis(Ogre::Degree(90), Ogre::Vector3::UNIT_X);
603  offset_orientation = offset_orientation * rotX;
604 
605  entity = Shape::createEntity(entity_name, Shape::Cylinder, scene_manager_);
606  scale = Ogre::Vector3(cylinder.radius * 2, cylinder.length, cylinder.radius * 2);
607  break;
608  }
609  case urdf::Geometry::MESH:
610  {
611  const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
612 
613  if (mesh.filename.empty())
614  return;
615 
616 
617  scale = Ogre::Vector3(mesh.scale.x, mesh.scale.y, mesh.scale.z);
618 
619  const std::string& model_name = mesh.filename;
620 
621  try
622  {
623  if (loadMeshFromResource(model_name).isNull())
624  addError("Could not load mesh resource '%s'", model_name.c_str());
625  else
626  entity = scene_manager_->createEntity(ss.str(), model_name);
627  }
628  catch (Ogre::InvalidParametersException& e)
629  {
630  addError("Could not convert mesh resource '%s': %s", model_name.c_str(), e.what());
631  }
632  catch (Ogre::Exception& e)
633  {
634  addError("Could not load model '%s': %s", model_name.c_str(), e.what());
635  }
636  break;
637  }
638  default:
639  ROS_WARN("Unsupported geometry type for element: %d", geom.type);
640  break;
641  }
642 
643  if (entity)
644  {
645  offset_node->attachObject(entity);
646  offset_node->setScale(scale);
647  offset_node->setPosition(offset_position);
648  offset_node->setOrientation(offset_orientation);
649 
650  if (default_material_.isNull() || material)
651  {
652  // latest used material becomes the default for this link
653  default_material_ = getMaterialForLink(link, material);
654  }
655 
656  for (uint32_t i = 0; i < entity->getNumSubEntities(); ++i)
657  {
658  // Assign materials only if the submesh does not have one already
659  Ogre::SubEntity* sub = entity->getSubEntity(i);
660  const std::string& material_name = sub->getMaterialName();
661 
662  if (material_name == "BaseWhite" || material_name == "BaseWhiteNoLighting")
663  {
664  sub->setMaterial(default_material_);
665  }
666  else
667  {
668  // create a new material copy for each instance of a RobotLink
669  Ogre::MaterialPtr mat =
670  Ogre::MaterialPtr(new Ogre::Material(nullptr, material_name, 0, ROS_PACKAGE_NAME));
671  *mat = *sub->getMaterial();
672  sub->setMaterial(mat);
673  }
674  materials_[sub] = sub->getMaterial();
675  }
676  }
677 }
678 
679 void RobotLink::createCollision(const urdf::LinkConstSharedPtr& link)
680 {
681  bool valid_collision_found = false;
682 #if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
683  std::map<std::string, boost::shared_ptr<std::vector<urdf::CollisionSharedPtr> > >::const_iterator mi;
684  for (mi = link->collision_groups.begin(); mi != link->collision_groups.end(); mi++)
685  {
686  if (mi->second)
687  {
688  std::vector<urdf::CollisionSharedPtr>::const_iterator vi;
689  for (vi = mi->second->begin(); vi != mi->second->end(); vi++)
690  {
691  urdf::CollisionSharedPtr collision = *vi;
692  if (collision && collision->geometry)
693  {
694  Ogre::Entity* collision_mesh = NULL;
695  createEntityForGeometryElement(link, *collision->geometry, urdf::MaterialSharedPtr(),
696  collision->origin, collision_node_, collision_mesh);
697  if (collision_mesh)
698  {
699  collision_meshes_.push_back(collision_mesh);
700  valid_collision_found = true;
701  }
702  }
703  }
704  }
705  }
706 #else
707  std::vector<urdf::CollisionSharedPtr>::const_iterator vi;
708  for (vi = link->collision_array.begin(); vi != link->collision_array.end(); vi++)
709  {
710  urdf::CollisionSharedPtr collision = *vi;
711  if (collision && collision->geometry)
712  {
713  Ogre::Entity* collision_mesh = nullptr;
714  createEntityForGeometryElement(link, *collision->geometry, urdf::MaterialSharedPtr(),
715  collision->origin, collision_node_, collision_mesh);
716  if (collision_mesh)
717  {
718  collision_meshes_.push_back(collision_mesh);
719  }
720  valid_collision_found |= collision == link->collision; // don't consider the same geometry twice
721  }
722  }
723 #endif
724 
725  if (!valid_collision_found && link->collision && link->collision->geometry)
726  {
727  Ogre::Entity* collision_mesh = nullptr;
728  createEntityForGeometryElement(link, *link->collision->geometry, urdf::MaterialSharedPtr(),
729  link->collision->origin, collision_node_, collision_mesh);
730  if (collision_mesh)
731  {
732  collision_meshes_.push_back(collision_mesh);
733  }
734  }
735 
736  collision_node_->setVisible(getEnabled());
737 }
738 
739 void RobotLink::createVisual(const urdf::LinkConstSharedPtr& link)
740 {
741  bool valid_visual_found = false;
742 #if URDF_MAJOR_VERSION == 0 && URDF_MINOR_VERSION == 2
743  std::map<std::string, boost::shared_ptr<std::vector<urdf::VisualSharedPtr> > >::const_iterator mi;
744  for (mi = link->visual_groups.begin(); mi != link->visual_groups.end(); mi++)
745  {
746  if (mi->second)
747  {
748  std::vector<urdf::VisualSharedPtr>::const_iterator vi;
749  for (vi = mi->second->begin(); vi != mi->second->end(); vi++)
750  {
751  urdf::VisualSharedPtr visual = *vi;
752  if (visual && visual->geometry)
753  {
754  Ogre::Entity* visual_mesh = NULL;
755  createEntityForGeometryElement(link, *visual->geometry, visual->material, visual->origin,
756  visual_node_, visual_mesh);
757  if (visual_mesh)
758  {
759  visual_meshes_.push_back(visual_mesh);
760  valid_visual_found = true;
761  }
762  }
763  }
764  }
765  }
766 #else
767  std::vector<urdf::VisualSharedPtr>::const_iterator vi;
768  for (vi = link->visual_array.begin(); vi != link->visual_array.end(); vi++)
769  {
770  urdf::VisualSharedPtr visual = *vi;
771  if (visual && visual->geometry)
772  {
773  Ogre::Entity* visual_mesh = nullptr;
774  createEntityForGeometryElement(link, *visual->geometry, visual->material, visual->origin,
775  visual_node_, visual_mesh);
776  if (visual_mesh)
777  {
778  visual_meshes_.push_back(visual_mesh);
779  }
780  valid_visual_found |= visual == link->visual; // don't consider the same geometry again
781  }
782  }
783 #endif
784 
785  if (!valid_visual_found && link->visual && link->visual->geometry)
786  {
787  Ogre::Entity* visual_mesh = nullptr;
788  createEntityForGeometryElement(link, *link->visual->geometry, link->visual->material,
789  link->visual->origin, visual_node_, visual_mesh);
790  if (visual_mesh)
791  {
792  visual_meshes_.push_back(visual_mesh);
793  }
794  }
795 
796  visual_node_->setVisible(getEnabled());
797 }
798 
800 {
802  for (size_t i = 0; i < visual_meshes_.size(); i++)
803  {
804  selection_handler_->addTrackedObject(visual_meshes_[i]);
805  }
806  for (size_t i = 0; i < collision_meshes_.size(); i++)
807  {
808  selection_handler_->addTrackedObject(collision_meshes_[i]);
809  }
810 }
811 
813 {
814  if (trail_property_->getValue().toBool())
815  {
816  if (!trail_)
817  {
818  if (visual_node_)
819  {
820  static int count = 0;
821  std::stringstream ss;
822  ss << "Trail for link " << name_ << count++;
823  trail_ = scene_manager_->createRibbonTrail(ss.str());
824  trail_->setMaxChainElements(100);
825  trail_->setInitialWidth(0, 0.01f);
826  trail_->setInitialColour(0, 0.0f, 0.5f, 0.5f);
827  trail_->addNode(visual_node_);
828  trail_->setTrailLength(2.0f);
829  trail_->setVisible(getEnabled());
830  robot_->getOtherNode()->attachObject(trail_);
831  }
832  else
833  {
834  ROS_WARN("No visual node for link %s, cannot create a trail", name_.c_str());
835  }
836  }
837  }
838  else
839  {
840  if (trail_)
841  {
842  scene_manager_->destroyRibbonTrail(trail_);
843  trail_ = nullptr;
844  }
845  }
846 }
847 
849 {
850  if (axes_property_->getValue().toBool())
851  {
852  if (!axes_)
853  {
854  static int count = 0;
855  std::stringstream ss;
856  ss << "Axes for link " << name_ << count++;
857  axes_ = new Axes(scene_manager_, robot_->getOtherNode(), 0.1, 0.01);
858  axes_->getSceneNode()->setVisible(getEnabled());
859 
862  }
863  }
864  else
865  {
866  if (axes_)
867  {
868  delete axes_;
869  axes_ = nullptr;
870  }
871  }
872 }
873 
874 void RobotLink::setTransforms(const Ogre::Vector3& visual_position,
875  const Ogre::Quaternion& visual_orientation,
876  const Ogre::Vector3& collision_position,
877  const Ogre::Quaternion& collision_orientation)
878 {
879  if (visual_node_)
880  {
881  visual_node_->setPosition(visual_position);
882  visual_node_->setOrientation(visual_orientation);
883  }
884 
885  if (collision_node_)
886  {
887  collision_node_->setPosition(collision_position);
888  collision_node_->setOrientation(collision_orientation);
889  }
890 
891  position_property_->setVector(visual_position);
892  orientation_property_->setQuaternion(visual_orientation);
893 
894  if (axes_)
895  {
896  axes_->setPosition(visual_position);
897  axes_->setOrientation(visual_orientation);
898  }
899 }
900 
902 {
904 }
905 
907 {
909 }
910 
911 void RobotLink::setMaterialMode(unsigned char mode_flags)
912 {
913  if (material_mode_flags_ == mode_flags)
914  return; // nothing to change
915 
916  material_mode_flags_ = mode_flags;
917 
918  if (mode_flags == ORIGINAL)
919  {
920  for (const auto& item : materials_)
921  item.first->setMaterial(item.second);
922  return;
923  }
924 
925  auto error_material = Ogre::MaterialManager::getSingleton().getByName(
926  "BaseWhiteNoLighting", Ogre::ResourceGroupManager::INTERNAL_RESOURCE_GROUP_NAME);
927  auto material = mode_flags == COLOR ? color_material_ : error_material;
928 
929  for (const auto& mesh : visual_meshes_)
930  mesh->setMaterial(material);
931  for (const auto& mesh : collision_meshes_)
932  mesh->setMaterial(material);
933 }
934 
935 void RobotLink::setColor(float red, float green, float blue)
936 {
937  Ogre::ColourValue color = color_material_->getTechnique(0)->getPass(0)->getDiffuse();
938  color.r = red;
939  color.g = green;
940  color.b = blue;
941  color_material_->getTechnique(0)->setAmbient(0.5 * color);
942  color_material_->getTechnique(0)->setDiffuse(color);
943 
945 }
946 
948 {
950 }
951 
952 bool RobotLink::setSelectable(bool selectable)
953 {
954  bool old = is_selectable_;
955  is_selectable_ = selectable;
956  return old;
957 }
958 
960 {
961  return is_selectable_;
962 }
963 
965 {
968  trail_property_->setHidden(hide);
969  axes_property_->setHidden(hide);
970  alpha_property_->setHidden(hide);
971 }
972 
973 Ogre::Vector3 RobotLink::getPosition()
974 {
975  return position_property_->getVector();
976 }
977 
978 Ogre::Quaternion RobotLink::getOrientation()
979 {
981 }
982 
984 {
985  Property* old_parent = link_property_->getParent();
986  if (old_parent)
987  old_parent->takeChild(link_property_);
988 
989  if (new_parent)
990  new_parent->addChild(link_property_);
991 }
992 
993 // if use_detail:
994 // - all sub properties become children of details_ property.
995 // - details_ property becomes a child of link_property_
996 // else (!use_detail)
997 // - all sub properties become children of link_property_.
998 // details_ property does not have a parent.
999 void RobotLink::useDetailProperty(bool use_detail)
1000 {
1001  Property* old_parent = details_->getParent();
1002  if (old_parent)
1003  old_parent->takeChild(details_);
1004 
1005  if (use_detail)
1006  {
1007  while (link_property_->numChildren() > 0)
1008  {
1009  Property* child = link_property_->childAt(0);
1010  link_property_->takeChild(child);
1011  details_->addChild(child);
1012  }
1013 
1015  }
1016  else
1017  {
1018  while (details_->numChildren() > 0)
1019  {
1020  Property* child = details_->childAt(0);
1021  details_->takeChild(child);
1022  link_property_->addChild(child);
1023  }
1024  }
1025 }
1026 
1027 void RobotLink::expandDetails(bool expand)
1028 {
1030  if (expand)
1031  {
1032  parent->expand();
1033  }
1034  else
1035  {
1036  parent->collapse();
1037  }
1038 }
1039 
1040 
1041 } // namespace rviz
#define NULL
Definition: global.h:37
void setOrientation(const Ogre::Quaternion &orientation) override
Set the orientation of the object.
Definition: axes.cpp:88
filename
VectorProperty * position_property_
Definition: robot_link.cpp:88
void updateProperties() override
Override to update property values.
Definition: robot_link.cpp:116
virtual void expand()
Expand (show the children of) this Property.
Definition: property.cpp:555
static Ogre::Entity * createEntity(const std::string &name, Type shape_type, Ogre::SceneManager *scene_manager)
Definition: shape.cpp:45
virtual bool setVector(const Ogre::Vector3 &vector)
f
Ogre::SceneNode * getOtherNode()
Definition: robot.h:180
Property * getParent() const
Return the parent Property.
Definition: property.cpp:231
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:134
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
void setReadOnly(bool read_only) override
Overridden from Property to propagate read-only-ness to children.
virtual Ogre::Vector3 getVector() const
Ogre::MeshPtr loadMeshFromResource(const std::string &resource_path)
Property specialized to enforce floating point max/min.
#define ROS_WARN(...)
void setReadOnly(bool read_only) override
Overridden from Property to propagate read-only-ness to children.
virtual void setDescription(const QString &description)
Set the description.
Definition: property.cpp:169
bool isCollisionVisible()
Returns whether or not the collision representation is set to be visible To be visible this and isVis...
Definition: robot.cpp:170
void calculateJointCheckboxes()
Definition: robot.cpp:646
void createProperties(const Picked &obj, Property *parent_property) override
Override to create properties of the given picked object(s).
Definition: robot_link.cpp:101
Property * takeChild(Property *child)
Remove a given child object and return a pointer to it.
Definition: property.cpp:321
virtual void collapse()
Collapse (hide the children of) this Property.
Definition: property.cpp:563
Ogre::SceneNode * getCollisionNode()
Definition: robot.h:176
virtual void setIcon(const QIcon &icon)
Set the icon to be displayed next to the property.
Definition: property.h:193
Pure-virtual base class for objects which give Display subclasses context in which to work...
boost::shared_array< uint8_t > data
DisplayContext * context_
bool isVisible()
Returns whether anything is visible.
Definition: robot.cpp:160
virtual bool setQuaternion(const Ogre::Quaternion &quaternion)
An object that displays a set of X/Y/Z axes, with X=Red, Y=Green, Z=Blue.
Definition: axes.h:59
Ogre::SceneNode * getVisualNode()
Definition: robot.h:172
void setPosition(const Ogre::Vector3 &position) override
Set the position of this object.
Definition: axes.cpp:83
MemoryResource get(const std::string &url)
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:543
virtual int numChildren() const
Return the number of child objects (Property or otherwise).
Definition: property.h:227
virtual float getFloat() const
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:202
virtual void addChild(Property *child, int index=-1)
Add a child property.
Definition: property.cpp:355
QList< Property * > properties_
void preRenderPass(uint32_t pass) override
Definition: robot_link.cpp:123
RobotLinkSelectionHandler(RobotLink *link, DisplayContext *context)
Definition: robot_link.cpp:92
static std::map< const RobotLink *, std::string > errors
Definition: robot_link.cpp:154
void postRenderPass(uint32_t pass) override
Definition: robot_link.cpp:146
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:398
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:150
Ogre::SceneNode * getSceneNode()
Get the scene node associated with this object.
Definition: axes.h:95
#define ROS_ERROR(...)
QPixmap loadPixmap(QString url, bool fill_cache)
bool isVisualVisible()
Returns whether or not the visual representation is set to be visible To be visible this and isVisibl...
Definition: robot.cpp:165
virtual Ogre::Quaternion getQuaternion() const
QuaternionProperty * orientation_property_
Definition: robot_link.cpp:89


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Aug 15 2022 02:05:17