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