fps_motion_view_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <OgreViewport.h>
00031 #include <OgreQuaternion.h>
00032 #include <OgreVector3.h>
00033 #include <OgreSceneNode.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreCamera.h>
00036 
00037 #include <rviz/uniform_string_stream.h>
00038 #include <rviz/display_context.h>
00039 #include <rviz/viewport_mouse_event.h>
00040 #include <rviz/geometry.h>
00041 #include <rviz/ogre_helpers/shape.h>
00042 #include <rviz/properties/float_property.h>
00043 #include <rviz/properties/vector_property.h>
00044 #include <rviz/properties/bool_property.h>
00045 
00046 #include <fps_motion_view_controller.h>
00047 
00048 namespace rviz
00049 {
00050 
00051 static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION =
00052   Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Y ) *
00053   Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Z );
00054 
00055 static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001;
00056 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001;
00057 
00058 FPSMotionViewController::FPSMotionViewController()
00059 {
00060   yaw_property_ = new FloatProperty( "Yaw", 0, "Rotation of the camera around the Z (up) axis.", this );
00061   pitch_property_ = new FloatProperty( "Pitch", 0, "How much the camera is tipped downward.", this);
00062   pitch_property_->setMax( Ogre::Math::HALF_PI - 0.001 );
00063   pitch_property_->setMin( -pitch_property_->getMax() );
00064 
00065   position_property_ = new VectorProperty( "Position", Ogre::Vector3( 5, 5, 10 ), "Position of the camera.", this );
00066 
00067 }
00068 
00069 FPSMotionViewController::~FPSMotionViewController()
00070 {
00071 }
00072 
00073 void FPSMotionViewController::onInitialize()
00074 {
00075   FramePositionTrackingViewController::onInitialize();
00076   camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00077 }
00078 
00079 void FPSMotionViewController::reset()
00080 {
00081   camera_->setPosition( Ogre::Vector3( 0.0, 0.0, 1.75 ));
00082   camera_->lookAt( 0.0, 0.0, 0.0);
00083   setPropertiesFromCamera( camera_ );
00084 
00085   updateCamera();
00086   camera_->lookAt( -1.0, 0.0, 1.75);
00087   setPropertiesFromCamera( camera_ );
00088 }
00089 
00090 void FPSMotionViewController::handleMouseEvent(ViewportMouseEvent& event)
00091 {
00092   bool moved = false;
00093 
00094   int32_t diff_x = 0;
00095   int32_t diff_y = 0;
00096 
00097   if( event.type == QEvent::MouseMove )
00098   {
00099     diff_x = event.x - event.last_x;
00100     diff_y = event.y - event.last_y;
00101     moved = true;
00102   }
00103 
00104   if( event.left() )
00105   {
00106     setCursor( Rotate3D );
00107     yaw( -diff_x * 0.005 );
00108     pitch( diff_y * 0.005 );
00109   }
00110 
00111   if (moved)
00112   {
00113     context_->queueRender();
00114   }
00115 }
00116 
00117 void FPSMotionViewController::setPropertiesFromCamera( Ogre::Camera* source_camera )
00118 {
00119   Ogre::Quaternion quat = source_camera->getOrientation() * ROBOT_TO_CAMERA_ROTATION.Inverse();
00120   float yaw = quat.getRoll( false ).valueRadians(); // OGRE camera frame looks along -Z, so they call rotation around Z "roll".
00121   float pitch = quat.getYaw( false ).valueRadians(); // OGRE camera frame has +Y as "up", so they call rotation around Y "yaw".
00122 
00123   Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z;
00124 
00125   if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
00126   {
00127     if ( pitch > Ogre::Math::HALF_PI )
00128     {
00129       pitch -= Ogre::Math::PI;
00130     }
00131     else if ( pitch < -Ogre::Math::HALF_PI )
00132     {
00133       pitch += Ogre::Math::PI;
00134     }
00135 
00136     yaw = -yaw;
00137 
00138     if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 )
00139     {
00140       yaw -= Ogre::Math::PI;
00141     }
00142     else
00143     {
00144       yaw += Ogre::Math::PI;
00145     }
00146   }
00147 
00148   pitch_property_->setFloat( pitch );
00149   yaw_property_->setFloat( mapAngleTo0_2Pi( yaw ));
00150   position_property_->setVector( source_camera->getPosition() );
00151 }
00152 
00153 void FPSMotionViewController::mimic( ViewController* source_view )
00154 {
00155   FramePositionTrackingViewController::mimic( source_view );
00156   setPropertiesFromCamera( source_view->getCamera() );
00157 }
00158 
00159 void FPSMotionViewController::update(float dt, float ros_dt)
00160 {
00161   FramePositionTrackingViewController::update( dt, ros_dt );
00162   updateCamera();
00163 }
00164 
00165 void FPSMotionViewController::lookAt( const Ogre::Vector3& point )
00166 {
00167   camera_->lookAt( point );
00168   setPropertiesFromCamera( camera_ );
00169 }
00170 
00171 void FPSMotionViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00172 {
00173   position_property_->add( old_reference_position - reference_position_ );
00174 }
00175 
00176 void FPSMotionViewController::updateCamera()
00177 {
00178   camera_->setOrientation( getOrientation() );
00179   camera_->setPosition( position_property_->getVector() );
00180 }
00181 
00182 void FPSMotionViewController::updateCamera(Ogre::Vector3& position, Ogre::Quaternion& orientation )
00183 {
00184   camera_->setOrientation(orientation);
00185   camera_->setPosition(position);
00186   context_->queueRender();
00187 }
00188 
00189 void FPSMotionViewController::yaw( float angle )
00190 {
00191   yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() + angle ));
00192 }
00193 
00194 void FPSMotionViewController::pitch( float angle )
00195 {
00196   pitch_property_->add( angle );
00197 }
00198 
00199 Ogre::Quaternion FPSMotionViewController::getOrientation()
00200 {
00201   Ogre::Quaternion pitch, yaw;
00202 
00203   yaw.FromAngleAxis( Ogre::Radian( yaw_property_->getFloat() ), Ogre::Vector3::UNIT_Z );
00204   pitch.FromAngleAxis( Ogre::Radian( pitch_property_->getFloat() ), Ogre::Vector3::UNIT_Y );
00205 
00206   return yaw * pitch * ROBOT_TO_CAMERA_ROTATION;
00207 }
00208 
00209 void FPSMotionViewController::move( float x, float y, float z )
00210 {
00211   Ogre::Vector3 translate( x, y, z );
00212   Ogre::Vector3 update = getOrientation() * translate;
00213   update.z = 0.0;
00214   position_property_->add( update );
00215 }
00216 
00217 void FPSMotionViewController::changeZ(float z)
00218 {
00219   Ogre::Vector3 translate( 0.0, 0.0, z );
00220   Ogre::Vector3 update = getOrientation() * translate;
00221   position_property_->add( translate );
00222 }
00223 
00224 void FPSMotionViewController::fly( float x, float y, float z )
00225 {
00226   Ogre::Vector3 translate( x, y, z );
00227   Ogre::Vector3 update = getOrientation() * translate;
00228   position_property_->add( update );
00229 }
00230 
00231 } // end namespace rviz
00232 
00233 #include <pluginlib/class_list_macros.h>
00234 PLUGINLIB_EXPORT_CLASS(rviz::FPSMotionViewController, rviz::ViewController)


rviz_fps_plugin
Author(s): hdeeken
autogenerated on Thu Jun 6 2019 20:43:58