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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31