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