├── .github └── workflows │ ├── main.yml │ └── ubuntu_20_04.yml ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── rviz_camera_stream.rviz ├── include └── rviz_camera_stream │ └── camera_display.h ├── launch └── demo.launch ├── package.xml ├── plugin_description.xml └── src └── camera_display.cpp /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: ROS 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 libyaml-cpp-dev 30 | sudo apt-get install -y qt5-default 31 | sudo apt-get install -y ros-cmake-modules 32 | # sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager 33 | # sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge 34 | # sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure 35 | # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure 36 | sudo apt-get install -y ros-$ROS_DISTRO-image-transport 37 | # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core 38 | # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions 39 | # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros 40 | sudo apt-get install -y ros-$ROS_DISTRO-pluginlib 41 | sudo apt-get install -y ros-$ROS_DISTRO-roscpp 42 | sudo apt-get install -y ros-$ROS_DISTRO-roslint 43 | sudo apt-get install -y ros-$ROS_DISTRO-rospy 44 | sudo apt-get install -y ros-$ROS_DISTRO-rostest 45 | sudo apt-get install -y ros-$ROS_DISTRO-rviz 46 | # sudo apt-get install -y ros-$ROS_DISTRO-shape-msgs 47 | sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs 48 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs 49 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs 50 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-py 51 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros 52 | # sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs 53 | # sudo apt-get install -y ros-$ROS_DISTRO-visualization-msgs 54 | source /opt/ros/$ROS_DISTRO/setup.bash 55 | # Prepare rosdep to install dependencies. 56 | sudo rosdep init 57 | rosdep update --include-eol-distros # Support EOL distros. 58 | 59 | - name: build 60 | run: | 61 | source /opt/ros/$ROS_DISTRO/setup.bash 62 | mkdir -p ~/catkin_ws/src 63 | cd ~/catkin_ws 64 | catkin build 65 | source devel/setup.bash 66 | # echo "::warning $CI_SOURCE_PATH" 67 | # echo "::warning `ls -l`" 68 | cd src 69 | ln -s ~/work # $CI_SOURCE_PATH 70 | cd .. 71 | catkin build 72 | - name: lint 73 | run: | 74 | cd ~/catkin_ws 75 | catkin build rviz_camera_stream --no-deps --catkin-make-args roslint 76 | -------------------------------------------------------------------------------- /.github/workflows/ubuntu_20_04.yml: -------------------------------------------------------------------------------- 1 | name: ROS 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 | - name: git clone git@github.com:lucasw/rviz_camera_stream 20 | uses: actions/checkout@v2 21 | with: 22 | path: catkin_ws/src/rviz_camera_stream 23 | 24 | - name: Install ROS 25 | run: | 26 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 27 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 28 | sudo apt-get update -qq 29 | sudo apt-get install dpkg 30 | sudo apt-get install -y catkin-lint 31 | sudo apt-get install -y python3-catkin-pkg 32 | sudo apt-get install -y python3-catkin-tools 33 | sudo apt-get install -y python3-osrf-pycommon 34 | sudo apt-get install -y python3-rosdep 35 | sudo apt-get install -y python3-wstool 36 | sudo apt-get install -y libboost-dev 37 | sudo apt-get install -y libyaml-cpp-dev 38 | sudo apt-get install -y qt5-default 39 | sudo apt-get install -y ros-cmake-modules 40 | sudo apt-get install -y ros-$ROS_DISTRO-ros-base 41 | source /opt/ros/$ROS_DISTRO/setup.bash 42 | # Prepare rosdep to install dependencies. 43 | sudo rosdep init 44 | rosdep update --include-eol-distros # Support EOL distros. 45 | 46 | - name: rosdep install additional ros dependencies 47 | run: | 48 | source /opt/ros/$ROS_DISTRO/setup.bash 49 | cd catkin_ws 50 | rosdep install --from-paths src --ignore-src -r -s # do a dry-run first 51 | rosdep install --from-paths src --ignore-src -r -y 52 | 53 | - name: build 54 | run: | 55 | source /opt/ros/$ROS_DISTRO/setup.bash 56 | cd catkin_ws 57 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -Wno-deprecated 58 | catkin build --no-status 59 | source devel/setup.bash 60 | 61 | - name: lint 62 | run: | 63 | cd catkin_ws 64 | source devel/setup.bash 65 | catkin build rviz_camera_stream --no-status --no-deps --catkin-make-args roslint 66 | catkin_lint --pkg rviz_camera_stream -W2 --error unsorted_list --ignore uninstalled_script --error launch_depend --error description_meaningless 67 | 68 | - name: build install version 69 | run: | 70 | source /opt/ros/$ROS_DISTRO/setup.bash 71 | cd catkin_ws 72 | rm -rf build devel logs 73 | catkin config --install --cmake-args -DCMAKE_BUILD_TYPE=Release -Wno-deprecated 74 | catkin build --no-status 75 | source install/setup.bash 76 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Build files 2 | msg_gen/ 3 | srv_gen/ 4 | build/ 5 | doc/ 6 | bin/ 7 | lib/ 8 | cmake_install.cmake 9 | _gtest_from_src/ 10 | #QT 11 | CMakeLists.txt.user 12 | qtcreator-build/ 13 | 14 | 15 | # Dynamic reconfigure 16 | docs/ 17 | *.cfgc 18 | **/cfg/cpp/ 19 | **/src/*/cfg/ 20 | 21 | # Eclipse project 22 | .project 23 | .cproject 24 | .pydevproject 25 | .settings 26 | 27 | # Python 28 | *.pyc 29 | *.pkl 30 | 31 | 32 | # Temporary files 33 | *.swp 34 | *~ 35 | 36 | # Mac os 37 | .DS_Store 38 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rviz_camera_stream 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Added github action 8 | * Cleanup according to roslint and catkin_lint 9 | * Modifications by Lucas Walter 10 | * initial: adds a copy of rviz Camera plugin 11 | * Contributors: Florent Lamiraux, John Wason, Julian, Lucas Walter, Mikhail Medvedev, Nikolas Engelhard, Thibaud Lasguignes 12 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rviz_camera_stream) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | image_transport 6 | roscpp 7 | roslint 8 | rviz 9 | sensor_msgs 10 | ) 11 | 12 | roslint_cpp() 13 | ## Compiler flags 14 | # if(CMAKE_COMPILER_IS_GNUCXX) 15 | # set(CMAKE_CXX_FLAGS "-std=c++0x") ## Enable C++11 16 | # endif() 17 | # set(CMAKE_CXX_FLAGS "-Wall -Werror ${CMAKE_CXX_FLAGS}") 18 | set(CMAKE_CXX_FLAGS "-Wall ${CMAKE_CXX_FLAGS}") 19 | 20 | ## This plugin includes Qt widgets, so we must include Qt. 21 | ## We'll use the version that rviz used so they are compatible. 22 | if(rviz_QT_VERSION VERSION_LESS "5") 23 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 24 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 25 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 26 | include(${QT_USE_FILE}) 27 | else() 28 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 29 | # find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 30 | find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) 31 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 32 | set(QT_LIBRARIES Qt5::Widgets) 33 | endif() 34 | 35 | ## Avoid defining "emit", "slots", 36 | ## etc for Qt signals because they can conflict with boost signals 37 | add_definitions(-DQT_NO_KEYWORDS) 38 | 39 | catkin_package( 40 | CATKIN_DEPENDS 41 | roscpp 42 | rviz 43 | ) 44 | 45 | include_directories( 46 | include 47 | ${catkin_INCLUDE_DIRS} 48 | ) 49 | 50 | if(rviz_QT_VERSION VERSION_LESS "5") 51 | qt4_wrap_cpp(MOC_FILES 52 | include/${PROJECT_NAME}/camera_display.h 53 | ) 54 | else() 55 | qt5_wrap_cpp(MOC_FILES 56 | include/${PROJECT_NAME}/camera_display.h 57 | ) 58 | endif() 59 | 60 | add_library(${PROJECT_NAME} 61 | src/camera_display.cpp 62 | ${MOC_FILES} 63 | ) 64 | 65 | target_link_libraries(${PROJECT_NAME} 66 | ${catkin_LIBRARIES} 67 | ${QT_LIBRARIES} 68 | ) 69 | 70 | # install 71 | install (TARGETS ${PROJECT_NAME} 72 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 74 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 75 | ) 76 | 77 | install(FILES 78 | plugin_description.xml 79 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 80 | ) 81 | 82 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 83 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 84 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009, Willow Garage, Inc. 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 | 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the Willow Garage, Inc. nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rviz_camera_stream 2 | 3 | Custom rviz camera plugin that publishes rendered camera video stream 4 | -------------------------------------------------------------------------------- /config/rviz_camera_stream.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 242 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: Image1 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 | - Class: rviz/Axes 51 | Enabled: true 52 | Length: 1 53 | Name: Axes 54 | Radius: 0.100000001 55 | Reference Frame: 56 | Value: true 57 | - Class: rviz/TF 58 | Enabled: true 59 | Frame Timeout: 15 60 | Frames: 61 | All Enabled: true 62 | camera1: 63 | Value: true 64 | camera2: 65 | Value: true 66 | map: 67 | Value: true 68 | Marker Scale: 1 69 | Name: TF 70 | Show Arrows: true 71 | Show Axes: true 72 | Show Names: true 73 | Tree: 74 | map: 75 | camera1: 76 | {} 77 | camera2: 78 | {} 79 | Update Interval: 0 80 | Value: true 81 | - Camera Info Topic: /camera1/camera_info 82 | Class: rviz_camera_stream/CameraPub 83 | Enabled: true 84 | Frame Rate: -1 85 | Image Topic: /rviz1/camera1/image 86 | Name: CameraPub1 87 | Queue Size: 2 88 | Value: true 89 | Visibility: 90 | Axes: true 91 | CameraPub2: true 92 | Grid: true 93 | Image1: true 94 | Image2: true 95 | TF: true 96 | Value: true 97 | - Class: rviz/Image 98 | Enabled: true 99 | Image Topic: /rviz1/camera1/image 100 | Max Value: 1 101 | Median window: 5 102 | Min Value: 0 103 | Name: Image1 104 | Normalize Range: true 105 | Queue Size: 2 106 | Transport Hint: raw 107 | Unreliable: false 108 | Value: true 109 | - Camera Info Topic: /camera2/camera_info 110 | Class: rviz_camera_stream/CameraPub 111 | Enabled: true 112 | Frame Rate: -1 113 | Image Topic: /rviz1/camera2/image 114 | Name: CameraPub2 115 | Queue Size: 2 116 | Value: true 117 | Visibility: 118 | Axes: true 119 | CameraPub1: true 120 | Grid: true 121 | Image1: true 122 | Image2: true 123 | TF: true 124 | Value: true 125 | - Class: rviz/Image 126 | Enabled: true 127 | Image Topic: /rviz1/camera2/image 128 | Max Value: 1 129 | Median window: 5 130 | Min Value: 0 131 | Name: Image2 132 | Normalize Range: true 133 | Queue Size: 2 134 | Transport Hint: raw 135 | Unreliable: false 136 | Value: true 137 | Enabled: true 138 | Global Options: 139 | Background Color: 48; 48; 90 140 | Fixed Frame: map 141 | Frame Rate: 30 142 | Name: root 143 | Tools: 144 | - Class: rviz/Interact 145 | Hide Inactive Objects: true 146 | - Class: rviz/MoveCamera 147 | - Class: rviz/Select 148 | - Class: rviz/FocusCamera 149 | - Class: rviz/Measure 150 | - Class: rviz/SetInitialPose 151 | Topic: /initialpose 152 | - Class: rviz/SetGoal 153 | Topic: /move_base_simple/goal 154 | - Class: rviz/PublishPoint 155 | Single click: true 156 | Topic: /clicked_point 157 | Value: true 158 | Views: 159 | Current: 160 | Class: rviz/Orbit 161 | Distance: 1.62782335 162 | Enable Stereo Rendering: 163 | Stereo Eye Separation: 0.0599999987 164 | Stereo Focal Distance: 1 165 | Swap Stereo Eyes: false 166 | Value: false 167 | Focal Point: 168 | X: 0 169 | Y: 0 170 | Z: 0 171 | Name: Current View 172 | Near Clip Distance: 0.00999999978 173 | Pitch: -0.794796288 174 | Target Frame: 175 | Value: Orbit (rviz) 176 | Yaw: 4.71858883 177 | Saved: ~ 178 | Window Geometry: 179 | Displays: 180 | collapsed: false 181 | Height: 1003 182 | Hide Left Dock: false 183 | Hide Right Dock: true 184 | Image1: 185 | collapsed: false 186 | Image2: 187 | collapsed: false 188 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000348fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000181000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002d6000000160000000000000000fb0000001200430061006d0065007200610050007500620100000199000000920000000000000000fb0000001200430061006d006500720061005000750062010000022b000000c10000000000000000fb0000001200430061006d006500720061005000750062010000022b000000c10000000000000000fb0000000c0049006d006100670065003101000001c8000000c40000001600fffffffb0000000c0049006d00610067006500320100000292000000f70000001600ffffff000000010000010f00000348fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000348000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f80000003efc0100000002fb0000000800540069006d00650100000000000004f80000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000034800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 189 | Selection: 190 | collapsed: false 191 | Time: 192 | collapsed: false 193 | Tool Properties: 194 | collapsed: false 195 | Views: 196 | collapsed: true 197 | Width: 1272 198 | X: 78 199 | Y: 24 200 | -------------------------------------------------------------------------------- /include/rviz_camera_stream/camera_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef RVIZ_CAMERA_STREAM_CAMERA_DISPLAY_H 31 | #define RVIZ_CAMERA_STREAM_CAMERA_DISPLAY_H 32 | 33 | #include 34 | #include 35 | 36 | #ifndef Q_MOC_RUN 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | # include 43 | 44 | # include "rviz/image/image_display_base.h" 45 | #include 46 | #endif 47 | 48 | namespace Ogre 49 | { 50 | class SceneNode; 51 | class ManualObject; 52 | class Rectangle2D; 53 | class Camera; 54 | } 55 | 56 | namespace video_export 57 | { 58 | class VideoPublisher; 59 | } 60 | 61 | namespace rviz 62 | { 63 | 64 | class EnumProperty; 65 | class FloatProperty; 66 | class IntProperty; 67 | class RenderPanel; 68 | class RosTopicProperty; 69 | class DisplayGroupVisibilityProperty; 70 | class ColorProperty; 71 | 72 | /** 73 | * \class CameraPub 74 | * 75 | */ 76 | class CameraPub: public Display, public Ogre::RenderTargetListener 77 | { 78 | Q_OBJECT 79 | public: 80 | CameraPub(); 81 | virtual ~CameraPub(); 82 | 83 | // Overrides from Display 84 | virtual void onInitialize(); 85 | virtual void fixedFrameChanged(); 86 | virtual void update(float wall_dt, float ros_dt); 87 | virtual void reset(); 88 | 89 | // Overrides from Ogre::RenderTargetListener 90 | virtual void preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt); 91 | virtual void postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt); 92 | 93 | static const QString BACKGROUND; 94 | static const QString OVERLAY; 95 | static const QString BOTH; 96 | 97 | protected: 98 | // overrides from Display 99 | virtual void onEnable(); 100 | virtual void onDisable(); 101 | 102 | private Q_SLOTS: 103 | void forceRender(); 104 | 105 | void updateTopic(); 106 | virtual void updateQueueSize(); 107 | virtual void updateFrameRate(); 108 | virtual void updateBackgroundColor(); 109 | virtual void updateDisplayNamespace(); 110 | virtual void updateImageEncoding(); 111 | virtual void updateNearClipDistance(); 112 | 113 | private: 114 | std::string camera_trigger_name_; 115 | ros::NodeHandle nh_; 116 | 117 | void subscribe(); 118 | void unsubscribe(); 119 | 120 | ros::ServiceServer trigger_service_; 121 | bool triggerCallback(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); 122 | bool trigger_activated_ = false; 123 | ros::Time last_image_publication_time_ = ros::Time(0); 124 | 125 | void caminfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg); 126 | 127 | bool updateCamera(); 128 | 129 | void clear(); 130 | void updateStatus(); 131 | 132 | ros::Subscriber caminfo_sub_; 133 | 134 | RosTopicProperty* topic_property_; 135 | RosTopicProperty* camera_info_property_; 136 | DisplayGroupVisibilityProperty* visibility_property_; 137 | IntProperty* queue_size_property_; 138 | StringProperty* namespace_property_; 139 | 140 | FloatProperty* frame_rate_property_; 141 | ColorProperty* background_color_property_; 142 | EnumProperty* image_encoding_property_; 143 | FloatProperty* near_clip_property_; 144 | 145 | sensor_msgs::CameraInfo::ConstPtr current_caminfo_; 146 | boost::mutex caminfo_mutex_; 147 | 148 | bool new_caminfo_ = false; 149 | 150 | bool caminfo_ok_ = false; 151 | 152 | bool force_render_ = false; 153 | 154 | static inline int count_ = 0; 155 | 156 | uint32_t vis_bit_; 157 | 158 | video_export::VideoPublisher* video_publisher_ = nullptr; 159 | 160 | // render to texture 161 | // from http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Intermediate+Tutorial+7 162 | Ogre::Camera* camera_; 163 | Ogre::TexturePtr rtt_texture_; 164 | Ogre::RenderTexture* render_texture_; 165 | }; 166 | 167 | } // namespace rviz 168 | 169 | #endif // RVIZ_CAMERA_STREAM_CAMERA_DISPLAY_H 170 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10 | 11 | 12 | 14 | 25 | 26 | 27 | 28 | 30 | 41 | 42 | 43 | 44 | 45 | - 'image_transport/compressed' 46 | - 'image_transport/compressedDepth' 47 | - 'image_transport/theora' 48 | 49 | 50 | 51 | 52 | 53 | - 'image_transport/compressed' 54 | - 'image_transport/compressedDepth' 55 | - 'image_transport/theora' 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | rviz_camera_stream 3 | 1.0.1 4 | 5 | Publish a camera view as rendered by rviz on an Image topic 6 | 7 | 8 | Lucas Walter 9 | Mikhail Medvedev 10 | 11 | BSD 12 | 13 | https://github.com/lucasw/rviz_camera_stream 14 | 15 | catkin 16 | image_transport 17 | roscpp 18 | roslint 19 | rviz 20 | sensor_msgs 21 | 22 | roscpp 23 | rviz 24 | tf 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Same as standard RViz camera, with addition of the ability to 7 | publish the rendered image to the Image topic. 8 | 9 | sensor_msgs/Image 10 | sensor_msgs/CompressedImage 11 | theora_image_transport/Packet 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /src/camera_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 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 | 62 | #include "rviz_camera_stream/camera_display.h" 63 | 64 | namespace video_export 65 | { 66 | class VideoPublisher 67 | { 68 | private: 69 | ros::NodeHandle nh_; 70 | image_transport::ImageTransport it_; 71 | image_transport::CameraPublisher pub_; 72 | uint image_id_; 73 | public: 74 | sensor_msgs::CameraInfo camera_info_; 75 | VideoPublisher() : 76 | it_(nh_), 77 | image_id_(0) 78 | { 79 | } 80 | 81 | std::string get_topic() 82 | { 83 | return pub_.getTopic(); 84 | } 85 | 86 | bool is_active() 87 | { 88 | return !pub_.getTopic().empty(); 89 | } 90 | 91 | void setNodehandle(const ros::NodeHandle& nh) 92 | { 93 | shutdown(); 94 | nh_ = nh; 95 | it_ = image_transport::ImageTransport(nh_); 96 | } 97 | 98 | void shutdown() 99 | { 100 | if (pub_.getTopic() != "") 101 | { 102 | pub_.shutdown(); 103 | } 104 | } 105 | 106 | void advertise(std::string topic) 107 | { 108 | pub_ = it_.advertiseCamera(topic, 1); 109 | } 110 | 111 | // bool publishFrame(Ogre::RenderWindow * render_object, const std::string frame_id) 112 | bool publishFrame(Ogre::RenderTexture * render_object, const std::string frame_id, int encoding_option) 113 | { 114 | if (pub_.getTopic() == "") 115 | { 116 | return false; 117 | } 118 | if (frame_id == "") 119 | { 120 | return false; 121 | } 122 | // RenderTarget::writeContentsToFile() used as example 123 | // TODO(lucasw) make things const that can be 124 | int height = render_object->getHeight(); 125 | int width = render_object->getWidth(); 126 | // the suggested pixel format is most efficient, but other ones 127 | // can be used. 128 | sensor_msgs::Image image; 129 | Ogre::PixelFormat pf = Ogre::PF_BYTE_RGB; 130 | switch (encoding_option) 131 | { 132 | case 0: 133 | pf = Ogre::PF_BYTE_RGB; 134 | image.encoding = sensor_msgs::image_encodings::RGB8; 135 | break; 136 | case 1: 137 | pf = Ogre::PF_BYTE_RGBA; 138 | image.encoding = sensor_msgs::image_encodings::RGBA8; 139 | break; 140 | case 2: 141 | pf = Ogre::PF_BYTE_BGR; 142 | image.encoding = sensor_msgs::image_encodings::BGR8; 143 | break; 144 | case 3: 145 | pf = Ogre::PF_BYTE_BGRA; 146 | image.encoding = sensor_msgs::image_encodings::BGRA8; 147 | break; 148 | case 4: 149 | pf = Ogre::PF_L8; 150 | image.encoding = sensor_msgs::image_encodings::MONO8; 151 | break; 152 | case 5: 153 | pf = Ogre::PF_L16; 154 | image.encoding = sensor_msgs::image_encodings::MONO16; 155 | break; 156 | default: 157 | ROS_ERROR_STREAM("Invalid image encoding value specified"); 158 | return false; 159 | } 160 | 161 | uint pixelsize = Ogre::PixelUtil::getNumElemBytes(pf); 162 | uint datasize = width * height * pixelsize; 163 | 164 | // 1.05 multiplier is to avoid crash when the window is resized. 165 | // There should be a better solution. 166 | uchar *data = OGRE_ALLOC_T(uchar, static_cast(datasize * 1.05), Ogre::MEMCATEGORY_RENDERSYS); 167 | Ogre::PixelBox pb(width, height, 1, pf, data); 168 | render_object->copyContentsToMemory(pb, Ogre::RenderTarget::FB_AUTO); 169 | 170 | 171 | image.header.stamp = ros::Time::now(); 172 | image.header.seq = image_id_++; 173 | image.header.frame_id = frame_id; 174 | image.height = height; 175 | image.width = width; 176 | image.step = pixelsize * width; 177 | image.is_bigendian = (OGRE_ENDIAN == OGRE_ENDIAN_BIG); 178 | image.data.resize(datasize); 179 | memcpy(&image.data[0], data, datasize); 180 | camera_info_.header = image.header; 181 | pub_.publish(image, camera_info_); 182 | 183 | OGRE_FREE(data, Ogre::MEMCATEGORY_RENDERSYS); 184 | return true; 185 | } 186 | }; 187 | } // namespace video_export 188 | 189 | 190 | namespace rviz 191 | { 192 | 193 | const QString CameraPub::BACKGROUND("background"); 194 | const QString CameraPub::OVERLAY("overlay"); 195 | const QString CameraPub::BOTH("background and overlay"); 196 | 197 | bool validateFloats(const sensor_msgs::CameraInfo& msg) 198 | { 199 | bool valid = true; 200 | valid = valid && validateFloats(msg.D); 201 | valid = valid && validateFloats(msg.K); 202 | valid = valid && validateFloats(msg.R); 203 | valid = valid && validateFloats(msg.P); 204 | return valid; 205 | } 206 | 207 | CameraPub::CameraPub() 208 | : Display() 209 | , camera_trigger_name_("camera_trigger") 210 | , nh_() 211 | { 212 | topic_property_ = new RosTopicProperty("Image Topic", "", 213 | QString::fromStdString(ros::message_traits::datatype()), 214 | "sensor_msgs::Image topic to publish to.", this, SLOT(updateTopic())); 215 | 216 | namespace_property_ = new StringProperty("Display namespace", "", 217 | "Namespace for this display.", this, SLOT(updateDisplayNamespace())); 218 | 219 | camera_info_property_ = new RosTopicProperty("Camera Info Topic", "", 220 | QString::fromStdString(ros::message_traits::datatype()), 221 | "sensor_msgs::CameraInfo topic to subscribe to.", this, SLOT(updateTopic())); 222 | 223 | queue_size_property_ = new IntProperty( "Queue Size", 2, 224 | "Advanced: set the size of the incoming message queue. Increasing this " 225 | "is useful if your incoming TF data is delayed significantly from your" 226 | " image data, but it can greatly increase memory usage if the messages are big.", 227 | this, SLOT(updateQueueSize())); 228 | queue_size_property_->setMin(1); 229 | 230 | frame_rate_property_ = new FloatProperty("Frame Rate", -1, 231 | "Sets target frame rate. Set to < 0 for maximum speed, set to 0 to stop, you can " 232 | "trigger single images with the /rviz_camera_trigger service.", 233 | this, SLOT(updateFrameRate())); 234 | frame_rate_property_->setMin(-1); 235 | 236 | background_color_property_ = new ColorProperty("Background Color", Qt::black, 237 | "Sets background color, values from 0.0 to 1.0.", 238 | this, SLOT(updateBackgroundColor())); 239 | 240 | image_encoding_property_ = new EnumProperty("Image Encoding", "rgb8", 241 | "Sets the image encoding", this, SLOT(updateImageEncoding())); 242 | image_encoding_property_->addOption("rgb8", 0); 243 | image_encoding_property_->addOption("rgba8", 1); 244 | image_encoding_property_->addOption("bgr8", 2); 245 | image_encoding_property_->addOption("bgra8", 3); 246 | image_encoding_property_->addOption("mono8", 4); 247 | image_encoding_property_->addOption("mono16", 5); 248 | 249 | near_clip_property_ = new FloatProperty("Near Clip Distance", 0.01, "Set the near clip distance", 250 | this, SLOT(updateNearClipDistance())); 251 | near_clip_property_->setMin(0.01); 252 | } 253 | 254 | CameraPub::~CameraPub() 255 | { 256 | if (initialized()) 257 | { 258 | render_texture_->removeListener(this); 259 | 260 | unsubscribe(); 261 | 262 | context_->visibilityBits()->freeBits(vis_bit_); 263 | } 264 | } 265 | 266 | bool CameraPub::triggerCallback(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) 267 | { 268 | res.success = video_publisher_->is_active(); 269 | if (res.success) 270 | { 271 | trigger_activated_ = true; 272 | res.message = "New image will be published on: " + video_publisher_->get_topic(); 273 | } 274 | else 275 | { 276 | res.message = "Image publisher not configured"; 277 | } 278 | return true; 279 | } 280 | 281 | void CameraPub::onInitialize() 282 | { 283 | Display::onInitialize(); 284 | 285 | count_++; 286 | 287 | video_publisher_ = new video_export::VideoPublisher(); 288 | 289 | std::stringstream ss; 290 | ss << "RvizCameraPubCamera" << count_; 291 | camera_ = context_->getSceneManager()->createCamera(ss.str()); 292 | 293 | std::stringstream ss_tex; 294 | ss_tex << "RttTexInit" << count_; 295 | ROS_INFO_STREAM(ss_tex.str()); 296 | 297 | // render to texture 298 | rtt_texture_ = Ogre::TextureManager::getSingleton().createManual( 299 | ss_tex.str(), 300 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 301 | Ogre::TEX_TYPE_2D, 302 | 640, 480, 303 | 0, 304 | Ogre::PF_R8G8B8, 305 | Ogre::TU_RENDERTARGET); 306 | render_texture_ = rtt_texture_->getBuffer()->getRenderTarget(); 307 | render_texture_->addViewport(camera_); 308 | render_texture_->getViewport(0)->setClearEveryFrame(true); 309 | render_texture_->getViewport(0)->setBackgroundColour(Ogre::ColourValue::Black); 310 | render_texture_->getViewport(0)->setOverlaysEnabled(false); 311 | render_texture_->setAutoUpdated(false); 312 | render_texture_->setActive(false); 313 | render_texture_->addListener(this); 314 | 315 | camera_->setNearClipDistance(0.01f); 316 | camera_->setPosition(0, 10, 15); 317 | camera_->lookAt(0, 0, 0); 318 | 319 | // Thought this was optional but the plugin crashes without it 320 | vis_bit_ = context_->visibilityBits()->allocBit(); 321 | render_texture_->getViewport(0)->setVisibilityMask(vis_bit_); 322 | 323 | visibility_property_ = new DisplayGroupVisibilityProperty( 324 | vis_bit_, context_->getRootDisplayGroup(), this, "Visibility", true, 325 | "Changes the visibility of other Displays in the camera view."); 326 | 327 | visibility_property_->setIcon(loadPixmap("package://rviz/icons/visibility.svg", true)); 328 | 329 | this->addChild(visibility_property_, 0); 330 | updateDisplayNamespace(); 331 | } 332 | 333 | void CameraPub::updateTopic() 334 | { 335 | unsubscribe(); 336 | reset(); 337 | subscribe(); 338 | context_->queueRender(); 339 | } 340 | 341 | void CameraPub::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt) 342 | { 343 | // set view flags on all displays 344 | visibility_property_->update(); 345 | } 346 | 347 | void CameraPub::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt) 348 | { 349 | // Publish the rendered window video stream 350 | const ros::Time cur_time = ros::Time::now(); 351 | ros::Duration elapsed_duration = cur_time - last_image_publication_time_; 352 | const float frame_rate = frame_rate_property_->getFloat(); 353 | bool time_is_up = (frame_rate > 0.0) && (elapsed_duration.toSec() > 1.0 / frame_rate); 354 | // We want frame rate to be unlimited if we enter zero or negative values for frame rate 355 | if (frame_rate < 0.0) 356 | { 357 | time_is_up = true; 358 | } 359 | if (!(trigger_activated_ || time_is_up)) 360 | { 361 | return; 362 | } 363 | trigger_activated_ = false; 364 | last_image_publication_time_ = cur_time; 365 | render_texture_->getViewport(0)->setBackgroundColour(background_color_property_->getOgreColor()); 366 | 367 | std::string frame_id; 368 | { 369 | boost::mutex::scoped_lock lock(caminfo_mutex_); 370 | if (!current_caminfo_) 371 | return; 372 | frame_id = current_caminfo_->header.frame_id; 373 | video_publisher_->camera_info_ = *current_caminfo_; 374 | } 375 | 376 | int encoding_option = image_encoding_property_->getOptionInt(); 377 | 378 | // render_texture_->update(); 379 | video_publisher_->publishFrame(render_texture_, frame_id, encoding_option); 380 | } 381 | 382 | void CameraPub::onEnable() 383 | { 384 | subscribe(); 385 | render_texture_->setActive(true); 386 | } 387 | 388 | void CameraPub::onDisable() 389 | { 390 | render_texture_->setActive(false); 391 | unsubscribe(); 392 | clear(); 393 | } 394 | 395 | void CameraPub::subscribe() 396 | { 397 | if (!isEnabled()) 398 | return; 399 | 400 | std::string topic_name = topic_property_->getTopicStd(); 401 | if (topic_name.empty()) 402 | { 403 | setStatus(StatusProperty::Error, "Output Topic", "No topic set"); 404 | return; 405 | } 406 | 407 | std::string error; 408 | if (!ros::names::validate(topic_name, error)) 409 | { 410 | setStatus(StatusProperty::Error, "Output Topic", QString(error.c_str())); 411 | return; 412 | } 413 | 414 | 415 | std::string caminfo_topic = camera_info_property_->getTopicStd(); 416 | if (caminfo_topic.empty()) 417 | { 418 | setStatus(StatusProperty::Error, "Camera Info", "No topic set"); 419 | return; 420 | } 421 | 422 | // std::string target_frame = fixed_frame_.toStdString(); 423 | // Display::enableTFFilter(target_frame); 424 | 425 | 426 | try 427 | { 428 | // caminfo_sub_.subscribe(update_nh_, caminfo_topic, 1); 429 | caminfo_sub_ = update_nh_.subscribe(caminfo_topic, 1, &CameraPub::caminfoCallback, this); 430 | setStatus(StatusProperty::Ok, "Camera Info", "OK"); 431 | } 432 | catch (ros::Exception& e) 433 | { 434 | setStatus(StatusProperty::Error, "Camera Info", QString("Error subscribing: ") + e.what()); 435 | } 436 | 437 | video_publisher_->advertise(topic_name); 438 | setStatus(StatusProperty::Ok, "Output Topic", "Topic set"); 439 | } 440 | 441 | void CameraPub::unsubscribe() 442 | { 443 | video_publisher_->shutdown(); 444 | caminfo_sub_.shutdown(); 445 | } 446 | 447 | void CameraPub::forceRender() 448 | { 449 | force_render_ = true; 450 | context_->queueRender(); 451 | } 452 | 453 | void CameraPub::updateQueueSize() 454 | { 455 | } 456 | 457 | void CameraPub::updateFrameRate() 458 | { 459 | } 460 | 461 | void CameraPub::updateNearClipDistance() 462 | { 463 | } 464 | 465 | 466 | void CameraPub::updateBackgroundColor() 467 | { 468 | } 469 | 470 | void CameraPub::updateDisplayNamespace() 471 | { 472 | std::string name = namespace_property_->getStdString(); 473 | 474 | try 475 | { 476 | nh_ = ros::NodeHandle(name); 477 | } 478 | catch (ros::InvalidNameException& e) 479 | { 480 | setStatus(StatusProperty::Warn, "Display namespace", "Invalid namespace: " + QString(e.what())); 481 | ROS_ERROR("%s", e.what()); 482 | return; 483 | } 484 | 485 | video_publisher_->setNodehandle(nh_); 486 | 487 | // ROS_INFO("New namespace: '%s'", nh_.getNamespace().c_str()); 488 | trigger_service_.shutdown(); 489 | trigger_service_ = nh_.advertiseService(camera_trigger_name_, &CameraPub::triggerCallback, this); 490 | 491 | /// Check for service name collision 492 | if (trigger_service_.getService().empty()) 493 | { 494 | setStatus(StatusProperty::Warn, "Display namespace", 495 | "Could not create trigger. Make sure that display namespace is unique!"); 496 | return; 497 | } 498 | 499 | setStatus(StatusProperty::Ok, "Display namespace", "OK"); 500 | updateTopic(); 501 | } 502 | 503 | void CameraPub::updateImageEncoding() 504 | { 505 | } 506 | 507 | void CameraPub::clear() 508 | { 509 | force_render_ = true; 510 | context_->queueRender(); 511 | 512 | new_caminfo_ = false; 513 | current_caminfo_.reset(); 514 | 515 | setStatus(StatusProperty::Warn, "Camera Info", 516 | "No CameraInfo received on [" + 517 | QString::fromStdString(caminfo_sub_.getTopic()) + 518 | "]. Topic may not exist."); 519 | // setStatus(StatusProperty::Warn, "Camera Info", "No CameraInfo received"); 520 | 521 | camera_->setPosition(Ogre::Vector3(999999, 999999, 999999)); 522 | } 523 | 524 | void CameraPub::update(float wall_dt, float ros_dt) 525 | { 526 | #if 0 527 | try 528 | { 529 | #endif 530 | { 531 | caminfo_ok_ = updateCamera(); 532 | force_render_ = false; 533 | } 534 | 535 | #if 0 536 | } 537 | catch (UnsupportedImageEncoding& e) 538 | { 539 | setStatus(StatusProperty::Error, "Image", e.what()); 540 | } 541 | #endif 542 | 543 | if (caminfo_sub_.getNumPublishers() == 0) 544 | { 545 | setStatus(StatusProperty::Warn, "Camera Info", 546 | "No publishers on [" + 547 | QString::fromStdString(caminfo_sub_.getTopic()) + 548 | "]. Topic may not exist."); 549 | } 550 | render_texture_->update(); 551 | } 552 | 553 | bool CameraPub::updateCamera() 554 | { 555 | sensor_msgs::CameraInfo::ConstPtr info; 556 | { 557 | boost::mutex::scoped_lock lock(caminfo_mutex_); 558 | info = current_caminfo_; 559 | } 560 | 561 | if (!info) 562 | { 563 | return false; 564 | } 565 | 566 | if (!validateFloats(*info)) 567 | { 568 | setStatus(StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)"); 569 | return false; 570 | } 571 | 572 | // if we're in 'exact' time mode, only show image if the time is exactly right 573 | ros::Time rviz_time = context_->getFrameManager()->getTime(); 574 | if (context_->getFrameManager()->getSyncMode() == FrameManager::SyncExact && 575 | rviz_time != info->header.stamp) 576 | { 577 | std::ostringstream s; 578 | s << "Time-syncing active and no info at timestamp " << rviz_time.toSec() << "."; 579 | setStatus(StatusProperty::Warn, "Time", s.str().c_str()); 580 | return false; 581 | } 582 | 583 | // TODO(lucasw) this will make the img vs. texture size code below unnecessary 584 | if ((info->width != render_texture_->getWidth()) || 585 | (info->height != render_texture_->getHeight())) 586 | { 587 | std::stringstream ss_tex; 588 | ss_tex << "RttTex" << count_; 589 | ROS_INFO_STREAM(ss_tex.str()); 590 | 591 | rtt_texture_ = Ogre::TextureManager::getSingleton().createManual( 592 | ss_tex.str(), 593 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 594 | Ogre::TEX_TYPE_2D, 595 | info->width, info->height, 596 | 0, 597 | Ogre::PF_R8G8B8, 598 | Ogre::TU_RENDERTARGET); 599 | render_texture_ = rtt_texture_->getBuffer()->getRenderTarget(); 600 | render_texture_->addViewport(camera_); 601 | render_texture_->getViewport(0)->setClearEveryFrame(true); 602 | render_texture_->getViewport(0)->setBackgroundColour(Ogre::ColourValue::Black); 603 | render_texture_->getViewport(0)->setVisibilityMask(vis_bit_); 604 | 605 | render_texture_->getViewport(0)->setOverlaysEnabled(false); 606 | render_texture_->setAutoUpdated(false); 607 | render_texture_->setActive(false); 608 | render_texture_->addListener(this); 609 | } 610 | 611 | Ogre::Vector3 position; 612 | Ogre::Quaternion orientation; 613 | const bool success = context_->getFrameManager()->getTransform( 614 | info->header.frame_id, info->header.stamp, position, orientation); 615 | if (!success) 616 | { 617 | std::string error; 618 | const bool has_problems = context_->getFrameManager()->transformHasProblems( 619 | info->header.frame_id, 620 | info->header.stamp, error); 621 | if (has_problems) 622 | { 623 | setStatus(StatusProperty::Error, "getTransform", error.c_str()); 624 | return false; 625 | } 626 | } 627 | 628 | // printf( "CameraPub:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z ); 629 | 630 | // convert vision (Z-forward) frame to ogre frame (Z-out) 631 | orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X); 632 | 633 | float img_width = info->width; 634 | float img_height = info->height; 635 | 636 | // If the image width is 0 due to a malformed caminfo, try to grab the width from the image. 637 | if (img_width == 0) 638 | { 639 | ROS_DEBUG("Malformed CameraInfo on camera [%s], width = 0", qPrintable(getName())); 640 | img_width = 640; 641 | } 642 | 643 | if (img_height == 0) 644 | { 645 | ROS_DEBUG("Malformed CameraInfo on camera [%s], height = 0", qPrintable(getName())); 646 | img_height = 480; 647 | } 648 | 649 | if (img_height == 0.0 || img_width == 0.0) 650 | { 651 | setStatus(StatusProperty::Error, "Camera Info", 652 | "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)"); 653 | return false; 654 | } 655 | 656 | double fx = info->P[0]; 657 | double fy = info->P[5]; 658 | 659 | float win_width = render_texture_->getWidth(); 660 | float win_height = render_texture_->getHeight(); 661 | float zoom_x = 1.0; 662 | float zoom_y = zoom_x; 663 | 664 | // Preserve aspect ratio 665 | if (win_width != 0 && win_height != 0) 666 | { 667 | float img_aspect = (img_width / fx) / (img_height / fy); 668 | float win_aspect = win_width / win_height; 669 | 670 | if (img_aspect > win_aspect) 671 | { 672 | zoom_y = zoom_y / img_aspect * win_aspect; 673 | } 674 | else 675 | { 676 | zoom_x = zoom_x / win_aspect * img_aspect; 677 | } 678 | } 679 | 680 | // Add the camera's translation relative to the left camera (from P[3]); 681 | double tx = -1 * (info->P[3] / fx); 682 | Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X; 683 | position = position + (right * tx); 684 | 685 | double ty = -1 * (info->P[7] / fy); 686 | Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y; 687 | position = position + (down * ty); 688 | 689 | if (!validateFloats(position)) 690 | { 691 | setStatus(StatusProperty::Error, "Camera Info", 692 | "CameraInfo/P resulted in an invalid position calculation (nans or infs)"); 693 | return false; 694 | } 695 | 696 | const float near_clip_distance = near_clip_property_->getFloat(); 697 | 698 | camera_->setPosition(position); 699 | camera_->setOrientation(orientation); 700 | camera_->setNearClipDistance(near_clip_distance); 701 | 702 | // calculate the projection matrix 703 | double cx = info->P[2]; 704 | double cy = info->P[6]; 705 | 706 | double far_plane = 100; 707 | double near_plane = near_clip_distance; 708 | 709 | Ogre::Matrix4 proj_matrix; 710 | proj_matrix = Ogre::Matrix4::ZERO; 711 | 712 | proj_matrix[0][0] = 2.0 * fx / img_width * zoom_x; 713 | proj_matrix[1][1] = 2.0 * fy / img_height * zoom_y; 714 | 715 | proj_matrix[0][2] = 2.0 * (0.5 - cx / img_width) * zoom_x; 716 | proj_matrix[1][2] = 2.0 * (cy / img_height - 0.5) * zoom_y; 717 | 718 | proj_matrix[2][2] = -(far_plane + near_plane) / (far_plane - near_plane); 719 | proj_matrix[2][3] = -2.0 * far_plane * near_plane / (far_plane - near_plane); 720 | 721 | proj_matrix[3][2] = -1; 722 | 723 | camera_->setCustomProjectionMatrix(true, proj_matrix); 724 | 725 | setStatus(StatusProperty::Ok, "Camera Info", "OK"); 726 | 727 | #if 0 728 | static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01); 729 | debug_axes->setPosition(position); 730 | debug_axes->setOrientation(orientation); 731 | #endif 732 | 733 | setStatus(StatusProperty::Ok, "Time", "ok"); 734 | 735 | return true; 736 | } 737 | 738 | void CameraPub::caminfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg) 739 | { 740 | boost::mutex::scoped_lock lock(caminfo_mutex_); 741 | current_caminfo_ = msg; 742 | new_caminfo_ = true; 743 | } 744 | 745 | void CameraPub::fixedFrameChanged() 746 | { 747 | std::string targetFrame = fixed_frame_.toStdString(); 748 | Display::fixedFrameChanged(); 749 | } 750 | 751 | void CameraPub::reset() 752 | { 753 | Display::reset(); 754 | clear(); 755 | } 756 | 757 | } // namespace rviz 758 | 759 | #include 760 | PLUGINLIB_EXPORT_CLASS(rviz::CameraPub, rviz::Display) 761 | --------------------------------------------------------------------------------