selection_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 "selection_manager.h"
00031 #include "properties/property.h"
00032 #include "properties/property_manager.h"
00033 #include "visualization_manager.h"
00034 #include "render_panel.h"
00035 #include "view_controller.h"
00036 
00037 #include <ogre_helpers/shape.h>
00038 #include <ogre_helpers/axes.h>
00039 #include <ogre_helpers/arrow.h>
00040 #include <ogre_helpers/qt_ogre_render_window.h>
00041 
00042 #include <ros/assert.h>
00043 
00044 #include <OGRE/OgreCamera.h>
00045 #include <OGRE/OgreViewport.h>
00046 #include <OGRE/OgreRenderSystem.h>
00047 #include <OGRE/OgreRenderTexture.h>
00048 #include <OGRE/OgreTextureManager.h>
00049 #include <OGRE/OgreSceneNode.h>
00050 #include <OGRE/OgreSceneManager.h>
00051 #include <OGRE/OgreManualObject.h>
00052 #include <OGRE/OgreWireBoundingBox.h>
00053 #include <OGRE/OgreRoot.h>
00054 #include <OGRE/OgreHardwarePixelBuffer.h>
00055 #include <OGRE/OgreMaterialManager.h>
00056 #include <OGRE/OgreEntity.h>
00057 #include <OGRE/OgreSubEntity.h>
00058 
00059 #include <boost/scoped_array.hpp>
00060 
00061 #include <algorithm>
00062 
00063 namespace rviz
00064 {
00065 
00066 SelectionManager::SelectionManager(VisualizationManager* manager)
00067 : vis_manager_(manager)
00068 , highlight_enabled_(false)
00069 , uid_counter_(0)
00070 , interaction_enabled_(false)
00071 , current_viewport_(NULL)
00072 {
00073   for (uint32_t i = 0; i < s_num_render_textures_; ++i)
00074   {
00075     pixel_boxes_[i].data = 0;
00076   }
00077   depth_pixel_box_.data = 0;
00078 }
00079 
00080 SelectionManager::~SelectionManager()
00081 {
00082   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00083 
00084   setSelection(M_Picked());
00085 
00086   highlight_node_->getParentSceneNode()->removeAndDestroyChild(highlight_node_->getName());
00087   delete highlight_rectangle_;
00088 
00089   for (uint32_t i = 0; i < s_num_render_textures_; ++i)
00090   {
00091     delete [] (uint8_t*)pixel_boxes_[i].data;
00092   }
00093   delete [] (uint8_t*)depth_pixel_box_.data;
00094 
00095   vis_manager_->getSceneManager()->destroyCamera( camera_ );
00096 }
00097 
00098 void SelectionManager::initialize( bool debug )
00099 {
00100   debug_mode_ = debug;
00101 
00102   // Create our render textures
00103   setTextureSize(1);
00104 
00105   // Create our highlight rectangle
00106   Ogre::SceneManager* scene_manager = vis_manager_->getSceneManager();
00107   highlight_node_ = scene_manager->getRootSceneNode()->createChildSceneNode();
00108 
00109   std::stringstream ss;
00110   static int count = 0;
00111   ss << "SelectionRect" << count++;
00112   highlight_rectangle_ = new Ogre::Rectangle2D(true);
00113 
00114   const static uint32_t texture_data[1] = { 0xffff0080 };
00115   Ogre::DataStreamPtr pixel_stream;
00116   pixel_stream.bind(new Ogre::MemoryDataStream( (void*)&texture_data[0], 4 ));
00117 
00118   Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().loadRawData(ss.str() + "Texture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, 1, 1, Ogre::PF_R8G8B8A8, Ogre::TEX_TYPE_2D, 0);
00119 
00120   Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00121   material->setLightingEnabled(false);
00122   //material->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME);
00123   highlight_rectangle_->setMaterial(material->getName());
00124   Ogre::AxisAlignedBox aabInf;
00125   aabInf.setInfinite();
00126   highlight_rectangle_->setBoundingBox(aabInf);
00127   highlight_rectangle_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00128   material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00129   material->setCullingMode(Ogre::CULL_NONE);
00130 
00131   Ogre::TextureUnitState* tex_unit = material->getTechnique(0)->getPass(0)->createTextureUnitState();
00132   tex_unit->setTextureName(tex->getName());
00133   tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00134 
00135   highlight_node_->attachObject(highlight_rectangle_);
00136 
00137   // create picking camera
00138   camera_= scene_manager->createCamera( ss.str()+"_camera" );
00139 
00140   // create fallback picking material
00141   fallback_pick_material_ = Ogre::MaterialManager::getSingleton().create( "SelectionManagerFallbackMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00142   addPickTechnique( 0, fallback_pick_material_ );
00143   fallback_pick_material_->load();
00144   fallback_pick_technique_ = 0;
00145 
00146   for (uint32_t i = 0; i < fallback_pick_material_->getNumTechniques(); ++i)
00147   {
00148     Ogre::Technique* tech = fallback_pick_material_->getTechnique(i);
00149 
00150     if (tech->getSchemeName() == "Pick")
00151     {
00152       fallback_pick_technique_ = tech;
00153     }
00154   }
00155 }
00156 
00157 void SelectionManager::initDepthFinder()
00158 {
00159   ROS_DEBUG("SelectionManager::initDepthFinder()");
00160   std::string tex_name = "DepthTexture";
00161 
00162   if( depth_render_texture_.get() )
00163   {
00164     Ogre::TextureManager::getSingleton().remove( tex_name );
00165   }
00166 
00167   depth_texture_size_ = 1;
00168   depth_render_texture_ =
00169     Ogre::TextureManager::getSingleton().createManual( tex_name,
00170                                                        Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00171                                                        Ogre::TEX_TYPE_2D, depth_texture_size_, depth_texture_size_, 0,
00172                                                        Ogre::PF_R8G8B8,
00173                                                        Ogre::TU_RENDERTARGET );
00174   Ogre::RenderTexture* render_texture = depth_render_texture_->getBuffer()->getRenderTarget();
00175   render_texture->setAutoUpdated(false);
00176 
00177   if( debug_mode_ )
00178   {
00179     if ( debug_depth_material_.get() )
00180     {
00181       debug_depth_material_->removeAllTechniques();
00182     }
00183     else
00184     {
00185       Ogre::Rectangle2D* mini_screen = new Ogre::Rectangle2D(true);
00186       float size = 0.6;
00187 
00188       float left = 1.0-size;
00189       float top = 1.0 - size * (float)2 * 1.02;
00190       float right = left + size;
00191       float bottom = top - size;
00192 
00193       mini_screen->setCorners(left,top,right,bottom);
00194       Ogre::AxisAlignedBox aabInf;
00195       aabInf.setInfinite();
00196       mini_screen->setBoundingBox(aabInf);
00197       Ogre::SceneNode* mini_screen_node = vis_manager_->getSceneManager()->getRootSceneNode()->createChildSceneNode(tex_name + "MiniScreenNode");
00198       mini_screen_node->attachObject(mini_screen);
00199       debug_depth_node_ = mini_screen_node;
00200 
00201       debug_depth_material_ = Ogre::MaterialManager::getSingleton().create(tex_name + "RttMat",
00202                                                                            Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00203       mini_screen->setMaterial(debug_depth_material_->getName());
00204     }
00205 
00206     Ogre::Technique *technique = debug_depth_material_->createTechnique();
00207     technique->createPass();
00208     technique->getPass(0)->setLightingEnabled(false);
00209     technique->getPass(0)->createTextureUnitState(depth_render_texture_->getName());
00210     technique->getPass(0)->setTextureFiltering( Ogre::TFO_NONE );
00211   }
00212 }
00213 
00214 void SelectionManager::setDebugVisibility( bool visible )
00215 {
00216   if (debug_mode_)
00217   {
00218     for (unsigned i = 0; i < s_num_render_textures_; ++i)
00219     {
00220       debug_nodes_[i]->setVisible( visible );
00221     }
00222     debug_depth_node_->setVisible( visible );
00223   }
00224 }
00225 
00226 bool SelectionManager::get3DPoint( Ogre::Viewport* viewport, int x, int y, Ogre::Vector3& result_point )
00227 {
00228   ROS_DEBUG("SelectionManager.get3DPoint()");
00229 
00230   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00231 
00232   setDebugVisibility( false );
00233   current_viewport_ = viewport;
00234 
00235   M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
00236   M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();
00237   for (; handler_it != handler_end; ++handler_it)
00238   {
00239     const SelectionHandlerPtr& handler = handler_it->second;
00240     handler->preRenderPass(0);
00241   }
00242 
00243   bool success = false;
00244   if( render( viewport, depth_render_texture_, x, y, x + 1, y + 1, depth_pixel_box_, "Depth", depth_texture_size_ ))
00245   {
00246     uint8_t* data_ptr = (uint8_t*) depth_pixel_box_.data;
00247     uint8_t a = *data_ptr++;
00248     uint8_t b = *data_ptr++;
00249     uint8_t c = *data_ptr++;
00250 
00251     int int_depth = (c << 16) | (b << 8) | a;
00252     float normalized_depth = ((float) int_depth) / (float) 0xffffff;
00253 
00254     float depth = normalized_depth * camera_->getFarClipDistance();
00255     
00256     if( depth != 0 )
00257     {
00258       Ogre::Matrix4 projection = camera_->getProjectionMatrix();
00259       if( projection[3][3] == 0.0 ) // If this is a perspective projection
00260       {
00261         // We don't use camera_->getCameraToViewportRay() here because
00262         // it normalizes the ray direction vector.  We need the scale
00263         // of the direction vector to account for the fact that the
00264         // depth value we get is not a distance from the camera, it is
00265         // a depth coordinate.  If we used the normalized vector, a
00266         // sweep along a plane parallel to the camera plane would
00267         // yield an arc of points instead of a line.
00268         Ogre::Matrix4 view = camera_->getViewMatrix();
00269         Ogre::Matrix4 pv = projection * view;
00270         Ogre::Matrix4 ip = pv.inverse();
00271 
00272         Ogre::Vector4 near_point(0, 0, -1, 1);
00273         Ogre::Vector4 far_point(0, 0, 0, 1);
00274 
00275         Ogre::Vector4 ray_origin = ip * near_point;
00276         Ogre::Vector4 ray_target = ip * far_point;
00277       
00278         ray_origin /= ray_origin[3];
00279         ray_target /= ray_target[3];
00280 
00281         Ogre::Vector3 origin3( ray_origin[0], ray_origin[1], ray_origin[2] );
00282         Ogre::Vector3 target3( ray_target[0], ray_target[1], ray_target[2] );
00283 
00284         Ogre::Vector3 dir = target3 - origin3;
00285 
00286         // TODO: Not sure where this scale factor actually comes from nor its precise value. (hersh)
00287         float magic_scale_factor = 100;
00288         result_point = target3 + dir * magic_scale_factor * depth;
00289       }
00290       else // else this must be an orthographic projection.
00291       {
00292         // For orthographic projection, getCameraToViewportRay() does
00293         // the right thing for us, and the above math does not work.
00294         Ogre::Ray ray;
00295         camera_->getCameraToViewportRay( 0.5, 0.5, &ray );
00296 
00297         result_point = ray.getPoint( depth );
00298       }
00299 
00300       ROS_DEBUG("SelectionManager.get3DPoint(): point = %f, %f, %f", result_point.x, result_point.y, result_point.z);
00301 
00302       success = true;
00303     }
00304   }
00305 
00306   handler_it = objects_.begin();
00307   handler_end = objects_.end();
00308   for (; handler_it != handler_end; ++handler_it)
00309   {
00310     const SelectionHandlerPtr& handler = handler_it->second;
00311     handler->postRenderPass(0);
00312   }
00313   current_viewport_ = NULL;
00314   setDebugVisibility( true );
00315   return success;
00316 }
00317 
00318 void SelectionManager::setTextureSize( unsigned size )
00319 {
00320   if ( size > 1024 )
00321   {
00322     size = 1024;
00323   }
00324 
00325   texture_size_ = size;
00326 
00327   for (uint32_t pass = 0; pass < s_num_render_textures_; ++pass)
00328   {
00329     // check if we need to change the texture size
00330     if ( !render_textures_[pass].get() || render_textures_[pass]->getWidth() != size )
00331     {
00332       std::string tex_name;
00333       if ( render_textures_[pass].get() )
00334       {
00335         ROS_INFO_STREAM( "Texture for pass " << pass << " must be resized to " << size << " x " << size );
00336         tex_name = render_textures_[pass]->getName();
00337 
00338         // destroy old
00339         Ogre::TextureManager::getSingleton().remove( tex_name );
00340       }
00341       else
00342       {
00343         ROS_INFO_STREAM( "Texture for pass " << pass << ": creating with size " << size << " x " << size );
00344         std::stringstream ss;
00345         static int count = 0;
00346         ss << "SelectionTexture" << count++;
00347         tex_name = ss.str();
00348       }
00349 
00350       // create new texture
00351       render_textures_[pass] = Ogre::TextureManager::getSingleton().createManual( tex_name,
00352           Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, size, size, 0,
00353           Ogre::PF_R8G8B8, Ogre::TU_STATIC | Ogre::TU_RENDERTARGET);
00354 
00355       Ogre::RenderTexture* render_texture = render_textures_[pass]->getBuffer()->getRenderTarget();
00356       render_texture->setAutoUpdated(false);
00357 
00358       if (debug_mode_)
00359       {
00360         if ( debug_material_[pass].get() )
00361         {
00362           debug_material_[pass]->removeAllTechniques();
00363         }
00364         else
00365         {
00366           Ogre::Rectangle2D* mini_screen = new Ogre::Rectangle2D(true);
00367           float size = 0.6;
00368 
00369           float left = 1.0-size;
00370           float top = 1.0 - size * (float)pass * 1.02;
00371           float right = left + size;
00372           float bottom = top - size;
00373 
00374           mini_screen->setCorners(left,top,right,bottom);
00375           Ogre::AxisAlignedBox aabInf;
00376           aabInf.setInfinite();
00377           mini_screen->setBoundingBox(aabInf);
00378           Ogre::SceneNode* mini_screen_node = vis_manager_->getSceneManager()->getRootSceneNode()->createChildSceneNode(tex_name + "MiniScreenNode");
00379           mini_screen_node->attachObject(mini_screen);
00380           debug_nodes_[pass] = mini_screen_node;
00381 
00382           debug_material_[pass] = Ogre::MaterialManager::getSingleton().create(tex_name + "RttMat", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00383           mini_screen->setMaterial(debug_material_[pass]->getName());
00384         }
00385 
00386         Ogre::Technique *technique = debug_material_[pass]->createTechnique();
00387         technique->createPass();
00388         technique->getPass(0)->setLightingEnabled(false);
00389         technique->getPass(0)->createTextureUnitState(render_textures_[pass]->getName());
00390         technique->getPass(0)->setTextureFiltering( Ogre::TFO_NONE );
00391       }
00392     }
00393   }
00394 
00395   initDepthFinder();
00396 }
00397 
00398 void SelectionManager::clearHandlers()
00399 {
00400   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00401 
00402   objects_.clear();
00403 }
00404 
00405 void SelectionManager::enableInteraction( bool enable )
00406 {
00407   interaction_enabled_ = enable;
00408   M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
00409   M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();
00410   for (; handler_it != handler_end; ++handler_it)
00411   {
00412     const SelectionHandlerPtr& handler = handler_it->second;
00413     InteractiveObjectPtr object = handler->getInteractiveObject().lock();
00414     if( object )
00415     {
00416       object->enableInteraction( enable );
00417     }
00418   }
00419 }
00420 
00421 CollObjectHandle SelectionManager::createHandle()
00422 {
00423   uid_counter_++;
00424   if (uid_counter_ > 0x00ffffff)
00425   {
00426     uid_counter_ = 0;
00427   }
00428 
00429   uint32_t handle = 0;
00430 
00431   // shuffle around the bits so we get lots of colors
00432   // when we're displaying the selection buffer
00433   for ( unsigned int i=0; i<24; i++ )
00434   {
00435     uint32_t shift = (((23-i)%3)*8) + (23-i)/3;
00436     uint32_t bit = ( (uint32_t)(uid_counter_ >> i) & (uint32_t)1 ) << shift;
00437     handle |= bit;
00438   }
00439 
00440   return handle;
00441 }
00442 
00443 void SelectionManager::addObject(CollObjectHandle obj, const SelectionHandlerPtr& handler)
00444 {
00445   if (!obj)
00446   {
00447 //    ROS_BREAK();
00448     return;
00449   }
00450 
00451   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00452 
00453   handler->initialize(vis_manager_);
00454 
00455   InteractiveObjectPtr object = handler->getInteractiveObject().lock();
00456   if( object )
00457   {
00458     object->enableInteraction( interaction_enabled_ );
00459   }
00460 
00461   bool inserted = objects_.insert(std::make_pair(obj, handler)).second;
00462   ROS_ASSERT(inserted);
00463 }
00464 
00465 void SelectionManager::removeObject(CollObjectHandle obj)
00466 {
00467   if (!obj)
00468   {
00469     return;
00470   }
00471 
00472   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00473 
00474   M_Picked::iterator it = selection_.find(obj);
00475   if (it != selection_.end())
00476   {
00477     M_Picked objs;
00478     objs.insert(std::make_pair(it->first, it->second));
00479 
00480     removeSelection(objs);
00481   }
00482 
00483   objects_.erase(obj);
00484 }
00485 
00486 void SelectionManager::update()
00487 {
00488   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00489 
00490   highlight_node_->setVisible(highlight_enabled_);
00491 
00492   if (highlight_enabled_)
00493   {
00494     setHighlightRect(highlight_.viewport, highlight_.x1, highlight_.y1, highlight_.x2, highlight_.y2);
00495 
00496 #if 0
00497     M_Picked results;
00498     highlight_node_->setVisible(false);
00499     pick(highlight_.viewport, highlight_.x1, highlight_.y1, highlight_.x2, highlight_.y2, results);
00500     highlight_node_->setVisible(true);
00501 #endif
00502   }
00503 }
00504 
00505 void SelectionManager::highlight(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2)
00506 {
00507   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00508 
00509   highlight_enabled_ = true;
00510 
00511   highlight_.viewport = viewport;
00512   highlight_.x1 = x1;
00513   highlight_.y1 = y1;
00514   highlight_.x2 = x2;
00515   highlight_.y2 = y2;
00516 }
00517 
00518 void SelectionManager::removeHighlight()
00519 {
00520   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00521 
00522   highlight_enabled_ = false;
00523 }
00524 
00525 void SelectionManager::select(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, SelectType type)
00526 {
00527   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00528 
00529   highlight_enabled_ = false;
00530   highlight_node_->setVisible(false);
00531 
00532   M_Picked results;
00533   pick(viewport, x1, y1, x2, y2, results);
00534 
00535   if (type == Add)
00536   {
00537     addSelection(results);
00538   }
00539   else if (type == Remove)
00540   {
00541     removeSelection(results);
00542   }
00543   else if (type == Replace)
00544   {
00545     setSelection(results);
00546   }
00547 }
00548 
00549 void SelectionManager::setHighlightRect(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2)
00550 {
00551   float nx1 = ((float)x1 / viewport->getActualWidth()) * 2 - 1;
00552   float nx2 = ((float)x2 / viewport->getActualWidth()) * 2 - 1;
00553   float ny1 = -(((float)y1 / viewport->getActualHeight()) * 2 - 1);
00554   float ny2 = -(((float)y2 / viewport->getActualHeight()) * 2 - 1);
00555 
00556   nx1 = nx1 < -1 ? -1 : (nx1 > 1 ? 1 : nx1);
00557   ny1 = ny1 < -1 ? -1 : (ny1 > 1 ? 1 : ny1);
00558   nx2 = nx2 < -1 ? -1 : (nx2 > 1 ? 1 : nx2);
00559   ny2 = ny2 < -1 ? -1 : (ny2 > 1 ? 1 : ny2);
00560 
00561   highlight_rectangle_->setCorners(nx1, ny1, nx2, ny2);
00562 }
00563 
00564 void SelectionManager::unpackColors( const Ogre::PixelBox& box, V_Pixel& pixels)
00565 {
00566   int w = box.getWidth();
00567   int h = box.getHeight();
00568 
00569   pixels.clear();
00570   pixels.reserve( w*h );
00571 
00572   for (int y = 0; y < h; y ++)
00573   {
00574     for (int x = 0; x < w; x ++)
00575     {
00576       uint32_t pos = (x + y*w) * 4;
00577 
00578       uint32_t pix_val = *(uint32_t*)((uint8_t*)box.data + pos);
00579       uint32_t handle = colorToHandle(box.format, pix_val);
00580 
00581       //ROS_INFO( "%d %d = %u", x, y, handle );
00582 
00583       Pixel p;
00584       p.x = x;
00585       p.y = y;
00586       p.handle = handle;
00587 
00588       pixels.push_back(p);
00589     }
00590   }
00591 }
00592 
00593 void SelectionManager::renderAndUnpack(Ogre::Viewport* viewport, uint32_t pass, int x1, int y1, int x2, int y2, V_Pixel& pixels)
00594 {
00595   ROS_ASSERT(pass < s_num_render_textures_);
00596 
00597   std::stringstream scheme;
00598   scheme << "Pick";
00599   if (pass > 0)
00600   {
00601     scheme << pass;
00602   }
00603 
00604   if( render( viewport, render_textures_[pass], x1, y1, x2, y2, pixel_boxes_[pass], scheme.str(), texture_size_ ))
00605   {
00606     unpackColors(pixel_boxes_[pass], pixels);
00607   }
00608 }
00609 
00610 bool SelectionManager::render(Ogre::Viewport* viewport, Ogre::TexturePtr tex,
00611                               int x1, int y1, int x2, int y2,
00612                               Ogre::PixelBox& dst_box, std::string material_scheme,
00613                               unsigned texture_size)
00614 {
00615   vis_manager_->lockRender();
00616 
00617   if ( x2==x1 ) x2++;
00618   if ( y2==y1 ) y2++;
00619 
00620   if ( x1 > x2 ) std::swap( x1, x2 );
00621   if ( y1 > y2 ) std::swap( y1, y2 );
00622 
00623   if ( x1 < 0 ) x1 = 0;
00624   if ( y1 < 0 ) y1 = 0;
00625   if ( x1 > viewport->getActualWidth() ) x1 = viewport->getActualWidth();
00626   if ( y1 > viewport->getActualHeight() ) y1 = viewport->getActualHeight();
00627   if ( x2 < 0 ) x2 = 0;
00628   if ( y2 < 0 ) y2 = 0;
00629   if ( x2 > viewport->getActualWidth() ) x2 = viewport->getActualWidth();
00630   if ( y2 > viewport->getActualHeight() ) y2 = viewport->getActualHeight();
00631 
00632   if ( x2==x1 || y2==y1 )
00633   {
00634     ROS_WARN("SelectionManager::render(): not rendering 0 size area.");
00635     vis_manager_->unlockRender();
00636     return false;
00637   }
00638 
00639   unsigned w = x2-x1;
00640   unsigned h = y2-y1;
00641 
00642   Ogre::HardwarePixelBufferSharedPtr pixel_buffer = tex->getBuffer();
00643   Ogre::RenderTexture* render_texture = pixel_buffer->getRenderTarget();
00644 
00645   Ogre::Matrix4 proj_matrix = viewport->getCamera()->getProjectionMatrix();
00646   Ogre::Matrix4 scale_matrix = Ogre::Matrix4::IDENTITY;
00647   Ogre::Matrix4 trans_matrix = Ogre::Matrix4::IDENTITY;
00648 
00649   float x1_rel = (float)x1 / (float)(viewport->getActualWidth()-1) - 0.5;
00650   float y1_rel = (float)y1 / (float)(viewport->getActualHeight()-1) - 0.5;
00651   float x2_rel = (float)x2 / (float)(viewport->getActualWidth()-1) - 0.5;
00652   float y2_rel = (float)y2 / (float)(viewport->getActualHeight()-1) - 0.5;
00653 
00654   scale_matrix[0][0] = 1.0 / (x2_rel-x1_rel);
00655   scale_matrix[1][1] = 1.0 / (y2_rel-y1_rel);
00656 
00657   trans_matrix[0][3] -= x1_rel+x2_rel;
00658   trans_matrix[1][3] += y1_rel+y2_rel;
00659 
00660   camera_->setCustomProjectionMatrix( true, scale_matrix * trans_matrix * proj_matrix );
00661   camera_->setPosition( viewport->getCamera()->getDerivedPosition() );
00662   camera_->setOrientation( viewport->getCamera()->getDerivedOrientation() );
00663 
00664   // Note: if you change this far-clip distance, update
00665   // fixed_orientation_ortho_view_controller.cpp where it sets the
00666   // camera position Z value ot half of this.
00667   camera_->setFarClipDistance( 1000 );
00668   camera_->setNearClipDistance( 0.1 );
00669 
00670   // create a viewport if there is none
00671   if (render_texture->getNumViewports() == 0)
00672   {
00673     render_texture->removeAllViewports();
00674     render_texture->addViewport( camera_ );
00675     Ogre::Viewport* render_viewport = render_texture->getViewport(0);
00676     render_viewport->setClearEveryFrame(true);
00677     render_viewport->setBackgroundColour( Ogre::ColourValue::Black );
00678     render_viewport->setOverlaysEnabled(false);
00679     render_viewport->setMaterialScheme(material_scheme);
00680   }
00681 
00682   unsigned render_w = w;
00683   unsigned render_h = h;
00684 
00685   if ( w>h )
00686   {
00687     if ( render_w > texture_size )
00688     {
00689       render_w = texture_size;
00690       render_h = round( float(h) * (float)texture_size / (float)w );
00691     }
00692   }
00693   else
00694   {
00695     if ( render_h > texture_size )
00696     {
00697       render_h = texture_size;
00698       render_w = round( float(w) * (float)texture_size / (float)h );
00699     }
00700   }
00701 
00702   // safety clamping in case of rounding errors
00703   if ( render_w > texture_size ) render_w = texture_size;
00704   if ( render_h > texture_size ) render_h = texture_size;
00705 
00706   // set viewport to render to a subwindow of the texture
00707   Ogre::Viewport* render_viewport = render_texture->getViewport(0);
00708   render_viewport->setDimensions( 0, 0,
00709                                   (float)render_w / (float)texture_size,
00710                                   (float)render_h / (float)texture_size );
00711 
00712   ros::WallTime start = ros::WallTime::now();
00713 
00714   // update & force ogre to render the scene
00715   Ogre::MaterialManager::getSingleton().addListener(this);
00716 
00717   render_texture->update();
00718 
00719   // For some reason we need to pretend to render the main window in
00720   // order to get the picking render to show up in the pixelbox below.
00721   // If we don't do this, it will show up there the *next* time we
00722   // pick something, but not this time.  This object as a
00723   // render queue listener tells the scene manager to skip every
00724   // render step, so nothing actually gets drawn.
00725   // 
00726   // TODO: find out what part of _renderScene() actually makes this work.
00727   Ogre::Viewport* main_view = vis_manager_->getRenderPanel()->getViewport();
00728   vis_manager_->getSceneManager()->addRenderQueueListener(this);
00729   vis_manager_->getSceneManager()->_renderScene(main_view->getCamera(), main_view, false);
00730   vis_manager_->getSceneManager()->removeRenderQueueListener(this);
00731 
00732   ros::WallTime end = ros::WallTime::now();
00733   ros::WallDuration d = end - start;
00734 //  ROS_DEBUG("Render took [%f] msec", d.toSec() * 1000.0f);
00735 
00736   Ogre::MaterialManager::getSingleton().removeListener(this);
00737 
00738   render_w = render_viewport->getActualWidth();
00739   render_h = render_viewport->getActualHeight();
00740 
00741   Ogre::PixelFormat format = pixel_buffer->getFormat();
00742 
00743   int size = Ogre::PixelUtil::getMemorySize(render_w, render_h, 1, format);
00744   uint8_t* data = new uint8_t[size];
00745 
00746   delete [] (uint8_t*)dst_box.data;
00747   dst_box = Ogre::PixelBox(render_w, render_h, 1, format, data);
00748 
00749   pixel_buffer->blitToMemory(dst_box,dst_box);
00750 
00751   vis_manager_->unlockRender();
00752   return true;
00753 }
00754 
00755 void SelectionManager::renderQueueStarted( uint8_t queueGroupId,
00756                                            const std::string& invocation, 
00757                                            bool& skipThisInvocation )
00758 {
00759   // This render queue listener function tells the scene manager to
00760   // skip every render step, so nothing actually gets drawn.
00761 
00762 //  ROS_DEBUG("SelectionManager renderQueueStarted(%d, '%s') returning skip = true.", (int)queueGroupId, invocation.c_str());
00763   skipThisInvocation = true;
00764 }
00765 
00766 void SelectionManager::pick(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, M_Picked& results, bool single_render_pass)
00767 {
00768   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00769 
00770   setDebugVisibility( false );
00771 
00772   bool need_additional_render = false;
00773   current_viewport_ = viewport;
00774 
00775   V_CollObject handles_by_pixel;
00776   S_CollObject need_additional;
00777 
00778   V_Pixel& pixels = pixel_buffer_;
00779 
00780   // First render is special... does the initial object picking, determines which objects have been selected
00781   // After that, individual handlers can specify that they need additional renders (max # defined in s_num_render_textures_)
00782   {
00783     M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
00784     M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();
00785     for (; handler_it != handler_end; ++handler_it)
00786     {
00787       const SelectionHandlerPtr& handler = handler_it->second;
00788 
00789       handler->preRenderPass(0);
00790     }
00791 
00792     renderAndUnpack(viewport, 0, x1, y1, x2, y2, pixels);
00793 
00794     handler_it = objects_.begin();
00795     handler_end = objects_.end();
00796     for (; handler_it != handler_end; ++handler_it)
00797     {
00798       const SelectionHandlerPtr& handler = handler_it->second;
00799 
00800       handler->postRenderPass(0);
00801     }
00802 
00803     handles_by_pixel.reserve(pixels.size());
00804     V_Pixel::iterator it = pixels.begin();
00805     V_Pixel::iterator end = pixels.end();
00806     for (; it != end; ++it)
00807     {
00808       const Pixel& p = *it;
00809 
00810       CollObjectHandle handle = p.handle;
00811 
00812       handles_by_pixel.push_back(handle);
00813 
00814       if (handle == 0)
00815       {
00816         continue;
00817       }
00818 
00819       SelectionHandlerPtr handler = getHandler(handle);
00820 
00821       if (handle && handler)
00822       {
00823         std::pair<M_Picked::iterator, bool> insert_result = results.insert(std::make_pair(handle, Picked(handle)));
00824         if (insert_result.second)
00825         {
00826           if (handler->needsAdditionalRenderPass(1) && !single_render_pass)
00827           {
00828             need_additional.insert(handle);
00829             need_additional_render = true;
00830           }
00831         }
00832         else
00833         {
00834           insert_result.first->second.pixel_count++;
00835         }
00836       }
00837     }
00838   }
00839 
00840   uint32_t pass = 1;
00841 
00842   V_uint64 extra_by_pixel;
00843   extra_by_pixel.resize(handles_by_pixel.size());
00844   while (need_additional_render && pass < s_num_render_textures_)
00845   {
00846     {
00847       S_CollObject::iterator need_it = need_additional.begin();
00848       S_CollObject::iterator need_end = need_additional.end();
00849       for (; need_it != need_end; ++need_it)
00850       {
00851         SelectionHandlerPtr handler = getHandler(*need_it);
00852         ROS_ASSERT(handler);
00853 
00854         handler->preRenderPass(pass);
00855       }
00856     }
00857 
00858     renderAndUnpack(viewport, pass, x1, y1, x2, y2, pixels);
00859 
00860     {
00861       S_CollObject::iterator need_it = need_additional.begin();
00862       S_CollObject::iterator need_end = need_additional.end();
00863       for (; need_it != need_end; ++need_it)
00864       {
00865         SelectionHandlerPtr handler = getHandler(*need_it);
00866         ROS_ASSERT(handler);
00867 
00868         handler->postRenderPass(pass);
00869       }
00870     }
00871 
00872     int i = 0;
00873     V_Pixel::iterator pix_it = pixels.begin();
00874     V_Pixel::iterator pix_end = pixels.end();
00875     for (; pix_it != pix_end; ++pix_it, ++i)
00876     {
00877       const Pixel& p = *pix_it;
00878 
00879       CollObjectHandle handle = handles_by_pixel[i];
00880 
00881       if (pass == 1)
00882       {
00883         extra_by_pixel[i] = 0;
00884       }
00885 
00886       if (need_additional.find(handle) != need_additional.end())
00887       {
00888         CollObjectHandle extra_handle = p.handle;
00889         extra_by_pixel[i] |= extra_handle << (32 * (pass-1));
00890       }
00891       else
00892       {
00893         extra_by_pixel[i] = 0;
00894       }
00895     }
00896 
00897     need_additional_render = false;
00898     need_additional.clear();
00899     M_Picked::iterator handle_it = results.begin();
00900     M_Picked::iterator handle_end = results.end();
00901     for (; handle_it != handle_end; ++handle_it)
00902     {
00903       CollObjectHandle handle = handle_it->first;
00904 
00905       if (getHandler(handle)->needsAdditionalRenderPass(pass + 1))
00906       {
00907         need_additional_render = true;
00908         need_additional.insert(handle);
00909       }
00910     }
00911   }
00912 
00913   int i = 0;
00914   V_uint64::iterator pix_2_it = extra_by_pixel.begin();
00915   V_uint64::iterator pix_2_end = extra_by_pixel.end();
00916   for (; pix_2_it != pix_2_end; ++pix_2_it, ++i)
00917   {
00918     CollObjectHandle handle = handles_by_pixel[i];
00919 
00920     if (handle == 0)
00921     {
00922       continue;
00923     }
00924 
00925     M_Picked::iterator picked_it = results.find(handle);
00926     if (picked_it == results.end())
00927     {
00928       continue;
00929     }
00930 
00931     Picked& picked = picked_it->second;
00932 
00933     if (*pix_2_it)
00934     {
00935       picked.extra_handles.insert(*pix_2_it);
00936     }
00937   }
00938 
00939   current_viewport_ = NULL;
00940   setDebugVisibility( true );
00941 }
00942 
00943 Ogre::Technique *SelectionManager::handleSchemeNotFound(unsigned short scheme_index,
00944     const Ogre::String& scheme_name,
00945     Ogre::Material* original_material,
00946     unsigned short lod_index,
00947     const Ogre::Renderable* rend )
00948 {
00949   return fallback_pick_technique_;
00950 }
00951 
00952 Ogre::Technique *SelectionManager::addPickTechnique(CollObjectHandle handle, const Ogre::MaterialPtr& material)
00953 {
00954   Ogre::DataStreamPtr pixel_stream;
00955   pixel_stream.bind(new Ogre::MemoryDataStream( &handle, 3 ));
00956 
00957   Ogre::Technique* technique = 0;
00958 
00959   // Look for a technique in the material that has a "Pick" scheme.
00960   uint32_t num_techs = material->getNumTechniques();
00961   for (uint32_t i = 0; i < num_techs; ++i)
00962   {
00963     Ogre::Technique* tech = material->getTechnique(i);
00964 
00965     if (tech->getSchemeName() == "Pick")
00966     {
00967       technique = tech;
00968       break;
00969     }
00970   }
00971 
00972   // If we did not find a "Pick" techique, create one and add it to the material.
00973   if (!technique)
00974   {
00975     // try to preserve the culling mode
00976     Ogre::CullingMode culling_mode = Ogre::CULL_CLOCKWISE;
00977     if ( material->getTechnique(0) && material->getTechnique(0)->getNumPasses() > 0 )
00978     {
00979       culling_mode = material->getTechnique(0)->getPass(0)->getCullingMode();
00980     }
00981 
00982     technique = material->createTechnique();
00983     technique->setSchemeName("Pick");
00984     Ogre::Pass* pass = technique->createPass();
00985     pass->setLightingEnabled(false);
00986     pass->setSceneBlending(Ogre::SBT_REPLACE);
00987     pass->setCullingMode( culling_mode );
00988 
00989     Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().loadRawData(material->getName() + "PickTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, 1, 1, Ogre::PF_R8G8B8, Ogre::TEX_TYPE_2D, 0);
00990     Ogre::TextureUnitState* tex_unit = pass->createTextureUnitState();
00991     tex_unit->setTextureName(tex->getName());
00992     tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00993     tex_unit->setColourOperation(Ogre::LBO_REPLACE);
00994   }
00995   else
00996   {
00997     // We *did* find a Pick technique, so just set the texture data
00998     // (in pixel_stream) to be the single pixel from the
00999     // CollObjectHandle.
01000     Ogre::TextureUnitState* tex_unit = technique->getPass(0)->getTextureUnitState(0);
01001     std::string tex_name = tex_unit->getTextureName();
01002 
01003     Ogre::TexturePtr tex = Ogre::TextureManager::getSingleton().getByName(tex_name);
01004     tex->unload();
01005     tex->loadRawData(pixel_stream, 1, 1, Ogre::PF_R8G8B8);
01006   }
01007 
01008   technique->getPass(0)->_dirtyHash();
01009 
01010   //----- now add a technique for finding depth -----
01011 
01012   // Look for a technique in the material that has a "Depth" scheme.
01013   bool has_depth = false;
01014   num_techs = material->getNumTechniques();
01015   for (uint32_t i = 0; i < num_techs; ++i)
01016   {
01017     Ogre::Technique* tech = material->getTechnique(i);
01018 
01019     if (tech->getSchemeName() == "Depth")
01020     {
01021       has_depth = true;
01022       break;
01023     }
01024   }
01025 
01026   if( !has_depth )
01027   {
01028     // try to preserve the culling mode
01029     Ogre::CullingMode culling_mode = Ogre::CULL_CLOCKWISE;
01030     if ( material->getTechnique(0) && material->getTechnique(0)->getNumPasses() > 0 )
01031     {
01032       culling_mode = material->getTechnique(0)->getPass(0)->getCullingMode();
01033     }
01034 
01035     technique = material->createTechnique();
01036     technique->setSchemeName("Depth");
01037     Ogre::Pass* pass = technique->createPass();
01038     pass->setLightingEnabled(false);
01039     pass->setSceneBlending(Ogre::SBT_REPLACE);
01040     pass->setCullingMode( culling_mode );
01041     pass->setVertexProgram( "rviz/DepthVP" );
01042     pass->setFragmentProgram( "rviz/DepthFP" );
01043   }
01044   material->load(false);
01045 
01046   return technique;
01047 }
01048 
01049 CollObjectHandle SelectionManager::createCollisionForObject(Object* obj, const SelectionHandlerPtr& handler, CollObjectHandle coll)
01050 {
01051   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01052 
01053   bool use_original = false;
01054 
01055   if (coll)
01056   {
01057     use_original = true;
01058   }
01059   else
01060   {
01061     coll = createHandle();
01062   }
01063 
01064   if (Shape* shape = dynamic_cast<Shape*>(obj))
01065   {
01066     createCollisionForEntity(shape->getEntity(), handler, coll);
01067     if (!use_original)
01068     {
01069       handler->addTrackedObject(shape->getEntity());
01070     }
01071   }
01072   else if (Axes* axes = dynamic_cast<Axes*>(obj))
01073   {
01074     createCollisionForEntity(axes->getXShape()->getEntity(), handler, coll);
01075     createCollisionForEntity(axes->getYShape()->getEntity(), handler, coll);
01076     createCollisionForEntity(axes->getZShape()->getEntity(), handler, coll);
01077 
01078     if (!use_original)
01079     {
01080       handler->addTrackedObject(axes->getXShape()->getEntity());
01081       handler->addTrackedObject(axes->getYShape()->getEntity());
01082       handler->addTrackedObject(axes->getZShape()->getEntity());
01083     }
01084   }
01085   else if (Arrow* arrow = dynamic_cast<Arrow*>(obj))
01086   {
01087     createCollisionForEntity(arrow->getHead()->getEntity(), handler, coll);
01088     createCollisionForEntity(arrow->getShaft()->getEntity(), handler, coll);
01089 
01090     if (!use_original)
01091     {
01092       handler->addTrackedObject(arrow->getHead()->getEntity());
01093       handler->addTrackedObject(arrow->getShaft()->getEntity());
01094     }
01095   }
01096 
01097   if (coll)
01098   {
01099     if (!use_original)
01100     {
01101       addObject(coll, handler);
01102     }
01103   }
01104 
01105   return coll;
01106 }
01107 
01108 CollObjectHandle SelectionManager::createCollisionForEntity(Ogre::Entity* entity, const SelectionHandlerPtr& handler, CollObjectHandle coll)
01109 {
01110   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01111 
01112   bool use_original = false;
01113 
01114   if (coll)
01115   {
01116     use_original = true;
01117   }
01118   else
01119   {
01120     coll = createHandle();
01121   }
01122 
01123   typedef std::set<Ogre::Material*> M_Material;
01124   M_Material materials;
01125 
01126   uint32_t num_sub_entities = entity->getNumSubEntities();
01127   for (uint32_t i = 0; i < num_sub_entities; ++i)
01128   {
01129     Ogre::SubEntity* sub = entity->getSubEntity(i);
01130 
01131     Ogre::MaterialPtr material = sub->getMaterial();
01132 
01133     if (materials.insert(material.get()).second)
01134     {
01135       addPickTechnique(coll, material);
01136     }
01137   }
01138 
01139   if (!use_original)
01140   {
01141     handler->addTrackedObject(entity);
01142     addObject(coll, handler);
01143   }
01144 
01145   return coll;
01146 }
01147 
01148 SelectionHandlerPtr SelectionManager::getHandler(CollObjectHandle obj)
01149 {
01150   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01151 
01152   M_CollisionObjectToSelectionHandler::iterator it = objects_.find(obj);
01153   if (it != objects_.end())
01154   {
01155     return it->second;
01156   }
01157 
01158   return SelectionHandlerPtr();
01159 }
01160 
01161 void SelectionManager::removeSelection(const M_Picked& objs)
01162 {
01163   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01164 
01165   M_Picked::const_iterator it = objs.begin();
01166   M_Picked::const_iterator end = objs.end();
01167   for (; it != end; ++it)
01168   {
01169     removeSelection(it->second);
01170   }
01171 
01172   Q_EMIT selectionRemoved( objs );
01173 }
01174 
01175 void SelectionManager::addSelection(const M_Picked& objs)
01176 {
01177   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01178 
01179   M_Picked added;
01180   M_Picked::const_iterator it = objs.begin();
01181   M_Picked::const_iterator end = objs.end();
01182   for (; it != end; ++it)
01183   {
01184     std::pair<Picked, bool> ppb = addSelection(it->second);
01185     if (ppb.second)
01186     {
01187       added.insert(std::make_pair(it->first, ppb.first));
01188     }
01189   }
01190 
01191   Q_EMIT selectionAdded( added );
01192 }
01193 
01194 void SelectionManager::setSelection(const M_Picked& objs)
01195 {
01196   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01197 
01198   Q_EMIT selectionSetting();
01199 
01200   M_Picked original(selection_.begin(), selection_.end());
01201 
01202   removeSelection(original);
01203   addSelection(objs);
01204 
01205   Q_EMIT selectionSet( original, selection_ );
01206 }
01207 
01208 std::pair<Picked, bool> SelectionManager::addSelection(const Picked& obj)
01209 {
01210   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01211 
01212   std::pair<M_Picked::iterator, bool> pib = selection_.insert(std::make_pair(obj.handle, obj));
01213 
01214   SelectionHandlerPtr handler = getHandler(obj.handle);
01215 
01216   if (pib.second)
01217   {
01218     handler->onSelect(obj);
01219     return std::make_pair(obj, true);
01220   }
01221   else
01222   {
01223     Picked& cur = pib.first->second;
01224     Picked added(cur.handle);
01225 
01226     S_uint64::iterator it = obj.extra_handles.begin();
01227     S_uint64::iterator end = obj.extra_handles.end();
01228     for (; it != end; ++it)
01229     {
01230       if (cur.extra_handles.insert(*it).second)
01231       {
01232         added.extra_handles.insert(*it);
01233       }
01234     }
01235 
01236     if (!added.extra_handles.empty())
01237     {
01238       handler->onSelect(added);
01239 
01240       return std::make_pair(added, true);
01241     }
01242   }
01243 
01244   return std::make_pair(Picked(0), false);
01245 }
01246 
01247 void SelectionManager::removeSelection(const Picked& obj)
01248 {
01249   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01250 
01251   M_Picked::iterator sel_it = selection_.find(obj.handle);
01252   if (sel_it != selection_.end())
01253   {
01254     S_uint64::iterator extra_it = obj.extra_handles.begin();
01255     S_uint64::iterator extra_end = obj.extra_handles.end();
01256     for (; extra_it != extra_end; ++extra_it)
01257     {
01258       sel_it->second.extra_handles.erase(*extra_it);
01259     }
01260 
01261     if (sel_it->second.extra_handles.empty())
01262     {
01263       selection_.erase(sel_it);
01264     }
01265   }
01266 
01267   SelectionHandlerPtr handler = getHandler(obj.handle);
01268   handler->onDeselect(obj);
01269 }
01270 
01271 void SelectionManager::focusOnSelection()
01272 {
01273   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01274 
01275   if (selection_.empty())
01276   {
01277     return;
01278   }
01279 
01280   Ogre::AxisAlignedBox combined;
01281 
01282   M_Picked::iterator it = selection_.begin();
01283   M_Picked::iterator end = selection_.end();
01284   for (; it != end; ++it)
01285   {
01286     const Picked& p = it->second;
01287 
01288     SelectionHandlerPtr handler = getHandler(p.handle);
01289 
01290     V_AABB aabbs;
01291     handler->getAABBs(p, aabbs);
01292 
01293     V_AABB::iterator aabb_it = aabbs.begin();
01294     V_AABB::iterator aabb_end = aabbs.end();
01295     for (; aabb_it != aabb_end; ++aabb_it)
01296     {
01297       combined.merge(*aabb_it);
01298     }
01299   }
01300 
01301   if (!combined.isInfinite() && !combined.isNull())
01302   {
01303     Ogre::Vector3 center = combined.getCenter();
01304     ViewController* controller = vis_manager_->getCurrentViewController();
01305     controller->lookAt(center);
01306   }
01307 }
01308 
01309 } // namespace rviz


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32