tablet_view_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, JSK Lab, 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 JSK Lab, 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  * Author: Adam Leeper
00030  * Author: Ryohei Ueda
00031  */
00032 
00033 #include "tablet_view_controller.h"
00034 
00035 #include "rviz/load_resource.h"
00036 #include "rviz/uniform_string_stream.h"
00037 #include "rviz/display_context.h"
00038 #include "rviz/viewport_mouse_event.h"
00039 #include "rviz/frame_manager.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 #include "rviz/properties/tf_frame_property.h"
00046 #include "rviz/properties/editable_enum_property.h"
00047 #include "rviz/properties/ros_topic_property.h"
00048 
00049 #include "view_controller_msgs/CameraPlacement.h"
00050 #include "geometry_msgs/PointStamped.h"
00051 
00052 #include <OGRE/OgreViewport.h>
00053 #include <OGRE/OgreQuaternion.h>
00054 #include <OGRE/OgreVector3.h>
00055 #include <OGRE/OgreSceneNode.h>
00056 #include <OGRE/OgreSceneManager.h>
00057 #include <OGRE/OgreCamera.h>
00058 
00059 #include <rviz/render_panel.h>
00060 #include <rviz/view_manager.h>
00061 #include <rviz/ogre_helpers/render_widget.h>
00062 #include <OGRE/OgreRenderWindow.h>
00063 
00064 namespace jsk_rviz_plugins
00065 {
00066 using namespace view_controller_msgs;
00067 using namespace rviz;
00068 
00069 // Strings for selecting control mode styles
00070 static const std::string MODE_ORBIT = "Orbit";
00071 static const std::string MODE_FPS = "FPS";
00072 
00073 // Limits to prevent orbit controller singularity, but not currently used.
00074 //static const Ogre::Radian PITCH_LIMIT_LOW  = Ogre::Radian(-Ogre::Math::HALF_PI + 0.02);
00075 //static const Ogre::Radian PITCH_LIMIT_HIGH = Ogre::Radian( Ogre::Math::HALF_PI - 0.02);
00076 static const Ogre::Radian PITCH_LIMIT_LOW  = Ogre::Radian( 0.02 );
00077 static const Ogre::Radian PITCH_LIMIT_HIGH = Ogre::Radian( Ogre::Math::PI - 0.02);
00078 
00079 
00080 // Some convenience functions for Ogre / geometry_msgs conversions
00081 static inline Ogre::Vector3 vectorFromMsg(const geometry_msgs::Point &m) { return Ogre::Vector3(m.x, m.y, m.z); }
00082 static inline Ogre::Vector3 vectorFromMsg(const geometry_msgs::Vector3 &m) { return Ogre::Vector3(m.x, m.y, m.z); }
00083 static inline geometry_msgs::Point pointOgreToMsg(const Ogre::Vector3 &o)
00084 {
00085   geometry_msgs::Point m;
00086   m.x = o.x; m.y = o.y; m.z = o.z;
00087   return m;
00088 }
00089 static inline void pointOgreToMsg(const Ogre::Vector3 &o, geometry_msgs::Point &m)  { m.x = o.x; m.y = o.y; m.z = o.z; }
00090 
00091 static inline geometry_msgs::Vector3 vectorOgreToMsg(const Ogre::Vector3 &o)
00092 {
00093   geometry_msgs::Vector3 m;
00094   m.x = o.x; m.y = o.y; m.z = o.z;
00095   return m;
00096 }
00097 static inline void vectorOgreToMsg(const Ogre::Vector3 &o, geometry_msgs::Vector3 &m) { m.x = o.x; m.y = o.y; m.z = o.z; }
00098 
00099 // -----------------------------------------------------------------------------
00100 
00101 
00102 TabletViewController::TabletViewController()
00103   : nh_(""), animate_(false), dragging_( false )
00104 {
00105   interaction_disabled_cursor_ = makeIconCursor( "package://rviz/icons/forbidden.svg" );
00106 
00107   mouse_enabled_property_ = new BoolProperty("Mouse Enabled", true,
00108                                    "Enables mouse control of the camera.",
00109                                    this);
00110   interaction_mode_property_ = new EditableEnumProperty("Control Mode", QString::fromStdString(MODE_ORBIT),
00111                                    "Select the style of mouse interaction.",
00112                                    this);
00113   interaction_mode_property_->addOptionStd(MODE_ORBIT);
00114   interaction_mode_property_->addOptionStd(MODE_FPS);
00115   interaction_mode_property_->setStdString(MODE_ORBIT);
00116 
00117   fixed_up_property_ = new BoolProperty( "Maintain Vertical Axis", true,
00118                                          "If enabled, the camera is not allowed to roll side-to-side.",
00119                                           this);
00120   attached_frame_property_ = new TfFrameProperty("Target Frame",
00121                                                  TfFrameProperty::FIXED_FRAME_STRING,
00122                                                  "TF frame the camera is attached to.",
00123                                                  this, NULL, true );
00124   eye_point_property_    = new VectorProperty( "Eye", Ogre::Vector3( 5, 5, 10 ),
00125                                               "Position of the camera.", this );
00126   focus_point_property_ = new VectorProperty( "Focus", Ogre::Vector3::ZERO,
00127                                               "Position of the focus/orbit point.", this );
00128   up_vector_property_ = new VectorProperty( "Up", Ogre::Vector3::UNIT_Z,
00129                                             "The vector which maps to \"up\" in the camera image plane.", this );
00130   distance_property_    = new FloatProperty( "Distance", getDistanceFromCameraToFocalPoint(),
00131                                              "The distance between the camera position and the focus point.",
00132                                              this );
00133   distance_property_->setMin( 0.01 );
00134   default_transition_time_property_ = new FloatProperty( "Transition Time", 0.5,
00135                                                          "The default time to use for camera transitions.",
00136                                                          this );
00137   camera_placement_topic_property_ = new RosTopicProperty("Placement Topic", "/rviz/camera_placement",
00138                                                           QString::fromStdString(ros::message_traits::datatype<view_controller_msgs::CameraPlacement>() ),
00139                                                           "Topic for CameraPlacement messages", this, SLOT(updateTopics()));
00140   
00141   camera_placement_publish_topic_property_ = new RosTopicProperty("Placement Publish Topic", "/rviz/current_camera_placement",
00142                                                           QString::fromStdString(ros::message_traits::datatype<view_controller_msgs::CameraPlacement>() ),
00143                                                           "Publishing Topic for CameraPlacement messages", this, SLOT(updatePublishTopics()));
00144 
00145   mouse_point_publish_topic_property_ = new RosTopicProperty("Placement Mouse Point", "/rviz/current_mouse_point",
00146                                                              QString::fromStdString(ros::message_traits::datatype<geometry_msgs::PointStamped>() ),
00147                                                              "Publishing position of mouse", this, SLOT(updateMousePointPublishTopics()));
00148 
00149 //  camera_placement_trajectory_topic_property_ = new RosTopicProperty("Trajectory Topic", "/rviz/camera_placement_trajectory",
00150 //                                                          QString::fromStdString(ros::message_traits::datatype<view_controller_msgs::CameraPlacementTrajectory>() ),
00151 //                                                          "Topic for CameraPlacementTrajectory messages", this, SLOT(updateTopics()));
00152 }
00153 
00154 TabletViewController::~TabletViewController()
00155 {
00156     delete focal_shape_;
00157     context_->getSceneManager()->destroySceneNode( attached_scene_node_ );
00158 }
00159 
00160 void TabletViewController::updatePublishTopics()
00161 {
00162   placement_publisher_ = nh_.advertise<view_controller_msgs::CameraPlacement>
00163     (camera_placement_publish_topic_property_->getStdString(), 1);
00164 }
00165 
00166 void TabletViewController::updateMousePointPublishTopics()
00167 {
00168   mouse_point_publisher_ = nh_.advertise<geometry_msgs::PointStamped>
00169     (mouse_point_publish_topic_property_->getStdString(), 1);
00170 }
00171 
00172 void TabletViewController::publishMouseEvent(rviz::ViewportMouseEvent& event)
00173 {
00174   geometry_msgs::PointStamped msg;
00175   msg.header.frame_id = context_->getFixedFrame().toStdString();
00176   msg.header.stamp = ros::Time::now();
00177   rviz::ViewManager* manager = context_->getViewManager();
00178   rviz::RenderPanel* panel = manager->getRenderPanel();
00179   Ogre::RenderWindow* window = panel->getRenderWindow();
00180   msg.point.x = (double)event.x / window->getWidth();
00181   msg.point.y = (double)event.y / window->getHeight();
00182   msg.point.z = 0;
00183   mouse_point_publisher_.publish(msg);
00184 }
00185   
00186 void TabletViewController::publishCurrentPlacement()
00187 {
00188   view_controller_msgs::CameraPlacement msg;
00189   ros::Time now = ros::Time::now();
00190   msg.target_frame = attached_frame_property_->getFrameStd();
00191   std::string fixed_frame = context_->getFixedFrame().toStdString();
00192   // eye
00193   msg.eye.header.stamp = now;
00194   msg.eye.header.frame_id = fixed_frame;
00195   Ogre::Vector3 eye = eye_point_property_->getVector();
00196   msg.eye.point.x = eye[0];
00197   msg.eye.point.y = eye[1];
00198   msg.eye.point.z = eye[2];
00199   // focus
00200   msg.focus.header.stamp = now;
00201   msg.focus.header.frame_id = fixed_frame;
00202   Ogre::Vector3 focus = focus_point_property_->getVector();
00203   msg.focus.point.x = focus[0];
00204   msg.focus.point.y = focus[1];
00205   msg.focus.point.z = focus[2];
00206   // up
00207   msg.up.header.stamp = now;
00208   msg.up.header.frame_id = fixed_frame;
00209   Ogre::Vector3 up = up_vector_property_->getVector();
00210   msg.up.vector.x = up[0];
00211   msg.up.vector.y = up[1];
00212   msg.up.vector.z = up[2];
00213 
00214   placement_publisher_.publish(msg);
00215 }
00216   
00217 void TabletViewController::updateTopics()
00218 {
00219 //  trajectory_subscriber_ = nh_.subscribe<view_controller_msgs::CameraPlacementTrajectory>
00220 //                              (camera_placement_trajectory_topic_property_->getStdString(), 1,
00221 //                              boost::bind(&TabletViewController::cameraPlacementTrajectoryCallback, this, _1));
00222   placement_subscriber_  = nh_.subscribe<view_controller_msgs::CameraPlacement>
00223                               (camera_placement_topic_property_->getStdString(), 1,
00224                               boost::bind(&TabletViewController::cameraPlacementCallback, this, _1));
00225 }
00226 
00227 void TabletViewController::onInitialize()
00228 {
00229     attached_frame_property_->setFrameManager( context_->getFrameManager() );
00230     attached_scene_node_ = context_->getSceneManager()->getRootSceneNode()->createChildSceneNode();
00231     camera_->detachFromParent();
00232     attached_scene_node_->attachObject( camera_ );
00233 
00234     camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00235 
00236     focal_shape_ = new Shape(Shape::Sphere, context_->getSceneManager(), attached_scene_node_);
00237     focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
00238     focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
00239     focal_shape_->getRootNode()->setVisible(false);
00240 
00241 }
00242 
00243 void TabletViewController::onActivate()
00244 {
00245   updateAttachedSceneNode();
00246 
00247   // Before activation, changes to target frame property should have
00248   // no side-effects.  After activation, changing target frame
00249   // property has the side effect (typically) of changing an offset
00250   // property so that the view does not jump.  Therefore we make the
00251   // signal/slot connection from the property here in onActivate()
00252   // instead of in the constructor.
00253   connect( attached_frame_property_, SIGNAL( changed() ), this, SLOT( updateAttachedFrame() ));
00254   connect( fixed_up_property_,       SIGNAL( changed() ), this, SLOT( onUpPropertyChanged() ));
00255   connectPositionProperties();
00256 
00257   // Only do this once activated!
00258   updateTopics();
00259   updatePublishTopics();
00260   updateMousePointPublishTopics();
00261 }
00262 
00263 void TabletViewController::connectPositionProperties()
00264 {
00265   connect( distance_property_,    SIGNAL( changed() ), this, SLOT( onDistancePropertyChanged() ), Qt::UniqueConnection);
00266   connect( eye_point_property_,   SIGNAL( changed() ), this, SLOT( onEyePropertyChanged() ),      Qt::UniqueConnection);
00267   connect( focus_point_property_, SIGNAL( changed() ), this, SLOT( onFocusPropertyChanged() ),    Qt::UniqueConnection);
00268   connect( up_vector_property_,   SIGNAL( changed() ), this, SLOT( onUpPropertyChanged() ),       Qt::UniqueConnection);
00269 }
00270 
00271 void TabletViewController::disconnectPositionProperties()
00272 {
00273   disconnect( distance_property_,    SIGNAL( changed() ), this, SLOT( onDistancePropertyChanged() ));
00274   disconnect( eye_point_property_,   SIGNAL( changed() ), this, SLOT( onEyePropertyChanged() ));
00275   disconnect( focus_point_property_, SIGNAL( changed() ), this, SLOT( onFocusPropertyChanged() ));
00276   disconnect( up_vector_property_,   SIGNAL( changed() ), this, SLOT( onUpPropertyChanged() ));
00277 }
00278 
00279 void TabletViewController::onEyePropertyChanged()
00280 {
00281   distance_property_->setFloat(getDistanceFromCameraToFocalPoint());
00282 }
00283 
00284 void TabletViewController::onFocusPropertyChanged()
00285 {
00286   distance_property_->setFloat(getDistanceFromCameraToFocalPoint());
00287 }
00288 
00289 void TabletViewController::onDistancePropertyChanged()
00290 {
00291   disconnectPositionProperties();
00292   Ogre::Vector3 new_eye_position = focus_point_property_->getVector() + distance_property_->getFloat()* camera_->getOrientation().zAxis();
00293   eye_point_property_->setVector(new_eye_position);
00294   connectPositionProperties();
00295 }
00296 
00297 void TabletViewController::onUpPropertyChanged()
00298 {
00299   disconnect( up_vector_property_,   SIGNAL( changed() ), this, SLOT( onUpPropertyChanged() ));
00300   if(fixed_up_property_->getBool()){
00301     up_vector_property_->setVector(Ogre::Vector3::UNIT_Z);
00302     camera_->setFixedYawAxis(true, reference_orientation_ * Ogre::Vector3::UNIT_Z);
00303   }
00304   else {
00305     // force orientation to match up vector; first call doesn't actually change the quaternion
00306     camera_->setFixedYawAxis(true, reference_orientation_ * up_vector_property_->getVector());
00307     camera_->setDirection( reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector()));
00308     // restore normal behavior
00309     camera_->setFixedYawAxis(false);
00310   }
00311   connect( up_vector_property_,   SIGNAL( changed() ), this, SLOT( onUpPropertyChanged() ),       Qt::UniqueConnection);
00312 }
00313 
00314 void TabletViewController::updateAttachedFrame()
00315 {
00316   Ogre::Vector3 old_position = attached_scene_node_->getPosition();
00317   Ogre::Quaternion old_orientation = attached_scene_node_->getOrientation();
00318 
00319   updateAttachedSceneNode();
00320 
00321   onAttachedFrameChanged( old_position, old_orientation );
00322 }
00323 
00324 void TabletViewController::updateAttachedSceneNode()
00325 {
00326   Ogre::Vector3 new_reference_position;
00327   Ogre::Quaternion new_reference_orientation;
00328 
00329   bool queue = false;
00330   if( context_->getFrameManager()->getTransform( attached_frame_property_->getFrameStd(), ros::Time(),
00331                                                  new_reference_position, new_reference_orientation ))
00332   {
00333     attached_scene_node_->setPosition( new_reference_position );
00334     attached_scene_node_->setOrientation( new_reference_orientation );
00335     reference_position_ = new_reference_position;
00336     reference_orientation_ = new_reference_orientation;
00337     queue = true;
00338   }
00339   if(queue) context_->queueRender();
00340 }
00341 
00342 void TabletViewController::onAttachedFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00343 {
00344   Ogre::Vector3 fixed_frame_focus_position = old_reference_orientation*focus_point_property_->getVector() + old_reference_position;
00345   Ogre::Vector3 fixed_frame_eye_position = old_reference_orientation*eye_point_property_->getVector() + old_reference_position;
00346   Ogre::Vector3 new_focus_position = fixedFrameToAttachedLocal(fixed_frame_focus_position);
00347   Ogre::Vector3 new_eye_position = fixedFrameToAttachedLocal(fixed_frame_eye_position);
00348   Ogre::Vector3 new_up_vector = reference_orientation_.Inverse()*old_reference_orientation*up_vector_property_->getVector();
00349 
00350   //Ogre::Quaternion new_camera_orientation = reference_orientation_.Inverse()*old_reference_orientation*getOrientation();
00351 
00352   focus_point_property_->setVector(new_focus_position);
00353   eye_point_property_->setVector(new_eye_position);
00354   up_vector_property_->setVector(fixed_up_property_->getBool() ? Ogre::Vector3::UNIT_Z : new_up_vector);
00355   distance_property_->setFloat( getDistanceFromCameraToFocalPoint());
00356 
00357   // force orientation to match up vector; first call doesn't actually change the quaternion
00358   camera_->setFixedYawAxis(true, reference_orientation_ * up_vector_property_->getVector());
00359   camera_->setDirection( reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector()));
00360 }
00361 
00362 float TabletViewController::getDistanceFromCameraToFocalPoint()
00363 {
00364     return (eye_point_property_->getVector() - focus_point_property_->getVector()).length();
00365 }
00366 
00367 void TabletViewController::reset()
00368 {
00369     eye_point_property_->setVector(Ogre::Vector3(5, 5, 10));
00370     focus_point_property_->setVector(Ogre::Vector3::ZERO);
00371     up_vector_property_->setVector(Ogre::Vector3::UNIT_Z);
00372     distance_property_->setFloat( getDistanceFromCameraToFocalPoint());
00373     mouse_enabled_property_->setBool(true);
00374     interaction_mode_property_->setStdString(MODE_ORBIT);
00375 
00376 
00377   // Hersh says: why is the following junk necessary?  I don't know.
00378   // However, without this you need to call reset() twice after
00379   // switching from TopDownOrtho to FPS.  After the first call the
00380   // camera is in the right position but pointing the wrong way.
00381   updateCamera();
00382   camera_->lookAt( 0, 0, 0 );
00383   setPropertiesFromCamera( camera_ );
00384 }
00385 
00386 void TabletViewController::handleMouseEvent(ViewportMouseEvent& event)
00387 {
00388   if( !mouse_enabled_property_->getBool() )
00389   {
00390     setCursor( interaction_disabled_cursor_ );
00391     setStatus( "<b>Mouse interaction is disabled. You can enable it by checking the \"Mouse Enabled\" check-box in the Views panel." );
00392     return;
00393   }
00394   else if ( event.shift() )
00395   {
00396     setStatus( "TODO: Fix me! <b>Left-Click:</b> Move X/Y.  <b>Right-Click:</b>: Move Z." );
00397   }
00398   else if ( event.control() )
00399   {
00400     setStatus( "TODO: Fix me! <b>Left-Click:</b> Move X/Y.  <b>Right-Click:</b>: Move Z." );
00401   }
00402   else
00403   {
00404     setStatus( "TODO: Fix me! <b>Left-Click:</b> Rotate.  <b>Middle-Click:</b> Move X/Y.  <b>Right-Click:</b>: Zoom.  <b>Shift</b>: More options." );
00405   }
00406   if (event.type == QEvent::MouseButtonPress
00407       || event.type == QEvent::MouseButtonRelease
00408       || dragging_ && event.type == QEvent::MouseMove) {
00409     publishMouseEvent(event);
00410   }
00411   float distance = distance_property_->getFloat();
00412   int32_t diff_x = 0;
00413   int32_t diff_y = 0;
00414   bool moved = false;
00415 
00416   if( event.type == QEvent::MouseButtonPress )
00417   {
00418     focal_shape_->getRootNode()->setVisible(true);
00419     moved = true;
00420     dragging_ = true;
00421     cancelTransition();  // Stop any automated movement
00422   }
00423   else if( event.type == QEvent::MouseButtonRelease )
00424   {
00425     focal_shape_->getRootNode()->setVisible(false);
00426     moved = true;
00427     dragging_ = false;
00428   }
00429   else if( dragging_ && event.type == QEvent::MouseMove )
00430   {
00431     diff_x = event.x - event.last_x;
00432     diff_y = event.y - event.last_y;
00433     moved = true;
00434   }
00435 
00436   // regular left-button drag
00437   if( event.left() && !event.shift() )
00438   {
00439     setCursor( Rotate3D );
00440     yaw_pitch_roll( -diff_x*0.005, -diff_y*0.005, 0 );
00441   }
00442   // middle or shift-left drag
00443   else if( event.middle() || ( event.shift() && event.left() ))
00444   {
00445     setCursor( MoveXY );
00446     if(interaction_mode_property_->getStdString() == MODE_ORBIT)  // Orbit style
00447     {
00448         float fovY = camera_->getFOVy().valueRadians();
00449         float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );
00450 
00451         int width = camera_->getViewport()->getActualWidth();
00452         int height = camera_->getViewport()->getActualHeight();
00453 
00454         move_focus_and_eye( -((float)diff_x / (float)width) * distance * tan( fovX / 2.0f ) * 2.0f,
00455               ((float)diff_y / (float)height) * distance * tan( fovY / 2.0f ) * 2.0f,
00456               0.0f );
00457     }
00458     else if(interaction_mode_property_->getStdString() == MODE_FPS)  // Orbit style
00459     {
00460       move_focus_and_eye( diff_x*0.01, -diff_y*0.01, 0.0f );
00461     }
00462   }
00463   else if( event.right() )
00464   {
00465     if( event.shift() ||  (interaction_mode_property_->getStdString() == MODE_FPS) )
00466     {
00467       setCursor( MoveZ );
00468       move_focus_and_eye(0.0f, 0.0f, diff_y * 0.01 * distance);
00469     }
00470     else
00471     {
00472       setCursor( Zoom );
00473       move_eye( 0, 0, diff_y * 0.01 * distance );
00474     }
00475   }
00476   else
00477   {
00478     setCursor( event.shift() ? MoveXY : Rotate3D );
00479   }
00480 
00481   if ( event.wheel_delta != 0 )
00482   {
00483     int diff = event.wheel_delta;
00484 
00485     if( event.shift() )
00486     {
00487       move_focus_and_eye(0, 0, -diff * 0.001 * distance );
00488     }
00489     else if(event.control())
00490     {
00491       yaw_pitch_roll(0, 0, diff*0.001 );
00492     }
00493     else
00494     {
00495       move_eye( 0, 0, -diff * 0.001 * distance );
00496     }
00497     moved = true;
00498   }
00499 
00500   if(event.type == QEvent::MouseButtonPress && event.left() && event.control() && event.shift())
00501   {
00502     bool was_orbit = (interaction_mode_property_->getStdString() == MODE_ORBIT);
00503     interaction_mode_property_->setStdString(was_orbit ? MODE_FPS : MODE_ORBIT );
00504   }
00505 
00506   if (moved)
00507   {
00508     publishCurrentPlacement();
00509     context_->queueRender();
00510   }
00511 }
00512 
00513 //void TabletViewController::setUpVectorPropertyModeDependent( const Ogre::Vector3 &vector )
00514 //{
00515 //  if(fixed_up_property_->getBool())
00516 //  {
00517 //    //up_vector_property_->setVector(Ogre::Vector3::UNIT_Z);
00518 //  }
00519 //  else {
00520 //    up_vector_property_->setVector(vector);
00521 //  }
00522 //}
00523 
00524 
00525 void TabletViewController::setPropertiesFromCamera( Ogre::Camera* source_camera )
00526 {
00527   disconnectPositionProperties();
00528   Ogre::Vector3 direction = source_camera->getOrientation() * Ogre::Vector3::NEGATIVE_UNIT_Z;
00529   eye_point_property_->setVector( source_camera->getPosition() );
00530   focus_point_property_->setVector( source_camera->getPosition() + direction*distance_property_->getFloat());
00531   if(fixed_up_property_->getBool())
00532     up_vector_property_->setVector(Ogre::Vector3::UNIT_Z);
00533   else
00534     up_vector_property_->setVector(source_camera->getOrientation().yAxis());
00535 
00536   //setUpVectorPropertyModeDependent(source_camera->getOrientation().yAxis());
00537   connectPositionProperties();
00538 }
00539 
00540 void TabletViewController::mimic( ViewController* source_view )
00541 {
00542     QVariant target_frame = source_view->subProp( "Target Frame" )->getValue();
00543     if( target_frame.isValid() )
00544     {
00545       attached_frame_property_->setValue( target_frame );
00546     }
00547 
00548     Ogre::Camera* source_camera = source_view->getCamera();
00549     Ogre::Vector3 position = source_camera->getPosition();
00550     Ogre::Quaternion orientation = source_camera->getOrientation();
00551 
00552     if( source_view->getClassId() == "rviz/Orbit" )
00553     {
00554         distance_property_->setFloat( source_view->subProp( "Distance" )->getValue().toFloat() );
00555     }
00556     else
00557     {
00558         distance_property_->setFloat( position.length() );
00559     }
00560     interaction_mode_property_->setStdString( MODE_ORBIT );
00561 
00562     Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_property_->getFloat() );
00563     focus_point_property_->setVector( position + direction );
00564     eye_point_property_->setVector(position);
00565     updateCamera();
00566 }
00567 
00568 void TabletViewController::transitionFrom( ViewController* previous_view )
00569 {
00570   TabletViewController* fvc = dynamic_cast<TabletViewController*>(previous_view);
00571   if(fvc)
00572   {
00573     Ogre::Vector3 new_eye =   eye_point_property_->getVector();
00574     Ogre::Vector3 new_focus = focus_point_property_->getVector();
00575     Ogre::Vector3 new_up =    up_vector_property_->getVector();
00576 
00577     eye_point_property_->setVector(fvc->eye_point_property_->getVector());
00578     focus_point_property_->setVector(fvc->focus_point_property_->getVector());
00579     up_vector_property_->setVector(fvc->up_vector_property_->getVector());
00580 
00581     beginNewTransition(new_eye, new_focus, new_up, ros::Duration(default_transition_time_property_->getFloat()));
00582   }
00583 }
00584 
00585 void TabletViewController::beginNewTransition(const Ogre::Vector3 &eye, const Ogre::Vector3 &focus, const Ogre::Vector3 &up,
00586                                             const ros::Duration &transition_time)
00587 {
00588   if(ros::Duration(transition_time).isZero())
00589   {
00590     eye_point_property_->setVector(eye);
00591     focus_point_property_->setVector(focus);
00592     up_vector_property_->setVector(up);
00593     distance_property_->setFloat(getDistanceFromCameraToFocalPoint());
00594     return;
00595   }
00596 
00597   start_position_ = eye_point_property_->getVector();
00598   goal_position_ = eye;
00599 
00600   start_focus_ = focus_point_property_->getVector();
00601   goal_focus_ = focus;
00602 
00603 //  start_up_ = fixed_up_property_->getBool() ? (Ogre::Vector3::UNIT_Z) : getOrientation().yAxis();
00604 //  goal_up_ =  fixed_up_property_->getBool() ? (Ogre::Vector3::UNIT_Z) : up;
00605   start_up_ = up_vector_property_->getVector();
00606   goal_up_ =  up;
00607 
00608   current_transition_duration_ = ros::Duration(transition_time);
00609   transition_start_time_ = ros::Time::now();
00610 
00611   animate_ = true;
00612 }
00613 
00614 void TabletViewController::cancelTransition()
00615 {
00616   animate_ = false;
00617 }
00618 
00619 void TabletViewController::cameraPlacementCallback(const CameraPlacementConstPtr &cp_ptr)
00620 {
00621   CameraPlacement cp = *cp_ptr;
00622 
00623   // Handle control parameters
00624   mouse_enabled_property_->setBool( !cp.interaction_disabled );
00625   fixed_up_property_->setBool( !cp.allow_free_yaw_axis );
00626   if(cp.mouse_interaction_mode != cp.NO_CHANGE)
00627   {
00628     std::string name = "";
00629     if(cp.mouse_interaction_mode == cp.ORBIT) name = MODE_ORBIT;
00630     else if(cp.mouse_interaction_mode == cp.FPS) name = MODE_FPS;
00631     interaction_mode_property_->setStdString(name);
00632   }
00633 
00634   if(cp.target_frame != "")
00635   {
00636     attached_frame_property_->setStdString(cp.target_frame);
00637     updateAttachedFrame();
00638   }
00639 
00640   if(cp.time_from_start.toSec() >= 0)
00641   {
00642     ROS_DEBUG_STREAM("Received a camera placement request! \n" << cp);
00643     transformCameraPlacementToAttachedFrame(cp);
00644     ROS_DEBUG_STREAM("After transform, we have \n" << cp);
00645 
00646     Ogre::Vector3 eye = vectorFromMsg(cp.eye.point);
00647     Ogre::Vector3 focus = vectorFromMsg(cp.focus.point);
00648     Ogre::Vector3 up = vectorFromMsg(cp.up.vector);
00649 
00650     beginNewTransition(eye, focus, up, cp.time_from_start);
00651   }
00652 }
00653 
00654 //void TabletViewController::cameraPlacementTrajectoryCallback(const CameraPlacementTrajectoryConstPtr &cptptr)
00655 //{
00656 //  CameraPlacementTrajectory cpt = *cptptr;
00657 //  ROS_DEBUG_STREAM("Received a camera placement trajectory request! \n" << cpt);
00658   
00659 //  // Handle control parameters
00660 //  mouse_enabled_property_->setBool( cpt.interaction_enabled );
00661 //  fixed_up_property_->setBool( !cpt.allow_free_yaw_axis );
00662 //  if(cpt.mouse_interaction_mode != cpt.NO_CHANGE)
00663 //  {
00664 //    std::string name = "";
00665 //    if(cpt.mouse_interaction_mode == cpt.ORBIT) name = MODE_ORBIT;
00666 //    else if(cpt.mouse_interaction_mode == cpt.FPS) name = MODE_FPS;
00667 //    interaction_mode_property_->setStdString(name);
00668 //  }
00669 
00670 //  // TODO should transform the interpolated positions (later), or transform info will only reflect the TF tree state at the beginning...
00671 //  for(size_t i = 0; i<cpt.placements.size(); i++)
00672 //  {
00673 //    transformCameraPlacementToAttachedFrame(cpt.placements[i]);
00674 //  }
00675 
00676 //  // For now, just transition to the first placement until we put in the capacity for a trajectory
00677 //  CameraPlacement cp = cpt.placements[0];
00678 //  if(cp.target_frame != "")
00679 //  {
00680 //    attached_frame_property_->setStdString(cp.target_frame);
00681 //    updateAttachedFrame();
00682 //  }
00683 //  Ogre::Vector3 eye = vectorFromMsg(cp.eye.point);
00684 //  Ogre::Vector3 focus = vectorFromMsg(cp.focus.point);
00685 //  Ogre::Vector3 up = vectorFromMsg(cp.up.vector);
00686 
00687 //  beginNewTransition(eye, focus, up, cp.time_from_start);
00688 //}
00689 
00690 void TabletViewController::transformCameraPlacementToAttachedFrame(CameraPlacement &cp)
00691 {
00692   Ogre::Vector3 position_fixed_eye, position_fixed_focus, position_fixed_up; // position_fixed_attached;
00693   Ogre::Quaternion rotation_fixed_eye, rotation_fixed_focus, rotation_fixed_up; // rotation_fixed_attached;
00694 
00695   context_->getFrameManager()->getTransform(cp.eye.header.frame_id, ros::Time(0), position_fixed_eye, rotation_fixed_eye);
00696   context_->getFrameManager()->getTransform(cp.focus.header.frame_id,  ros::Time(0), position_fixed_focus, rotation_fixed_focus);
00697   context_->getFrameManager()->getTransform(cp.up.header.frame_id,  ros::Time(0), position_fixed_up, rotation_fixed_up);
00698   //context_->getFrameManager()->getTransform(attached_frame_property_->getStdString(),  ros::Time(0), position_fixed_attached, rotation_fixed_attached);
00699 
00700   Ogre::Vector3 eye = vectorFromMsg(cp.eye.point); 
00701   Ogre::Vector3 focus = vectorFromMsg(cp.focus.point); 
00702   Ogre::Vector3 up = vectorFromMsg(cp.up.vector); 
00703 
00704   eye = fixedFrameToAttachedLocal(position_fixed_eye + rotation_fixed_eye*eye);
00705   focus = fixedFrameToAttachedLocal(position_fixed_focus + rotation_fixed_focus*focus);
00706   up = reference_orientation_.Inverse()*rotation_fixed_up*up;
00707   //up = rotation_fixed_up*up;
00708 
00709   cp.eye.point = pointOgreToMsg(eye);
00710   cp.focus.point = pointOgreToMsg(focus);
00711   cp.up.vector = vectorOgreToMsg(up);
00712   cp.eye.header.frame_id = attached_frame_property_->getStdString();
00713   cp.focus.header.frame_id = attached_frame_property_->getStdString();
00714   cp.up.header.frame_id = attached_frame_property_->getStdString();
00715 }
00716 
00717 
00718 // We must assume that this point is in the Rviz Fixed frame since it came from Rviz...
00719 void TabletViewController::lookAt( const Ogre::Vector3& point )
00720 {
00721   if( !mouse_enabled_property_->getBool() ) return;
00722 
00723   Ogre::Vector3 new_point = fixedFrameToAttachedLocal(point);
00724 
00725   beginNewTransition(eye_point_property_->getVector(), new_point,
00726                      up_vector_property_->getVector(),
00727                      ros::Duration(default_transition_time_property_->getFloat()));
00728 
00729   //  // Just for easily testing the other movement styles:
00730   //  orbitCameraTo(point);
00731   //  moveCameraWithFocusTo(point);
00732 }
00733 
00734 void TabletViewController::orbitCameraTo( const Ogre::Vector3& point)
00735 {
00736   beginNewTransition(point, focus_point_property_->getVector(),
00737                      up_vector_property_->getVector(),
00738                      ros::Duration(default_transition_time_property_->getFloat()));
00739 }
00740 
00741 void TabletViewController::moveEyeWithFocusTo( const Ogre::Vector3& point)
00742 {
00743   beginNewTransition(point, focus_point_property_->getVector() + (point - eye_point_property_->getVector()),
00744                      up_vector_property_->getVector(),
00745                      ros::Duration(default_transition_time_property_->getFloat()));
00746 }
00747 
00748 
00749 void TabletViewController::update(float dt, float ros_dt)
00750 {
00751   updateAttachedSceneNode();
00752 
00753   if(animate_)
00754   {
00755     ros::Duration time_from_start = ros::Time::now() - transition_start_time_;
00756     float fraction = time_from_start.toSec()/current_transition_duration_.toSec();
00757     // make sure we get all the way there before turning off
00758     if(fraction > 1.0f)
00759     {
00760       fraction = 1.0f;
00761       animate_ = false;
00762     }
00763 
00764     // TODO remap progress to progress_out, which can give us a new interpolation profile.
00765     float progress = 0.5*(1-cos(fraction*M_PI));
00766 
00767     Ogre::Vector3 new_position = start_position_ + progress*(goal_position_ - start_position_);
00768     Ogre::Vector3 new_focus  = start_focus_ + progress*(goal_focus_ - start_focus_);
00769     Ogre::Vector3 new_up  = start_up_ + progress*(goal_up_ - start_up_);
00770 
00771     disconnectPositionProperties();
00772     eye_point_property_->setVector( new_position );
00773     focus_point_property_->setVector( new_focus );
00774     up_vector_property_->setVector(new_up);
00775     distance_property_->setFloat( getDistanceFromCameraToFocalPoint());
00776     connectPositionProperties();
00777 
00778     // This needs to happen so that the camera orientation will update properly when fixed_up_property == false
00779     camera_->setFixedYawAxis(true, reference_orientation_ * up_vector_property_->getVector());
00780     camera_->setDirection(reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector()));
00781   }
00782   updateCamera();
00783 }
00784 
00785 void TabletViewController::updateCamera()
00786 {
00787   camera_->setPosition( eye_point_property_->getVector() );
00788   camera_->setFixedYawAxis(fixed_up_property_->getBool(), reference_orientation_ * up_vector_property_->getVector());
00789   camera_->setDirection( reference_orientation_ * (focus_point_property_->getVector() - eye_point_property_->getVector()));
00790   //camera_->setDirection( (focus_point_property_->getVector() - eye_point_property_->getVector()));
00791   focal_shape_->setPosition( focus_point_property_->getVector() );
00792 }
00793 
00794 void TabletViewController::yaw_pitch_roll( float yaw, float pitch, float roll )
00795 {
00796   Ogre::Quaternion old_camera_orientation = camera_->getOrientation();
00797   Ogre::Radian old_pitch = old_camera_orientation.getPitch(false);// - Ogre::Radian(Ogre::Math::HALF_PI);
00798 
00799   if(fixed_up_property_->getBool()) yaw = cos(old_pitch.valueRadians() - Ogre::Math::HALF_PI)*yaw; // helps to reduce crazy spinning!
00800 
00801   Ogre::Quaternion yaw_quat, pitch_quat, roll_quat;
00802   yaw_quat.FromAngleAxis( Ogre::Radian( yaw ), Ogre::Vector3::UNIT_Y );
00803   pitch_quat.FromAngleAxis( Ogre::Radian( pitch ), Ogre::Vector3::UNIT_X );
00804   roll_quat.FromAngleAxis( Ogre::Radian( roll ), Ogre::Vector3::UNIT_Z );
00805   Ogre::Quaternion orientation_change = yaw_quat * pitch_quat * roll_quat;
00806   Ogre::Quaternion new_camera_orientation = old_camera_orientation * orientation_change;
00807   Ogre::Radian new_pitch = new_camera_orientation.getPitch(false);// - Ogre::Radian(Ogre::Math::HALF_PI);
00808 
00809   if( fixed_up_property_->getBool() &&
00810       ((new_pitch > PITCH_LIMIT_HIGH && new_pitch > old_pitch) || (new_pitch < PITCH_LIMIT_LOW && new_pitch < old_pitch)) )
00811   {
00812     orientation_change = yaw_quat * roll_quat;
00813     new_camera_orientation = old_camera_orientation * orientation_change;
00814   }
00815 
00816 //  Ogre::Radian new_roll = new_camera_orientation.getRoll(false);
00817 //  Ogre::Radian new_yaw = new_camera_orientation.getYaw(false);
00818   //ROS_INFO("old_pitch: %.3f, new_pitch: %.3f", old_pitch.valueRadians(), new_pitch.valueRadians());
00819 
00820   camera_->setOrientation( new_camera_orientation );
00821   if( interaction_mode_property_->getStdString() == MODE_ORBIT )
00822   {
00823     // In orbit mode the focal point stays fixed, so we need to compute the new camera position.
00824     Ogre::Vector3 new_eye_position = focus_point_property_->getVector() + distance_property_->getFloat()* new_camera_orientation.zAxis();
00825     eye_point_property_->setVector(new_eye_position);
00826     camera_->setPosition(new_eye_position);
00827     setPropertiesFromCamera(camera_);
00828   }
00829   else
00830   {
00831     // In FPS mode the camera stays fixed, so we can just apply the rotations and then rely on the property update to set the new focal point.
00832     setPropertiesFromCamera(camera_);
00833   }
00834 }
00835 
00836 Ogre::Quaternion TabletViewController::getOrientation()  // Do we need this?
00837 {
00838   return camera_->getOrientation();
00839 }
00840 
00841 void TabletViewController::move_focus_and_eye( float x, float y, float z )
00842 {
00843   Ogre::Vector3 translate( x, y, z );
00844   eye_point_property_->add( getOrientation() * translate );
00845   focus_point_property_->add( getOrientation() * translate );
00846 }
00847 
00848 void TabletViewController::move_eye( float x, float y, float z )
00849 {
00850   Ogre::Vector3 translate( x, y, z );
00851   // Only update the camera position if it won't "pass through" the origin
00852   Ogre::Vector3 new_position = eye_point_property_->getVector() + getOrientation() * translate;
00853   if( (new_position - focus_point_property_->getVector()).length() > distance_property_->getMin() )
00854     eye_point_property_->setVector(new_position);
00855   distance_property_->setFloat(getDistanceFromCameraToFocalPoint());
00856 }
00857 
00858 } // end namespace rviz
00859 
00860 #include <pluginlib/class_list_macros.h>
00861 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::TabletViewController, rviz::ViewController )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03