orbit_camera.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, 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 <rve_common_transformers/orbit_camera.h>
00031 #include <rve_transformer/transformer_manager.h>
00032 #include <rve_transformer/frame_manager.h>
00033 #include <rve_render_client/camera.h>
00034 #include <rve_render_client/perspective_projection_factory.h>
00035 #include <rve_properties/property_node.h>
00036 
00037 #include <rve_msgs/MouseEvent.h>
00038 
00039 #include <rve_common/eigen_conversions.h>
00040 #include <geometry_msgs/TransformStamped.h>
00041 #include <ros/callback_queue.h>
00042 #include <geometry_msgs/Pose.h>
00043 
00044 #include <sys/time.h>
00045 
00046 static const float MIN_DISTANCE = 0.01;
00047 
00048 namespace rve_common_transformers
00049 {
00050 
00051 OrbitCamera::OrbitCamera()
00052 : focal_point_(Eigen::Vector3f::Zero())
00053 , pitch_quat_(Eigen::Quaternionf::Identity())
00054 , yaw_quat_(Eigen::Quaternionf::Identity())
00055 , distance_(10.0f)
00056 , last_x_(-1)
00057 , last_y_(-1)
00058 , camera_projection_( new rve_render_client::PerspectiveProjectionFactory() )
00059 {
00060   setRelativePosition( Eigen::Vector3f( 0, 2, 2 ));
00061 }
00062 
00063 void OrbitCamera::onInit()
00064 {
00065   addSubscriber("mouse_input", 100, &OrbitCamera::mouseCallback, this);
00066   addSubscriber("set_camera_pose", 1, &OrbitCamera::setPoseCallback, this);
00067 
00068   std::string frame_id;
00069   getPropertyNode()->get(std::string("target_frame"), frame_id, std::string("map"));
00070   setReferenceFrame( frame_id );
00071   getPropertyNode()->addChangeCallback(boost::bind(&OrbitCamera::onPropertyChanged, this, _1, _2, _3), this, getCallbackQueue());
00072 }
00073 
00074 void OrbitCamera::setPoseCallback(const geometry_msgs::PoseConstPtr& pose)
00075 {
00076   // TODO: setting the focal point here may not really be what I want.
00077   // Maybe we need a separate message to set the focal point?  Need to
00078   // think more about the application behavior I want.  Maybe a new
00079   // type of CameraTransformer is what I really want.
00080   focal_point_.x() = pose->position.x;
00081   focal_point_.y() = pose->position.y;
00082   focal_point_.z() = pose->position.z;
00083   // TODO: translate pose->orientation into pitch_quat_ and yaw_quat_.
00084 }
00085 
00086 void OrbitCamera::mouseCallback(const rve_msgs::MouseEventConstPtr& evt)
00087 {
00088   if (last_x_ == -1)
00089   {
00090     last_x_ = evt->x;
00091     last_y_ = evt->y;
00092   }
00093 
00094   int32_t diff_x = evt->x - last_x_;
00095   int32_t diff_y = evt->y - last_y_;
00096   last_x_ = evt->x;
00097   last_y_ = evt->y;
00098 
00099   if (evt->type == rve_msgs::MouseEvent::EVENT_MOVE)
00100   {
00101     if (evt->buttons & rve_msgs::MouseEvent::BUTTON_LEFT)
00102     {
00103       yaw( diff_x*0.005 );
00104       pitch( -diff_y*0.005 );
00105     }
00106     if (evt->buttons & rve_msgs::MouseEvent::BUTTON_MIDDLE)
00107     {
00108       rve_msgs::Matrix4 proj_mat = getCamera()->getProjectionMatrix();
00109       float proj_width = 2 / proj_mat.m[0*4+0];
00110       float proj_height = 2 / proj_mat.m[1*4+1];
00111 
00112       uint32_t width = evt->window_width;
00113       uint32_t height = evt->window_height;
00114 
00115       move(0.0, ((float)diff_x / (float)width) * distance_ * proj_width, ((float)diff_y / (float)height) * distance_ * proj_height);
00116     }
00117     if (evt->buttons & rve_msgs::MouseEvent::BUTTON_RIGHT)
00118     {
00119       if (evt->modifiers & rve_msgs::MouseEvent::MODIFIER_SHIFT)
00120       {
00121         move(-diff_y * 0.1 * (distance_ / 10.0f), 0.0f, 0.0f);
00122       }
00123       else
00124       {
00125         zoom( -diff_y * 0.1 * (distance_ / 10.0f) );
00126       }
00127     }
00128   }
00129 }
00130 
00131 void OrbitCamera::onCameraSet()
00132 {
00133   FrameRelativeCameraController::onCameraSet();
00134   getCamera()->setProjectionMatrixFactory( camera_projection_ );
00135 }
00136 
00137 void OrbitCamera::onPropertyChanged(const rve_properties::PropertyNodePtr& node, const std::string& path, rve_properties::PropertyChangeType type)
00138 {
00139   if (type != rve_properties::PropertyChanged)
00140   {
00141     return;
00142   }
00143 
00144   if (path == "target_frame")
00145   {
00146     setReferenceFrame( node->get<std::string>() );
00147   }
00148 }
00149 
00150 void OrbitCamera::updateRelativePose()
00151 {
00152   Eigen::Vector3f mouse_pos;
00153   Eigen::Quaternionf mouse_rot;
00154 
00155   mouse_rot = yaw_quat_ * pitch_quat_;
00156   mouse_pos = focal_point_ - distance_ * (mouse_rot * Eigen::Vector3f::UnitX());
00157   
00158   setRelativePosition( mouse_pos );
00159   setRelativeOrientation( mouse_rot );
00160 }
00161 
00162 void OrbitCamera::yaw( float angle )
00163 {
00164   yaw_quat_ = yaw_quat_ * Eigen::AngleAxisf(-angle, Eigen::Vector3f::UnitZ());
00165 }
00166 
00167 void OrbitCamera::pitch( float angle )
00168 {
00169   pitch_quat_ = pitch_quat_ * Eigen::AngleAxisf(-angle, Eigen::Vector3f::UnitY());
00170 }
00171 
00172 void OrbitCamera::zoom( float amount )
00173 {
00174   distance_ -= amount;
00175 
00176   if ( distance_ <= MIN_DISTANCE )
00177   {
00178     distance_ = MIN_DISTANCE;
00179   }
00180 }
00181 
00182 void OrbitCamera::move( float x, float y, float z )
00183 {
00184   focal_point_ += getAbsoluteOrientation() * Eigen::Vector3f( x, y, z );
00185 }
00186 
00187 } // namespace rve_common_transformers


rve_common_transformers
Author(s): Josh Faust
autogenerated on Wed Dec 11 2013 14:31:58