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 <rve_common_transformers/camera_following_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/camera_info_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 namespace rve_common_transformers
00047 {
00048
00049 CameraFollowingCamera::CameraFollowingCamera()
00050 : orientation_(Eigen::Quaternionf::Identity())
00051 , position_(0.0, 0.0, 0.0)
00052 , camera_projection_( new rve_render_client::CameraInfoProjectionFactory() )
00053 , camera_info_changed_( false )
00054 {
00055 }
00056
00057 CameraFollowingCamera::~CameraFollowingCamera()
00058 {
00059 }
00060
00061 void CameraFollowingCamera::onInit()
00062 {
00063 addSubscriber("camera_info", 1, &CameraFollowingCamera::cameraInfoCallback, this);
00064 }
00065
00066 void CameraFollowingCamera::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
00067 {
00068
00069 camera_info_ = camera_info;
00070 camera_info_changed_ = true;
00071 }
00072
00073 void CameraFollowingCamera::onUpdate()
00074 {
00075 updateCamera();
00076 }
00077
00078 void CameraFollowingCamera::onCameraSet()
00079 {
00080 getCamera()->setPosition(rve_common::eigenToMsg(position_));
00081 getCamera()->setOrientation(rve_common::eigenToMsg(orientation_));
00082 getCamera()->setProjectionMatrixFactory( camera_projection_ );
00083 }
00084
00085 void CameraFollowingCamera::updateCamera()
00086 {
00087 sensor_msgs::CameraInfo info;
00088 {
00089
00090 if( !camera_info_changed_ || !camera_info_ )
00091 {
00092 return;
00093 }
00094 info = *camera_info_;
00095 camera_info_changed_ = false;
00096 }
00097
00098
00099 geometry_msgs::TransformStamped target = getManager()->getFrameManager()->lookupTransform( info.header.frame_id, ros::Time());
00100
00101 geometry_msgs::Vector3 p = target.transform.translation;
00102 position_ = Eigen::Vector3f(p.x, p.y, p.z);
00103
00104 geometry_msgs::Quaternion q = target.transform.rotation;
00105 orientation_ = Eigen::Quaternionf(q.w, q.x, q.y, q.z);
00106
00107
00108 orientation_ = orientation_ *
00109 Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitZ()) *
00110 Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitY()) *
00111 Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX());
00112
00113 float width = info.width;
00114 float height = info.height;
00115
00116
00117 if( width == 0 )
00118 {
00119
00120
00121 }
00122
00123 if( height == 0 )
00124 {
00125
00126
00127 }
00128
00129 if( height == 0 || width == 0 )
00130 {
00131 ROS_ERROR( "CameraInfo: bad size: width = %f, height = %f.", width, height );
00132 return;
00133 }
00134
00135 double fx = info.P[0];
00136 double fy = info.P[5];
00137
00138
00139 double tx = -1 * (info.P[3] / fx);
00140 Eigen::Vector3f right = orientation_ * Eigen::Vector3f::UnitX();
00141 position_ = position_ + (right * tx);
00142
00143 double ty = -1 * (info.P[7] / fy);
00144 Eigen::Vector3f down = orientation_ * Eigen::Vector3f::UnitY();
00145 position_ = position_ + (down * ty);
00146
00147 getCamera()->setPosition( rve_common::eigenToMsg( position_ ));
00148 getCamera()->setOrientation( rve_common::eigenToMsg( orientation_ ));
00149
00150 double cx = info.P[2];
00151 double cy = info.P[6];
00152 camera_projection_->setCameraInfo( cx, cy, fx, fy, width, height );
00153 }
00154
00155 }