textured_mesh_display.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Robot Operating System code by the University of Osnabrück
5  * Copyright (c) 2015, University of Osnabrück
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * 1. Redistributions of source code must retain the above
13  * copyright notice, this list of conditions and the following
14  * disclaimer.
15  *
16  * 2. Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  *
21  * 3. Neither the name of the copyright holder nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  *
26  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
28  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
29  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
30  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
31  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
32  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
33  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
34  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
35  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
36  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
37  *
38  *
39  *
40  * textured_mesh_display.cpp
41  *
42  *
43  * authors:
44  *
45  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
46  * Henning Deeken <hdeeken@uni-osnabrueck.de>
47  * Marcel Mrozinski
48  * Nils Oesting
49  * Kristin Schmidt <krschmidt@uni-osnabrueck.de>
50  * Jan Philipp Vogtherr <jvogtherr@uni-osnabrueck.de>
51  */
52 
53 #include "textured_mesh_display.h"
54 
55 #include <mesh_msgs/GetVertexColors.h>
56 #include <mesh_msgs/GetMaterials.h>
57 #include <mesh_msgs/GetGeometry.h>
58 #include <mesh_msgs/GetTexture.h>
59 #include <mesh_msgs/GetUUID.h>
60 
61 #include <OGRE/OgreSceneNode.h>
62 #include <OGRE/OgreSceneManager.h>
63 
64 #include <tf/transform_listener.h>
65 
67 #include <rviz/frame_manager.h>
68 #include <rviz/display_context.h>
77 
78 #include <boost/random/mersenne_twister.hpp>
79 #include <boost/random/uniform_int.hpp>
80 #include <boost/random/variate_generator.hpp>
81 
82 #include <ros/callback_queue.h>
83 
84 namespace rviz_mesh_plugin
85 {
86 
88 
90 {
92  m_tfMeshFilter = 0;
93  m_meshCounter = 0;
94 
95  // topic properties
97  "Geometry Topic",
98  "",
99  QString::fromStdString(ros::message_traits::datatype<mesh_msgs::MeshGeometryStamped>()),
100  "Geometry topic to subscribe to.",
101  this,
102  SLOT(updateTopic())
103  );
104 
105  // mesh buffer size property
107  "Mesh Buffer Size",
108  1,
109  "Number of prior meshes to display.",
110  this,
111  SLOT(updateMeshBufferSize())
112  );
114 
115  // Display type selection dropdown
117  "Display Type",
118  "Faces with fixed color",
119  "Select Display Type for Mesh",
120  this,
121  SLOT(updateMesh()),
122  this
123  );
124  m_displayType->addOption("Fixed Color", 0);
125  m_displayType->addOption("Vertex Color", 1);
126  m_displayType->addOption("Textures", 2);
127  m_displayType->addOption("Vertex Costs", 3);
128  m_displayType->addOption("Hide Faces", 4);
129 
131  "Color Scale",
132  "Rainbow",
133  "Select color scale for vertex costs. Mesh will update when new data arrives.",
135  SLOT(updateMesh()),
136  this
137  );
138  m_costColorType->addOption("Rainbow", 0);
139  m_costColorType->addOption("Red Green", 1);
140 
142  "Vertex Colors Topic",
143  "",
144  QString::fromStdString(ros::message_traits::datatype<mesh_msgs::MeshVertexColorsStamped>()),
145  "Vertex color topic to subscribe to.",
147  SLOT(updateTopic()),
148  this
149  );
150 
152  "Vertex Costs Topic",
153  "",
154  QString::fromStdString(ros::message_traits::datatype<mesh_msgs::MeshVertexCostsStamped>()),
155  "Vertex cost topic to subscribe to.",
157  SLOT(updateTopic()),
158  this
159  );
160 
162  "Show textured faces only",
163  false,
164  "Show textured faces only",
166  SLOT(updateMesh()),
167  this
168  );
169 
170  // face color properties
172  "Faces Color",
173  QColor(0, 255, 0),
174  "The color of the faces.",
176  SLOT(updateMesh()),
177  this
178  );
179 
180  // face alpha properties
182  "Faces Alpha",
183  1.0,
184  "The alpha-value of the faces",
186  SLOT(updateMesh()),
187  this
188  );
189  m_facesAlpha->setMin(0);
190  m_facesAlpha->setMax(1);
191 
192  // Vertex color service name property
194  "Vertex Color Service Name",
195  "get_vertex_colors",
196  "Name of the Vertex Color Service to request Vertex Colors from.",
198  SLOT(updateVertexColorService()),
199  this
200  );
201 
202  // Material service name property
204  "Material Service Name",
205  "get_materials",
206  "Name of the Matrial Service to request Materials from.",
209  this
210  );
211 
212  // Textures service name property
214  "Texture Service Name",
215  "get_texture",
216  "Name of the Texture Service to request Textures from.",
219  this
220  );
221 
223  "Vertex Costs Type",
224  "-- None --",
225  "Select the type of vertex cost map to be displayed. New types will appear here when a new message arrives.",
227  SLOT(updateVertexCosts()),
228  this
229  );
230  m_selectVertexCostMap->addOption("-- None --", 0);
231 
232 
234  "Show Wireframe",
235  true,
236  "Show Wireframe",
237  this,
238  SLOT(updateMesh()),
239  this
240  );
241 
242  // wireframe color property
244  "Wireframe Color",
245  QColor(0, 0, 0),
246  "The color of the wireframe.",
248  SLOT(updateMesh()),
249  this
250  );
251  // wireframe alpha property
253  "Wireframe Alpha",
254  1.0,
255  "The alpha-value of the wireframe",
257  SLOT(updateMesh()),
258  this
259  );
262 
263 
265  "Show Normals",
266  true,
267  "Show Normals",
268  this,
269  SLOT(updateMesh()),
270  this
271  );
272 
274  "Normals Color",
275  QColor(255, 0, 255),
276  "The color of the normals.",
278  SLOT(updateMesh()),
279  this
280  );
282  "Normals Alpha",
283  1.0,
284  "The alpha-value of the normals",
286  SLOT(updateMesh()),
287  this
288  );
292  "Normals Scaling Factor",
293  0.1,
294  "Scaling factor of the normals",
296  SLOT(updateMesh()),
297  this
298  );
299 
301  "Use Custom limits",
302  false,
303  "Use custom vertex cost limits",
305  SLOT(updateVertexCosts()),
306  this
307  );
308 
310  "Vertex Costs Lower Limit",
311  0.0,
312  "Vertex costs lower limit",
314  SLOT(updateVertexCosts()),
315  this
316  );
318 
320  "Vertex Costs Upper Limit",
321  1.0,
322  "Vertex costs upper limit",
324  SLOT(updateVertexCosts()),
325  this
326  );
328 
329 }
330 
332 {
333  unsubscribe();
334  delete m_tfMeshFilter;
335 }
336 
338 {
340  *rviz::Display::context_->getTF2BufferPtr(),
341  rviz::Display::fixed_frame_.toStdString(),
342  2,
344  );
347 
349  *rviz::Display::context_->getTF2BufferPtr(),
350  rviz::Display::fixed_frame_.toStdString(),
351  10,
353  );
356 
358  *rviz::Display::context_->getTF2BufferPtr(),
359  rviz::Display::fixed_frame_.toStdString(),
360  10,
362  );
365 
366  m_meshSynchronizer = 0;
369 
371  updateTopic();
372  initServices();
373  updateMesh();
374 }
375 
377 {
378  ros::NodeHandle n;
379  ros::ServiceClient m_uuidClient = n.serviceClient<mesh_msgs::GetUUID>("get_uuid");
380 
381  mesh_msgs::GetUUID srv_uuid;
382  if (m_uuidClient.call(srv_uuid))
383  {
384  std::string uuid = (std::string)srv_uuid.response.uuid;
385 
386  ROS_INFO_STREAM("Initial data available for UUID=" << uuid);
387 
388  ros::ServiceClient m_geometryClient = n.serviceClient<mesh_msgs::GetGeometry>("get_geometry");
389 
390  mesh_msgs::GetGeometry srv_geometry;
391  srv_geometry.request.uuid = uuid;
392  if (m_geometryClient.call(srv_geometry))
393  {
394  ROS_INFO_STREAM("Found geometry for UUID=" << uuid);
395  mesh_msgs::MeshGeometryStamped::ConstPtr geometry
396  = boost::make_shared<const mesh_msgs::MeshGeometryStamped>(srv_geometry.response.mesh_geometry_stamped);
397  processMessage(geometry);
398  }
399  else
400  {
401  ROS_INFO_STREAM("Could not load geometry. Waiting for callback to trigger ... ");
402  }
403  }
404  else
405  {
406  ROS_INFO("No initial data available, waiting for callback to trigger ...");
407  }
408 }
409 
411 {
412  rviz::Display::reset(); // TODO bad hack?!
414  m_messagesReceived = 0;
415  m_meshVisuals.clear();
416 }
417 
419 {
420  unsubscribe();
421  reset();
422  subscribe();
424 }
425 
427 {
428  if (!isEnabled())
429  {
430  return;
431  }
432 
433  try {
437  setStatus(rviz::StatusProperty::Ok, "Topic", "OK");
438  }
439  catch(ros::Exception& e)
440  {
441  setStatus(rviz::StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
442  }
443 
444  // Nothing
445  if (m_meshTopic->getTopicStd().empty())
446  {
448  return;
449  }
450  else
451  {
455  m_meshSubscriber, 10
456  );
458  boost::bind(&TexturedMeshDisplay::incomingGeometry, this, _1)
459  );
460 
464  );
466  boost::bind(&TexturedMeshDisplay::incomingVertexColors, this, _1)
467  );
468 
472  );
474  boost::bind(&TexturedMeshDisplay::incomingVertexCosts, this, _1)
475  );
476 
477  }
478 
480 }
481 
483 {
487 
488  if (m_meshSynchronizer)
489  {
490  delete m_meshSynchronizer;
491  m_meshSynchronizer = 0;
492  }
494  {
495  delete m_colorsSynchronizer;
497  }
499  {
500  delete m_costsSynchronizer;
502  }
503 }
504 
506 {
507  subscribe();
508 }
509 
511 {
512  unsubscribe();
513  reset();
514 }
515 
517 {
519  reset();
520 }
521 
522 void TexturedMeshDisplay::incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr& colorsStamped)
523 {
524  if (m_meshVisuals.empty())
525  {
526  ROS_ERROR("Received vertex colors, but no visual available!");
527  return;
528  }
529  if (colorsStamped->uuid.compare(m_lastUuid) != 0)
530  {
531  ROS_ERROR("Received vertex colors, but UUIDs dont match!");
532  return;
533  }
534 
535  getCurrentVisual()->setVertexColors(colorsStamped);
536 
537  updateMesh();
538 }
539 
540 void TexturedMeshDisplay::incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr& costsStamped)
541 {
542  if (m_meshVisuals.empty())
543  {
544  ROS_ERROR("Received vertex costs, but no visual available!");
545  return;
546  }
547  if (costsStamped->uuid.compare(m_lastUuid) != 0)
548  {
549  ROS_ERROR("Received vertex costs, but UUIDs dont match!");
550  return;
551  }
552 
553  cacheVertexCosts(costsStamped);
555 }
556 
558  const mesh_msgs::MeshVertexCostsStamped::ConstPtr costsStamped
559 )
560 {
561  ROS_INFO_STREAM("Cache vertex cost map '" << costsStamped->type << "' for UUID " << costsStamped->uuid);
562 
563  // insert into cache
564  std::pair<std::map<std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr>::iterator, bool> ret =
565  m_costCache.insert(
566  std::pair<std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr>(costsStamped->type, costsStamped));
567  if(ret.second)
568  {
569  ROS_INFO_STREAM("The cost layer \"" << costsStamped->type << "\" has been added.");
571  }
572  else
573  {
574  //m_selectVertexCostMap->addOptionStd(costsStamped->type, m_selectVertexCostMap->numChildren());
575  m_costCache.erase(ret.first);
576  m_costCache.insert(
577  std::pair<std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr>(costsStamped->type, costsStamped));
578  ROS_INFO_STREAM("The cost layer \"" << costsStamped->type << "\" has been updated.");
579  }
580 
581 }
582 
583 void TexturedMeshDisplay::incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg)
584 {
586  setStatus(rviz::StatusProperty::Ok, "Topic", QString::number(m_messagesReceived) + " messages received");
587  processMessage(meshMsg);
588 }
589 
591 {
592  reset();
593  m_meshVisuals.rset_capacity(m_meshBufferSize->getInt());
594 }
595 
597 {
599  {
603 
604  if (m_meshVisuals.size() > 0 && m_costCache.count(m_selectVertexCostMap->getStdString()) != 0)
605  {
606  getCurrentVisual()->setVertexCosts(
611  );
612  }
613  }
614  else
615  {
618  if (m_meshVisuals.size() > 0 && m_costCache.count(m_selectVertexCostMap->getStdString()) != 0)
619  {
620  getCurrentVisual()->setVertexCosts(
623  );
624  }
625  }
626  updateMesh();
627 }
628 
629 
631 {
632  bool showWireframe = m_showWireframe->getBool();
633  bool showNormals = m_showNormals->getBool();
634  bool showFaces = false;
635  bool showTextures = false;
636  bool useVertexColors = false;
637  bool showVertexCosts = false;
638 
640  m_facesColor->hide();
641  m_facesAlpha->hide();
650  deleteStatus("Services");
651 
652  switch (m_displayType->getOptionInt())
653  {
654  default:
655  case 0: // Faces with fixed color
656  showFaces = true;
657  m_facesColor->show();
658  m_facesAlpha->show();
659  break;
660  case 1: // Faces with vertex color
661  showFaces = true;
662  useVertexColors = true;
666  break;
667  case 2: // Faces with textures
668  showFaces = true;
669  showTextures = true;
674  break;
675  case 3: // Faces with vertex costs
676  showFaces = true;
677  showVertexCosts = true;
682  break;
683  case 4: // No Faces
684  break;
685  }
686 
687  if (!m_meshVisuals.empty())
688  {
689  for (
690  boost::circular_buffer<boost::shared_ptr<TexturedMeshVisual> >::iterator it = m_meshVisuals.begin();
691  it != m_meshVisuals.end();
692  it++
693  )
694  {
695  it->get()->updateMaterial(
697  showFaces, m_facesColor->getOgreColor(), m_facesAlpha->getFloat(), useVertexColors, showVertexCosts,
698  showTextures, m_showTexturedFacesOnly->getBool(),
700  );
701  }
702  }
703 }
704 
706 {
707  // Initialize service clients
708  ros::NodeHandle n;
709  m_vertexColorClient = n.serviceClient<mesh_msgs::GetVertexColors>(m_vertexColorServiceName->getStdString());
710 
711  m_materialsClient = n.serviceClient<mesh_msgs::GetMaterials>(m_materialServiceName->getStdString());
712 
713  m_textureClient = n.serviceClient<mesh_msgs::GetTexture>(m_textureServiceName->getStdString());
714 }
715 
717 {
718  // Check if the service name is valid
719  std::string error;
721  {
722  setStatus(rviz::StatusProperty::Warn, "Services", QString("The service name contains an invalid character."));
723  return;
724  }
725 
726  // Update vertex color service client
727  ros::NodeHandle n;
728  m_vertexColorClient = n.serviceClient<mesh_msgs::GetVertexColors>(m_vertexColorServiceName->getStdString());
730  {
731  setStatus(rviz::StatusProperty::Ok, "Services", "Vertex Color Service OK");
732  if (!m_meshVisuals.empty())
733  {
735  }
736  }
737  else
738  {
739  setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Vertex Color Service doesn't exist."));
740  }
741 }
742 
744 {
745  // Check if the service names are valid
746  std::string error;
749  {
750  setStatus(rviz::StatusProperty::Warn, "Services", QString("The service name contains an invalid character."));
751  return;
752  }
753 
754  // Update material and texture service clients
755  ros::NodeHandle n;
756  m_materialsClient = n.serviceClient<mesh_msgs::GetMaterials>(m_materialServiceName->getStdString());
757  m_textureClient = n.serviceClient<mesh_msgs::GetTexture>(m_textureServiceName->getStdString());
759  {
760  if (!m_meshVisuals.empty())
761  {
763  }
764  if (m_textureClient.exists())
765  {
766  setStatus(rviz::StatusProperty::Ok, "Services", "Material and Texture Service OK");
767  }
768  else
769  {
770  setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Texture Service doesn't exist."));
771  }
772  }
773  else
774  {
775  setStatus(rviz::StatusProperty::Warn, "Services", QString("The specified Material Service doesn't exist."));
776  }
777 }
778 
780 {
782  if (m_meshVisuals.full())
783  {
784  visual = m_meshVisuals.front();
785  m_meshVisuals.push_back(visual);
786  }
787  else
788  {
789  int randomId = (int)((double)rand() / RAND_MAX * 9998);
790 
791  visual.reset(new TexturedMeshVisual(context_, m_displayID, m_meshCounter, randomId));
792 
793  m_meshVisuals.push_back(visual);
794  m_meshCounter++;
795  }
796  return visual;
797 }
798 
800 {
801 
802  if (m_meshVisuals.empty())
803  {
804  ROS_ERROR("Requested current visual when none is available!");
805  }
806 
807  return m_meshVisuals.back();
808 }
809 
810 void TexturedMeshDisplay::processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg)
811 {
812  Ogre::Quaternion orientation;
813  Ogre::Vector3 position;
814 
816  meshMsg->header.frame_id,
817  meshMsg->header.stamp,
818  position, orientation)
819  )
820  {
821  ROS_ERROR(
822  "Error transforming from frame '%s' to frame '%s'",
823  meshMsg->header.frame_id.c_str(), qPrintable(rviz::Display::fixed_frame_)
824  );
825  return;
826  }
827 
828  if (!m_lastUuid.empty() && meshMsg->uuid.compare(m_lastUuid) != 0)
829  {
830  ROS_WARN("Received geometry with new UUID!");
831  m_costCache.clear();
832 
833  }
834 
835  m_lastUuid = meshMsg->uuid;
836 
838 
839  visual->setGeometry(meshMsg);
840  requestVertexColors(visual, meshMsg->uuid);
841  requestMaterials(visual, meshMsg->uuid);
842  updateMesh();
843  visual->setFramePosition(position);
844  visual->setFrameOrientation(orientation);
845 }
846 
848 {
849  mesh_msgs::GetVertexColors srv;
850  srv.request.uuid = uuid;
851  if (m_vertexColorClient.call(srv))
852  {
853  ROS_INFO("Successful vertex colors service call!");
854  mesh_msgs::MeshVertexColorsStamped::ConstPtr meshVertexColors =
855  boost::make_shared<const mesh_msgs::MeshVertexColorsStamped>(srv.response.mesh_vertex_colors_stamped);
856 
857  visual->setVertexColors(meshVertexColors);
858  }
859  else
860  {
861  ROS_INFO("Failed vertex colors service call!");
862  }
863 }
864 
866 {
867  mesh_msgs::GetMaterials srv;
868  srv.request.uuid = uuid;
869  if (m_materialsClient.call(srv))
870  {
871  ROS_INFO("Successful materials service call!");
872 
873  mesh_msgs::MeshMaterialsStamped::ConstPtr meshMaterialsStamped =
874  boost::make_shared<const mesh_msgs::MeshMaterialsStamped>(srv.response.mesh_materials_stamped);
875 
876 
877  visual->setMaterials(meshMaterialsStamped);
878 
879  for (mesh_msgs::MeshMaterial material : meshMaterialsStamped->mesh_materials.materials)
880  {
881  if (material.has_texture)
882  {
883  mesh_msgs::GetTexture texSrv;
884  texSrv.request.uuid = uuid;
885  texSrv.request.texture_index = material.texture_index;
886  if (m_textureClient.call(texSrv))
887  {
888  ROS_INFO("Successful texture service call with index %d!", material.texture_index);
889  mesh_msgs::MeshTexture::ConstPtr texture =
890  boost::make_shared<const mesh_msgs::MeshTexture>(texSrv.response.texture);
891 
892  visual->addTexture(texture);
893  }
894  else
895  {
896  ROS_INFO("Failed texture service call with index %d!", material.texture_index);
897  }
898  }
899  }
900  }
901  else
902  {
903  ROS_INFO("Failed materials service call!");
904  }
905 }
906 
907 } // end namespace rviz_mesh_plugin
908 
rviz_mesh_plugin::TexturedMeshDisplay::m_vertexColorsSubscriber
message_filters::Subscriber< mesh_msgs::MeshVertexColorsStamped > m_vertexColorsSubscriber
Subscriber for vertex colors.
Definition: textured_mesh_display.h:265
rviz::BoolProperty::getBool
virtual bool getBool() const
rviz_mesh_plugin::TexturedMeshDisplay::m_selectVertexCostMap
rviz::EnumProperty * m_selectVertexCostMap
Property to select different types of vertex cost maps to be shown.
Definition: textured_mesh_display.h:379
rviz::EnumProperty::getOptionInt
virtual int getOptionInt()
rviz::Display::isEnabled
bool isEnabled() const
rviz::IntProperty::setMin
void setMin(int min)
rviz_mesh_plugin::TexturedMeshDisplay::m_costUseCustomLimits
rviz::BoolProperty * m_costUseCustomLimits
Property for using custom limits for cost display.
Definition: textured_mesh_display.h:385
rviz_mesh_plugin::TexturedMeshDisplay::m_lastUuid
std::string m_lastUuid
Uuid of the last received message.
Definition: textured_mesh_display.h:301
rviz_mesh_plugin::TexturedMeshDisplay::updateMeshBufferSize
void updateMeshBufferSize()
Sets capacity of trianglemesh_visual.
Definition: textured_mesh_display.cpp:590
rviz::RosTopicProperty
rviz_mesh_plugin::TexturedMeshDisplay::m_showWireframe
rviz::BoolProperty * m_showWireframe
Property to select the wireframe.
Definition: textured_mesh_display.h:355
rviz::DisplayContext::queueRender
virtual void queueRender()=0
rviz_mesh_plugin::TexturedMeshDisplay::getNewVisual
boost::shared_ptr< TexturedMeshVisual > getNewVisual()
Creates a new visual and returns shared pointer.
Definition: textured_mesh_display.cpp:779
tf2_ros::MessageFilter< mesh_msgs::MeshGeometryStamped >
rviz::Display::deleteStatus
virtual void deleteStatus(const QString &name)
rviz_mesh_plugin::TexturedMeshDisplay::m_meshCounter
size_t m_meshCounter
Counter for the meshes.
Definition: textured_mesh_display.h:295
boost::shared_ptr
rviz_mesh_plugin::TexturedMeshDisplay::m_uuidClient
ros::ServiceClient m_uuidClient
Client to request the UUID.
Definition: textured_mesh_display.h:370
rviz::StatusProperty::Error
Error
ros::ServiceClient::exists
bool exists()
rviz_mesh_plugin::TexturedMeshDisplay::onDisable
void onDisable()
Calls unsubscribe() and reset() if display is disabled.
Definition: textured_mesh_display.cpp:510
rviz::PropertyTreeModel::expandProperty
void expandProperty(Property *property)
rviz_mesh_plugin::TexturedMeshDisplay
Class to show options in rviz window.
Definition: textured_mesh_display.h:105
rviz_mesh_plugin::TexturedMeshDisplay::m_vertexColorsTopic
rviz::RosTopicProperty * m_vertexColorsTopic
Property to handle topic for vertex colors.
Definition: textured_mesh_display.h:307
textured_mesh_display.h
rviz_mesh_plugin::TexturedMeshVisual
Class to display mesh data in the main panel of rviz.
Definition: textured_mesh_visual.h:94
rviz_mesh_plugin::TexturedMeshDisplay::updateMaterialAndTextureServices
void updateMaterialAndTextureServices()
Updates the material and texture services.
Definition: textured_mesh_display.cpp:743
rviz::Property::show
void show()
rviz_mesh_plugin::TexturedMeshDisplay::fixedFrameChanged
void fixedFrameChanged()
Sets the fixed frame.
Definition: textured_mesh_display.cpp:516
rviz_mesh_plugin::TexturedMeshDisplay::m_meshSubscriber
message_filters::Subscriber< mesh_msgs::MeshGeometryStamped > m_meshSubscriber
Subscriber for meshMsg.
Definition: textured_mesh_display.h:262
rviz::BoolProperty
rviz::FloatProperty::setMax
void setMax(float max)
message_filters::Cache< mesh_msgs::MeshGeometryStamped >
int_property.h
frame_manager.h
rviz_mesh_plugin::TexturedMeshDisplay::getCurrentVisual
boost::shared_ptr< TexturedMeshVisual > getCurrentVisual()
Creates a new visual and returns shared pointer.
Definition: textured_mesh_display.cpp:799
rviz_mesh_plugin::TexturedMeshDisplay::m_tfVertexCostsFilter
tf2_ros::MessageFilter< mesh_msgs::MeshVertexCostsStamped > * m_tfVertexCostsFilter
Messagefilter for vertex costs.
Definition: textured_mesh_display.h:277
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
rviz_mesh_plugin::TexturedMeshDisplay::m_meshVisuals
boost::circular_buffer< boost::shared_ptr< TexturedMeshVisual > > m_meshVisuals
Shared pointer to store the created objects of trianglemesh_visual.
Definition: textured_mesh_display.h:292
enum_property.h
rviz_mesh_plugin::TexturedMeshDisplay::m_normalsColor
rviz::ColorProperty * m_normalsColor
Property to set the color of the normals.
Definition: textured_mesh_display.h:346
rviz::Property::numChildren
virtual int numChildren() const
rviz_mesh_plugin::TexturedMeshDisplay::m_displayType
rviz::EnumProperty * m_displayType
Property to select the display type.
Definition: textured_mesh_display.h:352
ros::Exception
rviz_mesh_plugin::TexturedMeshDisplay::subscribe
void subscribe()
Set the topics to subscribe.
Definition: textured_mesh_display.cpp:426
rviz::Display::fixed_frame_
QString fixed_frame_
rviz_mesh_plugin::TexturedMeshDisplay::displayCounter
static size_t displayCounter
Definition: textured_mesh_display.h:109
rviz_mesh_plugin::TexturedMeshDisplay::m_facesAlpha
rviz::FloatProperty * m_facesAlpha
Property to set faces transparency.
Definition: textured_mesh_display.h:334
message_filters::Subscriber::unsubscribe
void unsubscribe()
ros::names::validate
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
rviz::FloatProperty::setMin
void setMin(float min)
rviz_mesh_plugin::TexturedMeshDisplay::m_vertexCostsTopic
rviz::RosTopicProperty * m_vertexCostsTopic
Property to handle topic for vertex cost maps.
Definition: textured_mesh_display.h:310
float_property.h
rviz::ColorProperty
visualization_manager.h
rviz::Display
rviz::EnumProperty
rviz_mesh_plugin::TexturedMeshDisplay::m_wireframeColor
rviz::ColorProperty * m_wireframeColor
Property to set wireframe color.
Definition: textured_mesh_display.h:325
rviz::FloatProperty
rviz_mesh_plugin::TexturedMeshDisplay::updateTopic
void updateTopic()
Updates the subscribed topic.
Definition: textured_mesh_display.cpp:418
class_list_macros.h
rviz::Property::hide
void hide()
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
rviz_mesh_plugin::TexturedMeshDisplay::m_colorsSynchronizer
message_filters::Cache< mesh_msgs::MeshVertexColorsStamped > * m_colorsSynchronizer
Synchronizer for vertex colors.
Definition: textured_mesh_display.h:283
rviz_mesh_plugin::TexturedMeshDisplay::reset
void reset()
Clears whole stored data.
Definition: textured_mesh_display.cpp:410
bool_property.h
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
tf2_ros::MessageFilter::clear
void clear()
rviz::FloatProperty::getFloat
virtual float getFloat() const
rviz::EnumProperty::addOption
virtual void addOption(const QString &option, int value=0)
rviz_mesh_plugin::TexturedMeshDisplay::requestMaterials
void requestMaterials(boost::shared_ptr< TexturedMeshVisual > visual, std::string uuid)
Requests materials from the specified service.
Definition: textured_mesh_display.cpp:865
property_tree_model.h
rviz_mesh_plugin::TexturedMeshDisplay::m_textureServiceName
rviz::StringProperty * m_textureServiceName
Property to handle service name for textures.
Definition: textured_mesh_display.h:319
rviz_mesh_plugin::TexturedMeshDisplay::incomingVertexColors
void incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr &colorsStamped)
Handler for incoming vertex color messages. Validate data and update mesh.
Definition: textured_mesh_display.cpp:522
rviz::StringProperty
rviz_mesh_plugin::TexturedMeshDisplay::initialServiceCall
void initialServiceCall()
initial service call for UUID & geometry
Definition: textured_mesh_display.cpp:376
rviz::StringProperty::getStdString
std::string getStdString()
rviz::StatusProperty::Ok
Ok
rviz_mesh_plugin::TexturedMeshDisplay::unsubscribe
void unsubscribe()
Unsubscribes all topics.
Definition: textured_mesh_display.cpp:482
rviz::StatusProperty::Warn
Warn
rviz_mesh_plugin::TexturedMeshDisplay::onEnable
void onEnable()
Calls subscribe() if display is enabled.
Definition: textured_mesh_display.cpp:505
rviz_mesh_plugin::TexturedMeshDisplay::m_vertexColorClient
ros::ServiceClient m_vertexColorClient
Client to request the vertex colors.
Definition: textured_mesh_display.h:361
rviz::Property::model_
PropertyTreeModel * model_
rviz_mesh_plugin::TexturedMeshDisplay::m_geometryClient
ros::ServiceClient m_geometryClient
Client to request the geometry.
Definition: textured_mesh_display.h:373
rviz_mesh_plugin::TexturedMeshDisplay::updateVertexColorService
void updateVertexColorService()
Updates the vertex color service.
Definition: textured_mesh_display.cpp:716
rviz_mesh_plugin::TexturedMeshDisplay::m_costColorType
rviz::EnumProperty * m_costColorType
Property for selecting the color type for cost display.
Definition: textured_mesh_display.h:382
rviz_mesh_plugin::TexturedMeshDisplay::m_showTexturedFacesOnly
rviz::BoolProperty * m_showTexturedFacesOnly
Property to only show textured faces when texturizing is enabled.
Definition: textured_mesh_display.h:376
ros::ServiceClient
ROS_WARN
#define ROS_WARN(...)
rviz_mesh_plugin::TexturedMeshDisplay::cacheVertexCosts
void cacheVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr costsStamped)
Cache function for vertex cost messages.
Definition: textured_mesh_display.cpp:557
rviz_mesh_plugin::TexturedMeshDisplay::m_meshTopic
rviz::RosTopicProperty * m_meshTopic
Property to handle topic for meshMsg.
Definition: textured_mesh_display.h:304
message_filters::SimpleFilter::registerCallback
Connection registerCallback(const boost::function< void(P)> &callback)
tf2_ros::MessageFilter::setTargetFrame
void setTargetFrame(const std::string &target_frame)
rviz_mesh_plugin::TexturedMeshDisplay::m_facesColor
rviz::ColorProperty * m_facesColor
Property to set faces color.
Definition: textured_mesh_display.h:331
rviz_mesh_plugin::TexturedMeshDisplay::m_tfMeshFilter
tf2_ros::MessageFilter< mesh_msgs::MeshGeometryStamped > * m_tfMeshFilter
Messagefilter for meshMsg.
Definition: textured_mesh_display.h:271
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
rviz_mesh_plugin::TexturedMeshDisplay::m_meshSynchronizer
message_filters::Cache< mesh_msgs::MeshGeometryStamped > * m_meshSynchronizer
Synchronizer for meshMsgs.
Definition: textured_mesh_display.h:280
rviz::DisplayContext::getFrameManager
virtual FrameManager * getFrameManager() const=0
rviz::RosTopicProperty::getTopicStd
std::string getTopicStd() const
rviz_mesh_plugin::TexturedMeshDisplay::incomingVertexCosts
void incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr &costsStamped)
Handler for incoming vertex cost messages. Validate data and update mesh.
Definition: textured_mesh_display.cpp:540
rviz::EnumProperty::addOptionStd
void addOptionStd(const std::string &option, int value=0)
rviz_mesh_plugin::TexturedMeshDisplay::m_tfVertexColorsFilter
tf2_ros::MessageFilter< mesh_msgs::MeshVertexColorsStamped > * m_tfVertexColorsFilter
Messagefilter for vertex colors.
Definition: textured_mesh_display.h:274
rviz_mesh_plugin::TexturedMeshDisplay::m_materialServiceName
rviz::StringProperty * m_materialServiceName
Property to handle service name for materials.
Definition: textured_mesh_display.h:316
callback_queue.h
tf2_ros::MessageFilter::connectInput
void connectInput(F &f)
rviz_mesh_plugin::TexturedMeshDisplay::updateMesh
void updateMesh()
Updates material for each mesh displayed by trianglemesh_visual.
Definition: textured_mesh_display.cpp:630
message_filters::Subscriber::subscribe
void subscribe()
transform_listener.h
rviz_mesh_plugin::TexturedMeshDisplay::m_meshBufferSize
rviz::IntProperty * m_meshBufferSize
Property to set meshBufferSize.
Definition: textured_mesh_display.h:322
rviz_mesh_plugin::TexturedMeshDisplay::m_costLowerLimit
rviz::FloatProperty * m_costLowerLimit
Property for setting the lower limit of cost display.
Definition: textured_mesh_display.h:388
rviz::FrameManager::getTransform
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
rviz_mesh_plugin
Definition: face_selection_tool.h:117
rviz_mesh_plugin::TexturedMeshDisplay::m_displayID
size_t m_displayID
DisplayID.
Definition: textured_mesh_display.h:298
rviz_mesh_plugin::TexturedMeshDisplay::updateVertexCosts
void updateVertexCosts()
Update the vertex costs.
Definition: textured_mesh_display.cpp:596
rviz::Display::context_
DisplayContext * context_
rviz_mesh_plugin::TexturedMeshDisplay::m_vertexColorServiceName
rviz::StringProperty * m_vertexColorServiceName
Property to handle service name for vertexColors.
Definition: textured_mesh_display.h:313
rviz_mesh_plugin::TexturedMeshDisplay::incomingGeometry
void incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr &meshMsg)
Handler for incoming geometry messages. Validate data and update mesh.
Definition: textured_mesh_display.cpp:583
rviz_mesh_plugin::TexturedMeshDisplay::TexturedMeshDisplay
TexturedMeshDisplay()
Constructor.
Definition: textured_mesh_display.cpp:89
rviz::FrameManager::registerFilterForTransformStatusCheck
void registerFilterForTransformStatusCheck(tf2_ros::MessageFilter< M > *filter, Display *display)
ROS_ERROR
#define ROS_ERROR(...)
ros::ServiceClient::call
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
rviz_mesh_plugin::TexturedMeshDisplay::requestVertexColors
void requestVertexColors(boost::shared_ptr< TexturedMeshVisual > visual, std::string uuid)
Requests vertex colors from the specified service.
Definition: textured_mesh_display.cpp:847
rviz_mesh_plugin::TexturedMeshDisplay::m_wireframeAlpha
rviz::FloatProperty * m_wireframeAlpha
Property to set wireframe transparency.
Definition: textured_mesh_display.h:328
rviz_mesh_plugin::TexturedMeshDisplay::m_textureClient
ros::ServiceClient m_textureClient
Client to request the textures.
Definition: textured_mesh_display.h:367
rviz::Display::reset
virtual void reset()
rviz::IntProperty::getInt
virtual int getInt() const
rviz_mesh_plugin::TexturedMeshDisplay::m_scalingFactor
rviz::FloatProperty * m_scalingFactor
Property to set the size of the normals.
Definition: textured_mesh_display.h:343
rviz_mesh_plugin::TexturedMeshDisplay::m_vertexCostsSubscriber
message_filters::Subscriber< mesh_msgs::MeshVertexCostsStamped > m_vertexCostsSubscriber
Subscriber for vertex costs.
Definition: textured_mesh_display.h:268
rviz_mesh_plugin::TexturedMeshDisplay::m_costUpperLimit
rviz::FloatProperty * m_costUpperLimit
Property for setting the upper limit of cost display.
Definition: textured_mesh_display.h:391
string_property.h
rviz_mesh_plugin::TexturedMeshDisplay::m_costCache
std::map< std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr > m_costCache
Cache for received vertex cost messages.
Definition: textured_mesh_display.h:394
ROS_INFO
#define ROS_INFO(...)
rviz_mesh_plugin::TexturedMeshDisplay::m_showNormals
rviz::BoolProperty * m_showNormals
Property to select the normals.
Definition: textured_mesh_display.h:358
rviz_mesh_plugin::TexturedMeshDisplay::initServices
void initServices()
Initializes the used services.
Definition: textured_mesh_display.cpp:705
rviz_mesh_plugin::TexturedMeshDisplay::processMessage
void processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr &meshMsg)
Sets data for trianglemesh_visual and updates the mesh.
Definition: textured_mesh_display.cpp:810
rviz_mesh_plugin::TexturedMeshDisplay::m_costsSynchronizer
message_filters::Cache< mesh_msgs::MeshVertexCostsStamped > * m_costsSynchronizer
Synchronizer for vertex costs.
Definition: textured_mesh_display.h:286
rviz_mesh_plugin::TexturedMeshDisplay::m_messagesReceived
uint32_t m_messagesReceived
Counter for the received messages.
Definition: textured_mesh_display.h:289
rviz_mesh_plugin::TexturedMeshDisplay::m_normalsAlpha
rviz::FloatProperty * m_normalsAlpha
Property to set the transparency of the normals.
Definition: textured_mesh_display.h:349
color_property.h
rviz::Display::update_nh_
ros::NodeHandle update_nh_
ros_topic_property.h
rviz::ColorProperty::getOgreColor
Ogre::ColourValue getOgreColor() const
ros::NodeHandle
rviz_mesh_plugin::TexturedMeshDisplay::onInitialize
void onInitialize()
Initialises all nessessary things to get started.
Definition: textured_mesh_display.cpp:337
rviz::IntProperty
display_context.h
rviz_mesh_plugin::TexturedMeshDisplay::~TexturedMeshDisplay
~TexturedMeshDisplay()
Destructor.
Definition: textured_mesh_display.cpp:331
rviz_mesh_plugin::TexturedMeshDisplay::m_materialsClient
ros::ServiceClient m_materialsClient
Client to request the materials.
Definition: textured_mesh_display.h:364


rviz_mesh_plugin
Author(s): Sebastian Pütz , Henning Deeken , Marcel Mrozinski , Kristin Schmidt , Jan Philipp Vogtherr
autogenerated on Fri Feb 12 2021 04:03:57