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 <algorithm>
00031 
00032 #include <QTimer>
00033 
00034 #include <OGRE/OgreCamera.h>
00035 #include <OGRE/OgreEntity.h>
00036 #include <OGRE/OgreHardwarePixelBuffer.h>
00037 #include <OGRE/OgreManualObject.h>
00038 #include <OGRE/OgreMaterialManager.h>
00039 #include <OGRE/OgreRenderSystem.h>
00040 #include <OGRE/OgreRenderTexture.h>
00041 #include <OGRE/OgreRoot.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 #include <OGRE/OgreSceneNode.h>
00044 #include <OGRE/OgreSubEntity.h>
00045 #include <OGRE/OgreTextureManager.h>
00046 #include <OGRE/OgreViewport.h>
00047 #include <OGRE/OgreWireBoundingBox.h>
00048 
00049 #include <sensor_msgs/image_encodings.h>
00050 #include <sensor_msgs/Image.h>
00051 
00052 #include <ros/assert.h>
00053 #include <ros/node_handle.h>
00054 #include <ros/publisher.h>
00055 
00056 #include "rviz/ogre_helpers/arrow.h"
00057 #include "rviz/ogre_helpers/axes.h"
00058 #include "rviz/ogre_helpers/custom_parameter_indices.h"
00059 #include "rviz/ogre_helpers/qt_ogre_render_window.h"
00060 #include "rviz/ogre_helpers/shape.h"
00061 #include "rviz/properties/property.h"
00062 #include "rviz/properties/property_tree_model.h"
00063 #include "rviz/render_panel.h"
00064 #include "rviz/view_controller.h"
00065 #include "rviz/view_manager.h"
00066 #include "rviz/visualization_manager.h"
00067 
00068 #include "rviz/selection/selection_manager.h"
00069 #include <vector>
00070 
00071 
00072 namespace rviz
00073 {
00074 
00075 SelectionManager::SelectionManager(VisualizationManager* manager)
00076   : vis_manager_(manager)
00077   , highlight_enabled_(false)
00078   , uid_counter_(0)
00079   , interaction_enabled_(false)
00080   , debug_mode_( false )
00081   , property_model_( new PropertyTreeModel( new Property( "root" )))
00082 {
00083   for (uint32_t i = 0; i < s_num_render_textures_; ++i)
00084   {
00085     pixel_boxes_[i].data = 0;
00086   }
00087   depth_pixel_box_.data = 0;
00088 
00089   QTimer* timer = new QTimer( this );
00090   connect( timer, SIGNAL( timeout() ), this, SLOT( updateProperties() ));
00091   timer->start( 200 );
00092 }
00093 
00094 SelectionManager::~SelectionManager()
00095 {
00096   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00097 
00098   setSelection(M_Picked());
00099 
00100   highlight_node_->getParentSceneNode()->removeAndDestroyChild(highlight_node_->getName());
00101   delete highlight_rectangle_;
00102 
00103   for (uint32_t i = 0; i < s_num_render_textures_; ++i)
00104   {
00105     delete [] (uint8_t*)pixel_boxes_[i].data;
00106   }
00107   delete [] (uint8_t*)depth_pixel_box_.data;
00108 
00109   vis_manager_->getSceneManager()->destroyCamera( camera_ );
00110 
00111   delete property_model_;
00112 }
00113 
00114 void SelectionManager::setDebugMode( bool debug )
00115 {
00116   debug_mode_ = debug;
00117 }
00118 
00119 void SelectionManager::initialize()
00120 {
00121   // Create our render textures
00122   setTextureSize(1);
00123 
00124   // Create our highlight rectangle
00125   Ogre::SceneManager* scene_manager = vis_manager_->getSceneManager();
00126   highlight_node_ = scene_manager->getRootSceneNode()->createChildSceneNode();
00127 
00128   std::stringstream ss;
00129   static int count = 0;
00130   ss << "SelectionRect" << count++;
00131   highlight_rectangle_ = new Ogre::Rectangle2D(true);
00132 
00133   const static uint32_t texture_data[1] = { 0xffff0080 };
00134   Ogre::DataStreamPtr pixel_stream;
00135   pixel_stream.bind(new Ogre::MemoryDataStream( (void*)&texture_data[0], 4 ));
00136 
00137   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);
00138 
00139   Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().create(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00140   material->setLightingEnabled(false);
00141   //material->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME);
00142   highlight_rectangle_->setMaterial(material->getName());
00143   Ogre::AxisAlignedBox aabInf;
00144   aabInf.setInfinite();
00145   highlight_rectangle_->setBoundingBox(aabInf);
00146   highlight_rectangle_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00147   material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00148   material->setCullingMode(Ogre::CULL_NONE);
00149 
00150   Ogre::TextureUnitState* tex_unit = material->getTechnique(0)->getPass(0)->createTextureUnitState();
00151   tex_unit->setTextureName(tex->getName());
00152   tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00153 
00154   highlight_node_->attachObject(highlight_rectangle_);
00155 
00156   // create picking camera
00157   camera_= scene_manager->createCamera( ss.str()+"_camera" );
00158 
00159   // create fallback picking material
00160   fallback_pick_material_ = Ogre::MaterialManager::getSingleton().getByName( "rviz/DefaultPickAndDepth" );
00161   fallback_pick_material_->load();
00162 
00163   fallback_pick_cull_technique_ = fallback_pick_material_->getTechnique( "PickCull" );
00164   fallback_black_cull_technique_ = fallback_pick_material_->getTechnique( "BlackCull" );
00165   fallback_depth_cull_technique_ = fallback_pick_material_->getTechnique( "DepthCull" );
00166 
00167   fallback_pick_technique_ = fallback_pick_material_->getTechnique( "Pick" );
00168   fallback_black_technique_ = fallback_pick_material_->getTechnique( "Black" );
00169   fallback_depth_technique_ = fallback_pick_material_->getTechnique( "Depth" );
00170 }
00171 
00172 
00173 bool SelectionManager::get3DPoint( Ogre::Viewport* viewport, int x, int y, Ogre::Vector3& result_point )
00174 {
00175   ROS_DEBUG("SelectionManager.get3DPoint()");
00176   
00177   std::vector<Ogre::Vector3> result_points_temp;
00178   bool success = get3DPatch( viewport, x, y, 1, 1, true, result_points_temp);
00179   if (result_points_temp.size() == 0)
00180   {
00181     // return result_point unmodified if get point fails.
00182     return false;
00183 
00184   }
00185   result_point = result_points_temp[0];
00186   
00187   return success;
00188 }
00189 
00190 
00191 bool SelectionManager::getPatchDepthImage( Ogre::Viewport* viewport, int x, int y, unsigned width, unsigned height, std::vector<float> & depth_vector )
00192 {
00193 
00194   unsigned int num_pixels = width*height;
00195   depth_vector.reserve(num_pixels);
00196 
00197   setDepthTextureSize( width, height );
00198   
00199   
00200   M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
00201   M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();
00202 
00203   for (; handler_it != handler_end; ++handler_it)
00204   {
00205     handler_it->second->preRenderPass(0);
00206   }
00207   
00208   bool success = false;
00209   if( render( viewport, depth_render_texture_, x, y, x + width, 
00210               y + height, depth_pixel_box_, "Depth", depth_texture_width_, depth_texture_height_ ) )
00211   {
00212     uint8_t* data_ptr = (uint8_t*) depth_pixel_box_.data;
00213 
00214     for(uint32_t pixel = 0; pixel < num_pixels; ++pixel)
00215     {      
00216       uint8_t a = data_ptr[4*pixel]; 
00217       uint8_t b = data_ptr[4*pixel + 1];
00218       uint8_t c = data_ptr[4*pixel + 2];
00219       
00220       int int_depth = (c << 16) | (b << 8) | a;
00221       float normalized_depth = ((float) int_depth) / (float) 0xffffff;        
00222       depth_vector.push_back(normalized_depth * camera_->getFarClipDistance());
00223     }      
00224   }
00225   else
00226   {
00227     ROS_WARN("Failed to render depth patch\n");
00228     return false;
00229   }
00230 
00231   handler_it = objects_.begin();
00232   handler_end = objects_.end();
00233   for (; handler_it != handler_end; ++handler_it)
00234   {
00235       handler_it->second->postRenderPass(0);
00236   } 
00237   
00238   return true;
00239 }
00240 
00241 
00242 bool SelectionManager::get3DPatch( Ogre::Viewport* viewport, int x, int y, unsigned width, 
00243                                    unsigned height, bool skip_missing, std::vector<Ogre::Vector3> &result_points )
00244 {
00245   boost::recursive_mutex::scoped_lock lock(global_mutex_);  
00246   ROS_DEBUG("SelectionManager.get3DPatch()");
00247   
00248   std::vector<float> depth_vector;
00249 
00250   
00251   if ( !getPatchDepthImage( viewport, x, y,  width, height, depth_vector ) )
00252     return false;
00253   
00254   
00255   unsigned int pixel_counter = 0;
00256   Ogre::Matrix4 projection = camera_->getProjectionMatrix();
00257   float depth;
00258   
00259   for(int y_iter = 0; y_iter < height; ++y_iter)
00260     for(int x_iter = 0 ; x_iter < width; ++x_iter)
00261     {
00262       depth = depth_vector[pixel_counter];      
00263       
00264       //Deal with missing or invalid points
00265       if( ( depth > camera_->getFarClipDistance() ) || ( depth == 0 ) )
00266       {
00267         ++pixel_counter;
00268         if (!skip_missing)
00269         {
00270           result_points.push_back(Ogre::Vector3(NAN,NAN,NAN));
00271         }
00272         continue;
00273       }          
00274       
00275       
00276       Ogre::Vector3 result_point;
00277       // We want to shoot rays through the center of pixels, not the corners, 
00278       // so add .5 pixels to the x and y coordinate to get to the center
00279       // instead of the top left of the pixel.
00280       Ogre::Real screenx = float(x_iter + .5)/float(width);
00281       Ogre::Real screeny = float(y_iter + .5)/float(height); 
00282       if( projection[3][3] == 0.0 ) // If this is a perspective projection
00283       {
00284         // get world-space ray from camera & mouse coord
00285         Ogre::Ray vp_ray = camera_->getCameraToViewportRay(screenx, screeny );
00286         
00287         // transform ray direction back into camera coords
00288         Ogre::Vector3 dir_cam = camera_->getDerivedOrientation().Inverse() * vp_ray.getDirection();
00289         
00290         // normalize, so dir_cam.z == -depth
00291         dir_cam = dir_cam / dir_cam.z * depth * -1;
00292         
00293         // compute 3d point from camera origin and direction*/        
00294         result_point = camera_->getDerivedPosition() + camera_->getDerivedOrientation() * dir_cam;      
00295       }
00296       else // else this must be an orthographic projection.
00297       {
00298         // For orthographic projection, getCameraToViewportRay() does
00299         // the right thing for us, and the above math does not work.
00300         Ogre::Ray ray;
00301         camera_->getCameraToViewportRay(screenx, screeny, &ray);
00302 
00303         result_point = ray.getPoint(depth);        
00304       }
00305       
00306       result_points.push_back(result_point);
00307       ++pixel_counter;
00308     }      
00309 
00310   return result_points.size() > 0;
00311 
00312 }
00313 
00314 
00315 void SelectionManager::setDepthTextureSize(unsigned width, unsigned height)
00316 {
00317   // Cap and store requested texture size
00318   // It's probably an error if an invalid size is requested. 
00319   if ( width > 1024 )
00320   {
00321     width = 1024;
00322     ROS_ERROR_STREAM("SelectionManager::setDepthTextureSize invalid width requested. Max Width: 1024 -- Width requested: " << width << ".  Capping Width at 1024.");
00323   }
00324   
00325   if ( depth_texture_width_ != width )
00326     depth_texture_width_ = width;
00327 
00328   if ( height > 1024 )
00329   {
00330     height = 1024;
00331     ROS_ERROR_STREAM("SelectionManager::setDepthTextureSize invalid height requested. Max Height: 1024 -- Height requested: " << width << ".  Capping Height at 1024.");
00332   }
00333   
00334   if ( depth_texture_height_ != height )
00335     depth_texture_height_ = height;
00336   
00337   if ( !depth_render_texture_.get() || depth_render_texture_->getWidth() != width || depth_render_texture_->getHeight() != height)
00338     {
00339       std::string tex_name = "DepthTexture";
00340       if ( depth_render_texture_.get() )
00341       {
00342         tex_name = depth_render_texture_->getName();
00343 
00344         // destroy old
00345         Ogre::TextureManager::getSingleton().remove( tex_name );
00346       }
00347       
00348       depth_render_texture_ =
00349         Ogre::TextureManager::getSingleton().createManual( tex_name,
00350                                                        Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00351                                                        Ogre::TEX_TYPE_2D, depth_texture_width_, depth_texture_height_, 0,
00352                                                        Ogre::PF_R8G8B8,
00353                                                        Ogre::TU_RENDERTARGET );
00354 
00355       Ogre::RenderTexture* render_texture = depth_render_texture_->getBuffer()->getRenderTarget();
00356       render_texture->setAutoUpdated(false);
00357     }
00358 }
00359 
00360 
00361 void SelectionManager::setTextureSize( unsigned size )
00362 {
00363   if ( size > 1024 )
00364   {
00365     size = 1024;
00366   }
00367   
00368   texture_size_ = size;
00369 
00370   for (uint32_t pass = 0; pass < s_num_render_textures_; ++pass)
00371   {
00372     // check if we need to change the texture size
00373     if ( !render_textures_[pass].get() || render_textures_[pass]->getWidth() != size )
00374     {
00375       std::string tex_name;
00376       if ( render_textures_[pass].get() )
00377       {
00378         tex_name = render_textures_[pass]->getName();
00379 
00380         // destroy old
00381         Ogre::TextureManager::getSingleton().remove( tex_name );
00382       }
00383       else
00384       {
00385         std::stringstream ss;
00386         static int count = 0;
00387         ss << "SelectionTexture" << count++;
00388         tex_name = ss.str();
00389       }
00390 
00391       // create new texture
00392       render_textures_[pass] = Ogre::TextureManager::getSingleton().createManual( tex_name,
00393           Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, size, size, 0,
00394           Ogre::PF_R8G8B8, Ogre::TU_STATIC | Ogre::TU_RENDERTARGET);
00395 
00396       Ogre::RenderTexture* render_texture = render_textures_[pass]->getBuffer()->getRenderTarget();
00397       render_texture->setAutoUpdated(false);
00398     }
00399   }
00400 }
00401 
00402 void SelectionManager::clearHandlers()
00403 {
00404   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00405 
00406   objects_.clear();
00407 }
00408 
00409 void SelectionManager::enableInteraction( bool enable )
00410 {
00411   interaction_enabled_ = enable;
00412   M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
00413   M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();
00414   for (; handler_it != handler_end; ++handler_it)
00415   {
00416     if( InteractiveObjectPtr object = handler_it->second->getInteractiveObject().lock() )
00417     {
00418       object->enableInteraction( enable );
00419     }
00420   }
00421 }
00422 
00423 CollObjectHandle SelectionManager::createHandle()
00424 {
00425   uid_counter_++;
00426   if (uid_counter_ > 0x00ffffff)
00427   {
00428     uid_counter_ = 0;
00429   }
00430 
00431   uint32_t handle = 0;
00432 
00433   // shuffle around the bits so we get lots of colors
00434   // when we're displaying the selection buffer
00435   for ( unsigned int i=0; i<24; i++ )
00436   {
00437     uint32_t shift = (((23-i)%3)*8) + (23-i)/3;
00438     uint32_t bit = ( (uint32_t)(uid_counter_ >> i) & (uint32_t)1 ) << shift;
00439     handle |= bit;
00440   }
00441 
00442   return handle;
00443 }
00444 
00445 void SelectionManager::addObject(CollObjectHandle obj, SelectionHandler* handler)
00446 {
00447   if (!obj)
00448   {
00449 //    ROS_BREAK();
00450     return;
00451   }
00452 
00453   boost::recursive_mutex::scoped_lock lock(global_mutex_);
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_CollObject& 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       pixels.push_back(handle);
00582     }
00583   }
00584 }
00585 
00586 void SelectionManager::renderAndUnpack(Ogre::Viewport* viewport, uint32_t pass, int x1, int y1, int x2, int y2, V_CollObject& pixels)
00587 {
00588   ROS_ASSERT(pass < s_num_render_textures_);
00589 
00590   std::stringstream scheme;
00591   scheme << "Pick";
00592   if (pass > 0)
00593   {
00594     scheme << pass;
00595   }
00596 
00597   if( render( viewport, render_textures_[pass], x1, y1, x2, y2, pixel_boxes_[pass], scheme.str(), texture_size_, texture_size_ ))
00598   {
00599     unpackColors(pixel_boxes_[pass], pixels);
00600   }
00601 }
00602 
00603 
00604 bool SelectionManager::render(Ogre::Viewport* viewport, Ogre::TexturePtr tex,
00605                               int x1, int y1, int x2, int y2,
00606                               Ogre::PixelBox& dst_box, std::string material_scheme,
00607                               unsigned texture_width, unsigned texture_height)
00608 {
00609   vis_manager_->lockRender();
00610 
00611   if ( x1 > x2 ) std::swap( x1, x2 );
00612   if ( y1 > y2 ) std::swap( y1, y2 );
00613 
00614   if ( x1 < 0 ) x1 = 0;
00615   if ( y1 < 0 ) y1 = 0;
00616   if ( x1 > viewport->getActualWidth()-2 ) x1 = viewport->getActualWidth()-2;
00617   if ( y1 > viewport->getActualHeight()-2 ) y1 = viewport->getActualHeight()-2;
00618   if ( x2 < 0 ) x2 = 0;
00619   if ( y2 < 0 ) y2 = 0;
00620   if ( x2 > viewport->getActualWidth()-2 ) x2 = viewport->getActualWidth()-2;
00621   if ( y2 > viewport->getActualHeight()-2 ) y2 = viewport->getActualHeight()-2;
00622 
00623   if ( x2==x1 ) x2++;
00624   if ( y2==y1 ) y2++;
00625 
00626   if ( x2==x1 || y2==y1 )
00627   {
00628     ROS_WARN("SelectionManager::render(): not rendering 0 size area.");
00629     vis_manager_->unlockRender();
00630     return false;
00631   }
00632 
00633   unsigned w = x2-x1;
00634   unsigned h = y2-y1;
00635 
00636   Ogre::HardwarePixelBufferSharedPtr pixel_buffer = tex->getBuffer();
00637   Ogre::RenderTexture* render_texture = pixel_buffer->getRenderTarget();
00638 
00639   Ogre::Matrix4 proj_matrix = viewport->getCamera()->getProjectionMatrix();
00640   Ogre::Matrix4 scale_matrix = Ogre::Matrix4::IDENTITY;
00641   Ogre::Matrix4 trans_matrix = Ogre::Matrix4::IDENTITY;
00642 
00643   float x1_rel = static_cast<float>(x1) / static_cast<float>(viewport->getActualWidth() - 1) - 0.5f;
00644   float y1_rel = static_cast<float>(y1) / static_cast<float>(viewport->getActualHeight() - 1) - 0.5f;
00645   float x2_rel = static_cast<float>(x2) / static_cast<float>(viewport->getActualWidth() - 1) - 0.5f;
00646   float y2_rel = static_cast<float>(y2) / static_cast<float>(viewport->getActualHeight() - 1) - 0.5f;
00647 
00648   scale_matrix[0][0] = 1.0 / (x2_rel-x1_rel);
00649   scale_matrix[1][1] = 1.0 / (y2_rel-y1_rel);
00650 
00651   trans_matrix[0][3] -= x1_rel+x2_rel;
00652   trans_matrix[1][3] += y1_rel+y2_rel;
00653 
00654   camera_->setCustomProjectionMatrix( true, scale_matrix * trans_matrix * proj_matrix );
00655 
00656   camera_->setPosition( viewport->getCamera()->getDerivedPosition() );
00657   camera_->setOrientation( viewport->getCamera()->getDerivedOrientation() );
00658 
00659   // create a viewport if there is none
00660   if (render_texture->getNumViewports() == 0)
00661   {
00662     render_texture->removeAllViewports();
00663     render_texture->addViewport( camera_ );
00664     Ogre::Viewport* render_viewport = render_texture->getViewport(0);
00665     render_viewport->setClearEveryFrame(true);
00666     render_viewport->setBackgroundColour( Ogre::ColourValue::Black );
00667     render_viewport->setOverlaysEnabled(false);
00668     render_viewport->setMaterialScheme(material_scheme);
00669   }
00670 
00671   unsigned render_w = w;
00672   unsigned render_h = h;
00673 
00674   if ( w>h )
00675   {
00676     if ( render_w > texture_width )
00677     {
00678       render_w = texture_width;
00679       render_h = round( float(h) * (float)texture_width / (float)w );
00680     }
00681   }
00682   else
00683   {
00684     if ( render_h > texture_height )
00685     {
00686       render_h = texture_height;
00687       render_w = round( float(w) * (float)texture_height / (float)h );
00688     }
00689   }
00690 
00691   // safety clamping in case of rounding errors
00692   if ( render_w > texture_width ) render_w = texture_width;
00693   if ( render_h > texture_height ) render_h = texture_height;
00694 
00695   // set viewport to render to a subwindow of the texture
00696   Ogre::Viewport* render_viewport = render_texture->getViewport(0);
00697   render_viewport->setDimensions( 0, 0,
00698                                   (float)render_w / (float)texture_width,
00699                                   (float)render_h / (float)texture_height );
00700 
00701   // make sure the same objects are visible as in the original viewport
00702   render_viewport->setVisibilityMask( viewport->getVisibilityMask() );
00703 
00704   ros::WallTime start = ros::WallTime::now();
00705 
00706   // update & force ogre to render the scene
00707   Ogre::MaterialManager::getSingleton().addListener(this);
00708 
00709   render_texture->update();
00710 
00711   // For some reason we need to pretend to render the main window in
00712   // order to get the picking render to show up in the pixelbox below.
00713   // If we don't do this, it will show up there the *next* time we
00714   // pick something, but not this time.  This object as a
00715   // render queue listener tells the scene manager to skip every
00716   // render step, so nothing actually gets drawn.
00717   // 
00718   // TODO: find out what part of _renderScene() actually makes this work.
00719   Ogre::Viewport* main_view = vis_manager_->getRenderPanel()->getViewport();
00720   vis_manager_->getSceneManager()->addRenderQueueListener(this);
00721   vis_manager_->getSceneManager()->_renderScene(main_view->getCamera(), main_view, false);
00722   vis_manager_->getSceneManager()->removeRenderQueueListener(this);
00723 
00724   ros::WallTime end = ros::WallTime::now();
00725   ros::WallDuration d = end - start;
00726 //  ROS_DEBUG("Render took [%f] msec", d.toSec() * 1000.0f);
00727 
00728   Ogre::MaterialManager::getSingleton().removeListener(this);
00729 
00730   render_w = render_viewport->getActualWidth();
00731   render_h = render_viewport->getActualHeight();
00732 
00733   Ogre::PixelFormat format = pixel_buffer->getFormat();
00734 
00735   int size = Ogre::PixelUtil::getMemorySize(render_w, render_h, 1, format);
00736   uint8_t* data = new uint8_t[size];
00737 
00738   delete [] (uint8_t*)dst_box.data;
00739   dst_box = Ogre::PixelBox(render_w, render_h, 1, format, data);
00740 
00741   pixel_buffer->blitToMemory(dst_box,dst_box);
00742 
00743   vis_manager_->unlockRender();
00744 
00745   if( debug_mode_ )
00746   {
00747     publishDebugImage( dst_box, material_scheme );
00748   }
00749 
00750   return true;
00751 }
00752 
00753 void SelectionManager::publishDebugImage( const Ogre::PixelBox& pixel_box, const std::string& label )
00754 {
00755   ros::Publisher pub;
00756   ros::NodeHandle nh;
00757   PublisherMap::const_iterator iter = debug_publishers_.find( label );
00758   if( iter == debug_publishers_.end() )
00759   {
00760     pub = nh.advertise<sensor_msgs::Image>( "/rviz_debug/" + label, 2 );
00761     debug_publishers_[ label ] = pub;
00762   }
00763   else
00764   {
00765     pub = iter->second;
00766   }
00767 
00768   sensor_msgs::Image msg;
00769   msg.header.stamp = ros::Time::now();
00770   msg.width = pixel_box.getWidth();
00771   msg.height = pixel_box.getHeight();
00772   msg.encoding = sensor_msgs::image_encodings::RGB8;
00773   msg.is_bigendian = false;
00774   msg.step = msg.width * 3;
00775   int dest_byte_count = msg.width * msg.height * 3;
00776   msg.data.resize( dest_byte_count );
00777   int dest_index = 0;
00778   uint8_t* source_ptr = (uint8_t*)pixel_box.data;
00779   int pre_pixel_padding = 0;
00780   int post_pixel_padding = 0;
00781   switch( pixel_box.format )
00782   {
00783   case Ogre::PF_A8R8G8B8:
00784   case Ogre::PF_X8R8G8B8:
00785     post_pixel_padding = 1;
00786     break;
00787   case Ogre::PF_R8G8B8A8:
00788     pre_pixel_padding = 1;
00789     break;
00790   default:
00791     ROS_ERROR( "SelectionManager::publishDebugImage(): Incompatible pixel format [%d]", pixel_box.format );
00792     return;
00793   }
00794   uint8_t r, g, b;
00795   while( dest_index < dest_byte_count )
00796   {
00797     source_ptr += pre_pixel_padding;
00798     b = *source_ptr++;
00799     g = *source_ptr++;
00800     r = *source_ptr++;
00801     source_ptr += post_pixel_padding;
00802     msg.data[ dest_index++ ] = r;
00803     msg.data[ dest_index++ ] = g;
00804     msg.data[ dest_index++ ] = b;
00805   }
00806 
00807   pub.publish( msg );
00808 }
00809 
00810 void SelectionManager::renderQueueStarted( uint8_t queueGroupId,
00811                                            const std::string& invocation, 
00812                                            bool& skipThisInvocation )
00813 {
00814   // This render queue listener function tells the scene manager to
00815   // skip every render step, so nothing actually gets drawn.
00816 
00817 //  ROS_DEBUG("SelectionManager renderQueueStarted(%d, '%s') returning skip = true.", (int)queueGroupId, invocation.c_str());
00818   skipThisInvocation = true;
00819 }
00820 
00821 void SelectionManager::pick(Ogre::Viewport* viewport, int x1, int y1, int x2, int y2, M_Picked& results, bool single_render_pass)
00822 {
00823   boost::recursive_mutex::scoped_lock lock(global_mutex_);
00824 
00825   bool need_additional_render = false;
00826 
00827   V_CollObject handles_by_pixel;
00828   S_CollObject need_additional;
00829 
00830   V_CollObject& pixels = pixel_buffer_;
00831 
00832   // First render is special... does the initial object picking, determines which objects have been selected
00833   // After that, individual handlers can specify that they need additional renders (max # defined in s_num_render_textures_)
00834   {
00835     M_CollisionObjectToSelectionHandler::iterator handler_it = objects_.begin();
00836     M_CollisionObjectToSelectionHandler::iterator handler_end = objects_.end();
00837     for (; handler_it != handler_end; ++handler_it)
00838     {
00839       handler_it->second->preRenderPass( 0 );
00840     }
00841 
00842     renderAndUnpack(viewport, 0, x1, y1, x2, y2, pixels);
00843 
00844     handler_it = objects_.begin();
00845     handler_end = objects_.end();
00846     for (; handler_it != handler_end; ++handler_it)
00847     {
00848       handler_it->second->postRenderPass(0);
00849     }
00850 
00851     handles_by_pixel.reserve(pixels.size());
00852     V_CollObject::iterator it = pixels.begin();
00853     V_CollObject::iterator end = pixels.end();
00854     for (; it != end; ++it)
00855     {
00856       const CollObjectHandle& p = *it;
00857 
00858       CollObjectHandle handle = p;
00859 
00860       handles_by_pixel.push_back(handle);
00861 
00862       if (handle == 0)
00863       {
00864         continue;
00865       }
00866 
00867       SelectionHandler* handler = getHandler( handle );
00868 
00869       if( handler )
00870       {
00871         std::pair<M_Picked::iterator, bool> insert_result = results.insert(std::make_pair(handle, Picked(handle)));
00872         if (insert_result.second)
00873         {
00874           if (handler->needsAdditionalRenderPass(1) && !single_render_pass)
00875           {
00876             need_additional.insert(handle);
00877             need_additional_render = true;
00878           }
00879         }
00880         else
00881         {
00882           insert_result.first->second.pixel_count++;
00883         }
00884       }
00885     }
00886   }
00887 
00888   uint32_t pass = 1;
00889 
00890   V_uint64 extra_by_pixel;
00891   extra_by_pixel.resize(handles_by_pixel.size());
00892   while (need_additional_render && pass < s_num_render_textures_)
00893   {
00894     {
00895       S_CollObject::iterator need_it = need_additional.begin();
00896       S_CollObject::iterator need_end = need_additional.end();
00897       for (; need_it != need_end; ++need_it)
00898       {
00899         SelectionHandler* handler = getHandler( *need_it );
00900         ROS_ASSERT(handler);
00901 
00902         handler->preRenderPass(pass);
00903       }
00904     }
00905 
00906     renderAndUnpack(viewport, pass, x1, y1, x2, y2, pixels);
00907 
00908     {
00909       S_CollObject::iterator need_it = need_additional.begin();
00910       S_CollObject::iterator need_end = need_additional.end();
00911       for (; need_it != need_end; ++need_it)
00912       {
00913         SelectionHandler* handler = getHandler( *need_it );
00914         ROS_ASSERT(handler);
00915 
00916         handler->postRenderPass(pass);
00917       }
00918     }
00919 
00920     int i = 0;
00921     V_CollObject::iterator pix_it = pixels.begin();
00922     V_CollObject::iterator pix_end = pixels.end();
00923     for (; pix_it != pix_end; ++pix_it, ++i)
00924     {
00925       const CollObjectHandle& p = *pix_it;
00926 
00927       CollObjectHandle handle = handles_by_pixel[i];
00928 
00929       if (pass == 1)
00930       {
00931         extra_by_pixel[i] = 0;
00932       }
00933 
00934       if (need_additional.find(handle) != need_additional.end())
00935       {
00936         CollObjectHandle extra_handle = p;
00937         extra_by_pixel[i] |= extra_handle << (32 * (pass-1));
00938       }
00939       else
00940       {
00941         extra_by_pixel[i] = 0;
00942       }
00943     }
00944 
00945     need_additional_render = false;
00946     need_additional.clear();
00947     M_Picked::iterator handle_it = results.begin();
00948     M_Picked::iterator handle_end = results.end();
00949     for (; handle_it != handle_end; ++handle_it)
00950     {
00951       CollObjectHandle handle = handle_it->first;
00952 
00953       if (getHandler(handle)->needsAdditionalRenderPass(pass + 1))
00954       {
00955         need_additional_render = true;
00956         need_additional.insert(handle);
00957       }
00958     }
00959   }
00960 
00961   int i = 0;
00962   V_uint64::iterator pix_2_it = extra_by_pixel.begin();
00963   V_uint64::iterator pix_2_end = extra_by_pixel.end();
00964   for (; pix_2_it != pix_2_end; ++pix_2_it, ++i)
00965   {
00966     CollObjectHandle handle = handles_by_pixel[i];
00967 
00968     if (handle == 0)
00969     {
00970       continue;
00971     }
00972 
00973     M_Picked::iterator picked_it = results.find(handle);
00974     if (picked_it == results.end())
00975     {
00976       continue;
00977     }
00978 
00979     Picked& picked = picked_it->second;
00980 
00981     if (*pix_2_it)
00982     {
00983       picked.extra_handles.insert(*pix_2_it);
00984     }
00985   }
00986 }
00987 
00988 Ogre::Technique *SelectionManager::handleSchemeNotFound(unsigned short scheme_index,
00989     const Ogre::String& scheme_name,
00990     Ogre::Material* original_material,
00991     unsigned short lod_index,
00992     const Ogre::Renderable* rend )
00993 {
00994   // Find the original culling mode
00995   Ogre::CullingMode culling_mode = Ogre::CULL_CLOCKWISE;
00996   Ogre::Technique* orig_tech = original_material->getTechnique( 0 );
00997   if( orig_tech && orig_tech->getNumPasses() > 0 )
00998   {
00999     culling_mode = orig_tech->getPass( 0 )->getCullingMode();
01000   }
01001 
01002   // find out if the renderable has the picking param set
01003   bool has_pick_param = ! rend->getUserObjectBindings().getUserAny( "pick_handle" ).isEmpty();
01004 
01005   // NOTE: it is important to avoid changing the culling mode of the
01006   // fallback techniques here, because that change then propagates to
01007   // other uses of these fallback techniques in unpredictable ways.
01008   // If you want to change the technique programmatically (like with
01009   // Pass::setCullingMode()), make sure you do it on a cloned material
01010   // which doesn't get shared with other objects.
01011 
01012   // Use the technique with the right name and culling mode.
01013   if( culling_mode == Ogre::CULL_CLOCKWISE )
01014   {
01015     if( scheme_name == "Pick" )
01016     {
01017       return has_pick_param ? fallback_pick_cull_technique_ : fallback_black_cull_technique_;
01018     }
01019     else if( scheme_name == "Depth" )
01020     {
01021       return fallback_depth_cull_technique_;
01022     }
01023     if( scheme_name == "Pick1" )
01024     {
01025       return fallback_black_cull_technique_;
01026     }
01027     else
01028     {
01029       return NULL;
01030     }
01031   }
01032   else // Must be CULL_NONE because we never use CULL_ANTICLOCKWISE
01033   {
01034     if( scheme_name == "Pick" )
01035     {
01036       return has_pick_param ? fallback_pick_technique_ : fallback_black_technique_;
01037     }
01038     else if( scheme_name == "Depth" )
01039     {
01040       return fallback_depth_technique_;
01041     }
01042     if( scheme_name == "Pick1" )
01043     {
01044       return fallback_black_technique_;
01045     }
01046     else
01047     {
01048       return NULL;
01049     }
01050   }
01051 }
01052 
01053 Ogre::ColourValue SelectionManager::handleToColor( CollObjectHandle handle )
01054 {
01055   float r = ((handle >> 16) & 0xff) / 255.0f;
01056   float g = ((handle >> 8) & 0xff) / 255.0f;
01057   float b = (handle & 0xff) / 255.0f;
01058   return Ogre::ColourValue( r, g, b, 1.0f );
01059 }
01060 
01061 void SelectionManager::setPickData( CollObjectHandle handle, const Ogre::ColourValue& color, Ogre::SceneNode* node )
01062 {
01063   if (!node)
01064   {
01065     return;
01066   }
01067   // Loop over all objects attached to this node.
01068   Ogre::SceneNode::ObjectIterator obj_it = node->getAttachedObjectIterator();
01069   while( obj_it.hasMoreElements() )
01070   {
01071     Ogre::MovableObject* obj = obj_it.getNext();
01072     setPickData( handle, color, obj );
01073   }
01074   // Loop over and recurse into all child nodes.
01075   Ogre::SceneNode::ChildNodeIterator child_it = node->getChildIterator();
01076   while( child_it.hasMoreElements() )
01077   {
01078     Ogre::SceneNode* child = dynamic_cast<Ogre::SceneNode*>( child_it.getNext() );
01079     setPickData( handle, color, child );
01080   }
01081 }
01082 
01083 class PickColorSetter: public Ogre::Renderable::Visitor
01084 {
01085 public:
01086   PickColorSetter( CollObjectHandle handle, const Ogre::ColourValue& color )
01087     : color_vector_( color.r, color.g, color.b, 1.0 ), handle_(handle) {}
01088 
01089   virtual void visit( Ogre::Renderable* rend, ushort lodIndex, bool isDebug, Ogre::Any* pAny = 0 )
01090   {
01091     rend->setCustomParameter( PICK_COLOR_PARAMETER, color_vector_ );
01092     rend->getUserObjectBindings().setUserAny( "pick_handle", Ogre::Any( handle_ ));
01093   }
01094 
01095   Ogre::Vector4 color_vector_;
01096   CollObjectHandle handle_;
01097 };
01098 
01099 void SelectionManager::setPickData( CollObjectHandle handle, const Ogre::ColourValue& color, Ogre::MovableObject* object )
01100 {
01101   PickColorSetter visitor( handle, color );
01102   object->visitRenderables( &visitor );
01103   object->getUserObjectBindings().setUserAny( "pick_handle", Ogre::Any( handle ));
01104 }
01105 
01106 SelectionHandler* SelectionManager::getHandler( CollObjectHandle obj )
01107 {
01108   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01109 
01110   M_CollisionObjectToSelectionHandler::iterator it = objects_.find(obj);
01111   if (it != objects_.end())
01112   {
01113     return it->second;
01114   }
01115 
01116   return NULL;
01117 }
01118 
01119 void SelectionManager::removeSelection(const M_Picked& objs)
01120 {
01121   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01122 
01123   M_Picked::const_iterator it = objs.begin();
01124   M_Picked::const_iterator end = objs.end();
01125   for (; it != end; ++it)
01126   {
01127     removeSelectedObject(it->second);
01128   }
01129 
01130   selectionRemoved( objs );
01131 }
01132 
01133 void SelectionManager::addSelection(const M_Picked& objs)
01134 {
01135   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01136 
01137   M_Picked added;
01138   M_Picked::const_iterator it = objs.begin();
01139   M_Picked::const_iterator end = objs.end();
01140   for (; it != end; ++it)
01141   {
01142     std::pair<Picked, bool> ppb = addSelectedObject(it->second);
01143     if (ppb.second)
01144     {
01145       added.insert(std::make_pair(it->first, ppb.first));
01146     }
01147   }
01148 
01149   selectionAdded( added );
01150 }
01151 
01152 void SelectionManager::setSelection(const M_Picked& objs)
01153 {
01154   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01155 
01156   M_Picked original(selection_.begin(), selection_.end());
01157 
01158   removeSelection(original);
01159   addSelection(objs);
01160 }
01161 
01162 std::pair<Picked, bool> SelectionManager::addSelectedObject(const Picked& obj)
01163 {
01164   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01165 
01166   std::pair<M_Picked::iterator, bool> pib = selection_.insert(std::make_pair(obj.handle, obj));
01167 
01168   SelectionHandler* handler = getHandler( obj.handle );
01169 
01170   if (pib.second)
01171   {
01172     handler->onSelect(obj);
01173     return std::make_pair(obj, true);
01174   }
01175   else
01176   {
01177     Picked& cur = pib.first->second;
01178     Picked added(cur.handle);
01179 
01180     S_uint64::iterator it = obj.extra_handles.begin();
01181     S_uint64::iterator end = obj.extra_handles.end();
01182     for (; it != end; ++it)
01183     {
01184       if (cur.extra_handles.insert(*it).second)
01185       {
01186         added.extra_handles.insert(*it);
01187       }
01188     }
01189 
01190     if (!added.extra_handles.empty())
01191     {
01192       handler->onSelect(added);
01193 
01194       return std::make_pair(added, true);
01195     }
01196   }
01197 
01198   return std::make_pair(Picked(0), false);
01199 }
01200 
01201 void SelectionManager::removeSelectedObject(const Picked& obj)
01202 {
01203   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01204 
01205   M_Picked::iterator sel_it = selection_.find(obj.handle);
01206   if (sel_it != selection_.end())
01207   {
01208     S_uint64::iterator extra_it = obj.extra_handles.begin();
01209     S_uint64::iterator extra_end = obj.extra_handles.end();
01210     for (; extra_it != extra_end; ++extra_it)
01211     {
01212       sel_it->second.extra_handles.erase(*extra_it);
01213     }
01214 
01215     if (sel_it->second.extra_handles.empty())
01216     {
01217       selection_.erase(sel_it);
01218     }
01219   }
01220 
01221   SelectionHandler* handler = getHandler( obj.handle );
01222   handler->onDeselect(obj);
01223 }
01224 
01225 void SelectionManager::focusOnSelection()
01226 {
01227   boost::recursive_mutex::scoped_lock lock(global_mutex_);
01228 
01229   if (selection_.empty())
01230   {
01231     return;
01232   }
01233 
01234   Ogre::AxisAlignedBox combined;
01235 
01236   M_Picked::iterator it = selection_.begin();
01237   M_Picked::iterator end = selection_.end();
01238   for (; it != end; ++it)
01239   {
01240     const Picked& p = it->second;
01241 
01242     SelectionHandler* handler = getHandler( p.handle );
01243 
01244     V_AABB aabbs;
01245     handler->getAABBs(p, aabbs);
01246 
01247     V_AABB::iterator aabb_it = aabbs.begin();
01248     V_AABB::iterator aabb_end = aabbs.end();
01249     for (; aabb_it != aabb_end; ++aabb_it)
01250     {
01251       combined.merge(*aabb_it);
01252     }
01253   }
01254 
01255   if (!combined.isInfinite() && !combined.isNull())
01256   {
01257     Ogre::Vector3 center = combined.getCenter();
01258     ViewController* controller = vis_manager_->getViewManager()->getCurrent();
01259     if( controller )
01260     {
01261       controller->lookAt(center);
01262     }
01263   }
01264 }
01265 
01266 void SelectionManager::selectionRemoved( const M_Picked& removed )
01267 {
01268   M_Picked::const_iterator it = removed.begin();
01269   M_Picked::const_iterator end = removed.end();
01270   for (; it != end; ++it)
01271   {
01272     const Picked& picked = it->second;
01273     SelectionHandler* handler = getHandler( picked.handle );
01274     ROS_ASSERT(handler);
01275 
01276     handler->destroyProperties( picked, property_model_->getRoot() );
01277   }
01278 }
01279 
01280 void SelectionManager::selectionAdded( const M_Picked& added )
01281 {
01282   M_Picked::const_iterator it = added.begin();
01283   M_Picked::const_iterator end = added.end();
01284   for (; it != end; ++it)
01285   {
01286     const Picked& picked = it->second;
01287     SelectionHandler* handler = getHandler( picked.handle );
01288     ROS_ASSERT(handler);
01289 
01290     handler->createProperties( picked, property_model_->getRoot() );
01291   }
01292   property_model_->sort( 0, Qt::AscendingOrder );
01293 }
01294 
01295 void SelectionManager::updateProperties()
01296 {
01297   M_Picked::const_iterator it = selection_.begin();
01298   M_Picked::const_iterator end = selection_.end();
01299   for (; it != end; ++it)
01300   {
01301     CollObjectHandle handle = it->first;
01302     SelectionHandler* handler = getHandler( handle );
01303 
01304     handler->updateProperties();
01305   }
01306 }
01307 
01308 
01309 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:36