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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:10