├── CMakeLists.txt ├── README.md ├── gifs └── rviz_demo.gif ├── mesh_display_custom_plugin_description.xml ├── msg ├── TexturedQuad.msg └── TexturedQuadArray.msg ├── package.xml ├── src ├── mesh_display_custom.cpp ├── mesh_display_custom.h ├── plugin_init.cpp └── text_node.h └── tests ├── display_images_test.py └── textures ├── Decal.png ├── Decal_filter.png ├── bebop_drone.jpg └── red.jpg /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_textured_quads) 3 | 4 | ## Find catkin and any catkin packages on which 5 | ## this package depends at build time 6 | find_package(catkin REQUIRED COMPONENTS roscpp rospy roslib std_msgs sensor_msgs shape_msgs rviz pluginlib class_loader cv_bridge message_generation geometry_msgs sensor_msgs cmake_modules eigen_conversions rviz) 7 | find_package(Eigen REQUIRED) 8 | 9 | ## This plugin includes Qt widgets, so we must include Qt like so: 10 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 11 | include(${QT_USE_FILE}) 12 | 13 | find_package(OpenGL REQUIRED) 14 | 15 | ## I (original author) prefer the Qt signals and slots to avoid defining "emit", "slots", 16 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 17 | add_definitions(-DQT_NO_KEYWORDS) 18 | 19 | include_directories(SYSTEM "msg_gen/cpp/include") 20 | 21 | add_message_files( 22 | FILES 23 | TexturedQuad.msg 24 | TexturedQuadArray.msg 25 | ) 26 | 27 | generate_messages( 28 | DEPENDENCIES 29 | std_msgs 30 | geometry_msgs 31 | sensor_msgs 32 | ) 33 | 34 | 35 | catkin_package( 36 | LIBRARIES 37 | rviz_textured_quads_custom 38 | ${OPENGL_LIBRARIES} 39 | INCLUDE_DIRS 40 | src 41 | ${OPENGL_INCLUDE_DIR} 42 | CATKIN_DEPENDS 43 | roscpp 44 | rospy 45 | roslib 46 | std_msgs 47 | sensor_msgs 48 | shape_msgs 49 | rviz 50 | pluginlib 51 | class_loader 52 | cv_bridge 53 | geometry_msgs 54 | sensor_msgs 55 | eigen_conversions 56 | rviz 57 | ) 58 | 59 | 60 | include_directories( 61 | src 62 | ${catkin_INCLUDE_DIRS} 63 | ${Eigen_INCLUDE_DIRS}) 64 | 65 | include_directories(SYSTEM 66 | ${OPENGL_INCLUDE_DIR} 67 | ${QT_INCLUDE_DIR}) 68 | 69 | link_directories(${catkin_LIBRARY_DIRS}) 70 | 71 | 72 | # Convert the Qt Signals and Slots for QWidget events 73 | qt4_wrap_cpp(MOC_SOURCES src/mesh_display_custom.h) 74 | 75 | include_directories(${catkin_INCLUDE_DIRS} ${CMAKE_CURRENT_BINARY_DIR}) 76 | 77 | set(TEXTURED_QUAD_LIB rviz_textured_quads_custom) 78 | 79 | add_library(${TEXTURED_QUAD_LIB}_core src/text_node.h src/mesh_display_custom.cpp ${MOC_SOURCES}) 80 | target_link_libraries(${TEXTURED_QUAD_LIB}_core ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Eigen_LIBRARIES}) 81 | 82 | add_dependencies(${TEXTURED_QUAD_LIB}_core ${catkin_EXPORTED_TARGETS}) 83 | 84 | add_library(${TEXTURED_QUAD_LIB} src/plugin_init.cpp) 85 | target_link_libraries(${TEXTURED_QUAD_LIB} ${TEXTURED_QUAD_LIB}_core ${catkin_LIBRARIES}) 86 | 87 | add_dependencies(${TEXTURED_QUAD_LIB} ${catkin_EXPORTED_TARGETS}) 88 | 89 | install(TARGETS ${TEXTURED_QUAD_LIB} ${TEXTURED_QUAD_LIB}_core 90 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 91 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 92 | 93 | install(DIRECTORY textures 94 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz_textured_quads_custom 95 | ) 96 | 97 | install(FILES 98 | mesh_display_custom_plugin_description.xml 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 100 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RVIZ Textured Quads 2 | Plugin: Displays textured images (realtime or static sensor_msgs/Image topics) in 3D space 3 | 4 | ![Demo](https://github.com/MohitShridhar/rviz_textured_quads/blob/master/gifs/rviz_demo.gif) 5 | 6 | ## Usage 7 | 8 | See TexturedQuad.msg -------------------------------------------------------------------------------- /gifs/rviz_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MohitShridhar/rviz_textured_quads/a1e60b38eeb878c23c32d9d0597629f0524bb0df/gifs/rviz_demo.gif -------------------------------------------------------------------------------- /mesh_display_custom_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Customized display plugin for meshes. It also supports overlay of camera images. 7 | 8 | 9 | -------------------------------------------------------------------------------- /msg/TexturedQuad.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image image # texture 2 | geometry_msgs/Pose pose # 6DOF pose of the center of the quad to be displayed 3 | float32 width # physical width of the quad in Rviz unit (usually meters) 4 | float32 height # physical height of the quad in Rviz unit (usually meters) 5 | string caption # [OPTIONAL] text description to appear below the quad 6 | float32 border_size # [OPTIONAL] size of the quad border 7 | float32[] border_color # [OPTIONAL] (r,g,b,a) color value of the quad border (Size = 4) -------------------------------------------------------------------------------- /msg/TexturedQuadArray.msg: -------------------------------------------------------------------------------- 1 | TexturedQuad[] quads -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rviz_textured_quads 4 | 0.1.0 5 | RVIZ Image Mesh Plugin 6 | Mohit Shridhar 7 | TORC 8 | Mohit Shridhar 9 | 10 | catkin 11 | 12 | roscpp 13 | rospy 14 | roslib 15 | std_msgs 16 | sensor_msgs 17 | shape_msgs 18 | pluginlib 19 | class_loader 20 | rviz 21 | cv_bridge 22 | message_generation 23 | geometry_msgs 24 | sensor_msgs 25 | cmake_modules 26 | eigen_conversions 27 | rviz 28 | 29 | roscpp 30 | rospy 31 | roslib 32 | std_msgs 33 | sensor_msgs 34 | shape_msgs 35 | pluginlib 36 | class_loader 37 | rviz 38 | cv_bridge 39 | message_runtime 40 | geometry_msgs 41 | sensor_msgs 42 | cmake_modules 43 | eigen_conversions 44 | rviz 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/mesh_display_custom.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MeshDisplayCustom class implementation. 3 | * 4 | * Author: Felipe Bacim. 5 | * 6 | * Based on the rviz image display class. 7 | * 8 | * Latest changes (12/11/2012): 9 | * - fixed segfault issues 10 | */ 11 | /* 12 | * Copyright (c) 2008, Willow Garage, Inc. 13 | * All rights reserved. 14 | * 15 | * Redistribution and use in source and binary forms, with or without 16 | * modification, are permitted provided that the following conditions are met: 17 | * 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in the 22 | * documentation and/or other materials provided with the distribution. 23 | * * Neither the name of the Willow Garage, Inc. nor the names of its 24 | * contributors may be used to endorse or promote products derived from 25 | * this software without specific prior written permission. 26 | * 27 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 28 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 29 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 30 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 31 | * LIABLE FOR ANY DImesh, INDImesh, INCIDENTAL, SPECIAL, EXEMPLARY, OR 32 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 33 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 34 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 35 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 36 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include "rviz/display_context.h" 47 | #include "rviz/robot/robot.h" 48 | #include "rviz/robot/tf_link_updater.h" 49 | #include "rviz/properties/color_property.h" 50 | #include "rviz/properties/vector_property.h" 51 | #include "rviz/properties/ros_topic_property.h" 52 | #include "rviz/properties/float_property.h" 53 | #include "rviz/properties/string_property.h" 54 | #include "rviz/properties/quaternion_property.h" 55 | #include "rviz/render_panel.h" 56 | #include "rviz/validate_floats.h" 57 | #include "rviz/view_manager.h" 58 | #include "rviz/visualization_manager.h" 59 | 60 | #include 61 | 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | #include "mesh_display_custom.h" 68 | 69 | namespace rviz 70 | { 71 | 72 | bool validateFloats(const sensor_msgs::CameraInfo& msg) 73 | { 74 | bool valid = true; 75 | valid = valid && validateFloats( msg.D ); 76 | valid = valid && validateFloats( msg.K ); 77 | valid = valid && validateFloats( msg.R ); 78 | valid = valid && validateFloats( msg.P ); 79 | return valid; 80 | } 81 | 82 | MeshDisplayCustom::MeshDisplayCustom() 83 | : Display() 84 | , time_since_last_transform_( 0.0f ) 85 | , initialized_(false) 86 | { 87 | display_images_topic_property_ = new RosTopicProperty( "Display Images Topic", "", 88 | QString::fromStdString( ros::message_traits::datatype() ), 89 | "shape_msgs::Mesh topic to subscribe to.", 90 | this, SLOT( updateDisplayImages() )); 91 | 92 | text_color_property_ = new ColorProperty ( "Text Color", QColor( 255, 255, 255 ), 93 | "caption color.", this, SLOT( updateMeshProperties() ) ); 94 | 95 | text_height_property_ = new FloatProperty( "Text Height", 0.1f, 96 | "font size of caption", this, SLOT( updateMeshProperties() ) ); 97 | 98 | text_bottom_offset_ = new FloatProperty( "Text Bottom Offset", 0.18f, 99 | "text placement offset below", this, SLOT( updateMeshProperties() ) ); 100 | } 101 | 102 | MeshDisplayCustom::~MeshDisplayCustom() 103 | { 104 | unsubscribe(); 105 | 106 | // TODO: Why am I doing this? switch to shared ptrs Argh!!!!!! 107 | 108 | // clear manual objects 109 | for (std::vector::iterator it = manual_objects_.begin() ; it != manual_objects_.end(); ++it) 110 | { 111 | delete (*it); 112 | } 113 | manual_objects_.clear(); 114 | 115 | // clear decal frustrums 116 | for (std::vector::iterator it = decal_frustums_.begin() ; it != decal_frustums_.end(); ++it) 117 | { 118 | delete (*it); 119 | } 120 | decal_frustums_.clear(); 121 | 122 | // clear textures 123 | for (std::vector::iterator it = textures_.begin() ; it != textures_.end(); ++it) 124 | { 125 | delete (*it); 126 | } 127 | textures_.clear(); 128 | 129 | // clear textures 130 | for (std::vector::iterator it = mesh_nodes_.begin() ; it != mesh_nodes_.end(); ++it) 131 | { 132 | delete (*it); 133 | } 134 | mesh_nodes_.clear(); 135 | 136 | // clear text nodes 137 | for (std::vector::iterator it = text_nodes_.begin() ; it != text_nodes_.end(); ++it) 138 | { 139 | delete (*it); 140 | } 141 | text_nodes_.clear(); 142 | 143 | // TODO: clean up other things 144 | } 145 | 146 | void MeshDisplayCustom::onInitialize() 147 | { 148 | Display::onInitialize(); 149 | } 150 | 151 | void MeshDisplayCustom::createProjector(int index) 152 | { 153 | decal_frustums_[index] = new Ogre::Frustum(); 154 | 155 | projector_nodes_[index] = scene_manager_->getRootSceneNode()->createChildSceneNode(); 156 | projector_nodes_[index]->attachObject(decal_frustums_[index]); 157 | 158 | Ogre::SceneNode* filter_node; 159 | 160 | //back filter 161 | filter_frustums_[index].push_back(new Ogre::Frustum()); 162 | filter_frustums_[index].back()->setProjectionType(Ogre::PT_ORTHOGRAPHIC); 163 | filter_node = projector_nodes_[index]->createChildSceneNode(); 164 | filter_node->attachObject(filter_frustums_[index].back()); 165 | filter_node->setOrientation(Ogre::Quaternion(Ogre::Degree(90),Ogre::Vector3::UNIT_Y)); 166 | } 167 | 168 | void MeshDisplayCustom::addDecalToMaterial(int index, const Ogre::String& matName) 169 | { 170 | Ogre::MaterialPtr mat = (Ogre::MaterialPtr)Ogre::MaterialManager::getSingleton().getByName(matName); 171 | mat->setCullingMode(Ogre::CULL_NONE); 172 | Ogre::Pass* pass = mat->getTechnique(0)->createPass(); 173 | 174 | pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 175 | pass->setDepthBias(1); 176 | //pass->setLightingEnabled(true); 177 | 178 | // need the decal_filter to avoid back projection 179 | Ogre::String resource_group_name = "decal_textures_folder"; 180 | Ogre::ResourceGroupManager& resource_manager = Ogre::ResourceGroupManager::getSingleton(); 181 | if(!resource_manager.resourceGroupExists(resource_group_name)) 182 | { 183 | resource_manager.createResourceGroup(resource_group_name); 184 | resource_manager.addResourceLocation(ros::package::getPath("rviz_textured_quads")+"/tests/textures/", "FileSystem", resource_group_name, false); 185 | resource_manager.initialiseResourceGroup(resource_group_name); 186 | } 187 | // loads files into our resource manager 188 | resource_manager.loadResourceGroup(resource_group_name); 189 | 190 | Ogre::TextureUnitState* tex_state = pass->createTextureUnitState();//"Decal.png"); 191 | tex_state->setTextureName(textures_[index]->getTexture()->getName()); 192 | tex_state->setProjectiveTexturing(true, decal_frustums_[index]); 193 | 194 | tex_state->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); 195 | tex_state->setTextureFiltering(Ogre::FO_POINT, Ogre::FO_LINEAR, Ogre::FO_NONE); 196 | tex_state->setColourOperation(Ogre::LBO_REPLACE); //don't accept additional effects 197 | 198 | for(int i = 0; i < filter_frustums_[index].size(); i++) 199 | { 200 | tex_state = pass->createTextureUnitState("Decal_filter.png"); 201 | tex_state->setProjectiveTexturing(true, filter_frustums_[index][i]); 202 | tex_state->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); 203 | tex_state->setTextureFiltering(Ogre::TFO_NONE); 204 | } 205 | } 206 | 207 | shape_msgs::Mesh MeshDisplayCustom::constructMesh( geometry_msgs::Pose mesh_origin, float width, float height, float border_size ) 208 | { 209 | shape_msgs::Mesh mesh; 210 | 211 | Eigen::Affine3d trans_mat; 212 | tf::poseMsgToEigen(mesh_origin, trans_mat); 213 | 214 | // Rviz Coordinate System: x-right, y-forward, z-down 215 | // create mesh vertices and tranform them to the specified pose 216 | 217 | Eigen::Vector4d top_left(-width/2.0f - border_size, 0.0f, -height/2.0f - border_size, 1.0f); 218 | Eigen::Vector4d top_right(width/2.0f + border_size, 0.0f, -height/2.0f - border_size, 1.0f); 219 | Eigen::Vector4d bottom_left(-width/2.0f - border_size, 0.0f, height/2.0f + border_size, 1.0f); 220 | Eigen::Vector4d bottom_right(width/2.0f + border_size, 0.0f, height/2.0f + border_size, 1.0f); 221 | 222 | Eigen::Vector4d trans_top_left = trans_mat.matrix() * top_left; 223 | Eigen::Vector4d trans_top_right = trans_mat.matrix() * top_right; 224 | Eigen::Vector4d trans_bottom_left = trans_mat.matrix() * bottom_left; 225 | Eigen::Vector4d trans_bottom_right = trans_mat.matrix() * bottom_right; 226 | 227 | std::vector vertices(4); 228 | vertices.at(0).x = trans_top_left[0]; 229 | vertices.at(0).y = trans_top_left[1]; 230 | vertices.at(0).z = trans_top_left[2]; 231 | vertices.at(1).x = trans_top_right[0]; 232 | vertices.at(1).y = trans_top_right[1]; 233 | vertices.at(1).z = trans_top_right[2]; 234 | vertices.at(2).x = trans_bottom_left[0]; 235 | vertices.at(2).y = trans_bottom_left[1]; 236 | vertices.at(2).z = trans_bottom_left[2]; 237 | vertices.at(3).x = trans_bottom_right[0]; 238 | vertices.at(3).y = trans_bottom_right[1]; 239 | vertices.at(3).z = trans_bottom_right[2]; 240 | mesh.vertices = vertices; 241 | 242 | std::vector triangles(2); 243 | triangles.at(0).vertex_indices[0] = 0; 244 | triangles.at(0).vertex_indices[1] = 1; 245 | triangles.at(0).vertex_indices[2] = 2; 246 | triangles.at(1).vertex_indices[0] = 1; 247 | triangles.at(1).vertex_indices[1] = 2; 248 | triangles.at(1).vertex_indices[2] = 3; 249 | mesh.triangles = triangles; 250 | 251 | return mesh; 252 | } 253 | 254 | void MeshDisplayCustom::clearStates(int num_quads) 255 | { 256 | for (int q=0; qclear(); 259 | } 260 | 261 | for (int q=0; qclear(); 264 | } 265 | 266 | // resize state vectors 267 | mesh_poses_.resize(num_quads); 268 | img_widths_.resize(num_quads); 269 | img_heights_.resize(num_quads); 270 | physical_widths_.resize(num_quads); 271 | physical_heights_.resize(num_quads); 272 | 273 | manual_objects_.resize(num_quads); 274 | last_meshes_.resize(num_quads); 275 | 276 | last_images_.resize(num_quads); 277 | textures_.resize(num_quads); 278 | decal_frustums_.resize(num_quads); 279 | projector_nodes_.resize(num_quads); 280 | filter_frustums_.resize(num_quads); 281 | mesh_materials_.resize(num_quads); 282 | mesh_nodes_.resize(num_quads); 283 | text_nodes_.resize(num_quads); 284 | 285 | border_colors_.resize(num_quads); 286 | border_sizes_.resize(num_quads); 287 | } 288 | 289 | void MeshDisplayCustom::constructQuads( const rviz_textured_quads::TexturedQuadArray::ConstPtr& images ) 290 | { 291 | int num_quads = images->quads.size(); 292 | 293 | clearStates(num_quads); 294 | 295 | for (int q=0; qquads[q].image); 298 | 299 | geometry_msgs::Pose mesh_origin = images->quads[q].pose; 300 | 301 | // Rotate from x-y to x-z plane: 302 | Eigen::Affine3d trans_mat; 303 | tf::poseMsgToEigen(mesh_origin, trans_mat); 304 | trans_mat = trans_mat * Eigen::Quaterniond(0.70710678, -0.70710678f, 0.0f, 0.0f); 305 | 306 | Eigen::Quaterniond xz_quat(trans_mat.rotation()); 307 | mesh_origin.orientation.x = xz_quat.x(); 308 | mesh_origin.orientation.y = xz_quat.y(); 309 | mesh_origin.orientation.z = xz_quat.z(); 310 | mesh_origin.orientation.w = xz_quat.w(); 311 | 312 | float width = images->quads[q].width; 313 | float height = images->quads[q].height; 314 | 315 | // set properties 316 | mesh_poses_[q] = mesh_origin; 317 | img_widths_[q] = images->quads[q].image.width; 318 | img_heights_[q] = images->quads[q].image.height; 319 | 320 | border_colors_[q].resize(4); 321 | 322 | if (images->quads[q].border_color.size() == 4) 323 | { 324 | border_colors_[q][0] = images->quads[q].border_color[0]; 325 | border_colors_[q][1] = images->quads[q].border_color[1]; 326 | border_colors_[q][2] = images->quads[q].border_color[2]; 327 | border_colors_[q][3] = images->quads[q].border_color[3]; 328 | } 329 | else 330 | { 331 | // default white border 332 | border_colors_[q][0] = 255.0f; 333 | border_colors_[q][1] = 255.0f; 334 | border_colors_[q][2] = 255.0f; 335 | border_colors_[q][3] = 255.0f; 336 | } 337 | 338 | if (images->quads[q].border_size >= 0.0f) 339 | { 340 | border_sizes_[q] = images->quads[q].border_size; 341 | } 342 | else 343 | { 344 | // default border size (no border) 345 | border_sizes_[q] = 0.0f; 346 | } 347 | 348 | shape_msgs::Mesh mesh = constructMesh(mesh_origin, width, height, border_sizes_[q]); 349 | 350 | physical_widths_[q] = width; 351 | physical_heights_[q] = height; 352 | 353 | boost::mutex::scoped_lock lock( mesh_mutex_ ); 354 | 355 | // create our scenenode and material 356 | load(q); 357 | 358 | Ogre::Vector3 caption_position = Ogre::Vector3(mesh_origin.position.x, mesh_origin.position.y + 0.5f*height + text_bottom_offset_->getFloat(), mesh_origin.position.z); 359 | 360 | if (!manual_objects_[q]) 361 | { 362 | static uint32_t count = 0; 363 | std::stringstream ss; 364 | ss << "MeshObject" << count++ << "Index" << q; 365 | manual_objects_[q] = context_->getSceneManager()->createManualObject(ss.str()); 366 | mesh_nodes_[q]->attachObject(manual_objects_[q]); 367 | } 368 | 369 | // If we have the same number of tris as previously, just update the object 370 | if (last_meshes_[q].vertices.size() > 0 && mesh.vertices.size()*2 == last_meshes_[q].vertices.size()) 371 | { 372 | manual_objects_[q]->beginUpdate(0); 373 | } 374 | else // Otherwise clear it and begin anew 375 | { 376 | manual_objects_[q]->clear(); 377 | manual_objects_[q]->estimateVertexCount(mesh.vertices.size()*2); 378 | manual_objects_[q]->begin(mesh_materials_[q]->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); 379 | } 380 | 381 | const std::vector& points = mesh.vertices; 382 | for(size_t i = 0; i < mesh.triangles.size(); i++) 383 | { 384 | // make sure we have front-face/back-face triangles 385 | for(int side = 0; side < 2; side++) 386 | { 387 | std::vector corners(3); 388 | for(size_t c = 0; c < 3; c++) 389 | { 390 | size_t corner = side ? 2-c : c; // order of corners if side == 1 391 | corners[corner] = Ogre::Vector3(points[mesh.triangles[i].vertex_indices[corner]].x, points[mesh.triangles[i].vertex_indices[corner]].y, points[mesh.triangles[i].vertex_indices[corner]].z); 392 | } 393 | Ogre::Vector3 normal = (corners[1] - corners[0]).crossProduct(corners[2] - corners[0]); 394 | normal.normalise(); 395 | 396 | for(size_t c = 0; c < 3; c++) 397 | { 398 | manual_objects_[q]->position(corners[c]); 399 | manual_objects_[q]->normal(normal); 400 | } 401 | } 402 | } 403 | 404 | manual_objects_[q]->end(); 405 | 406 | mesh_materials_[q]->setCullingMode(Ogre::CULL_NONE); 407 | 408 | last_meshes_[q] = mesh; 409 | 410 | Ogre::ColourValue text_color(text_color_property_->getColor().redF(), text_color_property_->getColor().greenF(), text_color_property_->getColor().blueF(), 1.0f); 411 | 412 | if (!text_nodes_[q]) 413 | { 414 | text_nodes_[q] = new rviz_textured_quads::TextNode(context_->getSceneManager(), manual_objects_[q]->getParentSceneNode(), caption_position); 415 | text_nodes_[q]->setCaption(images->quads[q].caption); 416 | text_nodes_[q]->setCharacterHeight(text_height_property_->getFloat()); 417 | text_nodes_[q]->setColor(text_color); 418 | } 419 | else 420 | { 421 | text_nodes_[q]->setCaption(images->quads[q].caption); 422 | text_nodes_[q]->setPosition(caption_position); 423 | text_nodes_[q]->setCharacterHeight(text_height_property_->getFloat()); 424 | text_nodes_[q]->setColor(text_color); 425 | 426 | } 427 | 428 | } 429 | } 430 | 431 | void MeshDisplayCustom::updateImageMeshes( const rviz_textured_quads::TexturedQuadArray::ConstPtr& images ) 432 | { 433 | constructQuads(images); 434 | updateMeshProperties(); 435 | } 436 | 437 | void MeshDisplayCustom::updateMeshProperties() 438 | { 439 | for (int i=0; igetTechnique(0); 443 | Ogre::Pass* pass = technique->getPass(0); 444 | 445 | Ogre::ColourValue self_illumination_color(0.0f, 0.0f, 0.0f, 0.0f);// border_colors_[i][3]); 446 | pass->setSelfIllumination(self_illumination_color); 447 | 448 | Ogre::ColourValue diffuse_color(0.0f, 0.0f, 0.0f, 1.0f/*border_colors_[i][0], border_colors_[i][1], border_colors_[i][2], border_colors_[i][3]*/); 449 | pass->setDiffuse(diffuse_color); 450 | 451 | Ogre::ColourValue ambient_color(border_colors_[i][0], border_colors_[i][1], border_colors_[i][2], border_colors_[i][3]); 452 | pass->setAmbient(ambient_color); 453 | 454 | Ogre::ColourValue specular_color(0.0f, 0.0f, 0.0f, 1.0f); 455 | pass->setSpecular(specular_color); 456 | 457 | Ogre::Real shininess = 64.0f; 458 | pass->setShininess(shininess); 459 | 460 | pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 461 | pass->setDepthWriteEnabled(false); 462 | 463 | context_->queueRender(); 464 | } 465 | 466 | } 467 | 468 | void MeshDisplayCustom::updateDisplayImages() 469 | { 470 | unsubscribe(); 471 | subscribe(); 472 | } 473 | 474 | void MeshDisplayCustom::subscribe() 475 | { 476 | if ( !isEnabled() ) 477 | { 478 | return; 479 | } 480 | 481 | if( !display_images_topic_property_->getTopic().isEmpty() ) 482 | { 483 | try 484 | { 485 | rviz_display_images_sub_ = nh_.subscribe(display_images_topic_property_->getTopicStd(), 1, &MeshDisplayCustom::updateImageMeshes, this); 486 | setStatus( StatusProperty::Ok, "Display Images Topic", "OK" ); 487 | } 488 | catch( ros::Exception& e ) 489 | { 490 | setStatus( StatusProperty::Error, "Display Images Topic", QString( "Error subscribing: " ) + e.what() ); 491 | } 492 | } 493 | } 494 | 495 | void MeshDisplayCustom::unsubscribe() 496 | { 497 | rviz_display_images_sub_.shutdown(); 498 | } 499 | 500 | void MeshDisplayCustom::load(int index) 501 | { 502 | if(mesh_nodes_[index] != NULL) 503 | return; 504 | 505 | static int count = 0; 506 | std::stringstream ss; 507 | ss << "MeshNode" << count++ << "GroupIndex" << index; 508 | Ogre::MaterialManager& material_manager = Ogre::MaterialManager::getSingleton(); 509 | Ogre::String resource_group_name = ss.str(); 510 | 511 | Ogre::ResourceGroupManager& rg_mgr = Ogre::ResourceGroupManager::getSingleton(); 512 | 513 | Ogre::String material_name = resource_group_name+"MeshMaterial"; 514 | 515 | if(!rg_mgr.resourceGroupExists(resource_group_name)) 516 | { 517 | rg_mgr.createResourceGroup(resource_group_name); 518 | 519 | mesh_materials_[index] = material_manager.create(material_name,resource_group_name); 520 | Ogre::Technique* technique = mesh_materials_[index]->getTechnique(0); 521 | Ogre::Pass* pass = technique->getPass(0); 522 | 523 | Ogre::ColourValue self_illumnation_color(0.0f, 0.0f, 0.0f, border_colors_[index][3]); 524 | pass->setSelfIllumination(self_illumnation_color); 525 | 526 | Ogre::ColourValue diffuse_color(border_colors_[index][0], border_colors_[index][1], border_colors_[index][2], border_colors_[index][3]); 527 | pass->setDiffuse(diffuse_color); 528 | 529 | Ogre::ColourValue ambient_color(border_colors_[index][0], border_colors_[index][1], border_colors_[index][2], border_colors_[index][3]); 530 | pass->setAmbient(ambient_color); 531 | 532 | Ogre::ColourValue specular_color(1.0f, 1.0f, 1.0f, 1.0f); 533 | pass->setSpecular(specular_color); 534 | 535 | Ogre::Real shininess = 64.0f; 536 | pass->setShininess(shininess); 537 | 538 | pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 539 | 540 | mesh_materials_[index]->setCullingMode(Ogre::CULL_NONE); 541 | } 542 | 543 | mesh_nodes_[index] = this->scene_node_->createChildSceneNode(); 544 | 545 | } 546 | 547 | void MeshDisplayCustom::onEnable() 548 | { 549 | subscribe(); 550 | } 551 | 552 | void MeshDisplayCustom::onDisable() 553 | { 554 | unsubscribe(); 555 | } 556 | 557 | void MeshDisplayCustom::update( float wall_dt, float ros_dt ) 558 | { 559 | time_since_last_transform_ += wall_dt; 560 | 561 | if( !display_images_topic_property_->getTopic().isEmpty() ) 562 | { 563 | try 564 | { 565 | for (int i=0; iupdate()); 568 | } 569 | } 570 | catch( UnsupportedImageEncoding& e ) 571 | { 572 | setStatus(StatusProperty::Error, "Display Image", e.what()); 573 | } 574 | } 575 | } 576 | 577 | bool MeshDisplayCustom::updateCamera(int index, bool update_image) 578 | { 579 | if(update_image) 580 | { 581 | last_images_[index] = textures_[index]->getImage(); 582 | } 583 | 584 | if(!img_heights_[index] || !img_widths_[index] || 585 | !physical_widths_[index] || !physical_heights_[index] || 586 | !last_images_[index]) 587 | { 588 | return false; 589 | } 590 | 591 | boost::mutex::scoped_lock lock( mesh_mutex_ ); 592 | 593 | float img_width = img_widths_[index]; 594 | float img_height = img_heights_[index]; 595 | 596 | Ogre::Vector3 position; 597 | Ogre::Quaternion orientation; 598 | 599 | context_->getFrameManager()->getTransform( last_images_[index]->header.frame_id, last_images_[index]->header.stamp, position, orientation ); 600 | 601 | Eigen::Affine3d trans_mat; 602 | tf::poseMsgToEigen(mesh_poses_[index], trans_mat); 603 | 604 | // Rotate by 90 deg to get xz plane 605 | trans_mat = trans_mat * Eigen::Quaterniond(0.70710678, -0.70710678f, 0.0f, 0.0f); 606 | 607 | float z_offset = (img_width > img_height) ? img_width : img_height; 608 | float scale_factor = 1.0f / (physical_widths_[index] > physical_heights_[index] ? physical_widths_[index] : physical_heights_[index]); 609 | 610 | Eigen::Vector4d projector_origin(0.0f, 0.0f, 1.0f / (z_offset * scale_factor), 1.0f); 611 | Eigen::Vector4d projector_point = trans_mat.matrix() * projector_origin; 612 | 613 | position = Ogre::Vector3(projector_point[0], projector_point[1], projector_point[2] ); 614 | orientation = Ogre::Quaternion(mesh_poses_[index].orientation.w, mesh_poses_[index].orientation.x, mesh_poses_[index].orientation.y, mesh_poses_[index].orientation.z); 615 | 616 | // Update orientation with 90 deg offset (xy to xz) 617 | orientation = orientation * Ogre::Quaternion( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_X ); 618 | 619 | // convert vision (Z-forward) frame to ogre frame (Z-out) 620 | orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_Z ); 621 | 622 | 623 | // std::cout << "CameraInfo dimensions: " << last_info_->width << " x " << last_info_->height << std::endl; 624 | // std::cout << "Texture dimensions: " << last_image_->width << " x " << last_image_->height << std::endl; 625 | //std::cout << "Original image dimensions: " << last_image_->width*full_image_binning_ << " x " << last_image_->height*full_image_binning_ << std::endl; 626 | 627 | // If the image width/height is 0 due to a malformed caminfo, try to grab the width from the image. 628 | if( img_width <= 0 ) 629 | { 630 | ROS_ERROR( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() )); 631 | // use texture size, but have to remove border from the perspective calculations 632 | img_width = textures_[index]->getWidth()-2; 633 | } 634 | 635 | if (img_height <= 0) 636 | { 637 | ROS_ERROR( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() )); 638 | // use texture size, but have to remove border from the perspective calculations 639 | img_height = textures_[index]->getHeight()-2; 640 | } 641 | 642 | // if even the texture has 0 size, return 643 | if( img_height <= 0.0 || img_width <= 0.0 ) 644 | { 645 | setStatus( StatusProperty::Error, "Camera Info", 646 | "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0) and texture." ); 647 | return false; 648 | } 649 | 650 | // projection matrix 651 | float P[12] = {1.0, 0.0, img_width / 2.0f, 0.0, 652 | 0.0, 1.0, img_height / 2.0f, 0.0, 653 | 0.0, 0.0, 1.0, 0.0 }; 654 | 655 | // calculate projection matrix 656 | double fx = P[0]; 657 | double fy = P[5]; 658 | 659 | // Add the camera's translation relative to the left camera (from P[3]); 660 | double tx = -1 * (P[3] / fx); 661 | Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X; 662 | position = position + (right * tx); 663 | 664 | double ty = -1 * (P[7] / fy); 665 | Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y; 666 | position = position + (down * ty); 667 | 668 | if( !validateFloats( position )) 669 | { 670 | ROS_ERROR( "position error"); 671 | setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" ); 672 | return false; 673 | } 674 | 675 | if(projector_nodes_[index] != NULL) 676 | { 677 | projector_nodes_[index]->setPosition( position ); 678 | projector_nodes_[index]->setOrientation( orientation ); 679 | } 680 | 681 | // calculate the projection matrix 682 | double cx = P[2]; 683 | double cy = P[6]; 684 | 685 | double far_plane = 100; 686 | double near_plane = 0.01; 687 | 688 | Ogre::Matrix4 proj_matrix; 689 | proj_matrix = Ogre::Matrix4::ZERO; 690 | 691 | proj_matrix[0][0]= 2.0 * fx/img_width; 692 | proj_matrix[1][1]= 2.0 * fy/img_height; 693 | 694 | proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width); 695 | proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5); 696 | 697 | proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane); 698 | proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane); 699 | 700 | proj_matrix[3][2]= -1; 701 | 702 | if(decal_frustums_[index] != NULL) 703 | decal_frustums_[index]->setCustomProjectionMatrix(true, proj_matrix); 704 | 705 | // ROS_INFO(" Camera (%f, %f)", proj_matrix[0][0], proj_matrix[1][1]); 706 | // ROS_INFO(" Render Panel: %x Viewport: %x", render_panel_, render_panel_->getViewport()); 707 | 708 | 709 | setStatus( StatusProperty::Ok, "Time", "ok" ); 710 | setStatus( StatusProperty::Ok, "Camera Info", "ok" ); 711 | 712 | if(mesh_nodes_[index] != NULL && filter_frustums_[index].size() == 0 && !mesh_materials_[index].isNull()) 713 | { 714 | createProjector(index); 715 | 716 | addDecalToMaterial(index, mesh_materials_[index]->getName()); 717 | } 718 | 719 | return true; 720 | } 721 | 722 | void MeshDisplayCustom::clear() 723 | { 724 | for (int i=0; iclear(); 727 | } 728 | 729 | context_->queueRender(); 730 | 731 | setStatus( StatusProperty::Warn, "Image", "No Image received"); 732 | } 733 | 734 | void MeshDisplayCustom::reset() 735 | { 736 | Display::reset(); 737 | clear(); 738 | } 739 | 740 | void MeshDisplayCustom::processImage(int index, const sensor_msgs::Image& msg) 741 | { 742 | //std::cout<<"camera image received"<image.rows; i++) 758 | // { 759 | // for(int j = 0; j < cv_ptr->image.cols; j++) 760 | // { 761 | // cv::Vec4b& pixel = cv_ptr->image.at(i,j); 762 | // pixel[3] = image_alpha_property_->getFloat()*255; 763 | // } 764 | // } 765 | 766 | // add completely white transparent border to the image so that it won't replicate colored pixels all over the mesh 767 | cv::Scalar value(255,255,255,0); 768 | cv::copyMakeBorder(cv_ptr->image,cv_ptr->image,1,1,1,1,cv::BORDER_CONSTANT,value); 769 | cv::flip(cv_ptr->image,cv_ptr->image,-1); 770 | 771 | // Output modified video stream 772 | if (textures_[index] == NULL) 773 | textures_[index] = new ROSImageTexture(); 774 | 775 | textures_[index]->addMessage(cv_ptr->toImageMsg()); 776 | } 777 | 778 | } // namespace rviz 779 | -------------------------------------------------------------------------------- /src/mesh_display_custom.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MeshDisplayCustom declaration. 3 | * 4 | * Author: Felipe Bacim. 5 | * 6 | * help with selection of robot parts 7 | */ 8 | /* 9 | * Copyright (c) 2008, Willow Garage, Inc. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions are met: 14 | * 15 | * * Redistributions of source code must retain the above copyright 16 | * notice, this list of conditions and the following disclaimer. 17 | * * Redistributions in binary form must reproduce the above copyright 18 | * notice, this list of conditions and the following disclaimer in the 19 | * documentation and/or other materials provided with the distribution. 20 | * * Neither the name of the Willow Garage, Inc. nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 27 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 28 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 29 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 30 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 31 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 32 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 33 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef RVIZ_MESH_DISPLAY_H 38 | #define RVIZ_MESH_DISPLAY_H 39 | 40 | #include "rviz/display.h" 41 | #include "rviz/frame_manager.h" 42 | #include "rviz/image/image_display_base.h" 43 | #include "rviz/image/ros_image_texture.h" 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include 56 | 57 | #include 58 | #include "OGRE/OgreRoot.h" 59 | #include "OGRE/OgreRenderSystem.h" 60 | #include "OGRE/OgreRenderWindow.h" 61 | #include "OGRE/OgreWindowEventUtilities.h" 62 | #include "OGRE/OgreManualObject.h" 63 | #include "OGRE/OgreEntity.h" 64 | #include 65 | #include 66 | #include 67 | 68 | #include 69 | #include 70 | 71 | #include 72 | #include 73 | 74 | #include 75 | #include 76 | #include 77 | 78 | #include 79 | #include 80 | 81 | #include "text_node.h" 82 | 83 | namespace Ogre 84 | { 85 | class Entity; 86 | class SceneNode; 87 | class ManualObject; 88 | } 89 | 90 | namespace rviz 91 | { 92 | class Axes; 93 | class RenderPanel; 94 | class FloatProperty; 95 | class RosTopicProperty; 96 | class ColorProperty; 97 | class VectorProperty; 98 | class StringProperty; 99 | class QuaternionProperty; 100 | } 101 | 102 | namespace rviz 103 | { 104 | 105 | /** 106 | * \class MeshDisplayCustom 107 | * \brief Uses a pose from topic + offset to render a bounding object with shape, size and color 108 | */ 109 | class MeshDisplayCustom: public rviz::Display, public Ogre::RenderTargetListener, public Ogre::RenderQueueListener 110 | { 111 | Q_OBJECT 112 | public: 113 | MeshDisplayCustom(); 114 | virtual ~MeshDisplayCustom(); 115 | 116 | // Overrides from Display 117 | virtual void onInitialize(); 118 | virtual void update( float wall_dt, float ros_dt ); 119 | virtual void reset(); 120 | 121 | private Q_SLOTS: 122 | void updateMeshProperties(); 123 | void updateDisplayImages(); 124 | 125 | protected: 126 | virtual void load(int index); 127 | 128 | // overrides from Display 129 | virtual void onEnable(); 130 | virtual void onDisable(); 131 | 132 | // This is called by incomingMessage(). 133 | void processImage(int index, const sensor_msgs::Image& msg); 134 | 135 | virtual void subscribe(); 136 | virtual void unsubscribe(); 137 | 138 | private: 139 | void clear(); 140 | bool updateCamera(int index, bool update_image); 141 | 142 | void createProjector(int index); 143 | void addDecalToMaterial(int index, const Ogre::String& matName); 144 | void updateImageMeshes( const rviz_textured_quads::TexturedQuadArray::ConstPtr& images ); 145 | 146 | void constructQuads( const rviz_textured_quads::TexturedQuadArray::ConstPtr& images ); 147 | shape_msgs::Mesh constructMesh( geometry_msgs::Pose mesh_origin, float width, float height, float border_size ); 148 | void clearStates(int num_quads); 149 | 150 | float time_since_last_transform_; 151 | 152 | RosTopicProperty* display_images_topic_property_; 153 | ColorProperty* text_color_property_; 154 | FloatProperty* text_height_property_; 155 | FloatProperty* text_bottom_offset_; 156 | 157 | std::vector last_meshes_; 158 | std::vector mesh_poses_; 159 | std::vector img_widths_, img_heights_; 160 | std::vector physical_widths_, physical_heights_; 161 | std::vector > border_colors_; 162 | std::vector border_sizes_; 163 | std::vector text_nodes_; 164 | 165 | ros::NodeHandle nh_; 166 | 167 | std::vector last_images_; 168 | 169 | std::vector mesh_nodes_; 170 | std::vector manual_objects_; 171 | std::vector mesh_materials_; 172 | std::vector textures_; 173 | 174 | ros::Subscriber pose_sub_; 175 | ros::Subscriber rviz_display_images_sub_; 176 | 177 | std::vector decal_frustums_; 178 | std::vector > filter_frustums_; //need multiple filters (back, up, down, left, right) 179 | std::vector projector_nodes_; 180 | 181 | std::vector render_panel_list_; 182 | RenderPanel* render_panel_; // this is the active render panel 183 | 184 | bool initialized_; 185 | 186 | boost::mutex mesh_mutex_; 187 | }; 188 | 189 | } // namespace rviz 190 | 191 | #endif 192 | 193 | 194 | -------------------------------------------------------------------------------- /src/plugin_init.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013-2015, Team ViGIR ( TORC Robotics LLC, TU Darmstadt, Virginia Tech, Oregon State University, Cornell University, and Leibniz University Hanover ) 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Team ViGIR, TORC Robotics, nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | //@TODO_ADD_AUTHOR_INFO 35 | #include 36 | #include "mesh_display_custom.h" 37 | 38 | CLASS_LOADER_REGISTER_CLASS( rviz::MeshDisplayCustom, rviz::Display ) 39 | 40 | -------------------------------------------------------------------------------- /src/text_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright notice, this 11 | * list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * * Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef TEXT_NODE_H 32 | #define TEXT_NODE_H 33 | 34 | #include 35 | #include 36 | 37 | namespace rviz_textured_quads { 38 | class TextNode { 39 | public: 40 | TextNode(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode, Ogre::Vector3 position = Ogre::Vector3::ZERO) : m_sceneManager(sceneManager) 41 | { 42 | m_sceneNode = parentNode->createChildSceneNode(); 43 | 44 | m_text = new rviz::MovableText("text"); 45 | m_text->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW); 46 | m_sceneNode->attachObject(m_text); 47 | 48 | setCharacterHeight(0.1); 49 | setPosition(position); 50 | setVisible(true); 51 | } 52 | 53 | virtual ~TextNode() { 54 | m_sceneManager->destroySceneNode(m_sceneNode->getName()); 55 | delete m_text; 56 | }; 57 | 58 | void clear() { 59 | setVisible(false); 60 | } 61 | 62 | void setCharacterHeight(double characterHeight) { 63 | m_text->setCharacterHeight(characterHeight); 64 | m_text->setSpaceWidth(0.3 * characterHeight); 65 | } 66 | 67 | double getCharacterHeight() { 68 | return m_text->getCharacterHeight(); 69 | } 70 | 71 | void setCaption(const std::string& caption) { 72 | m_text->setCaption(caption); 73 | setVisible(true); 74 | } 75 | 76 | void setPosition(const Ogre::Vector3& position) { 77 | m_sceneNode->setPosition(position); 78 | } 79 | 80 | void setVisible(bool visible) { 81 | m_sceneNode->setVisible(visible, true); 82 | } 83 | 84 | void setColor(const Ogre::ColourValue& c) { 85 | m_text->setColor(c); 86 | } 87 | 88 | void showOnTop(bool onTop = true) { 89 | m_text->showOnTop(onTop); 90 | } 91 | 92 | private: 93 | Ogre::SceneManager* m_sceneManager; 94 | Ogre::SceneNode* m_sceneNode; 95 | rviz::MovableText* m_text; 96 | }; 97 | 98 | } 99 | 100 | #endif // TEXT_NODE_H -------------------------------------------------------------------------------- /tests/display_images_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from sensor_msgs.msg import Image 5 | from geometry_msgs.msg import Pose 6 | 7 | import copy 8 | import numpy as np 9 | import cv2 10 | from cv_bridge import CvBridge, CvBridgeError 11 | import tf 12 | import math 13 | 14 | from rviz_textured_quads.msg import TexturedQuad, TexturedQuadArray 15 | 16 | 17 | def pub_image(): 18 | 19 | rospy.init_node('rviz_display_image_test', anonymous=True) 20 | image_pub = rospy.Publisher("/semantic_targets", TexturedQuadArray, queue_size=10) 21 | 22 | img1 = cv2.imread('./textures/bebop_drone.jpg',cv2.IMREAD_COLOR) 23 | img_msg1 = CvBridge().cv2_to_imgmsg(img1, "bgr8") 24 | 25 | img2 = cv2.imread('./textures/Decal.png',cv2.IMREAD_COLOR) 26 | img_msg2 = CvBridge().cv2_to_imgmsg(img2, "bgr8") 27 | 28 | cap = cv2.VideoCapture('/home/mohitshridhar/Downloads/ICRA_2010.mov') 29 | 30 | display_image = TexturedQuad() 31 | 32 | pose = Pose() 33 | 34 | pose.position.x = -1.2 35 | pose.position.y = -0.5 36 | pose.position.z = 2.0 37 | 38 | # pose.orientation.x = 0.70710678 39 | # pose.orientation.y = 0.0 40 | # pose.orientation.z = 0.0 41 | # pose.orientation.w = 0.70710678 42 | 43 | pose.orientation.x = 0.0 44 | pose.orientation.y = 0.0 45 | pose.orientation.z = 0.0 46 | pose.orientation.w = 1.0 47 | 48 | scale = 0.5 49 | 50 | display_image.image = img_msg1 51 | display_image.pose = pose 52 | display_image.width = scale 53 | display_image.height = (scale * img_msg1.height)/img_msg1.width 54 | display_image.border_color = [1., 0., 0., 0.5] 55 | display_image.border_size = 0.05 56 | display_image.caption = 'ICRA Video' 57 | 58 | 59 | second_image = copy.deepcopy(display_image) 60 | second_image.image = img_msg2 61 | second_image.width = 1.0 62 | second_image.height = 1.0 63 | second_image.pose.position.x = 2.2 64 | second_image.pose.position.y = -0.3 65 | second_image.pose.position.z = 2.0 66 | 67 | second_image.pose.orientation.x = 0.0 68 | second_image.pose.orientation.y = 0.70710678 69 | second_image.pose.orientation.z = 0.0 70 | second_image.pose.orientation.w = 0.70710678 71 | second_image.border_color = [0.5, 1.0, 0.0, 0.5] 72 | second_image.border_size = 0.1 73 | second_image.caption = 'Decal' 74 | 75 | display_images = TexturedQuadArray() 76 | display_images = np.array([display_image, second_image]) 77 | 78 | rate = rospy.Rate(30) # 10hz 79 | deg_increment = 0.005 80 | angle = 0 81 | count = 0 82 | 83 | while not rospy.is_shutdown(): 84 | if (cap.isOpened()): 85 | ret, frame = cap.read() 86 | 87 | display_image.image = CvBridge().cv2_to_imgmsg(frame, "bgr8") 88 | 89 | angle += deg_increment 90 | 91 | second_image.pose.position.x = 2.0 * math.sin(angle) 92 | second_image.pose.position.y = 2.0 * math.cos(angle) 93 | 94 | 95 | q = tf.transformations.quaternion_from_euler(angle + deg_increment, 0., 0.); 96 | second_image.pose.orientation.x = q[0] 97 | second_image.pose.orientation.y = q[1] 98 | second_image.pose.orientation.z = q[2] 99 | second_image.pose.orientation.w = q[3] 100 | 101 | # q = tf.transformations.quaternion_from_euler(0., angle + deg_increment, 0.); 102 | # display_image.pose.orientation.x = q[0] 103 | # display_image.pose.orientation.y = q[1] 104 | # display_image.pose.orientation.z = q[2] 105 | # display_image.pose.orientation.w = q[3] 106 | 107 | 108 | count = count + 1 109 | 110 | image_pub.publish(display_images) 111 | rate.sleep() 112 | 113 | cap.release() 114 | 115 | if __name__ == '__main__': 116 | 117 | try: 118 | pub_image() 119 | except rospy.ROSInterruptException: 120 | pass -------------------------------------------------------------------------------- /tests/textures/Decal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MohitShridhar/rviz_textured_quads/a1e60b38eeb878c23c32d9d0597629f0524bb0df/tests/textures/Decal.png -------------------------------------------------------------------------------- /tests/textures/Decal_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MohitShridhar/rviz_textured_quads/a1e60b38eeb878c23c32d9d0597629f0524bb0df/tests/textures/Decal_filter.png -------------------------------------------------------------------------------- /tests/textures/bebop_drone.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MohitShridhar/rviz_textured_quads/a1e60b38eeb878c23c32d9d0597629f0524bb0df/tests/textures/bebop_drone.jpg -------------------------------------------------------------------------------- /tests/textures/red.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MohitShridhar/rviz_textured_quads/a1e60b38eeb878c23c32d9d0597629f0524bb0df/tests/textures/red.jpg --------------------------------------------------------------------------------