object_recognition_gui: object_recognition_gui: object_recognition_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 "object_recognition_gui/object_recognition_rviz_ui.h"
00031 
00032 #include "rviz_interaction_tools/image_tools.h"
00033 
00034 #include "rviz_interaction_tools/mesh_object_switcher.h"
00035 #include "rviz_interaction_tools/mesh_object.h"
00036 #include "rviz_interaction_tools/camera_tools.h"
00037 
00038 #include <OGRE/OgreManualObject.h>
00039 #include <OGRE/OgreMaterialManager.h>
00040 #include <OGRE/OgreRenderWindow.h>
00041 #include <OGRE/OgreSceneNode.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 #include <OGRE/OgreRoot.h>
00044 
00045 #include <rviz/render_panel.h>
00046 #include <rviz/window_manager_interface.h>
00047 #include <rviz/visualization_manager.h>
00048 
00049 #include "rviz_interaction_tools/image_overlay.h"
00050 #include "rviz_interaction_tools/camera_tools.h"
00051 #include "rviz_interaction_tools/unique_string_manager.h"
00052 
00053 using namespace rviz_interaction_tools;
00054 
00055 namespace object_recognition_gui
00056 {
00057 
00058 // material names for the different mesh colors
00059 static const std::string VALID_MAT_NAME = "object_recognition_valid";
00060 static const std::string VALID_SEL_MAT_NAME = "object_recognition_valid_selected";
00061 static const std::string INVALID_MAT_NAME = "object_recognition_invalid";
00062 static const std::string INVALID_SEL_MAT_NAME = "object_recognition_invalid_selected";
00063 
00064 
00065 ObjectRecognitionRvizUI::ObjectRecognitionRvizUI(rviz::VisualizationManager *visualization_manager) :
00066     ObjectRecognitionFrame( visualization_manager->getWindowManager()->getParentWindow() ),
00067     object_recognition_server_(0)
00068 {
00069   UniqueStringManager usm;
00070   
00071   //construct basic ogre scene
00072   scene_manager_ = Ogre::Root::getSingleton().createSceneManager( Ogre::ST_GENERIC, usm.unique("ObjectRecognitionRvizUI") );
00073 
00074   scene_root_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00075   
00076   image_overlay_ = new rviz_interaction_tools::ImageOverlay( scene_root_, Ogre::RENDER_QUEUE_BACKGROUND );
00077 
00078   createRenderPanel( visualization_manager );
00079 
00080   createMaterials();
00081 
00082   ray_scene_query_ = scene_manager_->createRayQuery(Ogre::Ray());
00083 }
00084 
00085 void ObjectRecognitionRvizUI::showBoundingBoxes( bool show )
00086 {
00087   scene_manager_->showBoundingBoxes(show);
00088 }
00089 
00090 bool ObjectRecognitionRvizUI::getShowBoundingBoxes()
00091 {
00092   return scene_manager_->getShowBoundingBoxes();
00093 }
00094 
00095 ObjectRecognitionRvizUI::~ObjectRecognitionRvizUI()
00096 {
00097   if ( object_recognition_server_ )
00098   {
00099     stopActionServer();
00100   }
00101 
00102   cleanupAndHide();
00103 
00104   scene_manager_->destroyQuery( ray_scene_query_ );
00105 
00106   render_panel_->getRenderWindow()->setActive(false);
00107 
00108   delete render_panel_;
00109   delete image_overlay_;
00110 }
00111 
00112 
00113 void ObjectRecognitionRvizUI::cleanupAndHide()
00114 {
00115   for ( size_t m=0; m<mesh_switchers_.size(); ++m )
00116   {
00117     delete mesh_switchers_[m];
00118   }
00119   mesh_switchers_.clear();
00120   Hide();
00121 }
00122 
00123 
00124 void ObjectRecognitionRvizUI::createRenderPanel( rviz::VisualizationManager *visualization_manager )
00125 {
00126   //put a render panel into the window
00127   render_panel_ = new rviz::RenderPanel(this, false);
00128 
00129   render_panel_->SetSize( panel_->GetRect() );
00130 
00131   // notify us when the user does something with the mouse
00132   render_panel_->Connect( wxEVT_MOTION,
00133                           wxMouseEventHandler( ObjectRecognitionRvizUI::onRenderWindowMouseEvents ), NULL, this );
00134   render_panel_->Connect( wxEVT_LEFT_DOWN,
00135                           wxMouseEventHandler( ObjectRecognitionRvizUI::onRenderWindowMouseEvents ), NULL, this );
00136   render_panel_->Connect( wxEVT_RIGHT_DOWN,
00137                           wxMouseEventHandler( ObjectRecognitionRvizUI::onRenderWindowMouseEvents ), NULL, this );
00138 
00139   render_panel_->createRenderWindow();
00140   render_panel_->initialize(scene_manager_, visualization_manager);
00141 
00142   render_panel_->setAutoRender(false);
00143   render_panel_->getViewport()->setOverlaysEnabled(false);
00144   render_panel_->getViewport()->setClearEveryFrame(true);
00145   render_panel_->getRenderWindow()->setAutoUpdated(false);
00146   render_panel_->getRenderWindow()->setActive(true);
00147 
00148   //fill camera matrix with dummy values to RaySceneQuery won't crash
00149   render_panel_->getCamera()->setPosition(0.0, 0.0, 0.0);
00150   render_panel_->getCamera()->lookAt(Ogre::Vector3(0, 0, 1));
00151   render_panel_->getCamera()->roll(Ogre::Radian(3.141592653));
00152   render_panel_->getCamera()->setFOVy(Ogre::Radian(0.518));
00153   render_panel_->getCamera()->setAspectRatio(640.0 / 480.0);
00154   render_panel_->getCamera()->setNearClipDistance( 0.01f );
00155 }
00156 
00157 
00158 void ObjectRecognitionRvizUI::createMaterials()
00159 {
00160   Ogre::MaterialPtr white = Ogre::MaterialManager::getSingleton().getByName("BaseWhiteNoLighting");
00161 
00162   Ogre::Pass *pass;
00163 
00164   // clone materials + adjust rendering settings
00165 
00166   // material for valid models
00167   Ogre::MaterialPtr valid_mat = white->clone( VALID_MAT_NAME );
00168   pass = valid_mat->getTechnique(0)->getPass(0);
00169 
00170   pass->setPolygonMode( Ogre::PM_WIREFRAME );
00171   pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00172   pass->setDepthWriteEnabled(false);
00173   pass->setDepthCheckEnabled(false);
00174   pass->setLightingEnabled(true);
00175   pass->setCullingMode( Ogre::CULL_NONE );
00176   pass->setAmbient(0.0, 0.0, 0.0);
00177   pass->setDiffuse(0.0, 0.0, 0.0, 0.3);
00178   pass->setSpecular(0.0, 0.0, 0.0, 0.3);
00179 
00180   pass->setSelfIllumination( 0.0, 0.9, 0.0 );
00181 
00182   // material for valid models when selected
00183   pass = valid_mat->clone( VALID_SEL_MAT_NAME )->getTechnique(0)->getPass(0);
00184   pass->setSelfIllumination( 0.4, 1.0, 0.4 );
00185 
00186   // material for invalid models (wrong recognition)
00187   pass = valid_mat->clone( INVALID_MAT_NAME )->getTechnique(0)->getPass(0);
00188   pass->setSelfIllumination( 0.9, 0.0, 0.0 );
00189 
00190   // material for invalid models (wrong recognition)
00191   pass = valid_mat->clone( INVALID_SEL_MAT_NAME )->getTechnique(0)->getPass(0);
00192   pass->setSelfIllumination( 1.0, 0.4, 0.4 );
00193 }
00194 
00195 
00196 void ObjectRecognitionRvizUI::parseMeshes(const std::vector<object_recognition_gui::ModelHypothesisList> &model_hyp_list)
00197 {
00198   ROS_ASSERT( mesh_switchers_.size() == 0 );
00199 
00200   //get all the meshes from the database
00201   int num_models = model_hyp_list.size();
00202   mesh_switchers_.resize(num_models);
00203   for ( int m=0; m<num_models; ++m )
00204   {
00205     const std::vector<object_recognition_gui::ModelHypothesis> &hyp_list =
00206         model_hyp_list[m].hypotheses;
00207 
00208     int num_hyp = hyp_list.size();
00209     mesh_switchers_[m] = new MeshObjectSwitcher( VALID_MAT_NAME, VALID_SEL_MAT_NAME, INVALID_MAT_NAME, INVALID_SEL_MAT_NAME );
00210 
00211     if ( hyp_list.size() == 0 || hyp_list[0].mesh.triangles.size() == 0 )
00212     {
00213       continue;
00214     }
00215 
00216     for ( int h=0; h<num_hyp; ++h )
00217     {
00218       const arm_navigation_msgs::Shape& mesh = hyp_list[h].mesh;
00219       const geometry_msgs::Pose pose = hyp_list[h].pose.pose;
00220 #if 0
00221       ROS_INFO_STREAM( "Model " << m << ", hypothesis " << h
00222           << ") has " << mesh.triangles.size()/3 << " triangles and "
00223           << mesh.vertices.size() << " vertices." );
00224       ROS_INFO_STREAM( " Position:" << pose.position.x << " "
00225           << pose.position.y << " " << pose.position.z <<
00226           ". Orientation: " << pose.orientation.w << " " << pose.orientation.x
00227           << " " << pose.orientation.y << " " << pose.orientation.z );
00228 #endif
00229       MeshObject* mesh_object = new MeshObject( scene_manager_, scene_root_ );
00230 
00231       std::ostringstream s;
00232       s << "obj_rec model " << m << " hypothesis " << h;
00233       std::string name = s.str();
00234 
00235       mesh_object->loadMesh( name, mesh );
00236       mesh_object->setPose( pose );
00237 
00238       Ogre::Entity* entity = mesh_object->getEntity();
00239       entity->getUserObjectBindings().setUserAny( "model", Ogre::Any(m) );
00240       entity->setRenderQueueGroup(Ogre::RENDER_QUEUE_MAIN);
00241 
00242       mesh_switchers_[m]->addObject( mesh_object );
00243     }
00244 
00245     mesh_switchers_[m]->setValid( model_hyp_list[m].accept );
00246     mesh_switchers_[m]->setVisible( 0 );
00247   }
00248 }
00249 
00250 
00251 void ObjectRecognitionRvizUI::update(float wall_dt, float ros_dt)
00252 {
00253   render_panel_->getRenderWindow()->update();
00254 }
00255 
00256 
00257 void ObjectRecognitionRvizUI::onRenderWindowMouseEvents( wxMouseEvent& event )
00258 {
00259   ROS_ASSERT( object_recognition_server_->isActive() );
00260 
00261   int x = event.GetX();
00262   int y = event.GetY();
00263 
00264   //convert to coordinates in the original image resolution
00265   wxSize size = render_panel_->GetSize();
00266   x = floor(x * image_overlay_->getWidth() / size.GetWidth() );
00267   y = floor(y * image_overlay_->getHeight() / size.GetHeight() );
00268 
00269   //check if one object has been selected or clicked
00270   Ogre::Ray mouse_ray = render_panel_->getCamera()->getCameraToViewportRay(x/float(size.GetWidth()), y/float(size.GetHeight()));
00271   ray_scene_query_->setRay( mouse_ray );
00272 
00273   for ( size_t m=0; m<mesh_switchers_.size(); ++m )
00274   {
00275     mesh_switchers_[m]->setSelected(false);
00276   }
00277 
00278   // Execute query
00279   ray_scene_query_->setSortByDistance(true);
00280   Ogre::RaySceneQueryResult& result = ray_scene_query_->execute();
00281   Ogre::RaySceneQueryResult::iterator iter = result.begin();
00282 
00283   int i=0;
00284   while( iter != result.end() )
00285   {
00286     Ogre::MovableObject* movable_object = iter->movable;
00287     if ( movable_object )
00288     {
00289       Ogre::Any model_any = movable_object->getUserObjectBindings().getUserAny( "model" );
00290 
00291       if ( !model_any.isEmpty() )
00292       {
00293         int model = Ogre::any_cast<int>(model_any);
00294         //ROS_INFO_STREAM( " model: " << model );
00295 
00296         if ( model >=0 && model < (int)mesh_switchers_.size() )
00297         {
00298           mesh_switchers_[model]->setSelected(true);
00299 
00300           if ( mesh_switchers_[model]->isValid() )
00301           {
00302             if (event.ButtonDown(wxMOUSE_BTN_LEFT))
00303             {
00304               mesh_switchers_[model]->next();
00305             }
00306             else if (event.ButtonDown(wxMOUSE_BTN_RIGHT))
00307             {
00308               mesh_switchers_[model]->setValid( false );
00309             }
00310           }
00311           else
00312           {
00313             if ( event.ButtonDown(wxMOUSE_BTN_LEFT) || event.ButtonDown(wxMOUSE_BTN_RIGHT ))
00314             {
00315               mesh_switchers_[model]->setValid( true );
00316             }
00317           }
00318         }
00319         break;
00320       }
00321     }
00322 
00323     i++;
00324     iter++;
00325   }
00326 }
00327 
00328 
00329 void ObjectRecognitionRvizUI::acceptButtonClicked( wxCommandEvent& )
00330 {
00331   ObjectRecognitionGuiResult result;
00332 
00333   result.selected_hypothesis_indices.resize( mesh_switchers_.size() );
00334   for ( unsigned i=0; i<mesh_switchers_.size(); ++i )
00335   {
00336     result.selected_hypothesis_indices[i] =
00337         mesh_switchers_[i]->isValid() ? mesh_switchers_[i]->getVisible() : -1;
00338   }
00339 
00340   object_recognition_server_->setSucceeded(result);
00341   cleanupAndHide();
00342 }
00343 
00344 
00345 void ObjectRecognitionRvizUI::cancelButtonClicked( wxCommandEvent& )
00346 {
00347   object_recognition_server_->setAborted();
00348   cleanupAndHide();
00349 }
00350 
00351 
00352 void ObjectRecognitionRvizUI::startActionServer( ros::NodeHandle &node_handle )
00353 {
00354   if ( object_recognition_server_ )
00355   {
00356     ROS_ERROR( "ObjectRecognitionGuiAction server already started!" );
00357     return;
00358   }
00359 
00360   //create non-threaded action server
00361   object_recognition_server_ =
00362       new actionlib::SimpleActionServer<ObjectRecognitionGuiAction>( node_handle, "object_recognition_popup", false );
00363 
00364   object_recognition_server_->registerGoalCallback( boost::bind(&ObjectRecognitionRvizUI::acceptNewGoal, this) );
00365   object_recognition_server_->registerPreemptCallback( boost::bind(&ObjectRecognitionRvizUI::preempt, this) );
00366 
00367   object_recognition_server_->start();
00368 }
00369 
00370 
00371 void ObjectRecognitionRvizUI::stopActionServer()
00372 {
00373   if ( !object_recognition_server_ )
00374   {
00375     ROS_ERROR("ObjectRecognitionGuiAction server cannot be stopped because it is not running.");
00376     return;
00377   }
00378 
00379   //if we're currently being active, we have to cancel everything, clean up & hide the window
00380   if ( object_recognition_server_->isActive() )
00381   {
00382     ROS_WARN("Aborting ObjectRecognitionGuiAction goal.");
00383     object_recognition_server_->setAborted();
00384     cleanupAndHide();
00385   }
00386 
00387   delete object_recognition_server_;
00388   object_recognition_server_ = 0;
00389 }
00390 
00391 void ObjectRecognitionRvizUI::acceptNewGoal()
00392 {
00393   const ObjectRecognitionGuiGoal::ConstPtr &goal = object_recognition_server_->acceptNewGoal();
00394 
00395   rviz_interaction_tools::updateCamera( render_panel_->getCamera(), goal->camera_info );
00396 
00397   image_overlay_->setImage( goal->image );
00398   image_overlay_->update();
00399 
00400   parseMeshes( goal->model_hypotheses );
00401 
00402   Show();
00403 }
00404 
00405 void ObjectRecognitionRvizUI::preempt()
00406 {
00407   object_recognition_server_->setPreempted();
00408   cleanupAndHide();
00409 }
00410 
00411 
00412 
00413 
00414 }
object_recognition_rviz_ui.cpp
