├── .github └── workflows │ ├── ubuntu_18_04.yml │ └── ubuntu_20_04.yml ├── CMakeLists.txt ├── README.md ├── config └── rviz_textured_quads.rviz ├── gifs └── rviz_demo.gif ├── include └── rviz_textured_quads │ └── mesh_display_custom.h ├── launch └── demo.launch ├── mesh_display_custom_plugin_description.xml ├── package.xml ├── src └── mesh_display_custom.cpp └── tests ├── display_images_test.py └── textures ├── Decal.png ├── Decal_filter.png ├── bebop_drone.jpg └── red.jpg /.github/workflows/ubuntu_18_04.yml: -------------------------------------------------------------------------------- 1 | name: Ubuntu 18.04 Melodic CI 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-18.04 8 | env: 9 | ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] 10 | CI_SOURCE_PATH: $(pwd) 11 | ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall 12 | CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options 13 | ROS_PARALLEL_JOBS: '-j8 -l6' 14 | # Set the python path manually to include /usr/-/python2.7/dist-packages 15 | # as this is where apt-get installs python packages. 16 | PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 17 | ROS_DISTRO: melodic 18 | steps: 19 | - uses: actions/checkout@v1 20 | - name: Install ROS 21 | run: | 22 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 23 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 24 | sudo apt-get update -qq 25 | sudo apt-get install dpkg 26 | sudo apt-get install -y python-catkin-tools python-catkin-pkg 27 | sudo apt-get install -y python-rosdep python-wstool 28 | sudo apt-get install -y libboost-dev 29 | sudo apt-get install -y libeigen3-dev 30 | sudo apt-get install -y libyaml-cpp-dev 31 | sudo apt-get install -y qt5-default 32 | sudo apt-get install -y ros-cmake-modules 33 | # sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager 34 | sudo apt-get install -y ros-$ROS_DISTRO-class-loader 35 | sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules 36 | sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge 37 | # sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure 38 | # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure 39 | sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions 40 | sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs 41 | # sudo apt-get install -y ros-$ROS_DISTRO-image-transport 42 | sudo apt-get install -y ros-$ROS_DISTRO-interactive-markers 43 | sudo apt-get install -y ros-$ROS_DISTRO-message-generation 44 | sudo apt-get install -y ros-$ROS_DISTRO-message-runtime 45 | # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core 46 | # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions 47 | # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros 48 | sudo apt-get install -y ros-$ROS_DISTRO-pluginlib 49 | sudo apt-get install -y ros-$ROS_DISTRO-roscpp 50 | sudo apt-get install -y ros-$ROS_DISTRO-roslib 51 | sudo apt-get install -y ros-$ROS_DISTRO-roslint 52 | sudo apt-get install -y ros-$ROS_DISTRO-rospy 53 | sudo apt-get install -y ros-$ROS_DISTRO-rostest 54 | sudo apt-get install -y ros-$ROS_DISTRO-rviz 55 | sudo apt-get install -y ros-$ROS_DISTRO-shape-msgs 56 | sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs 57 | sudo apt-get install -y ros-$ROS_DISTRO-std-msgs 58 | sudo apt-get install -y ros-$ROS_DISTRO-tf 59 | sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions 60 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs 61 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs 62 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-py 63 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros 64 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs 65 | sudo apt-get install -y ros-$ROS_DISTRO-visualization-msgs 66 | source /opt/ros/$ROS_DISTRO/setup.bash 67 | # Prepare rosdep to install dependencies. 68 | sudo rosdep init 69 | rosdep update --include-eol-distros # Support EOL distros. 70 | 71 | - name: build 72 | run: | 73 | source /opt/ros/$ROS_DISTRO/setup.bash 74 | mkdir -p ~/catkin_ws/src 75 | cd ~/catkin_ws 76 | catkin build 77 | source devel/setup.bash 78 | # echo "::warning $CI_SOURCE_PATH" 79 | # echo "::warning `ls -l`" 80 | cd src 81 | ln -s ~/work # $CI_SOURCE_PATH 82 | cd .. 83 | catkin build 84 | - name: lint 85 | run: | 86 | cd ~/catkin_ws 87 | catkin build rviz_textured_quads --no-deps --catkin-make-args roslint 88 | -------------------------------------------------------------------------------- /.github/workflows/ubuntu_20_04.yml: -------------------------------------------------------------------------------- 1 | name: Ubuntu 20.04 Noetic CI 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-20.04 8 | env: 9 | ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] 10 | CI_SOURCE_PATH: $(pwd) 11 | ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall 12 | CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options 13 | ROS_PARALLEL_JOBS: '-j8 -l6' 14 | # Set the python path manually to include /usr/-/python2.7/dist-packages 15 | # as this is where apt-get installs python packages. 16 | PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 17 | ROS_DISTRO: noetic 18 | steps: 19 | - uses: actions/checkout@v1 20 | - name: Install ROS 21 | run: | 22 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 23 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 24 | sudo apt-get update -qq 25 | sudo apt-get install dpkg 26 | sudo apt-get install -y python3-catkin-pkg 27 | sudo apt-get install -y python3-catkin-tools 28 | sudo apt-get install -y python3-osrf-pycommon 29 | sudo apt-get install -y python3-rosdep 30 | sudo apt-get install -y python3-wstool 31 | sudo apt-get install -y libboost-dev 32 | sudo apt-get install -y libeigen3-dev 33 | sudo apt-get install -y libyaml-cpp-dev 34 | sudo apt-get install -y qt5-default 35 | sudo apt-get install -y ros-cmake-modules 36 | # sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager 37 | sudo apt-get install -y ros-$ROS_DISTRO-class-loader 38 | sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules 39 | sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge 40 | # sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure 41 | # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure 42 | sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions 43 | sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs 44 | # sudo apt-get install -y ros-$ROS_DISTRO-image-transport 45 | sudo apt-get install -y ros-$ROS_DISTRO-interactive-markers 46 | sudo apt-get install -y ros-$ROS_DISTRO-message-generation 47 | sudo apt-get install -y ros-$ROS_DISTRO-message-runtime 48 | # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core 49 | # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions 50 | # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros 51 | sudo apt-get install -y ros-$ROS_DISTRO-pluginlib 52 | sudo apt-get install -y ros-$ROS_DISTRO-roscpp 53 | sudo apt-get install -y ros-$ROS_DISTRO-roslib 54 | sudo apt-get install -y ros-$ROS_DISTRO-roslint 55 | sudo apt-get install -y ros-$ROS_DISTRO-rospy 56 | sudo apt-get install -y ros-$ROS_DISTRO-rostest 57 | sudo apt-get install -y ros-$ROS_DISTRO-rviz 58 | sudo apt-get install -y ros-$ROS_DISTRO-shape-msgs 59 | sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs 60 | sudo apt-get install -y ros-$ROS_DISTRO-std-msgs 61 | sudo apt-get install -y ros-$ROS_DISTRO-tf 62 | sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions 63 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs 64 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs 65 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-py 66 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros 67 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs 68 | sudo apt-get install -y ros-$ROS_DISTRO-visualization-msgs 69 | source /opt/ros/$ROS_DISTRO/setup.bash 70 | # Prepare rosdep to install dependencies. 71 | sudo rosdep init 72 | rosdep update --include-eol-distros # Support EOL distros. 73 | 74 | - name: build 75 | run: | 76 | source /opt/ros/$ROS_DISTRO/setup.bash 77 | mkdir -p ~/catkin_ws/src 78 | cd ~/catkin_ws 79 | catkin build 80 | source devel/setup.bash 81 | # echo "::warning $CI_SOURCE_PATH" 82 | # echo "::warning `ls -l`" 83 | cd src 84 | ln -s ~/work # $CI_SOURCE_PATH 85 | cd .. 86 | catkin build 87 | - name: lint 88 | run: | 89 | cd ~/catkin_ws 90 | catkin build rviz_textured_quads --no-deps --catkin-make-args roslint 91 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(rviz_textured_quads VERSION 0.1.0) 3 | 4 | ## Find catkin and any catkin packages on which 5 | ## this package depends at build time 6 | find_package(catkin REQUIRED COMPONENTS 7 | class_loader 8 | cmake_modules 9 | cv_bridge 10 | eigen_conversions 11 | geometry_msgs 12 | message_generation 13 | pluginlib 14 | roscpp 15 | roslib 16 | roslint 17 | rospy 18 | rviz 19 | rviz 20 | sensor_msgs 21 | sensor_msgs 22 | shape_msgs 23 | std_msgs 24 | tf_conversions 25 | ) 26 | 27 | find_package(Eigen REQUIRED) 28 | find_package(OpenGL REQUIRED) 29 | 30 | roslint_cpp() 31 | roslint_python() 32 | 33 | ## This plugin includes Qt widgets, so we must include Qt. 34 | ## We'll use the version that rviz used so they are compatible. 35 | if(rviz_QT_VERSION VERSION_LESS "5") 36 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 37 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 38 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 39 | include(${QT_USE_FILE}) 40 | else() 41 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 42 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 43 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 44 | set(QT_LIBRARIES Qt5::Widgets) 45 | endif() 46 | 47 | ## I (original author) prefer the Qt signals and slots to avoid defining "emit", "slots", 48 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 49 | add_definitions(-DQT_NO_KEYWORDS) 50 | 51 | # include_directories(SYSTEM "msg_gen/cpp/include") 52 | 53 | # add_message_files( 54 | # FILES 55 | # ) 56 | 57 | # generate_messages( 58 | # DEPENDENCIES 59 | # std_msgs 60 | # geometry_msgs 61 | # sensor_msgs 62 | # ) 63 | 64 | 65 | catkin_package( 66 | ) 67 | # LIBRARIES 68 | # rviz_textured_quads 69 | # ${OPENGL_LIBRARIES} 70 | # INCLUDE_DIRS 71 | # src 72 | # ${OPENGL_INCLUDE_DIR} 73 | # CATKIN_DEPENDS 74 | # roscpp 75 | # rospy 76 | # roslib 77 | # std_msgs 78 | # sensor_msgs 79 | # shape_msgs 80 | # rviz 81 | # pluginlib 82 | # class_loader 83 | # cv_bridge 84 | # geometry_msgs 85 | # sensor_msgs 86 | # eigen_conversions 87 | # rviz 88 | # ) 89 | 90 | include_directories( 91 | src 92 | ${catkin_INCLUDE_DIRS} 93 | ${Eigen_INCLUDE_DIRS}) 94 | 95 | # include_directories(SYSTEM 96 | # ${OPENGL_INCLUDE_DIR} 97 | # ${QT_INCLUDE_DIR}) 98 | # link_directories(${catkin_LIBRARY_DIRS}) 99 | 100 | 101 | if(rviz_QT_VERSION VERSION_LESS "5") 102 | qt4_wrap_cpp(MOC_FILES 103 | include/rviz_textured_quads/mesh_display_custom.h 104 | ) 105 | else() 106 | qt5_wrap_cpp(MOC_FILES 107 | include/rviz_textured_quads/mesh_display_custom.h 108 | ) 109 | endif() 110 | 111 | include_directories(include ${catkin_INCLUDE_DIRS}) 112 | 113 | set(TEXTURED_QUAD_LIB rviz_textured_quads) 114 | 115 | add_library(${TEXTURED_QUAD_LIB} src/mesh_display_custom.cpp ${MOC_FILES}) 116 | target_link_libraries(${TEXTURED_QUAD_LIB} ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Eigen_LIBRARIES}) 117 | 118 | add_dependencies(${TEXTURED_QUAD_LIB} ${catkin_EXPORTED_TARGETS}) 119 | 120 | install(TARGETS ${TEXTURED_QUAD_LIB} # ${TEXTURED_QUAD_LIB}_core 121 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 122 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 123 | 124 | # install(DIRECTORY textures 125 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz_textured_quads 126 | # ) 127 | 128 | install(FILES 129 | mesh_display_custom_plugin_description.xml 130 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 131 | -------------------------------------------------------------------------------- /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 | This uses rviz_camera_stream (https://github.com/lucasw/rviz_camera_stream), 9 | if not available set up another image topic (like from a webcam) and set the MeshDisplayCustom 10 | image property to that topic: 11 | 12 | roslaunch rviz_textured_quads demo.launch 13 | -------------------------------------------------------------------------------- /config/rviz_textured_quads.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /MeshDisplayCustom1 8 | Splitter Ratio: 0.5 9 | Tree Height: 212 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Image 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.0299999993 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Camera Info Topic: /camera1/camera_info 51 | Class: rviz_camera_stream/CameraPub 52 | Enabled: true 53 | Frame Rate: -1 54 | Image Topic: /image 55 | Name: CameraPub 56 | Queue Size: 2 57 | Value: true 58 | Visibility: 59 | Grid: true 60 | Image: true 61 | MeshDisplayCustom: true 62 | TF: true 63 | Value: true 64 | - Class: rviz/Image 65 | Enabled: true 66 | Image Topic: /image 67 | Max Value: 1 68 | Median window: 5 69 | Min Value: 0 70 | Name: Image 71 | Normalize Range: true 72 | Queue Size: 2 73 | Transport Hint: raw 74 | Unreliable: false 75 | Value: true 76 | - Class: rviz/TF 77 | Enabled: true 78 | Frame Timeout: 15 79 | Frames: 80 | All Enabled: true 81 | camera1: 82 | Value: true 83 | map: 84 | Value: true 85 | quad: 86 | Value: true 87 | Marker Scale: 1 88 | Name: TF 89 | Show Arrows: true 90 | Show Axes: true 91 | Show Names: true 92 | Tree: 93 | map: 94 | camera1: 95 | {} 96 | quad: 97 | {} 98 | Update Interval: 0 99 | Value: true 100 | - Class: rviz_textured_quads/MeshDisplayCustom 101 | Enabled: true 102 | Image Topic: /image 103 | Name: MeshDisplayCustom 104 | Quad Frame: quad 105 | Value: true 106 | Enabled: true 107 | Global Options: 108 | Background Color: 48; 48; 48 109 | Fixed Frame: map 110 | Frame Rate: 30 111 | Name: root 112 | Tools: 113 | - Class: rviz/Interact 114 | Hide Inactive Objects: true 115 | - Class: rviz/MoveCamera 116 | - Class: rviz/Select 117 | - Class: rviz/FocusCamera 118 | - Class: rviz/Measure 119 | - Class: rviz/SetInitialPose 120 | Topic: /initialpose 121 | - Class: rviz/SetGoal 122 | Topic: /move_base_simple/goal 123 | - Class: rviz/PublishPoint 124 | Single click: true 125 | Topic: /clicked_point 126 | Value: true 127 | Views: 128 | Current: 129 | Class: rviz/Orbit 130 | Distance: 4.28777599 131 | Enable Stereo Rendering: 132 | Stereo Eye Separation: 0.0599999987 133 | Stereo Focal Distance: 1 134 | Swap Stereo Eyes: false 135 | Value: false 136 | Focal Point: 137 | X: 0 138 | Y: 0 139 | Z: 0 140 | Name: Current View 141 | Near Clip Distance: 0.00999999978 142 | Pitch: 0.290397882 143 | Target Frame: 144 | Value: Orbit (rviz) 145 | Yaw: 6.19357872 146 | Saved: ~ 147 | Window Geometry: 148 | Displays: 149 | collapsed: false 150 | Height: 989 151 | Hide Left Dock: false 152 | Hide Right Dock: true 153 | Image: 154 | collapsed: false 155 | QMainWindow State: 000000ff00000000fd00000004000000000000027e0000033afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000163000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001aa000001d10000001600ffffff000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005c90000003efc0100000002fb0000000800540069006d00650100000000000005c90000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003450000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 156 | Selection: 157 | collapsed: false 158 | Time: 159 | collapsed: false 160 | Tool Properties: 161 | collapsed: false 162 | Views: 163 | collapsed: true 164 | Width: 1481 165 | X: 56 166 | Y: 24 167 | -------------------------------------------------------------------------------- /gifs/rviz_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasw/rviz_textured_quads/6e836abc2cdce9c7b832462a432767ffbf451a4d/gifs/rviz_demo.gif -------------------------------------------------------------------------------- /include/rviz_textured_quads/mesh_display_custom.h: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2013-2015 Team ViGIR ( TORC Robotics LLC, TU Darmstadt, Virginia Tech, Oregon State University, Cornell University, and Leibniz University Hanover ) 2 | * All rights reserved. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Willow Garage, Inc. nor the names of its 13 | * contributors may be used to endorse or promote products derived from 14 | * this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #ifndef RVIZ_TEXTURED_QUADS_MESH_DISPLAY_CUSTOM_H 30 | #define RVIZ_TEXTURED_QUADS_MESH_DISPLAY_CUSTOM_H 31 | 32 | #include 33 | // kinetic compatibility http://answers.ros.org/question/233786/parse-error-at-boost_join/ 34 | #ifndef Q_MOC_RUN 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | #endif // Q_MOC_RUN 69 | 70 | namespace Ogre 71 | { 72 | class Entity; 73 | class SceneNode; 74 | class ManualObject; 75 | } 76 | 77 | namespace rviz 78 | { 79 | class FloatProperty; 80 | class RenderPanel; 81 | class RosTopicProperty; 82 | class TfFrameProperty; 83 | 84 | /** 85 | * \class MeshDisplayCustom 86 | * \brief Uses a pose from topic + offset to render a bounding object with shape, size and color 87 | */ 88 | class MeshDisplayCustom: public rviz::Display, public Ogre::RenderTargetListener, public Ogre::RenderQueueListener 89 | { 90 | Q_OBJECT 91 | public: 92 | MeshDisplayCustom(); 93 | virtual ~MeshDisplayCustom(); 94 | 95 | // Overrides from Display 96 | virtual void onInitialize(); 97 | virtual void update(float wall_dt, float ros_dt); 98 | virtual void reset(); 99 | 100 | virtual void preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt); 101 | 102 | private Q_SLOTS: 103 | void updateMeshProperties(); 104 | void updateDisplayImages(); 105 | 106 | protected: 107 | virtual void load(); 108 | 109 | // overrides from Display 110 | virtual void onEnable(); 111 | virtual void onDisable(); 112 | 113 | // This is called by incomingMessage(). 114 | void processImage(int index, const sensor_msgs::Image& msg); 115 | 116 | virtual void subscribe(); 117 | virtual void unsubscribe(); 118 | 119 | private: 120 | void clear(); 121 | void updateCamera(bool update_image); 122 | 123 | void createProjector(int index); 124 | void addDecalToMaterial(int index, const Ogre::String& matName); 125 | 126 | void updateImage(const sensor_msgs::Image::ConstPtr& image); 127 | void constructQuads(const sensor_msgs::Image::ConstPtr& images); 128 | 129 | shape_msgs::Mesh constructMesh(geometry_msgs::Pose mesh_origin, float width, float height, float border_size); 130 | void clearStates(); 131 | 132 | RosTopicProperty* image_topic_property_; 133 | TfFrameProperty* tf_frame_property_; 134 | FloatProperty* meters_per_pixel_property_; 135 | ros::Subscriber image_sub_; 136 | 137 | std::vector last_meshes_; 138 | std::vector mesh_poses_; 139 | int img_widths_, img_heights_; 140 | float physical_widths_, physical_heights_; 141 | std::vector border_colors_; 142 | float border_sizes_; 143 | 144 | ros::NodeHandle nh_; 145 | 146 | bool new_image_; 147 | sensor_msgs::Image::ConstPtr cur_image_; 148 | std::vector last_images_; 149 | 150 | Ogre::SceneNode* mesh_nodes_; 151 | Ogre::ManualObject* manual_objects_; 152 | Ogre::MaterialPtr mesh_materials_; 153 | ROSImageTexture* textures_; 154 | 155 | Ogre::Frustum* decal_frustums_; 156 | // need multiple filters (back, up, down, left, right) 157 | std::vector filter_frustums_; 158 | Ogre::SceneNode* projector_nodes_; 159 | 160 | RenderPanel* render_panel_; // this is the active render panel 161 | 162 | boost::mutex mesh_mutex_; 163 | }; 164 | 165 | } // namespace rviz 166 | 167 | #endif // RVIZ_TEXTURED_QUADS_MESH_DISPLAY_CUSTOM_H 168 | 169 | 170 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 12 | 13 | 14 | 17 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /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 | 10 | -------------------------------------------------------------------------------- /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 | roslint 16 | std_msgs 17 | sensor_msgs 18 | shape_msgs 19 | pluginlib 20 | class_loader 21 | rviz 22 | cv_bridge 23 | message_generation 24 | geometry_msgs 25 | sensor_msgs 26 | cmake_modules 27 | eigen_conversions 28 | rviz 29 | tf_conversions 30 | 31 | roscpp 32 | rospy 33 | roslib 34 | roslint 35 | std_msgs 36 | sensor_msgs 37 | shape_msgs 38 | pluginlib 39 | class_loader 40 | rviz 41 | cv_bridge 42 | message_runtime 43 | geometry_msgs 44 | sensor_msgs 45 | cmake_modules 46 | eigen_conversions 47 | rviz 48 | tf_conversions 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /src/mesh_display_custom.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2013-2015 Team ViGIR ( TORC Robotics LLC, TU Darmstadt, Virginia Tech, Oregon State University, Cornell University, and Leibniz University Hanover ) 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 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | 64 | namespace rviz 65 | { 66 | 67 | bool validateFloats(const sensor_msgs::CameraInfo& msg) 68 | { 69 | bool valid = true; 70 | valid = valid && validateFloats(msg.D); 71 | valid = valid && validateFloats(msg.K); 72 | valid = valid && validateFloats(msg.R); 73 | valid = valid && validateFloats(msg.P); 74 | return valid; 75 | } 76 | 77 | MeshDisplayCustom::MeshDisplayCustom() 78 | : Display() 79 | , mesh_nodes_(NULL) 80 | , textures_(NULL) 81 | , projector_nodes_(NULL) 82 | , manual_objects_(NULL) 83 | , decal_frustums_(NULL) 84 | , new_image_(false) 85 | { 86 | image_topic_property_ = new RosTopicProperty("Image Topic", "", 87 | QString::fromStdString(ros::message_traits::datatype()), 88 | "Image topic to subscribe to.", 89 | this, SLOT(updateDisplayImages())); 90 | // TODO(lucasw) add controls to switch which plane to align to? 91 | tf_frame_property_ = new TfFrameProperty("Quad Frame", "map", 92 | "Align the image quad to the xy plane of this tf frame", 93 | this, 0, true); 94 | 95 | meters_per_pixel_property_ = new FloatProperty("Meters per pixel", 0.002, 96 | "Rviz meters per image pixel.", this); 97 | } 98 | 99 | MeshDisplayCustom::~MeshDisplayCustom() 100 | { 101 | unsubscribe(); 102 | // TODO(lucasw) switch to smart pointers 103 | delete manual_objects_; 104 | delete decal_frustums_; 105 | delete textures_; 106 | delete mesh_nodes_; 107 | 108 | for (size_t i = 0; i < filter_frustums_.size(); ++i) 109 | delete filter_frustums_[i]; 110 | 111 | // TODO(lucasw) clean up other things 112 | delete image_topic_property_; 113 | delete tf_frame_property_; 114 | delete meters_per_pixel_property_; 115 | } 116 | 117 | void MeshDisplayCustom::onInitialize() 118 | { 119 | tf_frame_property_->setFrameManager(context_->getFrameManager()); 120 | Display::onInitialize(); 121 | } 122 | 123 | void MeshDisplayCustom::createProjector(int index) 124 | { 125 | decal_frustums_ = new Ogre::Frustum(); 126 | 127 | projector_nodes_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 128 | projector_nodes_->attachObject(decal_frustums_); 129 | 130 | Ogre::SceneNode* filter_node; 131 | 132 | // back filter 133 | for (size_t i = 0; i < filter_frustums_.size(); ++i) 134 | delete filter_frustums_[i]; 135 | filter_frustums_.push_back(new Ogre::Frustum()); 136 | filter_frustums_.back()->setProjectionType(Ogre::PT_ORTHOGRAPHIC); 137 | filter_node = projector_nodes_->createChildSceneNode(); 138 | filter_node->attachObject(filter_frustums_.back()); 139 | filter_node->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Y)); 140 | } 141 | 142 | void MeshDisplayCustom::addDecalToMaterial(int index, const Ogre::String& matName) 143 | { 144 | Ogre::MaterialPtr mat = (Ogre::MaterialPtr)Ogre::MaterialManager::getSingleton().getByName(matName); 145 | mat->setCullingMode(Ogre::CULL_NONE); 146 | Ogre::Pass* pass = mat->getTechnique(0)->createPass(); 147 | 148 | pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 149 | pass->setDepthBias(1); 150 | // pass->setLightingEnabled(true); 151 | 152 | // need the decal_filter to avoid back projection 153 | Ogre::String resource_group_name = "decal_textures_folder"; 154 | Ogre::ResourceGroupManager& resource_manager = Ogre::ResourceGroupManager::getSingleton(); 155 | if (!resource_manager.resourceGroupExists(resource_group_name)) 156 | { 157 | resource_manager.createResourceGroup(resource_group_name); 158 | resource_manager.addResourceLocation(ros::package::getPath("rviz_textured_quads") + 159 | "/tests/textures/", "FileSystem", resource_group_name, false); 160 | resource_manager.initialiseResourceGroup(resource_group_name); 161 | } 162 | // loads files into our resource manager 163 | resource_manager.loadResourceGroup(resource_group_name); 164 | 165 | Ogre::TextureUnitState* tex_state = pass->createTextureUnitState(); // "Decal.png"); 166 | tex_state->setTextureName(textures_->getTexture()->getName()); 167 | tex_state->setProjectiveTexturing(true, decal_frustums_); 168 | 169 | tex_state->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); 170 | tex_state->setTextureFiltering(Ogre::FO_POINT, Ogre::FO_LINEAR, Ogre::FO_NONE); 171 | tex_state->setColourOperation(Ogre::LBO_REPLACE); // don't accept additional effects 172 | 173 | for (int i = 0; i < filter_frustums_.size(); i++) 174 | { 175 | tex_state = pass->createTextureUnitState("Decal_filter.png"); 176 | tex_state->setProjectiveTexturing(true, filter_frustums_[i]); 177 | tex_state->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); 178 | tex_state->setTextureFiltering(Ogre::TFO_NONE); 179 | } 180 | } 181 | 182 | shape_msgs::Mesh MeshDisplayCustom::constructMesh(geometry_msgs::Pose mesh_origin, 183 | float width, float height, float border_size) 184 | { 185 | shape_msgs::Mesh mesh; 186 | 187 | Eigen::Affine3d trans_mat; 188 | tf::poseMsgToEigen(mesh_origin, trans_mat); 189 | 190 | // Rviz Coordinate System: x-right, y-forward, z-down 191 | // create mesh vertices and tranform them to the specified pose 192 | 193 | Eigen::Vector4d top_left(-width / 2.0f - border_size, 0.0f, -height / 2.0f - border_size, 1.0f); 194 | Eigen::Vector4d top_right(width / 2.0f + border_size, 0.0f, -height / 2.0f - border_size, 1.0f); 195 | Eigen::Vector4d bottom_left(-width / 2.0f - border_size, 0.0f, height / 2.0f + border_size, 1.0f); 196 | Eigen::Vector4d bottom_right(width / 2.0f + border_size, 0.0f, height / 2.0f + border_size, 1.0f); 197 | 198 | Eigen::Vector4d trans_top_left = trans_mat.matrix() * top_left; 199 | Eigen::Vector4d trans_top_right = trans_mat.matrix() * top_right; 200 | Eigen::Vector4d trans_bottom_left = trans_mat.matrix() * bottom_left; 201 | Eigen::Vector4d trans_bottom_right = trans_mat.matrix() * bottom_right; 202 | 203 | std::vector vertices(4); 204 | vertices.at(0).x = trans_top_left[0]; 205 | vertices.at(0).y = trans_top_left[1]; 206 | vertices.at(0).z = trans_top_left[2]; 207 | vertices.at(1).x = trans_top_right[0]; 208 | vertices.at(1).y = trans_top_right[1]; 209 | vertices.at(1).z = trans_top_right[2]; 210 | vertices.at(2).x = trans_bottom_left[0]; 211 | vertices.at(2).y = trans_bottom_left[1]; 212 | vertices.at(2).z = trans_bottom_left[2]; 213 | vertices.at(3).x = trans_bottom_right[0]; 214 | vertices.at(3).y = trans_bottom_right[1]; 215 | vertices.at(3).z = trans_bottom_right[2]; 216 | mesh.vertices = vertices; 217 | 218 | std::vector triangles(2); 219 | triangles.at(0).vertex_indices[0] = 0; 220 | triangles.at(0).vertex_indices[1] = 1; 221 | triangles.at(0).vertex_indices[2] = 2; 222 | triangles.at(1).vertex_indices[0] = 1; 223 | triangles.at(1).vertex_indices[1] = 2; 224 | triangles.at(1).vertex_indices[2] = 3; 225 | mesh.triangles = triangles; 226 | 227 | return mesh; 228 | } 229 | 230 | void MeshDisplayCustom::clearStates() 231 | { 232 | if (manual_objects_) 233 | manual_objects_->clear(); 234 | 235 | const int num_quads = 1; 236 | // resize state vectors 237 | mesh_poses_.resize(num_quads); 238 | 239 | last_meshes_.resize(num_quads); 240 | 241 | last_images_.resize(num_quads); 242 | 243 | border_colors_.resize(4); 244 | for (size_t i = 0; i < 4; ++i) 245 | { 246 | border_colors_[i] = 1.0; 247 | } 248 | } 249 | 250 | void MeshDisplayCustom::constructQuads(const sensor_msgs::Image::ConstPtr& image) 251 | { 252 | clearStates(); 253 | 254 | int q = 0; 255 | { 256 | processImage(q, *image); 257 | 258 | geometry_msgs::Pose mesh_origin; 259 | 260 | // TODO(lucasw) get pose from tf 261 | std::string frame = tf_frame_property_->getFrameStd(); 262 | if (frame == "") 263 | { 264 | frame = image->header.frame_id; 265 | } 266 | // Lookup transform into fixed frame 267 | Ogre::Vector3 position; 268 | Ogre::Quaternion orientation; 269 | if (!context_->getFrameManager()->getTransform(frame, ros::Time(0), 270 | position, orientation)) 271 | { 272 | std::stringstream ss; 273 | ss << "Error transforming from fixed frame to frame " << frame.c_str(); 274 | throw ss.str(); 275 | } 276 | 277 | mesh_origin.position.x = position[0]; 278 | mesh_origin.position.y = position[1]; 279 | mesh_origin.position.z = position[2]; 280 | mesh_origin.orientation.w = orientation[0]; 281 | mesh_origin.orientation.x = orientation[1]; 282 | mesh_origin.orientation.y = orientation[2]; 283 | mesh_origin.orientation.z = orientation[3]; 284 | 285 | // Rotate from x-y to x-z plane: 286 | Eigen::Affine3d trans_mat; 287 | tf::poseMsgToEigen(mesh_origin, trans_mat); 288 | trans_mat = trans_mat * Eigen::Quaterniond(0.70710678, -0.70710678f, 0.0f, 0.0f); 289 | 290 | Eigen::Quaterniond xz_quat(trans_mat.rotation()); 291 | mesh_origin.orientation.x = xz_quat.x(); 292 | mesh_origin.orientation.y = xz_quat.y(); 293 | mesh_origin.orientation.z = xz_quat.z(); 294 | mesh_origin.orientation.w = xz_quat.w(); 295 | 296 | const float meters_per_pixel = meters_per_pixel_property_->getFloat(); 297 | float width = 1.0; 298 | float height = 1.0; 299 | if (meters_per_pixel > 0) 300 | { 301 | width = image->width * meters_per_pixel; 302 | height = image->height * meters_per_pixel; 303 | } 304 | else 305 | { 306 | height = width * image->height / image->width; 307 | } 308 | 309 | // set properties 310 | mesh_poses_[q] = mesh_origin; 311 | img_widths_ = image->width; 312 | img_heights_ = image->height; 313 | 314 | // default border size (no border) 315 | border_sizes_ = 0.0f; 316 | 317 | shape_msgs::Mesh mesh = constructMesh(mesh_origin, width, height, border_sizes_); 318 | 319 | physical_widths_ = width; 320 | physical_heights_ = height; 321 | 322 | boost::mutex::scoped_lock lock(mesh_mutex_); 323 | 324 | // create our scenenode and material 325 | load(); 326 | 327 | if (!manual_objects_) 328 | { 329 | static uint32_t count = 0; 330 | std::stringstream ss; 331 | ss << "MeshObject" << count++ << "Index" << q; 332 | manual_objects_ = context_->getSceneManager()->createManualObject(ss.str()); 333 | mesh_nodes_->attachObject(manual_objects_); 334 | } 335 | 336 | // If we have the same number of tris as previously, just update the object 337 | if (last_meshes_[q].vertices.size() > 0 && mesh.vertices.size() * 2 == last_meshes_[q].vertices.size()) 338 | { 339 | manual_objects_->beginUpdate(0); 340 | } 341 | else // Otherwise clear it and begin anew 342 | { 343 | manual_objects_->clear(); 344 | manual_objects_->estimateVertexCount(mesh.vertices.size() * 2); 345 | manual_objects_->begin(mesh_materials_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); 346 | } 347 | 348 | const std::vector& points = mesh.vertices; 349 | for (size_t i = 0; i < mesh.triangles.size(); i++) 350 | { 351 | // make sure we have front-face/back-face triangles 352 | for (int side = 0; side < 2; side++) 353 | { 354 | std::vector corners(3); 355 | for (size_t c = 0; c < 3; c++) 356 | { 357 | size_t corner = side ? 2 - c : c; // order of corners if side == 1 358 | corners[corner] = Ogre::Vector3(points[mesh.triangles[i].vertex_indices[corner]].x, 359 | points[mesh.triangles[i].vertex_indices[corner]].y, 360 | points[mesh.triangles[i].vertex_indices[corner]].z); 361 | } 362 | Ogre::Vector3 normal = (corners[1] - corners[0]).crossProduct(corners[2] - corners[0]); 363 | normal.normalise(); 364 | 365 | for (size_t c = 0; c < 3; c++) 366 | { 367 | manual_objects_->position(corners[c]); 368 | manual_objects_->normal(normal); 369 | } 370 | } 371 | } 372 | 373 | manual_objects_->end(); 374 | 375 | mesh_materials_->setCullingMode(Ogre::CULL_NONE); 376 | 377 | last_meshes_[q] = mesh; 378 | } 379 | } 380 | 381 | void MeshDisplayCustom::updateImage(const sensor_msgs::Image::ConstPtr& image) 382 | { 383 | cur_image_ = image; 384 | new_image_ = true; 385 | } 386 | 387 | void MeshDisplayCustom::updateMeshProperties() 388 | { 389 | { 390 | // update color/alpha 391 | Ogre::Technique* technique = mesh_materials_->getTechnique(0); 392 | Ogre::Pass* pass = technique->getPass(0); 393 | 394 | Ogre::ColourValue self_illumination_color(0.0f, 0.0f, 0.0f, 0.0f); 395 | pass->setSelfIllumination(self_illumination_color); 396 | 397 | Ogre::ColourValue diffuse_color(0.0f, 0.0f, 0.0f, 1.0f); 398 | pass->setDiffuse(diffuse_color); 399 | 400 | Ogre::ColourValue ambient_color(border_colors_[0], 401 | border_colors_[1], border_colors_[2], border_colors_[3]); 402 | pass->setAmbient(ambient_color); 403 | 404 | Ogre::ColourValue specular_color(0.0f, 0.0f, 0.0f, 1.0f); 405 | pass->setSpecular(specular_color); 406 | 407 | Ogre::Real shininess = 64.0f; 408 | pass->setShininess(shininess); 409 | 410 | pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 411 | pass->setDepthWriteEnabled(false); 412 | 413 | context_->queueRender(); 414 | } 415 | } 416 | 417 | void MeshDisplayCustom::updateDisplayImages() 418 | { 419 | unsubscribe(); 420 | subscribe(); 421 | } 422 | 423 | void MeshDisplayCustom::subscribe() 424 | { 425 | if (!isEnabled()) 426 | { 427 | return; 428 | } 429 | 430 | if (!image_topic_property_->getTopic().isEmpty()) 431 | { 432 | try 433 | { 434 | image_sub_ = nh_.subscribe(image_topic_property_->getTopicStd(), 435 | 1, &MeshDisplayCustom::updateImage, this); 436 | setStatus(StatusProperty::Ok, "Display Images Topic", "ok"); 437 | } 438 | catch (ros::Exception& e) 439 | { 440 | setStatus(StatusProperty::Error, "Display Images Topic", QString("Error subscribing: ") + e.what()); 441 | } 442 | } 443 | } 444 | 445 | void MeshDisplayCustom::unsubscribe() 446 | { 447 | image_sub_.shutdown(); 448 | } 449 | 450 | void MeshDisplayCustom::load() 451 | { 452 | if (mesh_nodes_ != NULL) 453 | return; 454 | 455 | static int count = 0; 456 | std::stringstream ss; 457 | ss << "MeshNode" << count++; 458 | Ogre::MaterialManager& material_manager = Ogre::MaterialManager::getSingleton(); 459 | Ogre::String resource_group_name = ss.str(); 460 | 461 | Ogre::ResourceGroupManager& rg_mgr = Ogre::ResourceGroupManager::getSingleton(); 462 | 463 | Ogre::String material_name = resource_group_name + "MeshMaterial"; 464 | 465 | if (!rg_mgr.resourceGroupExists(resource_group_name)) 466 | { 467 | rg_mgr.createResourceGroup(resource_group_name); 468 | 469 | mesh_materials_ = material_manager.create(material_name, resource_group_name); 470 | Ogre::Technique* technique = mesh_materials_->getTechnique(0); 471 | Ogre::Pass* pass = technique->getPass(0); 472 | 473 | Ogre::ColourValue self_illumnation_color(0.0f, 0.0f, 0.0f, border_colors_[3]); 474 | pass->setSelfIllumination(self_illumnation_color); 475 | 476 | Ogre::ColourValue diffuse_color(border_colors_[0], 477 | border_colors_[1], border_colors_[2], border_colors_[3]); 478 | pass->setDiffuse(diffuse_color); 479 | 480 | Ogre::ColourValue ambient_color(border_colors_[0], 481 | border_colors_[1], border_colors_[2], border_colors_[3]); 482 | pass->setAmbient(ambient_color); 483 | 484 | Ogre::ColourValue specular_color(1.0f, 1.0f, 1.0f, 1.0f); 485 | pass->setSpecular(specular_color); 486 | 487 | Ogre::Real shininess = 64.0f; 488 | pass->setShininess(shininess); 489 | 490 | pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 491 | mesh_materials_->setCullingMode(Ogre::CULL_NONE); 492 | } 493 | 494 | mesh_nodes_ = this->scene_node_->createChildSceneNode(); 495 | } 496 | 497 | void MeshDisplayCustom::onEnable() 498 | { 499 | subscribe(); 500 | } 501 | 502 | void MeshDisplayCustom::onDisable() 503 | { 504 | unsubscribe(); 505 | } 506 | 507 | void MeshDisplayCustom::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt) 508 | { 509 | } 510 | 511 | void MeshDisplayCustom::update(float wall_dt, float ros_dt) 512 | { 513 | if (cur_image_) 514 | { 515 | // need to run these every frame in case tf has changed, 516 | // but could detect that. 517 | try 518 | { 519 | constructQuads(cur_image_); 520 | } 521 | catch (std::string& e) 522 | { 523 | setStatus(StatusProperty::Error, "Display Image", e.c_str()); 524 | return; 525 | } 526 | catch (cv_bridge::Exception& e) 527 | { 528 | setStatus(StatusProperty::Error, "Display Image", e.what()); 529 | return; 530 | } 531 | updateMeshProperties(); 532 | // TODO(lucasw) do what is necessary for new image, but separate 533 | // other stuff. 534 | new_image_ = false; 535 | } 536 | 537 | if (textures_ && !image_topic_property_->getTopic().isEmpty()) 538 | { 539 | try 540 | { 541 | updateCamera(textures_->update()); 542 | } 543 | catch (UnsupportedImageEncoding& e) 544 | { 545 | setStatus(StatusProperty::Error, "Display Image", e.what()); 546 | return; 547 | } 548 | catch (std::string& e) 549 | { 550 | setStatus(StatusProperty::Error, "Display Image", e.c_str()); 551 | return; 552 | } 553 | } 554 | setStatus(StatusProperty::Ok, "Display Image", "ok"); 555 | } 556 | 557 | void MeshDisplayCustom::updateCamera(bool update_image) 558 | { 559 | int index = 0; 560 | if (update_image) 561 | { 562 | last_images_[index] = textures_->getImage(); 563 | } 564 | 565 | if (!img_heights_ || !img_widths_ || 566 | !physical_widths_ || !physical_heights_ || 567 | !last_images_[index]) 568 | { 569 | throw("sizes or image not ready"); 570 | // return false; 571 | } 572 | 573 | boost::mutex::scoped_lock lock(mesh_mutex_); 574 | 575 | float img_width = img_widths_; 576 | float img_height = img_heights_; 577 | 578 | Ogre::Vector3 position; 579 | Ogre::Quaternion orientation; 580 | 581 | context_->getFrameManager()->getTransform(last_images_[index]->header.frame_id, 582 | last_images_[index]->header.stamp, position, orientation); 583 | 584 | Eigen::Affine3d trans_mat; 585 | tf::poseMsgToEigen(mesh_poses_[index], trans_mat); 586 | 587 | // Rotate by 90 deg to get xz plane 588 | trans_mat = trans_mat * Eigen::Quaterniond(0.70710678, -0.70710678f, 0.0f, 0.0f); 589 | 590 | float z_offset = (img_width > img_height) ? img_width : img_height; 591 | float scale_factor = 1.0f / 592 | ((physical_widths_ > physical_heights_) ? 593 | physical_widths_ : physical_heights_); 594 | 595 | Eigen::Vector4d projector_origin(0.0f, 0.0f, 1.0f / (z_offset * scale_factor), 1.0f); 596 | Eigen::Vector4d projector_point = trans_mat.matrix() * projector_origin; 597 | 598 | position = Ogre::Vector3(projector_point[0], projector_point[1], projector_point[2]); 599 | orientation = Ogre::Quaternion(mesh_poses_[index].orientation.w, 600 | mesh_poses_[index].orientation.x, mesh_poses_[index].orientation.y, 601 | mesh_poses_[index].orientation.z); 602 | 603 | // Update orientation with 90 deg offset (xy to xz) 604 | orientation = orientation * Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); 605 | 606 | // convert vision (Z-forward) frame to ogre frame (Z-out) 607 | orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_Z); 608 | 609 | 610 | // std::cout << "CameraInfo dimensions: " << last_info_->width << " x " << last_info_->height << std::endl; 611 | // std::cout << "Texture dimensions: " << last_image_->width << " x " << last_image_->height << std::endl; 612 | // std::cout << "Original image dimensions: " 613 | // << last_image_->width*full_image_binning_ << " x " 614 | // << last_image_->height*full_image_binning_ << std::endl; 615 | 616 | // If the image width/height is 0 due to a malformed caminfo, try to grab the width from the image. 617 | if (img_width <= 0) 618 | { 619 | ROS_ERROR("Malformed CameraInfo on camera [%s], width = 0", qPrintable(getName())); 620 | // use texture size, but have to remove border from the perspective calculations 621 | img_width = textures_->getWidth() - 2; 622 | } 623 | 624 | if (img_height <= 0) 625 | { 626 | ROS_ERROR("Malformed CameraInfo on camera [%s], height = 0", qPrintable(getName())); 627 | // use texture size, but have to remove border from the perspective calculations 628 | img_height = textures_->getHeight() - 2; 629 | } 630 | 631 | // if even the texture has 0 size, return 632 | if (img_height <= 0.0 || img_width <= 0.0) 633 | { 634 | std::string text = "Could not determine width/height of image due to malformed "; 635 | text += "CameraInfo (either width or height is 0) and texture."; 636 | throw text; 637 | } 638 | 639 | // projection matrix 640 | float P[12] = 641 | { 642 | 1.0, 0.0, img_width / 2.0f, 0.0, 643 | 0.0, 1.0, img_height / 2.0f, 0.0, 644 | 0.0, 0.0, 1.0, 0.0 645 | }; 646 | 647 | // calculate projection matrix 648 | double fx = P[0]; 649 | double fy = P[5]; 650 | 651 | // Add the camera's translation relative to the left camera (from P[3]); 652 | double tx = -1 * (P[3] / fx); 653 | Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X; 654 | position = position + (right * tx); 655 | 656 | double ty = -1 * (P[7] / fy); 657 | Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y; 658 | position = position + (down * ty); 659 | 660 | if (!validateFloats(position)) 661 | { 662 | throw "CameraInfo/P resulted in an invalid position calculation (nans or infs)"; 663 | } 664 | 665 | if (projector_nodes_ != NULL) 666 | { 667 | projector_nodes_->setPosition(position); 668 | projector_nodes_->setOrientation(orientation); 669 | } 670 | 671 | // calculate the projection matrix 672 | double cx = P[2]; 673 | double cy = P[6]; 674 | 675 | double far_plane = 100; 676 | double near_plane = 0.01; 677 | 678 | Ogre::Matrix4 proj_matrix; 679 | proj_matrix = Ogre::Matrix4::ZERO; 680 | 681 | proj_matrix[0][0] = 2.0 * fx / img_width; 682 | proj_matrix[1][1] = 2.0 * fy / img_height; 683 | 684 | proj_matrix[0][2] = 2.0 * (0.5 - cx / img_width); 685 | proj_matrix[1][2] = 2.0 * (cy / img_height - 0.5); 686 | 687 | proj_matrix[2][2] = -(far_plane + near_plane) / (far_plane - near_plane); 688 | proj_matrix[2][3] = -2.0 * far_plane * near_plane / (far_plane - near_plane); 689 | 690 | proj_matrix[3][2] = -1; 691 | 692 | if (decal_frustums_ != NULL) 693 | decal_frustums_->setCustomProjectionMatrix(true, proj_matrix); 694 | 695 | // ROS_INFO(" Camera (%f, %f)", proj_matrix[0][0], proj_matrix[1][1]); 696 | // ROS_INFO(" Render Panel: %x Viewport: %x", render_panel_, render_panel_->getViewport()); 697 | 698 | 699 | setStatus(StatusProperty::Ok, "Time", "ok"); 700 | setStatus(StatusProperty::Ok, "Camera Info", "ok"); 701 | 702 | if (mesh_nodes_ != NULL && filter_frustums_.size() == 0 && !mesh_materials_.isNull()) 703 | { 704 | createProjector(index); 705 | 706 | addDecalToMaterial(index, mesh_materials_->getName()); 707 | } 708 | } 709 | 710 | void MeshDisplayCustom::clear() 711 | { 712 | textures_->clear(); 713 | 714 | context_->queueRender(); 715 | 716 | setStatus(StatusProperty::Warn, "Image", "No Image received"); 717 | } 718 | 719 | void MeshDisplayCustom::reset() 720 | { 721 | Display::reset(); 722 | clear(); 723 | } 724 | 725 | void MeshDisplayCustom::processImage(int index, const sensor_msgs::Image& msg) 726 | { 727 | // std::cout<<"camera image received"<image.rows; i++) 735 | // { 736 | // for(int j = 0; j < cv_ptr->image.cols; j++) 737 | // { 738 | // cv::Vec4b& pixel = cv_ptr->image.at(i,j); 739 | // pixel[3] = image_alpha_property_->getFloat()*255; 740 | // } 741 | // } 742 | 743 | // add completely white transparent border to the image so that it won't replicate colored pixels all over the mesh 744 | cv::Scalar value(255, 255, 255, 0); 745 | cv::copyMakeBorder(cv_ptr->image, cv_ptr->image, 1, 1, 1, 1, cv::BORDER_CONSTANT, value); 746 | cv::flip(cv_ptr->image, cv_ptr->image, -1); 747 | 748 | // Output modified video stream 749 | if (textures_ == NULL) 750 | textures_ = new ROSImageTexture(); 751 | 752 | textures_->addMessage(cv_ptr->toImageMsg()); 753 | } 754 | 755 | } // namespace rviz 756 | 757 | #include 758 | PLUGINLIB_EXPORT_CLASS(rviz::MeshDisplayCustom, rviz::Display) 759 | -------------------------------------------------------------------------------- /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 rospkg 12 | import tf 13 | import math 14 | 15 | # TODO(lucasw) these no longer exist, demo.launch supersedes this test 16 | # is there any reason to keep this around? 17 | # from rviz_textured_quads.msg import TexturedQuad, TexturedQuadArray 18 | 19 | 20 | def pub_image(): 21 | 22 | rospy.init_node('rviz_display_image_test', anonymous=True) 23 | rospack = rospkg.RosPack() 24 | 25 | image_pub = rospy.Publisher("/targets", Image, queue_size=10) 26 | 27 | texture_path = rospack.get_path('rviz_textured_quads') + '/tests/textures/' 28 | img1 = cv2.imread(texture_path + 'bebop_drone.jpg', cv2.IMREAD_COLOR) 29 | img_msg1 = CvBridge().cv2_to_imgmsg(img1, "bgr8") 30 | 31 | img2 = cv2.imread(texture_path + 'Decal.png', cv2.IMREAD_COLOR) 32 | img_msg2 = CvBridge().cv2_to_imgmsg(img2, "bgr8") 33 | 34 | rate = rospy.Rate(30) 35 | 36 | while not rospy.is_shutdown(): 37 | image_pub.publish(img_msg1) 38 | rate.sleep() 39 | 40 | 41 | if __name__ == '__main__': 42 | try: 43 | pub_image() 44 | except rospy.ROSInterruptException: 45 | pass 46 | -------------------------------------------------------------------------------- /tests/textures/Decal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasw/rviz_textured_quads/6e836abc2cdce9c7b832462a432767ffbf451a4d/tests/textures/Decal.png -------------------------------------------------------------------------------- /tests/textures/Decal_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasw/rviz_textured_quads/6e836abc2cdce9c7b832462a432767ffbf451a4d/tests/textures/Decal_filter.png -------------------------------------------------------------------------------- /tests/textures/bebop_drone.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasw/rviz_textured_quads/6e836abc2cdce9c7b832462a432767ffbf451a4d/tests/textures/bebop_drone.jpg -------------------------------------------------------------------------------- /tests/textures/red.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lucasw/rviz_textured_quads/6e836abc2cdce9c7b832462a432767ffbf451a4d/tests/textures/red.jpg --------------------------------------------------------------------------------