renderer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "rve_render_server/renderer.h"
00031 // TODO server should really hook into the renderer, rather than the renderer knowing about the server
00032 #include "rve_render_server/server.h"
00033 #include "rve_render_server/init.h"
00034 #include "rve_render_server/render_window.h"
00035 #include "rve_render_server/render_texture.h"
00036 #include "rve_render_server/scene.h"
00037 #include "rve_render_server/camera.h"
00038 #include "rve_render_server/disable_rendering_scheme_listener.h"
00039 #include "rve_render_server/mesh_loader.h"
00040 #include "rve_render_server/pickable.h"
00041 
00042 #include <OGRE/OgreRoot.h>
00043 #include <OGRE/OgreRenderSystem.h>
00044 #include <OGRE/OgreRenderWindow.h>
00045 #include <OGRE/OgreEntity.h>
00046 #include <OGRE/OgreCamera.h>
00047 #include <OGRE/OgreSceneManager.h>
00048 
00049 #include <ros/console.h>
00050 #include <ros/assert.h>
00051 #include <ros/package.h>
00052 #include <ros/callback_queue.h>
00053 #include <ros/node_handle.h>
00054 
00055 #include <rve_msgs/FrameStats.h>
00056 #include <std_msgs/Empty.h>
00057 
00058 #if USE_NVPERFSDK
00059 
00060 #include "NVPerfSDK.h"
00061 
00062 // On some versions of Linux NVPMInit() crashes in __strstr_sse42
00063 // This forces it to use our version instead
00064 // strstr implementation found at: http://en.wikibooks.org/wiki/C_Programming/Strings#The_strstr_function
00065 // nvidia developer forums post about the crash: http://developer.nvidia.com/forums/index.php?showtopic=4926
00066 const char *(strstr)(const char *haystack, const char *needle)
00067 {
00068    size_t needlelen;
00069    /* Check for the null needle case.  */
00070    if (*needle == '\0')
00071        return (char *) haystack;
00072    needlelen = strlen(needle);
00073    for (; (haystack = strchr(haystack, *needle)) != NULL; haystack++)
00074        if (strncmp(haystack, needle, needlelen) == 0)
00075            return (char *) haystack;
00076    return NULL;
00077 }
00078 
00079 #endif
00080 
00081 using namespace rve_common;
00082 
00083 namespace rve_render_server
00084 {
00085 
00086 Renderer::Renderer(bool enable_ogre_log)
00087 : running_(false)
00088 , first_window_created_(false)
00089 , enable_ogre_log_(enable_ogre_log)
00090 , callback_queue_(new ros::CallbackQueue)
00091 #if USE_NVPERFSDK
00092 , nvperfsdk_initialized_(false)
00093 , do_experiment_(false)
00094 , start_experiment_(false)
00095 #endif
00096 {
00097   pickable_id_gen_.seed(time(0));
00098 }
00099 
00100 Renderer::~Renderer()
00101 {
00102   stop();
00103 }
00104 
00105 void Renderer::start()
00106 {
00107   if (running_)
00108   {
00109     return;
00110   }
00111 
00112   init();
00113 
00114   running_ = true;
00115   render_thread_ = boost::thread(&Renderer::renderThread, this);
00116 }
00117 
00118 void Renderer::stop()
00119 {
00120   if (!running_)
00121   {
00122     return;
00123   }
00124 
00125   running_ = false;
00126   render_thread_.join();
00127 
00128   scenes_.clear();
00129   materials_.clear();
00130   meshes_.clear();
00131   render_targets_.clear();
00132   Ogre::Root::getSingleton().getRenderSystem()->destroyRenderWindow(primary_render_window_->getName());
00133   delete Ogre::Root::getSingletonPtr();
00134 }
00135 
00136 void Renderer::init()
00137 {
00138   Ogre::LogManager* log_manager = new Ogre::LogManager();
00139   log_manager->createLog( "Ogre.log", false, false, !enable_ogre_log_ );
00140 
00141   Ogre::Root* root = new Ogre::Root("");
00142   root->loadPlugin( "RenderSystem_GL" );
00143   root->loadPlugin( "Plugin_OctreeSceneManager" );
00144   root->loadPlugin( "Plugin_CgProgramManager" );
00145 
00146   // Taken from gazebo
00147   Ogre::RenderSystem* render_system = NULL;
00148 #if OGRE_VERSION_MAJOR >=1 && OGRE_VERSION_MINOR >= 7
00149   const Ogre::RenderSystemList& rsList = root->getAvailableRenderers();
00150   Ogre::RenderSystemList::const_iterator renderIt = rsList.begin();
00151   Ogre::RenderSystemList::const_iterator renderEnd = rsList.end();
00152 #else
00153   Ogre::RenderSystemList* rsList = root->getAvailableRenderers();
00154   Ogre::RenderSystemList::iterator renderIt = rsList->begin();
00155   Ogre::RenderSystemList::iterator renderEnd = rsList->end();
00156 #endif
00157   for ( ; renderIt != renderEnd; ++renderIt )
00158   {
00159     render_system = *renderIt;
00160 
00161     if ( render_system->getName() == "OpenGL Rendering Subsystem" )
00162     {
00163       break;
00164     }
00165   }
00166 
00167   if ( render_system == NULL )
00168   {
00169     throw std::runtime_error( "Could not find the opengl rendering subsystem!\n" );
00170   }
00171 
00172   //render_system->setConfigOption("FSAA","4");
00173   render_system->setConfigOption("RTT Preferred Mode", "FBO");
00174 
00175   root->setRenderSystem( render_system );
00176   root->initialise( false );
00177 
00178   scheme_listener_.reset(new DisableRenderingSchemeListener);
00179   Ogre::MaterialManager::getSingleton().addListener(scheme_listener_.get(), "GBuffer");
00180   Ogre::MaterialManager::getSingleton().addListener(scheme_listener_.get(), "GBufferStippleAlpha");
00181   Ogre::MaterialManager::getSingleton().addListener(scheme_listener_.get(), "AlphaBlend");
00182   Ogre::MaterialManager::getSingleton().addListener(scheme_listener_.get(), "WeightedAverageAlpha");
00183   Ogre::MaterialManager::getSingleton().addListener(scheme_listener_.get(), "Pick");
00184 
00185   Ogre::LogManager::getSingleton().getDefaultLog()->setDebugOutputEnabled(true);
00186   Ogre::LogManager::getSingleton().getDefaultLog()->setLogDetail(Ogre::LL_BOREME);
00187 
00188   frame_stats_pub_ = getServer()->getNodeHandle().advertise<rve_msgs::FrameStats>("frame_stats", 10);
00189 }
00190 
00191 template<typename F>
00192 void callAFunction(F f)
00193 {
00194   f();
00195 }
00196 
00197 void Renderer::threadInit()
00198 {
00199   Ogre::Root* root = Ogre::Root::getSingletonPtr();
00200   // Create the primary hidden render window.  Ogre uses the first-created render window to bind all its
00201   // gpu resources to, so you aren't allowed to destroy it, which is problematic if we use the first render window
00202   // that is created by a client.
00203   Ogre::NameValuePairList params;
00204   params["hidden"] = "true";
00205   primary_render_window_ = root->createRenderWindow("Primary", 1, 1, false, &params);
00206   Ogre::ResourceGroupManager::getSingleton().createResourceGroup(ROS_PACKAGE_NAME);
00207 
00208   std::string pkg_path = ros::package::getPath(ROS_PACKAGE_NAME);
00209   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/textures", "FileSystem", ROS_PACKAGE_NAME );
00210   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/fonts", "FileSystem", ROS_PACKAGE_NAME );
00211   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/models", "FileSystem", ROS_PACKAGE_NAME );
00212   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/materials/programs", "FileSystem", ROS_PACKAGE_NAME );
00213   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/materials/scripts", "FileSystem", ROS_PACKAGE_NAME );
00214   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/compositors", "FileSystem", ROS_PACKAGE_NAME );
00215   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/shaderlib", "FileSystem", ROS_PACKAGE_NAME );
00216   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/shaderlib/points", "FileSystem", ROS_PACKAGE_NAME );
00217   Ogre::ResourceGroupManager::getSingleton().addResourceLocation( pkg_path + "/media/shaderlib/lines", "FileSystem", ROS_PACKAGE_NAME );
00218   Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
00219 
00220   std::for_each(init_functions_.begin(), init_functions_.end(), callAFunction<InitFunction>);
00221   init_functions_.clear();
00222 }
00223 
00224 RenderWindow* Renderer::createRenderWindow(const rve_common::UUID& id, const std::string& parent_window, uint32_t width, uint32_t height)
00225 {
00226   if (render_targets_.count(id) > 0)
00227   {
00228     std::stringstream ss;
00229     ss << "Render target with id [" << id << "] already exists";
00230     throw std::runtime_error(ss.str());
00231   }
00232 
00233   RenderWindow* rw = new RenderWindow(id, this, parent_window, width, height);
00234   RenderTargetPtr ptr(rw);
00235   render_targets_[id] = ptr;
00236 
00237   return rw;
00238 }
00239 
00240 RenderWindow* Renderer::getRenderWindow(const rve_common::UUID& id)
00241 {
00242   RenderTarget* target = getRenderTarget(id);
00243   RenderWindow* win = dynamic_cast<RenderWindow*>(target);
00244   if (!win)
00245   {
00246     std::stringstream ss;
00247     ss << "Render target [" << id << "] is not a window";
00248     throw std::runtime_error(ss.str());
00249   }
00250 
00251   return win;
00252 }
00253 
00254 RenderTexture* Renderer::createRenderTexture(const rve_common::UUID& id, uint32_t width, uint32_t height)
00255 {
00256   if (render_targets_.count(id) > 0)
00257   {
00258     std::stringstream ss;
00259     ss << "Render target with id [" << id << "] already exists";
00260     throw std::runtime_error(ss.str());
00261   }
00262 
00263   RenderTexture* rt = new RenderTexture(id, this, width, height);
00264   RenderTargetPtr ptr(rt);
00265   render_targets_[id] = ptr;
00266 
00267   return rt;
00268 }
00269 
00270 void Renderer::destroyRenderTarget(const rve_common::UUID& id)
00271 {
00272   M_RenderTarget::iterator it = render_targets_.find(id);
00273   if (it == render_targets_.end())
00274   {
00275     std::stringstream ss;
00276     ss << "Render target [" << id << "] does not exist";
00277     throw std::runtime_error(ss.str());
00278   }
00279 
00280   render_targets_.erase(it);
00281 }
00282 
00283 RenderTarget* Renderer::getRenderTarget(const rve_common::UUID& id)
00284 {
00285   M_RenderTarget::iterator it = render_targets_.find(id);
00286   if (it == render_targets_.end())
00287   {
00288     std::stringstream ss;
00289     ss << "Render target [" << id << "] does not exist";
00290     throw std::runtime_error(ss.str());
00291   }
00292 
00293   return it->second.get();
00294 }
00295 
00296 Scene* Renderer::createScene(const UUID& id)
00297 {
00298   if (scenes_.count(id) > 0)
00299   {
00300     ROS_WARN_STREAM("UUID " << id << " collided when creating a scene!");
00301     return 0;
00302   }
00303 
00304   Ogre::Root* root = Ogre::Root::getSingletonPtr();
00305   Ogre::SceneManager* scene_manager = root->createSceneManager(Ogre::ST_GENERIC);
00306   ScenePtr scene(new Scene(id, scene_manager));
00307   scenes_[id] = scene;
00308 
00309   return scene.get();
00310 }
00311 
00312 void Renderer::destroyScene(const UUID& id)
00313 {
00314   M_Scene::iterator it = scenes_.find(id);
00315   if (it == scenes_.end())
00316   {
00317     std::stringstream ss;
00318     ss << "Scene " << id << " does not exist!";
00319     throw std::runtime_error(ss.str());
00320   }
00321 
00322   scenes_.erase(it);
00323 }
00324 
00325 Scene* Renderer::getScene(const UUID& id)
00326 {
00327   M_Scene::iterator it = scenes_.find(id);
00328   if (it == scenes_.end())
00329   {
00330     std::stringstream ss;
00331     ss << "Scene " << id << " does not exist!";
00332     throw std::runtime_error(ss.str());
00333   }
00334 
00335   return it->second.get();
00336 }
00337 
00338 Camera* Renderer::getCamera(const UUID& id)
00339 {
00340   M_Scene::iterator it = scenes_.begin();
00341   M_Scene::iterator end = scenes_.end();
00342   for (; it != end; ++it)
00343   {
00344     const ScenePtr& scene = it->second;
00345     Camera* cam = scene->getCamera(id);
00346     if (cam)
00347     {
00348       return cam;
00349     }
00350   }
00351 
00352   return 0;
00353 }
00354 
00355 void Renderer::addMaterial(const rve_common::UUID& id, const MaterialPtr& mat)
00356 {
00357   materials_[id] = mat;
00358 }
00359 
00360 void Renderer::removeMaterial(const rve_common::UUID& id)
00361 {
00362   materials_.erase(id);
00363 }
00364 
00365 MaterialPtr Renderer::getMaterial(const rve_common::UUID& id)
00366 {
00367   M_Material::iterator it = materials_.find(id);
00368   if (it == materials_.end())
00369   {
00370     std::stringstream ss;
00371     ss << "Material [" << id << "] does not exist";
00372     throw std::runtime_error(ss.str());
00373   }
00374 
00375   return it->second;
00376 }
00377 
00378 void Renderer::addMesh(const rve_common::UUID& id, const MeshPtr& mesh)
00379 {
00380   meshes_[id] = mesh;
00381 }
00382 
00383 void Renderer::removeMesh(const rve_common::UUID& id)
00384 {
00385   meshes_.erase(id);
00386 }
00387 
00388 MeshPtr Renderer::getMesh(const rve_common::UUID& id)
00389 {
00390   M_Mesh::iterator it = meshes_.find(id);
00391   if (it == meshes_.end())
00392   {
00393     std::stringstream ss;
00394     ss << "Mesh [" << id << "] does not exist";
00395     throw std::runtime_error(ss.str());
00396   }
00397 
00398   return it->second;
00399 }
00400 
00401 bool Renderer::meshExists(const rve_common::UUID& id)
00402 {
00403   return meshes_.find(id) != meshes_.end();
00404 }
00405 
00406 void Renderer::addTexture(const rve_common::UUID& id, const TexturePtr& tex)
00407 {
00408   textures_[id] = tex;
00409 }
00410 
00411 void Renderer::removeTexture(const rve_common::UUID& id)
00412 {
00413   textures_.erase(id);
00414 }
00415 
00416 TexturePtr Renderer::getTexture(const rve_common::UUID& id)
00417 {
00418   M_Texture::iterator it = textures_.find(id);
00419   if (it == textures_.end())
00420   {
00421     throw std::runtime_error("Texture [" + id.toString() + "] does not exist");
00422   }
00423 
00424   return it->second;
00425 }
00426 
00427 bool Renderer::textureExists(const rve_common::UUID& id)
00428 {
00429   return textures_.find(id) != textures_.end();
00430 }
00431 
00432 uint32_t Renderer::allocatePickID(Pickable* p)
00433 {
00434   ROS_ASSERT(p);
00435 
00436   uint32_t id = 0;
00437   while (id == 0 || (!pickables_.insert(std::make_pair(id, p)).second))
00438   {
00439     id = pickable_id_gen_();
00440   }
00441 
00442   return id;
00443 }
00444 
00445 void Renderer::deallocatePickID(uint32_t pick_id)
00446 {
00447   pickables_.erase(pick_id);
00448 }
00449 
00450 void Renderer::translatePickIDs(const V_PickRenderValues& ids, V_Picked& out_picked)
00451 {
00452   // TODO: if we assume ids is sorted we can cache the pickable
00453   // which should help performance if this turns out to be a bottleneck
00454   V_PickRenderValues::const_iterator it = ids.begin();
00455   V_PickRenderValues::const_iterator end = ids.end();
00456   for (; it != end; ++it)
00457   {
00458     const PickRenderValues& prv = *it;
00459     if (prv.isNull())
00460     {
00461       continue;
00462     }
00463 
00464     M_Pickable::iterator pickable_it = pickables_.find(prv.id);
00465     if (pickable_it != pickables_.end())
00466     {
00467       Pickable* p = pickable_it->second;
00468       Picked picked = p->translatePick(prv);
00469       if (picked.isNull())
00470       {
00471         continue;
00472       }
00473 
00474       out_picked.push_back(picked);
00475     }
00476   }
00477 }
00478 
00479 bool Renderer::useGeometryShaders()
00480 {
00481   //return false;
00482   return Ogre::Root::getSingleton().getRenderSystem()->getCapabilities()->hasCapability(Ogre::RSC_GEOMETRY_PROGRAM);
00483 }
00484 
00485 #if USE_NVPERFSDK
00486 std::vector<std::pair<std::string, UINT> > g_perf_counters;
00487 
00488 int enumerateNVPerfCounters(UINT index, char* name)
00489 {
00490   ROS_INFO_STREAM(index << " " << name);
00491   g_perf_counters.push_back(std::make_pair(name, index));
00492   return NVPM_OK;
00493 }
00494 
00495 void Renderer::doExperimentCallback(const std_msgs::EmptyConstPtr&)
00496 {
00497   start_experiment_ = true;
00498 }
00499 
00500 #endif
00501 
00502 void Renderer::renderThread()
00503 {
00504   threadInit();
00505 
00506 #if USE_NVPERFSDK
00507   {
00508     uint32_t ret = NVPMInit();
00509     if (ret == NVPM_OK)
00510     {
00511       nvperfsdk_initialized_ = true;
00512       NVPMEnumCounters(enumerateNVPerfCounters);
00513       for (uint32_t i = 0; i < g_perf_counters.size(); ++i)
00514       {
00515         //const std::string& name = g_perf_counters[i].first;
00516         UINT index = g_perf_counters[i].second;
00517         int ret = NVPMAddCounter(index);
00518         ROS_ASSERT_MSG(ret == NVPM_OK, "ret = %d", ret);
00519       }
00520 
00521       experiment_stats_pub_ = getServer()->getNodeHandle().advertise<rve_msgs::FrameStats>("experiment_stats", 10);
00522       do_experiment_sub_ = getServer()->getNodeHandle().subscribe("do_experiment", 1, &Renderer::doExperimentCallback, this);
00523     }
00524     else
00525     {
00526       ROS_ERROR("Failed to initialize NVPerfSDK: %d", ret);
00527     }
00528   }
00529 #endif
00530 
00531   while (running_)
00532   {
00533     if (render_targets_.empty())
00534     {
00535       callback_queue_->callAvailable(ros::WallDuration(0.1));
00536       continue;
00537     }
00538 
00539 #if USE_NVPERFSDK
00540     if (start_experiment_)
00541     {
00542       do_experiment_ = true;
00543       start_experiment_ = false;
00544     }
00545 #endif
00546 
00547     ros::WallTime start = ros::WallTime::now();
00548 
00549     rve_msgs::FrameStats frame_stats;
00550 
00551 #if 0
00552     Ogre::Root::getSingleton().renderOneFrame();
00553 #else
00554     if (Ogre::Root::getSingleton()._fireFrameStarted())
00555     {
00556 #if USE_NVPERFSDK
00557       int pass_count = 1;
00558 
00559       if (do_experiment_)
00560       {
00561         NVPMBeginExperiment(&pass_count);
00562         ROS_INFO("pass_count %d", pass_count);
00563       }
00564 
00565       for (int pass = 0; pass < pass_count; ++pass)
00566       {
00567         if (do_experiment_)
00568         {
00569           NVPMBeginPass(pass);
00570         }
00571 #endif
00572 
00573       M_RenderTarget::iterator it = render_targets_.begin();
00574       M_RenderTarget::iterator end = render_targets_.end();
00575       for (; it != end; ++it)
00576       {
00577         const RenderTargetPtr& wnd = it->second;
00578         wnd->beginRender();
00579       }
00580 
00581 #if USE_NVPERFSDK
00582       if (!do_experiment_)
00583 #endif
00584       {
00585         callback_queue_->callAvailable(ros::WallDuration(0.1));
00586       }
00587 
00588       it = render_targets_.begin();
00589       end = render_targets_.end();
00590       for (; it != end; ++it)
00591       {
00592         const RenderTargetPtr& wnd = it->second;
00593         wnd->finishRender();
00594 
00595         frame_stats.render_window_stats.push_back(wnd->getLastFrameStats());
00596       }
00597 
00598 #if USE_NVPERFSDK
00599         if (do_experiment_)
00600         {
00601           NVPMEndPass(pass);
00602         }
00603       }
00604 
00605       if (do_experiment_)
00606       {
00607         NVPMEndExperiment();
00608         callback_queue_->callAvailable();
00609 
00610         UINT64 value, cycles;
00611         NVPMGetCounterValueByName("GPU Bottleneck", 0, &value, &cycles);
00612         char bottleneck_name[1024];
00613         NVPMGetGPUBottleneckName(value, bottleneck_name);
00614         frame_stats.nv_perf_counters.gpu_bottleneck = bottleneck_name;
00615       }
00616 #endif
00617 
00618       Ogre::Root::getSingleton()._fireFrameEnded();
00619     }
00620     else
00621 #endif
00622     {
00623       callback_queue_->callAvailable();
00624     }
00625 
00626     ros::WallTime end = ros::WallTime::now();
00627     //ROS_INFO("Frame took %f", (end - start).toSec());
00628     frame_stats.total_frame_time = (end - start).toSec();
00629 
00630 #if USE_NVPERFSDK
00631     if (nvperfsdk_initialized_)
00632     {
00633       UINT count = 0;
00634       NVPMSample(0, &count);
00635       //ROS_INFO_STREAM("Samples: " << count);
00636 
00637       for (uint32_t i = 0; i < g_perf_counters.size(); ++i)
00638       {
00639         const std::string& name = g_perf_counters[i].first;
00640         UINT index = g_perf_counters[i].second;
00641         UINT64 value = 0;
00642         UINT64 cycles = 0;
00643         int ret = NVPMGetCounterValue(index, 0, &value, &cycles);
00644         if (ret == NVPM_OK)
00645         {
00646           rve_msgs::NVPerfCounter counter;
00647           counter.name = name;
00648           counter.value = value;
00649           counter.cycles = cycles;
00650           counter.value_as_pct = ((double)value / (double)cycles) * 100.0f;
00651           frame_stats.nv_perf_counters.counters.push_back(counter);
00652           //std::cout << name << ": " << "value: " << value << " cycles: " << cycles << std::endl;
00653         }
00654         else
00655         {
00656           ROS_ERROR("Failed to retrieve %s: %d", name.c_str(), ret);
00657         }
00658       }
00659     }
00660 
00661     if (do_experiment_)
00662     {
00663       experiment_stats_pub_.publish(frame_stats);
00664       do_experiment_ = false;
00665     }
00666     else
00667 #endif
00668     {
00669       frame_stats_pub_.publish(frame_stats);
00670     }
00671 
00672   }
00673 
00674 #if USE_NVPERFSDK
00675   if (nvperfsdk_initialized_)
00676   {
00677     NVPMShutdown();
00678   }
00679 #endif
00680 }
00681 
00682 void Renderer::registerInitFunction(const InitFunction& func)
00683 {
00684   init_functions_.push_back(func);
00685 }
00686 
00687 } // namespace rve_render_server


rve_render_server
Author(s): Josh Faust
autogenerated on Wed Dec 11 2013 14:31:15