Go to the documentation of this file.
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>
61 #include <OGRE/OgreSceneNode.h>
62 #include <OGRE/OgreSceneManager.h>
78 #include <boost/random/mersenne_twister.hpp>
79 #include <boost/random/uniform_int.hpp>
80 #include <boost/random/variate_generator.hpp>
99 QString::fromStdString(ros::message_traits::datatype<mesh_msgs::MeshGeometryStamped>()),
100 "Geometry topic to subscribe to.",
109 "Number of prior meshes to display.",
118 "Faces with fixed color",
119 "Select Display Type for Mesh",
133 "Select color scale for vertex costs. Mesh will update when new data arrives.",
142 "Vertex Colors Topic",
144 QString::fromStdString(ros::message_traits::datatype<mesh_msgs::MeshVertexColorsStamped>()),
145 "Vertex color topic to subscribe to.",
152 "Vertex Costs Topic",
154 QString::fromStdString(ros::message_traits::datatype<mesh_msgs::MeshVertexCostsStamped>()),
155 "Vertex cost topic to subscribe to.",
162 "Show textured faces only",
164 "Show textured faces only",
174 "The color of the faces.",
184 "The alpha-value of the faces",
194 "Vertex Color Service Name",
196 "Name of the Vertex Color Service to request Vertex Colors from.",
204 "Material Service Name",
206 "Name of the Matrial Service to request Materials from.",
214 "Texture Service Name",
216 "Name of the Texture Service to request Textures from.",
225 "Select the type of vertex cost map to be displayed. New types will appear here when a new message arrives.",
246 "The color of the wireframe.",
255 "The alpha-value of the wireframe",
276 "The color of the normals.",
284 "The alpha-value of the normals",
292 "Normals Scaling Factor",
294 "Scaling factor of the normals",
303 "Use custom vertex cost limits",
310 "Vertex Costs Lower Limit",
312 "Vertex costs lower limit",
320 "Vertex Costs Upper Limit",
322 "Vertex costs upper limit",
381 mesh_msgs::GetUUID srv_uuid;
384 std::string uuid = (std::string)srv_uuid.response.uuid;
390 mesh_msgs::GetGeometry srv_geometry;
391 srv_geometry.request.uuid = uuid;
395 mesh_msgs::MeshGeometryStamped::ConstPtr geometry
396 = boost::make_shared<const mesh_msgs::MeshGeometryStamped>(srv_geometry.response.mesh_geometry_stamped);
401 ROS_INFO_STREAM(
"Could not load geometry. Waiting for callback to trigger ... ");
406 ROS_INFO(
"No initial data available, waiting for callback to trigger ...");
526 ROS_ERROR(
"Received vertex colors, but no visual available!");
529 if (colorsStamped->uuid.compare(
m_lastUuid) != 0)
531 ROS_ERROR(
"Received vertex colors, but UUIDs dont match!");
544 ROS_ERROR(
"Received vertex costs, but no visual available!");
547 if (costsStamped->uuid.compare(
m_lastUuid) != 0)
549 ROS_ERROR(
"Received vertex costs, but UUIDs dont match!");
558 const mesh_msgs::MeshVertexCostsStamped::ConstPtr costsStamped
561 ROS_INFO_STREAM(
"Cache vertex cost map '" << costsStamped->type <<
"' for UUID " << costsStamped->uuid);
564 std::pair<std::map<std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr>::iterator,
bool> ret =
566 std::pair<std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr>(costsStamped->type, costsStamped));
569 ROS_INFO_STREAM(
"The cost layer \"" << costsStamped->type <<
"\" has been added.");
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.");
634 bool showFaces =
false;
635 bool showTextures =
false;
636 bool useVertexColors =
false;
637 bool showVertexCosts =
false;
662 useVertexColors =
true;
677 showVertexCosts =
true;
695 it->get()->updateMaterial(
789 int randomId = (int)((
double)rand() / RAND_MAX * 9998);
804 ROS_ERROR(
"Requested current visual when none is available!");
812 Ogre::Quaternion orientation;
813 Ogre::Vector3 position;
816 meshMsg->header.frame_id,
817 meshMsg->header.stamp,
818 position, orientation)
822 "Error transforming from frame '%s' to frame '%s'",
830 ROS_WARN(
"Received geometry with new UUID!");
839 visual->setGeometry(meshMsg);
843 visual->setFramePosition(position);
844 visual->setFrameOrientation(orientation);
849 mesh_msgs::GetVertexColors srv;
850 srv.request.uuid = uuid;
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);
857 visual->setVertexColors(meshVertexColors);
861 ROS_INFO(
"Failed vertex colors service call!");
867 mesh_msgs::GetMaterials srv;
868 srv.request.uuid = uuid;
871 ROS_INFO(
"Successful materials service call!");
873 mesh_msgs::MeshMaterialsStamped::ConstPtr meshMaterialsStamped =
874 boost::make_shared<const mesh_msgs::MeshMaterialsStamped>(srv.response.mesh_materials_stamped);
877 visual->setMaterials(meshMaterialsStamped);
879 for (mesh_msgs::MeshMaterial material : meshMaterialsStamped->mesh_materials.materials)
881 if (material.has_texture)
883 mesh_msgs::GetTexture texSrv;
884 texSrv.request.uuid = uuid;
885 texSrv.request.texture_index = material.texture_index;
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);
892 visual->addTexture(texture);
896 ROS_INFO(
"Failed texture service call with index %d!", material.texture_index);
903 ROS_INFO(
"Failed materials service call!");
message_filters::Subscriber< mesh_msgs::MeshVertexColorsStamped > m_vertexColorsSubscriber
Subscriber for vertex colors.
virtual bool getBool() const
rviz::EnumProperty * m_selectVertexCostMap
Property to select different types of vertex cost maps to be shown.
virtual int getOptionInt()
rviz::BoolProperty * m_costUseCustomLimits
Property for using custom limits for cost display.
std::string m_lastUuid
Uuid of the last received message.
void updateMeshBufferSize()
Sets capacity of trianglemesh_visual.
rviz::BoolProperty * m_showWireframe
Property to select the wireframe.
virtual void queueRender()=0
boost::shared_ptr< TexturedMeshVisual > getNewVisual()
Creates a new visual and returns shared pointer.
virtual void deleteStatus(const QString &name)
size_t m_meshCounter
Counter for the meshes.
ros::ServiceClient m_uuidClient
Client to request the UUID.
void onDisable()
Calls unsubscribe() and reset() if display is disabled.
void expandProperty(Property *property)
Class to show options in rviz window.
rviz::RosTopicProperty * m_vertexColorsTopic
Property to handle topic for vertex colors.
Class to display mesh data in the main panel of rviz.
void updateMaterialAndTextureServices()
Updates the material and texture services.
void fixedFrameChanged()
Sets the fixed frame.
message_filters::Subscriber< mesh_msgs::MeshGeometryStamped > m_meshSubscriber
Subscriber for meshMsg.
boost::shared_ptr< TexturedMeshVisual > getCurrentVisual()
Creates a new visual and returns shared pointer.
tf2_ros::MessageFilter< mesh_msgs::MeshVertexCostsStamped > * m_tfVertexCostsFilter
Messagefilter for vertex costs.
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
boost::circular_buffer< boost::shared_ptr< TexturedMeshVisual > > m_meshVisuals
Shared pointer to store the created objects of trianglemesh_visual.
rviz::ColorProperty * m_normalsColor
Property to set the color of the normals.
virtual int numChildren() const
rviz::EnumProperty * m_displayType
Property to select the display type.
void subscribe()
Set the topics to subscribe.
static size_t displayCounter
rviz::FloatProperty * m_facesAlpha
Property to set faces transparency.
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
rviz::RosTopicProperty * m_vertexCostsTopic
Property to handle topic for vertex cost maps.
rviz::ColorProperty * m_wireframeColor
Property to set wireframe color.
void updateTopic()
Updates the subscribed topic.
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
message_filters::Cache< mesh_msgs::MeshVertexColorsStamped > * m_colorsSynchronizer
Synchronizer for vertex colors.
void reset()
Clears whole stored data.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual float getFloat() const
virtual void addOption(const QString &option, int value=0)
void requestMaterials(boost::shared_ptr< TexturedMeshVisual > visual, std::string uuid)
Requests materials from the specified service.
rviz::StringProperty * m_textureServiceName
Property to handle service name for textures.
void incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr &colorsStamped)
Handler for incoming vertex color messages. Validate data and update mesh.
void initialServiceCall()
initial service call for UUID & geometry
std::string getStdString()
void unsubscribe()
Unsubscribes all topics.
void onEnable()
Calls subscribe() if display is enabled.
ros::ServiceClient m_vertexColorClient
Client to request the vertex colors.
PropertyTreeModel * model_
ros::ServiceClient m_geometryClient
Client to request the geometry.
void updateVertexColorService()
Updates the vertex color service.
rviz::EnumProperty * m_costColorType
Property for selecting the color type for cost display.
rviz::BoolProperty * m_showTexturedFacesOnly
Property to only show textured faces when texturizing is enabled.
void cacheVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr costsStamped)
Cache function for vertex cost messages.
rviz::RosTopicProperty * m_meshTopic
Property to handle topic for meshMsg.
Connection registerCallback(const boost::function< void(P)> &callback)
void setTargetFrame(const std::string &target_frame)
rviz::ColorProperty * m_facesColor
Property to set faces color.
tf2_ros::MessageFilter< mesh_msgs::MeshGeometryStamped > * m_tfMeshFilter
Messagefilter for meshMsg.
#define ROS_INFO_STREAM(args)
message_filters::Cache< mesh_msgs::MeshGeometryStamped > * m_meshSynchronizer
Synchronizer for meshMsgs.
virtual FrameManager * getFrameManager() const=0
std::string getTopicStd() const
void incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr &costsStamped)
Handler for incoming vertex cost messages. Validate data and update mesh.
void addOptionStd(const std::string &option, int value=0)
tf2_ros::MessageFilter< mesh_msgs::MeshVertexColorsStamped > * m_tfVertexColorsFilter
Messagefilter for vertex colors.
rviz::StringProperty * m_materialServiceName
Property to handle service name for materials.
void updateMesh()
Updates material for each mesh displayed by trianglemesh_visual.
rviz::IntProperty * m_meshBufferSize
Property to set meshBufferSize.
rviz::FloatProperty * m_costLowerLimit
Property for setting the lower limit of cost display.
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
size_t m_displayID
DisplayID.
void updateVertexCosts()
Update the vertex costs.
DisplayContext * context_
rviz::StringProperty * m_vertexColorServiceName
Property to handle service name for vertexColors.
void incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr &meshMsg)
Handler for incoming geometry messages. Validate data and update mesh.
TexturedMeshDisplay()
Constructor.
void registerFilterForTransformStatusCheck(tf2_ros::MessageFilter< M > *filter, Display *display)
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
void requestVertexColors(boost::shared_ptr< TexturedMeshVisual > visual, std::string uuid)
Requests vertex colors from the specified service.
rviz::FloatProperty * m_wireframeAlpha
Property to set wireframe transparency.
ros::ServiceClient m_textureClient
Client to request the textures.
virtual int getInt() const
rviz::FloatProperty * m_scalingFactor
Property to set the size of the normals.
message_filters::Subscriber< mesh_msgs::MeshVertexCostsStamped > m_vertexCostsSubscriber
Subscriber for vertex costs.
rviz::FloatProperty * m_costUpperLimit
Property for setting the upper limit of cost display.
std::map< std::string, const mesh_msgs::MeshVertexCostsStamped::ConstPtr > m_costCache
Cache for received vertex cost messages.
rviz::BoolProperty * m_showNormals
Property to select the normals.
void initServices()
Initializes the used services.
void processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr &meshMsg)
Sets data for trianglemesh_visual and updates the mesh.
message_filters::Cache< mesh_msgs::MeshVertexCostsStamped > * m_costsSynchronizer
Synchronizer for vertex costs.
uint32_t m_messagesReceived
Counter for the received messages.
rviz::FloatProperty * m_normalsAlpha
Property to set the transparency of the normals.
ros::NodeHandle update_nh_
Ogre::ColourValue getOgreColor() const
void onInitialize()
Initialises all nessessary things to get started.
~TexturedMeshDisplay()
Destructor.
ros::ServiceClient m_materialsClient
Client to request the materials.
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