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 "pr2_interactive_manipulation/camera_focus_display.h"
00031
00032 #include <rviz/window_manager_interface.h>
00033 #include <rviz/visualization_manager.h>
00034 #include <rviz/view_controller.h>
00035 #include <rviz/frame_manager.h>
00036
00037 #include <OGRE/OgreMatrix3.h>
00038
00039 #include <object_manipulator/tools/camera_configurations.h>
00040
00041 namespace pr2_interactive_manipulation {
00042
00043 CameraFocusDisplay::CameraFocusDisplay( const std::string& name,
00044 rviz::VisualizationManager* manager ) :
00045 Display( name, manager ),
00046 visualization_manager_(manager),
00047 name_(name),
00048 frame_(NULL)
00049 {
00050 window_manager_ = visualization_manager_->getWindowManager();
00051 ROS_ASSERT(window_manager_);
00052 wxWindow* parent = window_manager_->getParentWindow();
00053 ROS_ASSERT(parent);
00054 }
00055
00056 CameraFocusDisplay::~CameraFocusDisplay()
00057 {
00058 if ( frame_ )
00059 {
00060 window_manager_->removePane( frame_ );
00061 delete frame_;
00062 }
00063 }
00064
00065 void CameraFocusDisplay::onEnable()
00066 {
00067 if ( !frame_ )
00068 {
00069 frame_ = new CameraFocusFrame(window_manager_->getParentWindow(), vis_manager_);
00070 window_manager_->addPane( "Camera Focus", frame_ );
00071 }
00072 window_manager_->showPane(frame_);
00073 }
00074
00075 void CameraFocusDisplay::onDisable()
00076 {
00077 if ( frame_) window_manager_->closePane(frame_);
00078 }
00079
00080 void CameraFocusDisplay::createProperties()
00081 {
00082 }
00083
00084 void CameraFocusDisplay::update(float, float)
00085 {
00086 }
00087
00088
00089 CameraFocusFrame::CameraFocusFrame(wxWindow *parent,
00090 rviz::VisualizationManager* manager) :
00091 CameraFocusFrameBase(parent),
00092 root_nh_(""),
00093 vis_manager_(manager)
00094 {
00095 sub_ = root_nh_.subscribe("/camera_focus", 10, &CameraFocusFrame::callback, this);
00096 }
00097
00098 CameraFocusFrame::~CameraFocusFrame()
00099 {
00100 }
00101
00102 void CameraFocusFrame::callback(const pr2_object_manipulation_msgs::CameraFocusConstPtr &msg)
00103 {
00104 ROS_DEBUG("Camera focus message received");
00105 if (!accept_box_->IsChecked()) {ROS_INFO("Camera focus: not accepting external commands"); return;}
00106
00107
00108 geometry_msgs::PointStamped point = msg->focal_point;
00109 point.header.stamp = ros::Time(0);
00110 try {
00111 vis_manager_->getFrameManager()->getTFClient()->transformPoint(vis_manager_->getTargetFrame(),
00112 point, point );
00113 }
00114 catch(...)
00115 {
00116 ROS_ERROR("Camera focus: TF had a problem transforming between [%s] and [%s].",
00117 point.header.frame_id.c_str(), vis_manager_->getTargetFrame().c_str());
00118 return;
00119 }
00120 std::string params_string = vis_manager_->getCurrentViewController()->toString();
00121 ROS_DEBUG_STREAM("View params: " << params_string);
00122
00123
00124 std::istringstream params_str(params_string);
00125 std::vector<double> params;
00126 for (size_t i=0; i<6; i++)
00127 {
00128 double p;
00129 params_str >> p;
00130 if (params_str.fail())
00131 {
00132 ROS_ERROR("Camera focus: could not interpret params string");
00133 return;
00134 }
00135 params.push_back(p);
00136 }
00137
00138
00139
00140 params[2] = 1.5;
00141
00142 params[3] = -point.point.y;
00143 params[4] = point.point.z;
00144 params[5] = -point.point.x;
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156 float yaw_correction = 0;
00157 Ogre::Vector3 pos;
00158 Ogre::Quaternion orient;
00159
00160
00161 if (rviz::FrameManager::instance()->getTransform("base_link",
00162 ros::Time(), pos, orient) )
00163 {
00164 yaw_correction = orient.getRoll().valueRadians();
00165 }
00166
00167
00168
00169 Ogre::Vector3 focal_point(params[3], params[4], params[5]);
00170 Ogre::Matrix3 rot;
00171 rot.FromAxisAngle(Ogre::Vector3(0,1,0), Ogre::Radian(yaw_correction));
00172 focal_point = rot * focal_point;
00173 for(int i=0; i<3; i++) params[3+i] = focal_point[i];
00174
00175
00176 std::ostringstream os;
00177 for(int i=0; i<6; i++) os << params[i] << ' ';
00178 vis_manager_->setTargetFrame("base_link");
00179 vis_manager_->setCurrentViewControllerType( "Orbit" );
00180 vis_manager_->getCurrentViewController()->fromString(os.str());
00181 vis_manager_->queueRender();
00182 }
00183
00184
00185 void CameraFocusFrame::setCamera(std::vector<double> params)
00186 {
00187 float yaw_correction = 0;
00188 Ogre::Vector3 pos;
00189 Ogre::Quaternion orient;
00190
00191
00192 if (rviz::FrameManager::instance()->getTransform("base_link",
00193 ros::Time(), pos, orient) )
00194 {
00195 yaw_correction = orient.getRoll().valueRadians();
00196 }
00197 params[1] -= yaw_correction;
00198
00199
00200 Ogre::Vector3 focal_point(params[3], params[4], params[5]);
00201 Ogre::Matrix3 rot;
00202 rot.FromAxisAngle(Ogre::Vector3(0,1,0), Ogre::Radian(yaw_correction));
00203 focal_point = rot * focal_point;
00204 for(int i=0; i<3; i++) params[3+i] = focal_point[i];
00205
00206
00207 std::ostringstream os;
00208 for(int i=0; i<6; i++) os << params[i] << ' ';
00209 vis_manager_->setTargetFrame("base_link");
00210 vis_manager_->setCurrentViewControllerType( "Orbit" );
00211 vis_manager_->getCurrentViewController()->fromString(os.str());
00212 vis_manager_->queueRender();
00213 }
00214
00215 void CameraFocusFrame::leftButtonClicked( wxCommandEvent& )
00216 {
00217 try
00218 {
00219 setCamera( object_manipulator::cameraConfigurations().get_camera_pose("left") );
00220 }
00221 catch(object_manipulator::BadParamException &ex)
00222 {
00223 ROS_ERROR("Pre-defined camera position not found");
00224 }
00225 }
00226
00227 void CameraFocusFrame::topButtonClicked( wxCommandEvent& )
00228 {
00229 try
00230 {
00231 setCamera( object_manipulator::cameraConfigurations().get_camera_pose("top") );
00232 }
00233 catch(object_manipulator::BadParamException &ex)
00234 {
00235 ROS_ERROR("Pre-defined camera position not found");
00236 }
00237 }
00238
00239 void CameraFocusFrame::frontButtonClicked( wxCommandEvent& )
00240 {
00241 try
00242 {
00243 setCamera( object_manipulator::cameraConfigurations().get_camera_pose("front") );
00244 }
00245 catch(object_manipulator::BadParamException &ex)
00246 {
00247 ROS_ERROR("Pre-defined camera position not found");
00248 }
00249 }
00250
00251 void CameraFocusFrame::rightButtonClicked( wxCommandEvent& )
00252 {
00253 try
00254 {
00255 setCamera( object_manipulator::cameraConfigurations().get_camera_pose("right") );
00256 }
00257 catch(object_manipulator::BadParamException &ex)
00258 {
00259 ROS_ERROR("Pre-defined camera position not found");
00260 }
00261 }
00262
00263 void CameraFocusFrame::overheadButtonClicked( wxCommandEvent& )
00264 {
00265 try
00266 {
00267 setCamera( object_manipulator::cameraConfigurations().get_camera_pose("overhead") );
00268 }
00269 catch(object_manipulator::BadParamException &ex)
00270 {
00271 ROS_ERROR("Pre-defined camera position not found");
00272 }
00273 }
00274
00275 void CameraFocusFrame::acceptBoxChecked( wxCommandEvent& )
00276 {
00277 }
00278
00279 }