Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <math.h>
00031
00032 #include <rve_transformer/transformer_manager.h>
00033 #include <rve_transformer/frame_manager.h>
00034 #include <rve_render_client/camera.h>
00035 #include <rve_render_client/orthographic_projection_factory.h>
00036 #include <rve_properties/property_node.h>
00037
00038 #include <rve_msgs/MouseEvent.h>
00039
00040 #include <rve_common/eigen_conversions.h>
00041 #include <geometry_msgs/TransformStamped.h>
00042 #include <ros/callback_queue.h>
00043 #include <geometry_msgs/Pose.h>
00044
00045 #include <sys/time.h>
00046
00047 #include "rve_common_transformers/fixed_orientation_ortho_camera.h"
00048
00049 namespace rve_common_transformers
00050 {
00051
00052 FixedOrientationOrthoCamera::FixedOrientationOrthoCamera()
00053 : orientation_(1/sqrtf(2), 0, 1/sqrtf(2), 0)
00054 , position_rel_target_(50, 50, 100)
00055 , position_(0.0, 0.0, 100.0)
00056 , view_height_(100.0f)
00057 , last_x_(-1)
00058 , last_y_(-1)
00059 , camera_projection_( new rve_render_client::OrthographicProjectionFactory() )
00060 {
00061 }
00062
00063 FixedOrientationOrthoCamera::~FixedOrientationOrthoCamera()
00064 {
00065 }
00066
00067 void FixedOrientationOrthoCamera::onInit()
00068 {
00069 addSubscriber("mouse_input", 100, &FixedOrientationOrthoCamera::mouseCallback, this);
00070
00071 getPropertyNode()->get(std::string("target_frame"), target_frame_, std::string("map"));
00072 getPropertyNode()->addChangeCallback(boost::bind(&FixedOrientationOrthoCamera::onPropertyChanged, this, _1, _2, _3), this, getCallbackQueue());
00073 }
00074
00075 void FixedOrientationOrthoCamera::mouseCallback(const rve_msgs::MouseEventConstPtr& evt)
00076 {
00077 if (last_x_ == -1)
00078 {
00079 last_x_ = evt->x;
00080 last_y_ = evt->y;
00081 }
00082
00083 int32_t diff_x = evt->x - last_x_;
00084 int32_t diff_y = evt->y - last_y_;
00085 last_x_ = evt->x;
00086 last_y_ = evt->y;
00087
00088 if (evt->type == rve_msgs::MouseEvent::EVENT_MOVE)
00089 {
00090 if (evt->buttons & rve_msgs::MouseEvent::BUTTON_LEFT)
00091 {
00092 move( diff_y * view_height_ / float(evt->window_height),
00093 diff_x * view_height_ / float(evt->window_height),
00094 0 );
00095 }
00096 if (evt->buttons & rve_msgs::MouseEvent::BUTTON_RIGHT)
00097 {
00098 zoom( diff_y );
00099 }
00100 }
00101 }
00102
00103 void FixedOrientationOrthoCamera::onUpdate()
00104 {
00105 updateCamera();
00106 }
00107
00108 void FixedOrientationOrthoCamera::onCameraSet()
00109 {
00110 getCamera()->setPosition(rve_common::eigenToMsg(position_));
00111 getCamera()->setOrientation(rve_common::eigenToMsg(orientation_));
00112 getCamera()->setProjectionMatrixFactory( camera_projection_ );
00113 }
00114
00115 void FixedOrientationOrthoCamera::onPropertyChanged(const rve_properties::PropertyNodePtr& node, const std::string& path, rve_properties::PropertyChangeType type)
00116 {
00117 if (type != rve_properties::PropertyChanged)
00118 {
00119 return;
00120 }
00121
00122 if (path == "target_frame")
00123 {
00124 target_frame_ = node->get<std::string>();
00125 }
00126 }
00127
00128 void FixedOrientationOrthoCamera::updateCamera()
00129 {
00130
00131 geometry_msgs::TransformStamped target = getManager()->getFrameManager()->lookupTransform(target_frame_, ros::Time());
00132 geometry_msgs::Vector3 p = target.transform.translation;
00133 Eigen::Vector3f target_pos(p.x, p.y, p.z);
00134
00135
00136 position_ = position_rel_target_ + target_pos;
00137
00138
00139 getCamera()->setPosition(rve_common::eigenToMsg(position_));
00140 getCamera()->setOrientation(rve_common::eigenToMsg(orientation_));
00141 camera_projection_->setViewHeight( view_height_ );
00142 }
00143
00144 void FixedOrientationOrthoCamera::zoom( float amount )
00145 {
00146 view_height_ *= powf(2,amount/100.f);
00147 }
00148
00149 void FixedOrientationOrthoCamera::move( float x, float y, float z )
00150 {
00151 position_rel_target_ += Eigen::Vector3f( x, y, z );
00152 }
00153
00154 }