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