├── CMakeLists.txt ├── README.md ├── doc └── screenshot.png ├── icons └── classes │ └── OculusDisplay.png ├── include └── oculus_rviz_plugins │ ├── fixed_view_controller.h │ ├── oculus_display.h │ └── ogre_oculus.h ├── ogre_media ├── glsl120.program ├── oculus.compositor ├── warp.frag ├── warp.material ├── warp.vert └── warpWithChromeAb.frag ├── package.xml ├── plugin_description.xml └── src ├── fixed_view_controller.cpp ├── oculus_display.cpp └── ogre_oculus.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oculus_rviz_plugins) 3 | 4 | # Qt 5 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 6 | include(${QT_USE_FILE}) 7 | add_definitions(-DQT_NO_KEYWORDS -fPIC) 8 | 9 | # Catkin 10 | find_package(catkin REQUIRED COMPONENTS 11 | rviz 12 | roscpp 13 | tf 14 | ) 15 | 16 | # Oculus SDK 17 | 18 | FIND_PACKAGE( ros_ovr_sdk REQUIRED) 19 | 20 | # Run OVR off of a shared library 21 | 22 | if(NOT ros_ovr_sdk_FOUND) 23 | message("ros_ovr_sdk Library cannot be found.") 24 | endif(NOT ros_ovr_sdk_FOUND) 25 | 26 | # Boost 27 | find_package(Boost REQUIRED) 28 | 29 | catkin_package( 30 | INCLUDE_DIRS include 31 | LIBRARIES oculus_rviz_plugins 32 | CATKIN_DEPENDS rviz 33 | DEPENDS ros_ovr_sdk 34 | ) 35 | 36 | find_package(PkgConfig) 37 | pkg_check_modules(OGRE OGRE) 38 | 39 | include_directories(include 40 | ${catkin_INCLUDE_DIRS} 41 | ${ros_ovr_sdk_INCLUDE_DIRS} 42 | ${OGRE_INCLUDE_DIRS} 43 | ${BOOST_INCLUDE_DIRS} 44 | ) 45 | 46 | # run Qt moc generator 47 | qt4_wrap_cpp(MOC_FILES 48 | include/oculus_rviz_plugins/oculus_display.h 49 | include/oculus_rviz_plugins/fixed_view_controller.h 50 | ) 51 | 52 | add_library(oculus_rviz_plugins 53 | src/ogre_oculus.cpp 54 | src/oculus_display.cpp 55 | src/fixed_view_controller.cpp 56 | ${MOC_FILES} 57 | ) 58 | 59 | target_link_libraries(oculus_rviz_plugins 60 | ${catkin_LIBRARIES} 61 | ${QT_LIBRARIES} 62 | ${ros_ovr_sdk_LIBRARIES} 63 | ${BOOST_LIBRARIES} 64 | ) 65 | 66 | 67 | ############# 68 | ## Install ## 69 | ############# 70 | 71 | install(TARGETS oculus_rviz_plugins 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(DIRECTORY include/${PROJECT_NAME}/ 78 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 79 | FILES_MATCHING PATTERN "*.h" 80 | ) 81 | 82 | install(DIRECTORY ogre_media 83 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 84 | ) 85 | 86 | install(FILES 87 | plugin_description.xml 88 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 89 | ) 90 | 91 | install(DIRECTORY 92 | icons 93 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 94 | ) 95 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | oculus_rviz_plugins 2 | =========== 3 | 4 | This plugin is based on [OgreOculus](https://bitbucket.org/rajetic/ogreoculus) (C++) 5 | and [OsgOculusViewer](https://github.com/bjornblissing/osgoculusviewer) (GLSL shaders). 6 | 7 | Usage Instructions 8 | ================== 9 | 10 | Download the catkin_package [ros_ovr_sdk](https://github.com/OSUrobotics/ros_ovr_sdk.git) into your workspace and run catkin_make. 11 | 12 | Before using the OculusDisplay, run rosrun OculusSDK/build/bin/ovrd in a sourced terminal. 13 | 14 | In RViz, add the "OculusDisplay". This will create an additional window with a stereo rendering 15 | of the contents of the main RViz rendering area. Check "Render to Oculus" to 16 | render in full screen mode on your Oculus headset. It must be set up as secondary screen 17 | for this to work. 18 | 19 | By default, the Oculus view will be rendered from the same position as the main RViz camera while following 20 | your head's orientation. Alternatively, you can attach the camera to a tf frame. 21 | 22 | This is how the Display should look like in windowed mode: 23 | 24 | ![ScreenShot](doc/screenshot.png) 25 | 26 | See [the Wiki](https://github.com/OSUrobotics/oculus_rviz_plugins/wiki) for more details. 27 | -------------------------------------------------------------------------------- /doc/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OSUrobotics/oculus_rviz_plugins/e070cfc0db7fd919d95763f99058ed6076b518ce/doc/screenshot.png -------------------------------------------------------------------------------- /icons/classes/OculusDisplay.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OSUrobotics/oculus_rviz_plugins/e070cfc0db7fd919d95763f99058ed6076b518ce/icons/classes/OculusDisplay.png -------------------------------------------------------------------------------- /include/oculus_rviz_plugins/fixed_view_controller.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_ORBIT_VIEW_CONTROLLER_H 31 | #define RVIZ_ORBIT_VIEW_CONTROLLER_H 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include "rviz/frame_position_tracking_view_controller.h" 38 | 39 | namespace rviz 40 | { 41 | class FloatProperty; 42 | } 43 | 44 | namespace oculus_rviz_plugins 45 | { 46 | 47 | class FixedViewController: public rviz::FramePositionTrackingViewController 48 | { 49 | Q_OBJECT 50 | public: 51 | FixedViewController(); 52 | virtual ~FixedViewController(); 53 | 54 | virtual void onInitialize(); 55 | 56 | virtual void reset(); 57 | 58 | virtual void update(float dt, float ros_dt); 59 | 60 | virtual void handleMouseEvent(rviz::ViewportMouseEvent& event); 61 | 62 | void zoom( float amount ); 63 | 64 | private: 65 | 66 | rviz::FloatProperty *fov_property_; 67 | 68 | }; 69 | 70 | } 71 | 72 | #endif // RVIZ_VIEW_CONTROLLER_H 73 | -------------------------------------------------------------------------------- /include/oculus_rviz_plugins/oculus_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, 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_OCULUS_DISPLAY_H 31 | #define RVIZ_OCULUS_DISPLAY_H 32 | 33 | #ifndef Q_MOC_RUN 34 | #include 35 | #include 36 | #endif 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include "rviz/display.h" 43 | 44 | namespace Ogre 45 | { 46 | class SceneNode; 47 | class RenderWindow; 48 | } 49 | 50 | namespace rviz 51 | { 52 | class BoolProperty; 53 | class StringProperty; 54 | class RenderWidget; 55 | class FloatProperty; 56 | class VectorProperty; 57 | class TfFrameProperty; 58 | } 59 | 60 | namespace oculus_rviz_plugins 61 | { 62 | 63 | class Oculus; 64 | 65 | /** 66 | * \class OculusDisplay 67 | * 68 | */ 69 | class OculusDisplay: public rviz::Display, public Ogre::RenderTargetListener 70 | { 71 | Q_OBJECT 72 | public: 73 | OculusDisplay(); 74 | virtual ~OculusDisplay(); 75 | 76 | // Overrides from Display 77 | virtual void onInitialize(); 78 | virtual void update( float wall_dt, float ros_dt ); 79 | virtual void reset(); 80 | 81 | // Overrides from Ogre::RenderTargetListener 82 | virtual void preRenderTargetUpdate( const Ogre::RenderTargetEvent& evt ); 83 | virtual void postRenderTargetUpdate( const Ogre::RenderTargetEvent& evt ); 84 | 85 | protected: 86 | 87 | virtual void onEnable(); 88 | virtual void onDisable(); 89 | 90 | void updateCamera(); 91 | 92 | protected Q_SLOTS: 93 | 94 | void onFullScreenChanged(); 95 | void onPredictionDtChanged(); 96 | void onPubTfChanged(); 97 | void onFollowCamChanged(); 98 | 99 | void onScreenCountChanged( int newCount ); 100 | 101 | private: 102 | 103 | rviz::BoolProperty *fullscreen_property_; 104 | rviz::FloatProperty *prediction_dt_property_; 105 | 106 | rviz::BoolProperty *pub_tf_property_; 107 | rviz::StringProperty *pub_tf_frame_property_; 108 | 109 | rviz::BoolProperty *follow_cam_property_; 110 | rviz::BoolProperty *horizontal_property_; 111 | rviz::TfFrameProperty *tf_frame_property_; 112 | rviz::VectorProperty *offset_property_; 113 | 114 | rviz::FloatProperty *near_clip_property_; 115 | 116 | rviz::RenderWidget *render_widget_; 117 | Ogre::SceneNode *scene_node_; 118 | 119 | #ifndef Q_MOC_RUN 120 | tf::TransformBroadcaster tf_pub_; 121 | boost::shared_ptr oculus_; 122 | #endif 123 | }; 124 | 125 | } // namespace rviz 126 | 127 | #endif 128 | 129 | -------------------------------------------------------------------------------- /include/oculus_rviz_plugins/ogre_oculus.h: -------------------------------------------------------------------------------- 1 | /// Copyright (C) 2013 Kojack 2 | /// 3 | /// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | /// to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | /// and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | /// 7 | /// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | /// 9 | /// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | /// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 11 | /// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 12 | /// DEALINGS IN THE SOFTWARE. 13 | 14 | #pragma once 15 | 16 | #include "OGRE/OgreQuaternion.h" 17 | #include "OGRE/OgreVector3.h" 18 | #include "OVR_CAPI_0_5_0.h" 19 | #include "OVR_CAPI_Keys.h" 20 | 21 | #include 22 | 23 | namespace OVR 24 | { 25 | class HMDDevice; 26 | class SensorFusion; 27 | class DeviceManager; 28 | class SensorDevice; 29 | 30 | namespace Util 31 | { 32 | class MagCalibration; 33 | namespace Render 34 | { 35 | class StereoConfig; 36 | } 37 | } 38 | } 39 | 40 | namespace Ogre 41 | { 42 | class SceneManager; 43 | class RenderWindow; 44 | class Camera; 45 | class SceneNode; 46 | class Viewport; 47 | class CompositorInstance; 48 | } 49 | 50 | namespace oculus_rviz_plugins 51 | { 52 | 53 | class Oculus 54 | { 55 | public: 56 | Oculus(void); 57 | ~Oculus(void); 58 | bool setupOculus(); 59 | bool setupOgre(Ogre::SceneManager *sm, Ogre::RenderWindow *win, Ogre::SceneNode *parent = 0); 60 | void shutDownOculus(); 61 | void shutDownOgre(); 62 | bool isOgreReady() const; 63 | bool isOculusReady() const; 64 | 65 | float getAspectRatio(); 66 | 67 | /// Update camera node using current Oculus orientation. 68 | void update(); 69 | 70 | /// Reset orientation of the sensor. 71 | void resetOrientation(); 72 | 73 | /// Retrieve the SceneNode that contains the two cameras used for stereo rendering. 74 | Ogre::SceneNode *getCameraNode(); 75 | 76 | /// Retrieve the current orientation of the Oculus HMD. 77 | Ogre::Quaternion getOrientation() const; 78 | 79 | /// Retrieve the current position of the Oculus HMD. 80 | ovrPosef getPosition() const; 81 | 82 | /// Retrieve either of the two distortion compositors. 83 | Ogre::CompositorInstance *getCompositor(unsigned int i); 84 | 85 | /// Retrieve either of the two cameras. 86 | Ogre::Camera *getCamera(unsigned int i) 87 | { 88 | return m_cameras[i % 2]; 89 | } 90 | 91 | /// Retrieve either of the two viewports. 92 | Ogre::Viewport *getViewport(unsigned int i) 93 | { 94 | return m_viewports[i % 2]; 95 | } 96 | 97 | /// Retrieve the projection centre offset. 98 | float getCentreOffset() const; 99 | 100 | /* 101 | 102 | const OVR::HMDDevice *getHMDDevice() 103 | { 104 | return m_hmd; 105 | } 106 | */ 107 | 108 | /// Re-create projection matrices based on camera parameters 109 | void updateProjectionMatrices(); 110 | 111 | /// Set motion prediction time window (0 to disable) 112 | void setPredictionDt(float dt); 113 | 114 | /// Get status of mag calibration 115 | bool isMagCalibrated(); 116 | 117 | protected: 118 | 119 | /* OVR::DeviceManager *m_deviceManager; 120 | OVR::HMDDevice *m_hmd; 121 | OVR::Util::Render::StereoConfig *m_stereoConfig; 122 | OVR::Util::MagCalibration *m_magCalibration; 123 | OVR::SensorDevice *m_sensor; 124 | OVR::SensorFusion *m_sensorFusion; 125 | */ 126 | 127 | // new API structures 128 | 129 | // Oculus Rift 130 | ovrHmd m_hmd; 131 | 132 | // 133 | 134 | ovrEyeType m_eyeType; 135 | ovrEyeRenderDesc m_eyeRenderDescOut[2]; 136 | 137 | 138 | bool m_oculusReady; /// Has the oculus rift been fully initialised? 139 | bool m_ogreReady; /// Has ogre been fully initialised? 140 | Ogre::SceneManager *m_sceneManager; 141 | Ogre::RenderWindow *m_window; 142 | Ogre::SceneNode *m_cameraNode; 143 | Ogre::Quaternion m_orientation; 144 | float m_centreOffset; /// Projection centre offset. 145 | Ogre::Camera *m_cameras[2]; 146 | Ogre::Viewport *m_viewports[2]; 147 | Ogre::CompositorInstance *m_compositors[2]; 148 | }; 149 | 150 | } 151 | -------------------------------------------------------------------------------- /ogre_media/glsl120.program: -------------------------------------------------------------------------------- 1 | 2 | vertex_program oculus_rviz_plugins/glsl120/warp.vert glsl 3 | { 4 | source warp.vert 5 | } 6 | 7 | fragment_program rviz/glsl120/warp.frag glsl 8 | { 9 | source warp.frag 10 | default_params 11 | { 12 | param_named WarpTexture int 0.0 13 | param_named ScreenCenter float2 0.5 0.5 14 | param_named LensCenter float2 0.5 0.5 15 | param_named Scale float2 0.3 0.3 16 | param_named ScaleIn float2 2.0 2.0 17 | param_named HmdWarpParam float4 1.0 0.22 0.24 0.0 18 | } 19 | } 20 | 21 | fragment_program rviz/glsl120/warpWithChromeAb.frag glsl 22 | { 23 | source warpWithChromeAb.frag 24 | default_params 25 | { 26 | param_named WarpTexture int 0.0 27 | param_named ScreenCenter float2 0.5 0.5 28 | param_named LensCenter float2 0.5 0.5 29 | param_named Scale float2 0.3 0.3 30 | param_named ScaleIn float2 2.0 2.0 31 | param_named HmdWarpParam float4 1.0 0.22 0.24 0.0 32 | param_named ChromAbParam float4 0.996, -0.004, 1.014, 0 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /ogre_media/oculus.compositor: -------------------------------------------------------------------------------- 1 | compositor OculusLeft 2 | { 3 | technique 4 | { 5 | texture rt0 target_width_scaled 1.5 target_height_scaled 1.5 PF_R8G8B8 6 | 7 | target rt0 { input previous } 8 | 9 | target_output 10 | { 11 | // Start with clear output 12 | input none 13 | 14 | pass render_quad 15 | { 16 | material Ogre/Compositor/Oculus 17 | input 0 rt0 18 | } 19 | } 20 | } 21 | } 22 | 23 | compositor OculusRight 24 | { 25 | technique 26 | { 27 | texture rt0 target_width_scaled 1.5 target_height_scaled 1.5 PF_R8G8B8 28 | 29 | target rt0 { input previous } 30 | 31 | target_output 32 | { 33 | // Start with clear output 34 | input none 35 | 36 | pass render_quad 37 | { 38 | material Ogre/Compositor/Oculus 39 | input 0 rt0 40 | } 41 | } 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /ogre_media/warp.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | uniform sampler2D WarpTexture; 4 | 5 | uniform vec2 LensCenter; 6 | uniform vec2 ScreenCenter; 7 | uniform vec2 Scale; 8 | uniform vec2 ScaleIn; 9 | uniform vec4 HmdWarpParam; 10 | 11 | varying vec2 Texcoord; 12 | 13 | // Scales input texture coordinates for distortion. 14 | vec2 HmdWarp(vec2 texCoord) 15 | { 16 | vec2 theta = (texCoord - LensCenter) * ScaleIn; // Scales texture coordinates to [-1, 1] 17 | float rSq = theta.x * theta.x + theta.y * theta.y; 18 | vec2 rvector= theta * ( HmdWarpParam.x + 19 | HmdWarpParam.y * rSq + 20 | HmdWarpParam.z * rSq * rSq + 21 | HmdWarpParam.w * rSq * rSq * rSq); 22 | return LensCenter + Scale * rvector; 23 | } 24 | 25 | void main(void) 26 | { 27 | vec2 tc = HmdWarp(Texcoord); 28 | gl_FragColor = texture2D(WarpTexture, tc); 29 | 30 | // if (any(bvec2(clamp(tc, ScreenCenter - vec2(0.5, 0.5), ScreenCenter + vec2(0.5, 0.5)) - tc))) { 31 | // gl_FragColor = vec4(0, 0, 0, 0); 32 | // } else { 33 | // gl_FragColor = texture2D(WarpTexture, tc); 34 | // } 35 | } 36 | -------------------------------------------------------------------------------- /ogre_media/warp.material: -------------------------------------------------------------------------------- 1 | material Ogre/Compositor/Oculus 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | depth_check off 8 | 9 | vertex_program_ref oculus_rviz_plugins/glsl120/warp.vert {} 10 | // fragment_program_ref rviz/glsl120/warp.frag {} 11 | fragment_program_ref rviz/glsl120/warpWithChromeAb.frag {} 12 | 13 | texture_unit RT 14 | { 15 | tex_coord_set 0 16 | tex_address_mode border 17 | tex_border_colour 0 0 0 18 | filtering linear linear linear 19 | } 20 | } 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /ogre_media/warp.vert: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | varying vec2 Texcoord; 4 | 5 | void main( void ) 6 | { 7 | gl_Position = ftransform(); 8 | Texcoord = gl_MultiTexCoord0.xy; 9 | } 10 | -------------------------------------------------------------------------------- /ogre_media/warpWithChromeAb.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | uniform sampler2D WarpTexture; 4 | uniform vec2 LensCenter; 5 | uniform vec2 ScreenCenter; 6 | uniform vec2 Scale; 7 | uniform vec2 ScaleIn; 8 | uniform vec4 HmdWarpParam; 9 | uniform vec4 ChromAbParam; 10 | 11 | varying vec2 Texcoord; 12 | 13 | void main(void) 14 | { 15 | vec2 theta = (Texcoord - LensCenter) * ScaleIn; // Scales texture coordinates to [-1, 1] 16 | float rSq = theta.x * theta.x + theta.y * theta.y; 17 | vec2 theta1 = theta * ( HmdWarpParam.x + 18 | HmdWarpParam.y * rSq + 19 | HmdWarpParam.z * rSq * rSq + 20 | HmdWarpParam.w * rSq * rSq * rSq); 21 | // Detect whether blue texture coordinates are out of range since these will scaled out the furthest. 22 | vec2 thetaBlue = theta1 * (ChromAbParam.z + ChromAbParam.w * rSq); 23 | vec2 tcBlue = LensCenter + Scale * thetaBlue; 24 | 25 | if (any(bvec2(clamp(tcBlue, ScreenCenter - vec2(0.5, 0.5), ScreenCenter + vec2(0.5, 0.5)) - tcBlue))) { 26 | gl_FragColor = vec4(0, 0, 0, 0); 27 | return; 28 | } 29 | 30 | // Now do blue texture lookup. 31 | float blue = texture2D(WarpTexture, tcBlue).b; 32 | // Do green lookup (no scaling). 33 | vec2 tcGreen = LensCenter + Scale * theta1; 34 | float green = texture2D(WarpTexture, tcGreen).g; 35 | // Do red scale and lookup. 36 | vec2 thetaRed = theta1 * (ChromAbParam.x + ChromAbParam.y * rSq); 37 | vec2 tcRed = LensCenter + Scale * thetaRed; 38 | float red = texture2D(WarpTexture, tcRed).r; 39 | gl_FragColor = vec4(red, green, blue, 1); 40 | } 41 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oculus_rviz_plugins 4 | 0.0.8 5 | RViz plugins for the Oculus Rift. 6 | 7 | David Gossow 8 | David Gossow 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | ros_ovr_sdk 15 | ros_ovr_sdk 16 | 17 | libogre-dev 18 | libogre-dev 19 | 20 | libqt4-dev 21 | libqt4-dev 22 | 23 | libqt4-opengl-dev 24 | libqt4-opengl-dev 25 | 26 | boost 27 | boost 28 | 29 | rviz 30 | rviz 31 | 32 | tf 33 | tf 34 | 35 | roscpp 36 | roscpp 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Renders the rviz scene to the Oculus Rift. 7 | 8 | 9 | 12 | 13 | Renders the main rviz view from the origin of a particular tf frame. 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/fixed_view_controller.cpp: -------------------------------------------------------------------------------- 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 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "rviz/display_context.h" 40 | #include "rviz/geometry.h" 41 | #include "rviz/ogre_helpers/shape.h" 42 | #include "rviz/properties/float_property.h" 43 | #include "rviz/properties/vector_property.h" 44 | #include "rviz/uniform_string_stream.h" 45 | #include "rviz/viewport_mouse_event.h" 46 | #include "rviz/load_resource.h" 47 | #include "rviz/render_panel.h" 48 | 49 | #include "oculus_rviz_plugins/fixed_view_controller.h" 50 | 51 | using namespace rviz; 52 | 53 | namespace oculus_rviz_plugins 54 | { 55 | 56 | static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION = 57 | Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Y ) * 58 | Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Z ); 59 | 60 | FixedViewController::FixedViewController() 61 | { 62 | fov_property_ = new FloatProperty( "Vertical Field Of View", 50.0, "Vertical opening angle of the camera.", this ); 63 | fov_property_->setMin( 5.0 ); 64 | fov_property_->setMax( 130.0 ); 65 | } 66 | 67 | void FixedViewController::onInitialize() 68 | { 69 | FramePositionTrackingViewController::onInitialize(); 70 | camera_->setProjectionType( Ogre::PT_PERSPECTIVE ); 71 | camera_->setOrientation(ROBOT_TO_CAMERA_ROTATION);//.Inverse()); 72 | } 73 | 74 | FixedViewController::~FixedViewController() 75 | { 76 | } 77 | 78 | void FixedViewController::reset() 79 | { 80 | } 81 | 82 | void FixedViewController::update(float dt, float ros_dt) 83 | { 84 | FramePositionTrackingViewController::update( dt, ros_dt ); 85 | camera_->setFOVy( Ogre::Degree(fov_property_->getFloat()) ); 86 | target_scene_node_->setOrientation( reference_orientation_ ); 87 | } 88 | 89 | void FixedViewController::handleMouseEvent(ViewportMouseEvent& event) 90 | { 91 | setStatus( "Right-Click / Mouse Wheel:: Zoom. " ); 92 | 93 | int32_t diff_x = event.x - event.last_x; 94 | int32_t diff_y = event.y - event.last_y; 95 | 96 | if( event.right() ) 97 | { 98 | setCursor( Zoom ); 99 | zoom( -diff_y * 0.1); 100 | } 101 | 102 | if( event.wheel_delta != 0 ) 103 | { 104 | int diff = event.wheel_delta; 105 | zoom( diff * 0.001 ); 106 | } 107 | } 108 | 109 | void FixedViewController::zoom( float amount ) 110 | { 111 | fov_property_->setFloat( fov_property_->getFloat() * (1.0 + amount) ); 112 | } 113 | 114 | 115 | } // end namespace rviz 116 | 117 | #include 118 | PLUGINLIB_EXPORT_CLASS( oculus_rviz_plugins::FixedViewController, rviz::ViewController ) 119 | -------------------------------------------------------------------------------- /src/oculus_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 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | #include "oculus_rviz_plugins/oculus_display.h" 63 | #include "oculus_rviz_plugins/ogre_oculus.h" 64 | 65 | namespace oculus_rviz_plugins 66 | { 67 | 68 | OculusDisplay::OculusDisplay() 69 | : render_widget_(0) 70 | , scene_node_(0) 71 | { 72 | std::string rviz_path = ros::package::getPath(ROS_PACKAGE_NAME); 73 | Ogre::ResourceGroupManager::getSingleton().addResourceLocation( rviz_path + "/ogre_media", "FileSystem", ROS_PACKAGE_NAME ); 74 | Ogre::ResourceGroupManager::getSingleton().initialiseResourceGroup(ROS_PACKAGE_NAME); 75 | 76 | connect( QApplication::desktop(), SIGNAL( screenCountChanged ( int ) ), this, SLOT( onScreenCountChanged(int)) ); 77 | } 78 | 79 | OculusDisplay::~OculusDisplay() 80 | { 81 | oculus_.reset(); 82 | delete render_widget_; 83 | } 84 | 85 | 86 | void OculusDisplay::onInitialize() 87 | { 88 | fullscreen_property_ = new rviz::BoolProperty( "Render to Oculus", false, 89 | "If checked, will render fullscreen on your secondary screen. Otherwise, shows a window.", 90 | this, SLOT(onFullScreenChanged())); 91 | 92 | prediction_dt_property_ = new rviz::FloatProperty( "Motion prediction (ms)", 30.0, 93 | "Time in ms to predict head motion. Decreases overall latency and motion sickness.", 94 | this, SLOT(onPredictionDtChanged()) ); 95 | 96 | near_clip_property_ = new rviz::FloatProperty( "Near Clip Distance", 0.02, 97 | "Minimum rendering distance for Oculus camera.", 98 | this ); 99 | 100 | horizontal_property_ = new rviz::BoolProperty( "Fixed Horizon", true, 101 | "If checked, will ignore the pitch component of the RViz camera.", this); 102 | 103 | follow_cam_property_ = new rviz::BoolProperty( "Follow RViz Camera", true, 104 | "If checked, will set the Oculus camera to the same position as the main view camera.", 105 | this, SLOT( onFollowCamChanged() ) ); 106 | 107 | tf_frame_property_ = new rviz::TfFrameProperty( "Target Frame", "", 108 | "Tf frame that the Oculus camera should follow.", this, context_->getFrameManager(), true ); 109 | 110 | offset_property_ = new rviz::VectorProperty( "Offset", Ogre::Vector3(0,0,0), 111 | "Additional offset of the Oculus camera from the followed RViz camera or target frame.", this ); 112 | 113 | pub_tf_property_ = new rviz::BoolProperty( "Publish tf", true, 114 | "If checked, will publish the pose of the Oculus camera as a tf frame.", 115 | this, SLOT( onPubTfChanged() ) ); 116 | 117 | pub_tf_frame_property_ = new rviz::StringProperty( "Tf Frame", "oculus", 118 | "Name of the published tf frame.", this ); 119 | 120 | render_widget_ = new rviz::RenderWidget( rviz::RenderSystem::get() ); 121 | render_widget_->setVisible(false); 122 | render_widget_->setWindowTitle( "Oculus View" ); 123 | 124 | render_widget_->setParent( context_->getWindowManager()->getParentWindow() ); 125 | render_widget_->setWindowFlags( Qt::Window | Qt::CustomizeWindowHint | Qt::WindowTitleHint | Qt::WindowMaximizeButtonHint ); 126 | 127 | Ogre::RenderWindow *window = render_widget_->getRenderWindow(); 128 | window->setVisible(false); 129 | window->setAutoUpdated(false); 130 | window->addListener(this); 131 | 132 | scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); 133 | } 134 | 135 | 136 | void OculusDisplay::onFollowCamChanged() 137 | { 138 | tf_frame_property_->setHidden( follow_cam_property_->getBool() ); 139 | } 140 | 141 | void OculusDisplay::onPubTfChanged() 142 | { 143 | pub_tf_frame_property_->setHidden( !pub_tf_property_->getBool() ); 144 | } 145 | 146 | void OculusDisplay::onPredictionDtChanged( ) 147 | { 148 | if ( !oculus_ || !isEnabled() ) 149 | { 150 | return; 151 | } 152 | 153 | oculus_->setPredictionDt( prediction_dt_property_->getFloat() * 0.001 ); 154 | } 155 | 156 | 157 | void OculusDisplay::onScreenCountChanged( int newCount ) 158 | { 159 | if ( newCount == 1 ) 160 | { 161 | fullscreen_property_->setBool(false); 162 | fullscreen_property_->setHidden(true); 163 | setStatus( rviz::StatusProperty::Error, "Screen", "No secondary screen detected. Cannot render to Oculus device."); 164 | } 165 | else 166 | { 167 | fullscreen_property_->setHidden(false); 168 | setStatus( rviz::StatusProperty::Ok, "Screen", "Using screen #2."); 169 | } 170 | } 171 | 172 | 173 | void OculusDisplay::onFullScreenChanged() 174 | { 175 | if ( !oculus_ || !isEnabled() ) 176 | { 177 | return; 178 | } 179 | 180 | if ( fullscreen_property_->getBool() && QApplication::desktop()->numScreens() > 1 ) 181 | { 182 | QRect screen_res = QApplication::desktop()->screenGeometry(1); 183 | //render_widget->setWindowFlags(); 184 | render_widget_->setGeometry( screen_res ); 185 | //render_widget->show(); 186 | render_widget_->showFullScreen(); 187 | } 188 | else 189 | { 190 | int x_res = 1280; 191 | int y_res = 800; 192 | /* 193 | if ( oculus_->getHMDDevice() ) 194 | { 195 | OVR::HMDInfo info; 196 | oculus_->getHMDDevice()->GetDeviceInfo( &info ); 197 | x_res = info.HResolution; 198 | y_res = info.VResolution; 199 | } 200 | int primary_screen = QApplication::desktop()->primaryScreen(); 201 | QRect screen_res = QApplication::desktop()->screenGeometry( primary_screen ); 202 | render_widget_->setGeometry( screen_res.x(), screen_res.y(), x_res, y_res ); 203 | render_widget_->showNormal(); 204 | */ 205 | } 206 | } 207 | 208 | void OculusDisplay::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt) 209 | { 210 | if ( !oculus_ ) 211 | { 212 | return; 213 | } 214 | updateCamera(); 215 | } 216 | 217 | void OculusDisplay::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt) 218 | { 219 | if ( !oculus_ ) 220 | { 221 | return; 222 | } 223 | Ogre::RenderWindow *window = render_widget_->getRenderWindow(); 224 | window->swapBuffers(); 225 | } 226 | 227 | void OculusDisplay::onEnable() 228 | { 229 | if ( oculus_ ) 230 | { 231 | return; 232 | } 233 | 234 | oculus_.reset( new Oculus() ); 235 | oculus_->setupOculus(); 236 | 237 | if ( !oculus_->isOculusReady() ) 238 | { 239 | oculus_.reset(); 240 | setStatusStd( rviz::StatusProperty::Error, "Oculus", "No Oculus device found!" ); 241 | return; 242 | } 243 | 244 | setStatusStd( rviz::StatusProperty::Ok, "Oculus", "Oculus is ready." ); 245 | 246 | Ogre::RenderWindow *window = render_widget_->getRenderWindow(); 247 | oculus_->setupOgre( scene_manager_, window, scene_node_ ); 248 | 249 | render_widget_->setVisible( oculus_->isOculusReady() ); 250 | 251 | onScreenCountChanged( QApplication::desktop()->numScreens() ); 252 | onFullScreenChanged(); 253 | onPredictionDtChanged(); 254 | } 255 | 256 | void OculusDisplay::onDisable() 257 | { 258 | clearStatuses(); 259 | render_widget_->setVisible(false); 260 | if ( oculus_ ) 261 | { 262 | oculus_.reset(); 263 | } 264 | } 265 | 266 | void OculusDisplay::update( float wall_dt, float ros_dt ) 267 | { 268 | if ( !oculus_ ) 269 | { 270 | return; 271 | } 272 | 273 | updateCamera(); 274 | Ogre::RenderWindow *window = render_widget_->getRenderWindow(); 275 | window->update(false); 276 | 277 | if ( oculus_->isMagCalibrated() ) 278 | { 279 | setStatus( rviz::StatusProperty::Ok, "Magnetometer", "Magnetometer calibrated."); 280 | } 281 | else 282 | { 283 | setStatus( rviz::StatusProperty::Warn, "Magnetometer", "Magnetometer not calibrated. Look left/right/up/down to collect enough samples."); 284 | } 285 | } 286 | 287 | void OculusDisplay::updateCamera() 288 | { 289 | if (!oculus_ || !oculus_->isOculusReady()) 290 | { 291 | return; 292 | } 293 | 294 | Ogre::Vector3 pos; 295 | Ogre::Quaternion ori; 296 | 297 | if ( follow_cam_property_->getBool() ) 298 | { 299 | const Ogre::Camera *cam = context_->getViewManager()->getCurrent()->getCamera(); 300 | pos = cam->getDerivedPosition(); 301 | ori = cam->getDerivedOrientation(); 302 | } 303 | else 304 | { 305 | context_->getFrameManager()->getTransform( tf_frame_property_->getStdString(), ros::Time(), pos, ori ); 306 | 307 | Ogre::Quaternion r; 308 | r.FromAngleAxis( Ogre::Radian(M_PI*0.5), Ogre::Vector3::UNIT_X ); 309 | ori = ori * r; 310 | r.FromAngleAxis( Ogre::Radian(-M_PI*0.5), Ogre::Vector3::UNIT_Y ); 311 | ori = ori * r; 312 | } 313 | 314 | pos += offset_property_->getVector(); 315 | scene_node_->setPosition( pos ); // -> position of oculus_nav 316 | 317 | if ( horizontal_property_->getBool() ) 318 | { 319 | Ogre::Vector3 x_axis = ori * Ogre::Vector3(1,0,0); 320 | float yaw = atan2( x_axis.y, x_axis.x );// - M_PI*0.5; 321 | 322 | // we're working in OpenGL coordinates now 323 | ori.FromAngleAxis( Ogre::Radian(yaw), Ogre::Vector3::UNIT_Z ); 324 | 325 | Ogre::Quaternion r; 326 | r.FromAngleAxis( Ogre::Radian(M_PI*0.5), Ogre::Vector3::UNIT_X ); 327 | ori = ori * r; 328 | } 329 | 330 | scene_node_->setOrientation( ori ); 331 | 332 | Ogre::ColourValue bg_color = context_->getViewManager()->getRenderPanel()->getViewport()->getBackgroundColour(); 333 | 334 | for ( int i =0; i<2; i++ ) 335 | { 336 | oculus_->getViewport(i)->setBackgroundColour( bg_color ); 337 | oculus_->getCamera(i)->setNearClipDistance( near_clip_property_->getFloat() ); 338 | 339 | // this is a hack to circumvent a bug in Ogre 1.8 340 | // otherwise one of the viewports will not update it's background color 341 | bg_color.g += 0.0001; 342 | } 343 | 344 | oculus_->updateProjectionMatrices(); 345 | oculus_->update(); 346 | 347 | if ( pub_tf_property_->getBool() ) 348 | { 349 | tf::StampedTransform pose; 350 | pose.frame_id_ = context_->getFixedFrame().toStdString(); 351 | pose.child_frame_id_ = pub_tf_frame_property_->getStdString(); 352 | pose.stamp_ = ros::Time::now(); 353 | 354 | Ogre::Vector3 test_pos; 355 | Ogre::Quaternion test_ori; 356 | 357 | const Ogre::Camera *caml = oculus_->getCamera(0); // left eye position 358 | const Ogre::Camera *camr = oculus_->getCamera(1); // right eye position 359 | 360 | test_pos = (caml->getDerivedPosition() + camr->getDerivedPosition()) / 2; // eye position average 361 | test_ori = caml->getDerivedOrientation(); 362 | 363 | Ogre::Quaternion r; 364 | r.FromAngleAxis( Ogre::Radian(M_PI*0.5), Ogre::Vector3::UNIT_Y ); 365 | test_ori = test_ori * r; 366 | r.FromAngleAxis( Ogre::Radian(-M_PI*0.5), Ogre::Vector3::UNIT_X ); 367 | test_ori = test_ori * r; 368 | 369 | pose.setRotation( tf::Quaternion( test_ori.x, test_ori.y, test_ori.z, test_ori.w ) ); 370 | pose.setOrigin( tf::Vector3( test_pos.x, test_pos.y, test_pos.z ) ); 371 | 372 | tf_pub_.sendTransform( pose ); 373 | 374 | } 375 | } 376 | 377 | void OculusDisplay::reset() 378 | { 379 | rviz::Display::reset(); 380 | if ( oculus_ ) 381 | { 382 | onDisable(); 383 | onEnable(); 384 | } 385 | } 386 | 387 | } // namespace rviz 388 | 389 | #include 390 | PLUGINLIB_EXPORT_CLASS( oculus_rviz_plugins::OculusDisplay, rviz::Display ) 391 | -------------------------------------------------------------------------------- /src/ogre_oculus.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright (C) 2013 Kojack 2 | /// 3 | /// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 4 | /// to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 5 | /// and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 6 | /// 7 | /// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 8 | /// 9 | /// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 10 | /// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 11 | /// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 12 | /// DEALINGS IN THE SOFTWARE. 13 | 14 | #include "oculus_rviz_plugins/ogre_oculus.h" 15 | #include "OGRE/OgreSceneManager.h" 16 | #include "OGRE/OgreRenderWindow.h" 17 | #include "OGRE/OgreCompositorManager.h" 18 | #include "OGRE/OgreCompositorInstance.h" 19 | #include "OGRE/OgreCompositionTargetPass.h" 20 | #include "OGRE/OgreCompositionPass.h" 21 | #include 22 | using namespace OVR; 23 | 24 | namespace 25 | { 26 | const float g_defaultNearClip = 0.01f; 27 | const float g_defaultFarClip = 10000.0f; 28 | const float g_defaultIPD = 0.064f; 29 | const Ogre::ColourValue g_defaultViewportColour(97 / 255.0f, 97 / 255.0f, 200 / 255.0f); 30 | const float g_defaultProjectionCentreOffset = 0.14529906f; 31 | const float g_defaultDistortion[4] = {1.0f, 0.22f, 0.24f, 0.0f}; 32 | const float g_defaultChromAb[4] = {0.996, -0.004, 1.014, 0.0f}; 33 | } 34 | 35 | namespace oculus_rviz_plugins 36 | { 37 | 38 | Oculus::Oculus(void) : 39 | /* 40 | m_sensorFusion(0), 41 | m_stereoConfig(0), 42 | m_hmd(0), 43 | m_deviceManager(0), */ 44 | m_oculusReady(false), m_ogreReady(false), //m_sensor(0), 45 | m_centreOffset(g_defaultProjectionCentreOffset), m_window(0), m_sceneManager(0), m_cameraNode(0) 46 | { 47 | for (int i = 0; i < 2; ++i) 48 | { 49 | m_cameras[i] = 0; 50 | m_viewports[i] = 0; 51 | m_compositors[i] = 0; 52 | } 53 | } 54 | 55 | Oculus::~Oculus(void) 56 | { 57 | shutDownOgre(); 58 | shutDownOculus(); 59 | } 60 | 61 | void Oculus::shutDownOculus() 62 | { 63 | 64 | if (m_hmd){ 65 | ovrHmd_Destroy(m_hmd); 66 | ovr_Shutdown(); 67 | } 68 | /* 69 | delete m_stereoConfig; 70 | m_stereoConfig = 0; 71 | delete m_sensorFusion; 72 | m_sensorFusion = 0; 73 | 74 | if (m_sensor) 75 | { 76 | m_sensor->Release(); 77 | } 78 | if (m_hmd) 79 | { 80 | m_hmd->Release(); 81 | m_hmd = 0; 82 | } 83 | if (m_deviceManager) 84 | { 85 | m_deviceManager->Release(); 86 | m_deviceManager = 0; 87 | } 88 | */ 89 | if ( m_oculusReady) 90 | { 91 | 92 | } 93 | 94 | m_oculusReady = false; 95 | // System::Destroy(); 96 | } 97 | 98 | void Oculus::shutDownOgre() 99 | { 100 | m_ogreReady = false; 101 | for (int i = 0; i < 2; ++i) 102 | { 103 | if (m_compositors[i]) 104 | { 105 | Ogre::CompositorManager::getSingleton().removeCompositor(m_viewports[i], "Oculus"); 106 | m_compositors[i] = 0; 107 | } 108 | if (m_viewports[i]) 109 | { 110 | m_window->removeViewport(i); 111 | m_viewports[i] = 0; 112 | } 113 | if (m_cameras[i]) 114 | { 115 | m_cameras[i]->getParentSceneNode()->detachObject(m_cameras[i]); 116 | m_sceneManager->destroyCamera(m_cameras[i]); 117 | m_cameras[i] = 0; 118 | } 119 | } 120 | if (m_cameraNode) 121 | { 122 | m_cameraNode->getParentSceneNode()->removeChild(m_cameraNode); 123 | m_sceneManager->destroySceneNode(m_cameraNode); 124 | m_cameraNode = 0; 125 | } 126 | m_window = 0; 127 | m_sceneManager = 0; 128 | } 129 | 130 | bool Oculus::isOculusReady() const 131 | { 132 | return m_oculusReady; 133 | } 134 | 135 | bool Oculus::isOgreReady() const 136 | { 137 | return m_ogreReady; 138 | } 139 | 140 | bool Oculus::setupOculus() 141 | { 142 | if (m_oculusReady) 143 | { 144 | Ogre::LogManager::getSingleton().logMessage("Oculus: Already Initialised"); 145 | return true; 146 | } 147 | ovrInitParams params = {0, 0, NULL, 0}; 148 | bool success = ovr_Initialize(¶ms); 149 | // initialzes the OVR 150 | 151 | m_hmd = ovrHmd_Create(0); 152 | 153 | if (!success){ 154 | Ogre::LogManager::getSingleton().logMessage("Oculus: Initialization of OVR failed."); 155 | // no return here; the next step finds out why 156 | } 157 | 158 | // attempts to detect the HMD and dies if it doesn't. 159 | int numDevices = ovrHmd_Detect(); 160 | 161 | // special steps for Dk1? 162 | 163 | if (numDevices < 1){ 164 | Ogre::LogManager::getSingleton().logMessage("Oculus: Hmd not detected."); 165 | switch(numDevices){ 166 | case 0: 167 | Ogre::LogManager::getSingleton().logMessage("Check if the Oculus is plugged in / turned on."); 168 | return false; 169 | 170 | case -1: 171 | Ogre::LogManager::getSingleton().logMessage("Oculus: Please run ovrd in a seperate terminal."); 172 | return false; 173 | 174 | default: 175 | char msgstr[10]; 176 | sprintf(msgstr, "Unknown Error Code: %d",numDevices); 177 | Ogre::LogManager::getSingleton().logMessage(msgstr); 178 | return false; 179 | 180 | } 181 | } 182 | 183 | 184 | 185 | if (!m_hmd){ 186 | Ogre::LogManager::getSingleton().logMessage("Oculus:Could not set up virtual HMD."); 187 | return false; 188 | } 189 | 190 | ovrHmd_RecenterPose(m_hmd); 191 | 192 | // sets up tracking 193 | 194 | if (!ovrHmd_ConfigureTracking(m_hmd, 195 | ovrTrackingCap_Orientation | ovrTrackingCap_MagYawCorrection | ovrTrackingCap_Position, 196 | 0)) 197 | { 198 | Ogre::LogManager::getSingleton().logMessage("Oculus: Cannot configure OVR Tracking."); 199 | return false; 200 | } 201 | 202 | 203 | 204 | // sets up rendering information 205 | 206 | 207 | 208 | if (!ovrHmd_ConfigureRendering(m_hmd, 0, 0, 0, m_eyeRenderDescOut)){ 209 | Ogre::LogManager::getSingleton().logMessage("Oculus: Cannot configure OVR rendering."); 210 | return false; 211 | } 212 | 213 | unsigned int hmdCaps = ovrHmdCap_DynamicPrediction; 214 | ovrHmd_SetEnabledCaps(m_hmd, hmdCaps); 215 | 216 | // Set up whatever data structures are necessary. 217 | 218 | // Prints the version number 219 | Ogre::LogManager::getSingleton().logMessage(ovr_GetVersionString()); 220 | // Prints success 221 | Ogre::LogManager::getSingleton().logMessage("Oculus: Oculus setup completed successfully."); 222 | 223 | // TODO: figure out if additional calibration is needed. 224 | 225 | /* 226 | Ogre::LogManager::getSingleton().logMessage("Oculus: Initialising system"); 227 | System::Init(Log::ConfigureDefaultLog(LogMask_All)); 228 | m_deviceManager = DeviceManager::Create(); 229 | if (!m_deviceManager) 230 | { 231 | Ogre::LogManager::getSingleton().logMessage("Oculus: Failed to create Device Manager"); 232 | return false; 233 | } 234 | Ogre::LogManager::getSingleton().logMessage("Oculus: Created Device Manager"); 235 | m_stereoConfig = new Util::Render::StereoConfig(); 236 | if (!m_stereoConfig) 237 | { 238 | Ogre::LogManager::getSingleton().logMessage("Oculus: Failed to create StereoConfig"); 239 | return false; 240 | } 241 | m_centreOffset = m_stereoConfig->GetProjectionCenterOffset(); 242 | Ogre::LogManager::getSingleton().logMessage("Oculus: Created StereoConfig"); 243 | m_hmd = m_deviceManager->EnumerateDevices().CreateDevice(); 244 | if (!m_hmd) 245 | { 246 | Ogre::LogManager::getSingleton().logMessage("Oculus: Failed to create HMD"); 247 | return false; 248 | } 249 | Ogre::LogManager::getSingleton().logMessage("Oculus: Created HMD"); 250 | HMDInfo devinfo; 251 | m_hmd->GetDeviceInfo(&devinfo); 252 | m_stereoConfig->SetHMDInfo(devinfo); 253 | 254 | m_sensor = m_hmd->GetSensor(); 255 | if (!m_sensor) 256 | { 257 | Ogre::LogManager::getSingleton().logMessage("Oculus: Failed to create sensor"); 258 | return false; 259 | } 260 | Ogre::LogManager::getSingleton().logMessage("Oculus: Created sensor"); 261 | 262 | m_sensorFusion = new SensorFusion(); 263 | m_sensorFusion->AttachToSensor(m_sensor); 264 | Ogre::LogManager::getSingleton().logMessage("Oculus: Created SensorFusion"); 265 | 266 | m_magCalibration = new Util::MagCalibration(); 267 | m_magCalibration->BeginAutoCalibration( *m_sensorFusion ); 268 | Ogre::LogManager::getSingleton().logMessage("Oculus: Created MagCalibration"); 269 | */ 270 | m_oculusReady = true; 271 | Ogre::LogManager::getSingleton().logMessage("Oculus: Oculus setup completed successfully"); 272 | return true; 273 | } 274 | 275 | // Currently, only the DK2 is supported. Each DK has it's own resolution 276 | // per eye. Further revisions will include a way to account for other dk versions 277 | float Oculus::getAspectRatio(){ 278 | switch (m_hmd->Type){ 279 | case ovrHmd_DK1: 280 | return 640.0/800.0; 281 | case ovrHmd_DK2: 282 | return 960.0/1080.0; 283 | default: // release version 284 | return 2160.0/1200.0; 285 | } 286 | } 287 | 288 | bool Oculus::setupOgre(Ogre::SceneManager *sm, Ogre::RenderWindow *win, Ogre::SceneNode *parent) 289 | { 290 | m_window = win; 291 | m_sceneManager = sm; 292 | Ogre::LogManager::getSingleton().logMessage("Oculus: Setting up Ogre"); 293 | if (parent) 294 | m_cameraNode = parent->createChildSceneNode("StereoCameraNode"); 295 | else 296 | m_cameraNode = sm->getRootSceneNode()->createChildSceneNode("StereoCameraNode"); 297 | 298 | m_cameras[0] = sm->createCamera("CameraLeft"); 299 | m_cameras[1] = sm->createCamera("CameraRight"); 300 | 301 | Ogre::MaterialPtr matLeft = Ogre::MaterialManager::getSingleton().getByName("Ogre/Compositor/Oculus"); 302 | Ogre::MaterialPtr matRight = matLeft->clone("Ogre/Compositor/Oculus/Right"); 303 | Ogre::GpuProgramParametersSharedPtr pParamsLeft = 304 | matLeft->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); 305 | Ogre::GpuProgramParametersSharedPtr pParamsRight = 306 | matRight->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); 307 | Ogre::Vector4 hmdwarp; 308 | 309 | /* 310 | if (m_stereoConfig) 311 | { 312 | hmdwarp = Ogre::Vector4(m_stereoConfig->GetDistortionK(0), m_stereoConfig->GetDistortionK(1), 313 | m_stereoConfig->GetDistortionK(2), m_stereoConfig->GetDistortionK(3)); 314 | } 315 | else 316 | { 317 | hmdwarp = Ogre::Vector4(g_defaultDistortion[0], g_defaultDistortion[1], g_defaultDistortion[2], 318 | g_defaultDistortion[3]); 319 | } 320 | 321 | pParamsLeft->setNamedConstant("HmdWarpParam", hmdwarp); 322 | pParamsRight->setNamedConstant("HmdWarpParam", hmdwarp); 323 | 324 | Ogre::Vector4 hmdchrom; 325 | if (m_stereoConfig) 326 | { 327 | hmdchrom = Ogre::Vector4(m_stereoConfig->GetHMDInfo().ChromaAbCorrection); 328 | } 329 | else 330 | { 331 | hmdchrom = Ogre::Vector4(g_defaultChromAb); 332 | } 333 | pParamsLeft->setNamedConstant("ChromAbParam", hmdchrom); 334 | pParamsRight->setNamedConstant("ChromAbParam", hmdchrom); 335 | 336 | pParamsLeft->setNamedConstant("LensCenter", 0.5f + (m_stereoConfig->GetProjectionCenterOffset() / 2.0f)); 337 | pParamsRight->setNamedConstant("LensCenter", 0.5f - (m_stereoConfig->GetProjectionCenterOffset() / 2.0f)); 338 | 339 | Ogre::CompositorPtr comp = Ogre::CompositorManager::getSingleton().getByName("OculusRight"); 340 | comp->getTechnique(0)->getOutputTargetPass()->getPass(0)->setMaterialName("Ogre/Compositor/Oculus/Right"); 341 | 342 | */ 343 | for (int i = 0; i < 2; ++i) 344 | { 345 | m_cameraNode->attachObject(m_cameras[i]); 346 | 347 | // sets camera options 348 | m_cameras[i]->setNearClipDistance(g_defaultNearClip); 349 | m_cameras[i]->setFarClipDistance(g_defaultFarClip); 350 | 351 | // m_cameras[i]->setFarClipDistance(m_hmd->CameraFrustumFarZInMeters); 352 | 353 | // m_cameras[i]->setNearClipDistance(m_hmd->CameraFrustumNearZInMeters); 354 | // m_cameras[i]->setFarClipDistance(m_hmd->CameraFrustumFarZInMeters); 355 | m_cameras[i]->setPosition((i * 2 - 1) * OVR_DEFAULT_IPD * 0.5f, 0, 0); 356 | m_cameras[i]->setFOVy(Ogre::Radian(m_hmd->CameraFrustumVFovInRadians*2.)); 357 | // aspect ratio for DK2. Add in a more encapsulated way of setting this 358 | 359 | 360 | m_cameras[i]->setAspectRatio(getAspectRatio()); 361 | /* 362 | if (m_stereoConfig) 363 | { 364 | // Setup cameras. 365 | m_cameras[i]->setNearClipDistance(m_stereoConfig->GetEyeToScreenDistance()); 366 | m_cameras[i]->setFarClipDistance(g_defaultFarClip); 367 | m_cameras[i]->setPosition((i * 2 - 1) * m_stereoConfig->GetIPD() * 0.5f, 0, 0); 368 | m_cameras[i]->setAspectRatio(m_stereoConfig->GetAspect()); 369 | m_cameras[i]->setFOVy(Ogre::Radian(m_stereoConfig->GetYFOVRadians())); 370 | } 371 | else 372 | { 373 | m_cameras[i]->setNearClipDistance(g_defaultNearClip); 374 | m_cameras[i]->setFarClipDistance(g_defaultFarClip); 375 | m_cameras[i]->setPosition((i * 2 - 1) * g_defaultIPD * 0.5f, 0, 0); 376 | } 377 | */ 378 | m_viewports[i] = win->addViewport(m_cameras[i], i, 0.5f * i, 0, 0.5f, 1.0f); 379 | m_viewports[i]->setBackgroundColour(g_defaultViewportColour); 380 | m_compositors[i] = Ogre::CompositorManager::getSingleton().addCompositor(m_viewports[i], 381 | i == 0 ? "OculusLeft" : "OculusRight"); 382 | m_compositors[i]->setEnabled(true); 383 | } 384 | 385 | updateProjectionMatrices(); 386 | 387 | m_ogreReady = true; 388 | Ogre::LogManager::getSingleton().logMessage("Oculus: Oculus setup completed successfully"); 389 | return true; 390 | } 391 | 392 | void Oculus::updateProjectionMatrices() 393 | { 394 | 395 | 396 | 397 | // if (m_stereoConfig) 398 | // { 399 | 400 | ovrHmd_BeginFrameTiming(m_hmd, 0); 401 | 402 | 403 | for (int i = 0; i < 2; ++i) 404 | { 405 | 406 | Ogre::Matrix4 proj = Ogre::Matrix4::IDENTITY; 407 | m_cameras[i]->setCustomProjectionMatrix(false); 408 | ovrEyeType eye; 409 | 410 | switch(i){ 411 | case 0: 412 | eye = ovrEye_Left; 413 | break; 414 | case 1: 415 | eye = ovrEye_Right; 416 | break; 417 | } 418 | ovrEyeRenderDesc temp = ovrHmd_GetRenderDesc(m_hmd,eye,m_hmd->DefaultEyeFov[i]); 419 | //ovrPosef temp = ovrHmd_GetHmdPosePerEye(m_hmd, eye); 420 | 421 | //float temp = m_stereoConfig->GetProjectionCenterOffset(); 422 | // proj.setTrans(Ogre::Vector3(-m_stereoConfig->GetProjectionCenterOffset() * (2 * i - 1), 0, 0)); 423 | // proj.setTrans(Ogre::Vector3(temp.Orientation.x, temp.Orientation.y, temp.Orientation.z)); 424 | 425 | proj.setTrans(Ogre::Vector3(temp.HmdToEyeViewOffset.x, temp.HmdToEyeViewOffset.y, temp.HmdToEyeViewOffset.z)); 426 | 427 | // char msgstring[1024]; 428 | // sprintf(msgstring, "Orientation: %.2f %.2f %.2f %.2f", temp.HmdToEyeViewOffset.x,temp.HmdToEyeViewOffset.y,temp.HmdToEyeViewOffset.z); 429 | 430 | // Ogre::LogManager::getSingleton().logMessage(msgstring); 431 | 432 | m_cameras[i]->setCustomProjectionMatrix(true, proj * m_cameras[i]->getProjectionMatrix()); 433 | } 434 | 435 | ovrHmd_EndFrameTiming(m_hmd); 436 | 437 | 438 | // 439 | 440 | } 441 | 442 | void Oculus::update() 443 | { 444 | if (m_ogreReady) 445 | { 446 | Ogre::Quaternion orient = getOrientation(); 447 | 448 | // char msgstring[1024]; 449 | // sprintf(msgstring, "Orientation: %.2f %.2f %.2f %.2f", orient.x,orient.y,orient.z, orient.w); 450 | // Ogre::LogManager::getSingleton().logMessage(msgstring); 451 | 452 | ovrPosef currentPose = getPosition(); 453 | 454 | m_cameraNode->setPosition(currentPose.Position.x, currentPose.Position.y, currentPose.Position.z); // 455 | m_cameraNode->setOrientation(getOrientation()); 456 | /* 457 | if (m_magCalibration->IsAutoCalibrating()) 458 | { 459 | m_magCalibration->UpdateAutoCalibration( *m_sensorFusion ); 460 | if (m_magCalibration->IsCalibrated()) 461 | { 462 | m_sensorFusion->SetYawCorrectionEnabled(true); 463 | } 464 | 465 | } 466 | */ 467 | 468 | } 469 | 470 | } 471 | 472 | bool Oculus::isMagCalibrated() 473 | { 474 | 475 | //TODO: Figure out how to calibrate mag stuff if necessary. 476 | return true; 477 | 478 | /* 479 | return m_oculusReady && m_magCalibration->IsCalibrated(); 480 | */ 481 | } 482 | 483 | Ogre::SceneNode* Oculus::getCameraNode() 484 | { 485 | return m_cameraNode; 486 | } 487 | 488 | void Oculus::setPredictionDt(float dt) 489 | { 490 | if (m_oculusReady) 491 | { 492 | 493 | // m_sensorFusion->SetPrediction( dt, dt > 0.0f ); 494 | } 495 | } 496 | 497 | Ogre::Quaternion Oculus::getOrientation() const 498 | { 499 | if (m_oculusReady) 500 | { 501 | // get orientation 502 | 503 | ovrTrackingState state = ovrHmd_GetTrackingState(m_hmd, 0.0); 504 | if (!&state){ 505 | Ogre::LogManager::getSingleton().logMessage("Oculus: Sensor not found."); 506 | } else { 507 | ovrQuatf q = state.HeadPose.ThePose.Orientation;// get pose 508 | 509 | return Ogre::Quaternion(q.w, q.x, q.y, q.z); 510 | } 511 | 512 | // Quatf q = m_sensorFusion->GetPredictedOrientation(); 513 | // return Ogre::Quaternion(q.w, q.x, q.y, q.z); 514 | } 515 | else 516 | { 517 | } 518 | 519 | return Ogre::Quaternion::IDENTITY; 520 | } 521 | 522 | ovrPosef Oculus::getPosition() const 523 | { 524 | ovrEyeType eye; 525 | ovrPosef pose = ovrHmd_GetHmdPosePerEye(m_hmd, eye); 526 | 527 | return pose; 528 | } 529 | 530 | Ogre::CompositorInstance *Oculus::getCompositor(unsigned int i) 531 | { 532 | return m_compositors[i]; 533 | } 534 | 535 | float Oculus::getCentreOffset() const 536 | { 537 | return m_centreOffset; 538 | } 539 | 540 | void Oculus::resetOrientation() 541 | { 542 | // if (m_sensorFusion) 543 | // m_sensorFusion->Reset(); 544 | 545 | if (m_hmd){ 546 | ovrHmd_RecenterPose(m_hmd); 547 | } 548 | } 549 | } --------------------------------------------------------------------------------