pr2_gripper_click: gripper_click_rviz

00001 /*
00002  * Copyright (c) 2009, 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 "pr2_gripper_click/gripper_click_rviz_ui.h"
00031 
00032 #include <rviz/window_manager_interface.h>
00033 #include <rviz/render_panel.h>
00034 #include <rviz/visualization_manager.h>
00035 #include <rviz/validate_floats.h>
00036 #include <rviz/frame_manager.h>
00037 #include <rviz/common.h>
00038 
00039 #include <ogre_tools/point_cloud.h>
00040 
00041 #include <OGRE/OgreRenderWindow.h>
00042 #include <OGRE/OgreSceneNode.h>
00043 #include <OGRE/OgreSceneManager.h>
00044 #include <OGRE/OgreRoot.h>
00045 #include <OGRE/OgreRectangle2D.h>
00046 #include <OGRE/OgreMaterialManager.h>
00047 #include <OGRE/OgreTextureManager.h>
00048 #include <OGRE/OgreLight.h>
00049 
00050 #include <sensor_msgs/image_encodings.h>
00051 
00052 #include "rviz_interaction_tools/image_tools.h"
00053 #include "rviz_interaction_tools/image_overlay.h"
00054 #include "rviz_interaction_tools/camera_tools.h"
00055 #include "rviz_interaction_tools/disparity_renderer.h"
00056 #include "rviz_interaction_tools/unique_string_manager.h"
00057 
00058 using namespace rviz_interaction_tools;
00059 
00060 namespace pr2_gripper_click {
00061 
00062 template<class ActionClass>
00063 GripperClickRvizUI<ActionClass>::GripperClickRvizUI(rviz::VisualizationManager *visualization_manager) :
00064   GripperClickFrameBase(visualization_manager->getWindowManager()->getParentWindow()),
00065   action_state_(IDLE),
00066   vis_manager_(visualization_manager),
00067   active_plugin_(-1)
00068 {
00069   UniqueStringManager usm;
00070 
00071   //construct basic ogre scene
00072   scene_manager_ = Ogre::Root::getSingleton().createSceneManager( Ogre::ST_GENERIC, usm.unique("GripperClick") );
00073   //scene_manager_->showBoundingBoxes(true);
00074 
00075   scene_manager_->setShadowColour( Ogre::ColourValue(0.8,0.8,0.8) );
00076   scene_manager_->setShadowTechnique(Ogre::SHADOWTYPE_STENCIL_MODULATIVE);
00077 
00078   camera_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00079 
00080   //create gripper in our own window
00081   gripper_ = new rviz_interaction_tools::Gripper( scene_manager_, camera_node_  );
00082   gripper_->setDepthCheckEnabled(true);
00083   gripper_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN );
00084 
00085   gripper_light_ = scene_manager_->createLight(usm.unique("gripper_light"));
00086 
00087   gripper_light_->setType( Ogre::Light::LT_DIRECTIONAL );
00088   gripper_light_->setDirection( 0,-1,0 );
00089 
00090   //create gripper in main window
00091   main_win_camera_node_ = visualization_manager->getSceneManager()->getRootSceneNode()->createChildSceneNode();
00092   main_win_gripper_ = new rviz_interaction_tools::Gripper( visualization_manager->getSceneManager(), main_win_camera_node_ );
00093   main_win_gripper_->setColour(1, 1, 1, 0.9);
00094   image_overlay_ = new rviz_interaction_tools::ImageOverlay( camera_node_, Ogre::RENDER_QUEUE_BACKGROUND );
00095 
00096   disparity_renderer_ = new rviz_interaction_tools::DisparityRenderer( camera_node_, Ogre::RENDER_QUEUE_MAIN );
00097   main_win_disparity_renderer_ = new rviz_interaction_tools::DisparityRenderer( main_win_camera_node_, Ogre::RENDER_QUEUE_MAIN );
00098 
00099   //put a render panel into the window
00100   render_panel_ = new rviz::RenderPanel(this, false);
00101 
00102   render_panel_->SetSize( panel_->GetRect() );
00103 
00104   // notify us when the user does something with the mouse
00105   render_panel_->Connect( wxEVT_MOTION,
00106                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00107   render_panel_->Connect( wxEVT_LEFT_DOWN, 
00108                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00109   render_panel_->Connect( wxEVT_MIDDLE_DOWN, 
00110                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00111   render_panel_->Connect( wxEVT_RIGHT_DOWN, 
00112                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00113   render_panel_->Connect( wxEVT_LEFT_UP, 
00114                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00115   render_panel_->Connect( wxEVT_MIDDLE_UP, 
00116                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00117   render_panel_->Connect( wxEVT_RIGHT_UP, 
00118                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00119   render_panel_->Connect( wxEVT_MOUSEWHEEL, 
00120                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00121   render_panel_->Connect( wxEVT_LEFT_DCLICK, 
00122                           wxMouseEventHandler( GripperClickRvizUI::onRenderWindowMouseEvents ), NULL, this );
00123   
00124   render_panel_->createRenderWindow();
00125   render_panel_->initialize(scene_manager_, visualization_manager);
00126 
00127   //fill camera matrix with dummy values
00128   render_panel_->getCamera()->setFOVy(Ogre::Radian(0.518));
00129   render_panel_->getCamera()->setAspectRatio(640.0 / 480.0);
00130   render_panel_->getCamera()->setNearClipDistance( 0.01f );
00131 
00132   render_panel_->setAutoRender(false);
00133   render_panel_->getViewport()->setOverlaysEnabled(false);
00134   render_panel_->getViewport()->setClearEveryFrame(true);
00135   render_panel_->getRenderWindow()->setAutoUpdated(false);
00136   render_panel_->getRenderWindow()->setActive(true);
00137 
00138   hide();
00139 
00140 #ifdef DEBUG_DISPLAY
00141   line_ = new rviz_interaction_tools::Line( scene_manager_, visualization_manager->getSceneManager()->getRootSceneNode() );
00142 #endif
00143 }
00144 
00145 template<class ActionClass>
00146 GripperClickRvizUI<ActionClass>::~GripperClickRvizUI()
00147 {
00148   for ( size_t i=0; i<plugins_.size(); i++ )
00149   {
00150     delete plugins_[i];
00151   }
00152 
00153   render_panel_->getRenderWindow()->setActive(false);
00154   delete gripper_;
00155   delete render_panel_;
00156   delete image_overlay_;
00157   delete disparity_renderer_;
00158   delete main_win_disparity_renderer_;
00159   Hide();
00160 
00161   vis_manager_->getSceneManager()->destroySceneNode(main_win_camera_node_);
00162   delete main_win_gripper_;
00163 
00164   Ogre::Root::getSingleton().destroySceneManager( scene_manager_ );
00165 }
00166 
00167 template<class ActionClass>
00168 void GripperClickRvizUI<ActionClass>::update(float wall_dt, float ros_dt)
00169 {
00170   if ( active_plugin_ < plugins_.size() )
00171   {
00172     bool visible;
00173     Ogre::Vector3 position;
00174     Ogre::Quaternion orientation;
00175     float angle;
00176 
00177     if ( plugins_[active_plugin_]->descriptionChanged() )
00178     {
00179       setBottomLabelText( plugins_[active_plugin_]->getDescription() );
00180     }
00181 
00182     plugins_[active_plugin_]->getGripperParams( visible, position, orientation, angle );
00183 
00184     gripper_->setVisible(visible);
00185     main_win_gripper_->setVisible(visible);
00186 
00187     if (visible)
00188     {
00189       //update our window
00190       gripper_->setPosition( position );
00191       gripper_->setOrientation( orientation );
00192       gripper_->setGripperAngle( angle );
00193 
00194       //update main window
00195       main_win_gripper_->setPosition( position );
00196       main_win_gripper_->setOrientation( orientation );
00197       main_win_gripper_->setGripperAngle( angle );
00198     }
00199 
00200     plugins_[active_plugin_]->update( wall_dt, ros_dt );
00201     accept_button_->Enable( plugins_[active_plugin_]->hasValidResult() );
00202   }
00203   else
00204   {
00205     ROS_ERROR_STREAM( "Invalid plugin index found! # of plugins: " << plugins_.size() );
00206     active_plugin_ = 0;
00207     accept_button_->Enable( false );
00208   }
00209 
00210   render_panel_->getRenderWindow()->update();
00211 }
00212 
00213 template<class ActionClass>
00214 void GripperClickRvizUI<ActionClass>::show()
00215 {
00216   Show();
00217   main_win_camera_node_->setVisible(true);
00218 }
00219 
00220 template<class ActionClass>
00221 void GripperClickRvizUI<ActionClass>::hide()
00222 {
00223   Hide();
00224   main_win_camera_node_->setVisible(false);
00225 }
00226 
00227 template<class ActionClass>
00228 void GripperClickRvizUI<ActionClass>::setImage( const sensor_msgs::Image &image,
00229     const stereo_msgs::DisparityImage &disparity_image,
00230     const sensor_msgs::CameraInfo &camera_info )
00231 {
00232   image_overlay_->setImage( image, disparity_image );
00233   image_overlay_->update();
00234 
00235   disparity_renderer_->setDisparityImage( disparity_image, camera_info, &image );
00236   disparity_renderer_->update();
00237 
00238   main_win_disparity_renderer_->setDisparityImage( disparity_image, camera_info, &image );
00239   main_win_disparity_renderer_->update();
00240 
00241   camera_frame_ = image.header.frame_id;
00242 
00243   rviz_interaction_tools::updateCamera( render_panel_->getCamera(), camera_info );
00244 
00245   Ogre::Vector3 cam_position;
00246   Ogre::Quaternion cam_orientation;
00247 
00248   if ( getTransform(camera_frame_, cam_position, cam_orientation) )
00249   {
00250 
00251     main_win_camera_node_->setPosition(cam_position);
00252     main_win_camera_node_->setOrientation(cam_orientation);
00253 
00254     camera_node_->setPosition(cam_position);
00255     camera_node_->setOrientation(cam_orientation);
00256 
00257     // convert vision (Z-forward) frame to ogre frame (Z-out)
00258     cam_orientation = cam_orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);
00259 
00260     render_panel_->getCamera()->setPosition(cam_position);
00261     render_panel_->getCamera()->setOrientation(cam_orientation);
00262   }
00263   else
00264   {
00265     ROS_ERROR("Cannot transform to %s", camera_frame_.c_str());
00266     main_win_gripper_->setVisible(false);
00267   }
00268 }
00269 
00270 template<class ActionClass>
00271 bool GripperClickRvizUI<ActionClass>::getTransform( std::string frame, Ogre::Vector3 &position, Ogre::Quaternion &orientation )
00272 {
00273   if (!vis_manager_->getFrameManager()->getTransform(frame, ros::Time(), position, orientation, false))
00274   {
00275     std::string error;
00276     if (vis_manager_->getFrameManager()->transformHasProblems(frame, ros::Time(), error))
00277     {
00278       setStatus(rviz::status_levels::Error, error);
00279     }
00280     else
00281     {
00282       std::stringstream ss;
00283       ss << "Could not transform from [" << frame << "] to Fixed Frame [" << vis_manager_->getFrameManager()->getFixedFrame() << "] for an unknown reason";
00284       setStatus(rviz::status_levels::Error, ss.str());
00285     }
00286     return false;
00287   }
00288 
00289   setStatus(rviz::status_levels::Ok, "Transform OK");
00290   return true;
00291 }
00292 
00293 
00294 template<class ActionClass>
00295 void GripperClickRvizUI<ActionClass>::setStatus(rviz::status_levels::StatusLevel level, const std::string& text)
00296 {
00297   status_level_ = level;
00298   status_text_ = text;
00299   bottom_label_->SetLabel( wxString::FromAscii( text.c_str() ) );
00300 
00301   if ( status_level_ == rviz::status_levels::Ok && active_plugin_ < plugins_.size() )
00302   {
00303     bottom_label_->SetLabel( wxString::FromAscii( plugins_[active_plugin_]->getDescription().c_str() ) );
00304   }
00305 }
00306 
00307 template<class ActionClass>
00308 void GripperClickRvizUI<ActionClass>::setBottomLabelText(const std::string& text)
00309 {
00310   if ( status_level_ == rviz::status_levels::Ok )
00311   {
00312     bottom_label_->SetLabel( wxString::FromAscii( text.c_str() ) );
00313   }
00314 }
00315 
00316 
00317 template<class ActionClass>
00318 bool GripperClickRvizUI<ActionClass>::setGoal( const typename GoalType::ConstPtr &goal )
00319 {
00320   for ( size_t i=0; i<plugins_.size(); i++ )
00321   {
00322     if ( !plugins_[i]->setGoal( goal ) )
00323     {
00324       ROS_ERROR( "Couldn't set new goal on '%s'!", plugins_[i]->getName().c_str() );
00325       return false;
00326     }
00327   }
00328 
00329   action_state_ = IDLE;
00330   show();
00331   return true;
00332 }
00333 
00334 
00335 template<class ActionClass>
00336 void GripperClickRvizUI<ActionClass>::addPlugin( GripperClickPlugin<ActionClass>* plugin )
00337 {
00338   plugin->init( vis_manager_, scene_manager_, camera_node_ );
00339   plugin->hide();
00340   plugin_choice_->Append( wxString::FromAscii( plugin->getName().c_str() ) );
00341   plugins_.push_back( plugin );
00342 
00343   //auto-select the first plugin to be added
00344   if ( plugins_.size()==1 )
00345   {
00346     plugin_choice_->Select(0);
00347     setActivePlugin(0);
00348   }
00349 }
00350 
00351 template<class ActionClass>
00352 void GripperClickRvizUI<ActionClass>::setActivePlugin( unsigned index )
00353 {
00354   if ( index >= plugins_.size() )
00355   {
00356     ROS_ERROR( "Non-existent plugin index selected!" );
00357   }
00358   if ( index != active_plugin_ )
00359   {
00360     if ( active_plugin_ < plugins_.size() )
00361     {
00362       plugins_[active_plugin_]->hide();
00363     }
00364 
00365     plugins_[index]->show();
00366     active_plugin_ = index;
00367     setBottomLabelText( plugins_[index]->getDescription() );
00368   }
00369 }
00370 
00371 
00372 template<class ActionClass>
00373 bool GripperClickRvizUI<ActionClass>::getResult( ResultType &result )
00374 {
00375   if ( active_plugin_ >= plugins_.size() || !plugins_[active_plugin_]->hasValidResult() )
00376   {
00377     return false;
00378   }
00379 
00380   plugins_[active_plugin_]->getResult( result );
00381   return true;
00382 }
00383 
00384 
00385 template<class ActionClass>
00386 void GripperClickRvizUI<ActionClass>::acceptButtonClicked( wxCommandEvent& )
00387 {
00388   action_state_ = ACCEPTED;
00389   Hide();
00390 }
00391 
00392 template<class ActionClass>
00393 void GripperClickRvizUI<ActionClass>::cancelButtonClicked( wxCommandEvent& )
00394 {
00395   action_state_ = CANCELED;
00396   Hide();
00397 }
00398 
00399 template<class ActionClass>
00400 void GripperClickRvizUI<ActionClass>::onPluginChoice( wxCommandEvent& event )
00401 {
00402   setActivePlugin( event.GetInt() );
00403 }
00404 
00405 template<class ActionClass>
00406 void GripperClickRvizUI<ActionClass>::onRenderWindowMouseEvents( wxMouseEvent& event )
00407 {
00408   if ( active_plugin_ < plugins_.size() )
00409   {
00410     int x = event.GetX();
00411     int y = event.GetY();
00412 
00413     //convert to pixel coordinates in the original image resolution
00414     wxSize size = render_panel_->GetSize();
00415     x = floor(x * image_overlay_->getWidth() / size.GetWidth() );
00416     y = floor(y * image_overlay_->getHeight() / size.GetHeight() );
00417 
00418     wxMouseEvent event_normalized = event;
00419 
00420     event_normalized.m_x = x;
00421     event_normalized.m_y = y;
00422 
00423     Ogre::Ray mouse_ray = render_panel_->getCamera()->getCameraToViewportRay(event.GetX()/float(size.GetWidth()), event.GetY()/float(size.GetHeight()));
00424 
00425 #ifdef DEBUG_DISPLAY
00426     ROS_INFO_STREAM( "mouse " << event.GetX()/float(size.GetWidth()) << " " << event.GetY()/float(size.GetHeight())
00427         << "\n origin " << mouse_ray.getOrigin() << "\n direction " << mouse_ray.getDirection()
00428         << "\n camera pos " << render_panel_->getCamera()->getPosition() );
00429 
00430     line_->setLimits( mouse_ray.getOrigin(), mouse_ray.getOrigin() + mouse_ray.getDirection() );
00431 #endif
00432 
00433     plugins_[active_plugin_]->onRenderWindowMouseEvents( event_normalized, mouse_ray );
00434   }
00435 }
00436 
00437 //explicitly instantiate pickup/place classes
00438 template class GripperClickRvizUI<GripperClickPickupAction>;
00439 template class GripperClickRvizUI<GripperClickPlaceAction>;
00440 
00441 
00442 }
gripper_click_rviz_ui.cpp
