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