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