but_camcast.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Vit Stancl (stancl@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: dd/mm/2011
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00028 #include "but_camcast.h"
00029 
00030 // Ogre includes
00031 #include <OGRE/OgreSceneManager.h>
00032 #include "OGRE/OgreWindowEventUtilities.h"
00033 #include "OGRE/OgreManualObject.h"
00034 #include "OGRE/OgreEntity.h"
00035 
00036 // Rviz includes
00037 #include <rviz/render_panel.h>
00038 #include <rviz/visualization_manager.h>
00039 #include <rviz/window_manager_interface.h>
00040 
00041 #include <wx/filedlg.h>
00042 
00043 // Ros image message
00044 #include <sensor_msgs/Image.h>
00045 
00046 // Std includes
00047 #include <sstream>
00048 
00049 #define CAMERA_SCREENCAST_TOPIC_NAME std::string("rviz_cam_rgb")
00050 #define DEFAULT_PUBLISHING_PERIOD double(0.1)
00051 
00052 /*
00053  *  Constructor
00054  */
00055 srs_ui_but::CButCamCast::CButCamCast(const std::string & name,rviz::VisualizationManager * manager)
00056 : Display( name, manager )
00057 , m_bPublish(true)
00058 {
00059   // Create and connect pane
00060   rviz::WindowManagerInterface * wi( manager->getWindowManager() );
00061 
00062   if( wi != 0 )
00063   {
00064     // Arm manipulation controls
00065     m_pane = new CControllPane( wi->getParentWindow(), wxT("Screencasts manager"), wi);
00066 
00067     if( m_pane != 0 )
00068     {
00069       wi->addPane( "Screencasts manager", m_pane );
00070       wi->showPane( m_pane );
00071 
00072       // Connect signal
00073       m_pane->getSigChckBox().connect( boost::bind( &CButCamCast::onPublishStateChanged, this, _1) );
00074       m_pane->getSigSave().connect( boost::bind( &CButCamCast::onSave, this, _1 ) );
00075     }
00076 
00077   }else{
00078     std::cerr << "No window manager, no panes :( " << std::endl;
00079   }
00080 
00081 
00082   // Get node handle
00083   ros::NodeHandle private_nh("/");
00084 
00085   // Set parameters
00086 
00087   // Get camera screencasting topic name
00088   private_nh.param("rviz_screencast_topic_name", m_camCastPublisherName, CAMERA_SCREENCAST_TOPIC_NAME);
00089 
00090   // Get timer period
00091   private_nh.param("publishig_period", m_timerPeriod, DEFAULT_PUBLISHING_PERIOD );
00092 
00093   // Get scene node
00094   m_sceneNode = scene_manager_->getRootSceneNode()->createChildSceneNode();
00095 
00096   // Create publisher
00097   this->m_camCastPublisher = private_nh.advertise< sensor_msgs::Image >(m_camCastPublisherName, 100, false);
00098 
00099   // Create geometry
00100   createGeometry(private_nh);
00101 
00102   // Create publishing timer
00103   m_timer = private_nh.createTimer( ros::Duration(m_timerPeriod), boost::bind( &CButCamCast::onTimerPublish, this, _1) );
00104 
00105   // Set publishing parameters
00106   m_textureWithRtt->setFrame("/map");
00107 
00108 }
00109 
00110 /*
00111  *  Destructor
00112  */
00113 srs_ui_but::CButCamCast::~CButCamCast()
00114 {
00115   // Destroy all geometry
00116   destroyGeometry();
00117 
00118 }
00119 
00120 
00121 /*
00122  *  Display enablet callback
00123  */
00124 void srs_ui_but::CButCamCast::onEnable()
00125 {
00126   m_sceneNode->setVisible( true );
00127 }
00128 
00129 /*
00130  *  Display disabled callback
00131  */
00132 void srs_ui_but::CButCamCast::onDisable()
00133 {
00134   m_sceneNode->setVisible( false );
00135 }
00136 
00137 /*
00138  *  Create geometry
00139  */
00140 bool srs_ui_but::CButCamCast::createGeometry(const ros::NodeHandle & nh)
00141 {
00142   // Get camera.
00143   rviz::RenderPanel * panel = vis_manager_->getRenderPanel();
00144   if( panel == 0 )
00145   {
00146     ROS_DEBUG( "No render panel... ");
00147     return false;
00148   }
00149 
00150   Ogre::Camera * camera = panel->getCamera();
00151 
00152 
00153   // Create rtt texture
00154   m_textureWithRtt = new CRosRttTexture( 512, 512, camera );
00155 
00156   /*
00157   Ogre::String lNameOfTheMesh = "MeshCube";
00158   {
00159     // Here, I create a 3D element, by using the interface of ManualObject.
00160     // ManualObject is very close to the opengl old simple way to specify geometry.
00161     // There are other interfaces (Hardwarebuffers), you can check the ogremanual fo them and wiki.
00162     // For each vertex I will provide positions and attributes (normal, vertex color, texture coordinates...).
00163     // Then for each primitive (given its type : triangle, line, line strip etc...),
00164     // I give the corresponding group of vertex index.
00165     Ogre::ManualObject* lManualObject = NULL;
00166     {
00167       // The manualObject creation requires a name.
00168       Ogre::String lManualObjectName = "CubeWithAxes";
00169       lManualObject = scene_manager_->createManualObject(lManualObjectName);
00170 
00171       // Always tell if you want to update the 3D (vertex/index) later or not.
00172       bool lDoIWantToUpdateItLater = false;
00173       lManualObject->setDynamic(lDoIWantToUpdateItLater);
00174 
00175       // Here I create a cube in a first part with triangles, and then axes (in red/green/blue).
00176 
00177       // BaseWhiteNoLighting is the name of a material that already exist inside Ogre.
00178       // Ogre::RenderOperation::OT_TRIANGLE_LIST is a kind of primitive.
00179       float lSize = 0.7f;
00180       lManualObject->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST);
00181       {
00182         float cp = 1.0f * lSize ;
00183         float cm = -1.0f * lSize;
00184 
00185         lManualObject->position(cm, cp, cm);// a vertex
00186         lManualObject->colour(Ogre::ColourValue(0.0f,1.0f,0.0f,1.0f));
00187         lManualObject->textureCoord(0.0, 1.0);
00188         lManualObject->position(cp, cp, cm);// a vertex
00189         lManualObject->colour(Ogre::ColourValue(1.0f,1.0f,0.0f,1.0f));
00190         lManualObject->textureCoord(1.0, 1.0);
00191         lManualObject->position(cp, cm, cm);// a vertex
00192         lManualObject->colour(Ogre::ColourValue(1.0f,0.0f,0.0f,1.0f));
00193         lManualObject->textureCoord(1.0, 0.0);
00194         lManualObject->position(cm, cm, cm);// a vertex
00195         lManualObject->colour(Ogre::ColourValue(0.0f,0.0f,0.0f,1.0f));
00196         lManualObject->textureCoord(0.0, 0.0);
00197 
00198         lManualObject->position(cm, cp, cp);// a vertex
00199         lManualObject->colour(Ogre::ColourValue(0.0f,1.0f,1.0f,1.0f));
00200         lManualObject->textureCoord(0.0, 1.0);
00201         lManualObject->position(cp, cp, cp);// a vertex
00202         lManualObject->colour(Ogre::ColourValue(1.0f,1.0f,1.0f,1.0f));
00203         lManualObject->textureCoord(1.0, 1.0);
00204         lManualObject->position(cp, cm, cp);// a vertex
00205         lManualObject->colour(Ogre::ColourValue(1.0f,0.0f,1.0f,1.0f));
00206         lManualObject->textureCoord(1.0, 0.0);
00207         lManualObject->position(cm, cm, cp);// a vertex
00208         lManualObject->colour(Ogre::ColourValue(0.0f,0.0f,1.0f,1.0f));
00209         lManualObject->textureCoord(0.0, 0.0);
00210 
00211         // face behind / front
00212         lManualObject->triangle(0,1,2);
00213         lManualObject->triangle(2,3,0);
00214         lManualObject->triangle(4,6,5);
00215         lManualObject->triangle(6,4,7);
00216 
00217         // face top / down
00218         lManualObject->triangle(0,4,5);
00219         lManualObject->triangle(5,1,0);
00220         lManualObject->triangle(2,6,7);
00221         lManualObject->triangle(7,3,2);
00222 
00223         // face left / right
00224         lManualObject->triangle(0,7,4);
00225         lManualObject->triangle(7,0,3);
00226         lManualObject->triangle(1,5,6);
00227         lManualObject->triangle(6,2,1);
00228       }
00229       lManualObject->end();
00230       // Here I have finished my ManualObject construction.
00231       // It is possible to add more begin()-end() thing, in order to use
00232       // different rendering operation types, or different materials in 1 ManualObject.
00233     }
00234 
00235     lManualObject->convertToMesh(lNameOfTheMesh, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00236 
00237     // Now I can create several entities using that mesh
00238     int lNumberOfEntities = 5;
00239     for(int iter = 0; iter < lNumberOfEntities; ++iter)
00240     {
00241       Ogre::Entity* lEntity = scene_manager_->createEntity(lNameOfTheMesh);
00242       // Now I attach it to a scenenode, so that it becomes present in the scene.
00243       Ogre::SceneNode* lNode = scene_manager_->getRootSceneNode()->createChildSceneNode();
00244       lNode->attachObject(lEntity);
00245       // I move the SceneNode so that it is visible to the camera.
00246       float lPositionOffset = float(1+ iter * 2) - (float(lNumberOfEntities));
00247       lNode->translate(lPositionOffset, lPositionOffset, -10.0f);
00248     }
00249   }
00250 
00251   // Create entity on screen
00252 
00253   Ogre::Entity* lAdditionalEntity = scene_manager_->createEntity( lNameOfTheMesh );
00254   lAdditionalEntity->setMaterialName( m_textureWithRtt->getMaterialName() );
00255   Ogre::SceneNode* lVisibleOnlyByFirstCam = scene_manager_->getRootSceneNode()->createChildSceneNode();
00256   lVisibleOnlyByFirstCam->attachObject( lAdditionalEntity );
00257   lVisibleOnlyByFirstCam->setPosition( 1.5,-0.5,-5);
00258 */
00259   return true;
00260 }
00261 
00262 void srs_ui_but::CButCamCast::destroyGeometry()
00263 {
00264   // Destroy scene
00265   if( m_sceneNode != 0 )
00266     scene_manager_->destroySceneNode(m_sceneNode->getName());
00267 }
00268 
00270 void srs_ui_but::CButCamCast::update (float wall_dt, float ros_dt)
00271 {
00272   rviz::RenderPanel * panel = vis_manager_->getRenderPanel();
00273   if( panel == 0 )
00274   {
00275     ROS_DEBUG( "No render panel... ");
00276   }
00277 
00278   Ogre::Camera * camera = panel->getCamera();
00279 
00280   if( camera == 0 )
00281   {
00282     ROS_DEBUG( "No camera ");
00283   }
00284 }
00285 
00286 /*
00287  * Timer publishing callback function
00288  */
00289 
00290 void srs_ui_but::CButCamCast::onTimerPublish(const ros::TimerEvent&)
00291 {
00292 
00293 
00294   if( m_bPublish && (m_camCastPublisher.getNumSubscribers() > 0 ) && m_textureWithRtt->hasData() )
00295   {
00296     // Publish image
00297     m_camCastPublisher.publish( m_textureWithRtt->getImage() );
00298   }
00299 }
00300 
00304 void srs_ui_but::CButCamCast::onPublishStateChanged(bool state)
00305 {
00306   if( state )
00307     m_timer.start();
00308   else
00309     m_timer.stop();
00310 }
00311 
00315 void srs_ui_but::CButCamCast::onSave( std::string filename )
00316 {
00317   m_textureWithRtt->saveImage( filename );
00318 }
00319 
00320 
00322 const int ID_CHECKBOX(100);
00323 const int ID_SAVE_BUTTON(101);
00324 
00325 srs_ui_but::CButCamCast::CControllPane::CControllPane(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi): wxPanel( parent, wxID_ANY, wxDefaultPosition, wxSize(280, 180), wxTAB_TRAVERSAL, title)
00326 , m_wmi( wmi )
00327 {
00328   // Create controls
00329   m_button = new wxButton(this, ID_SAVE_BUTTON, wxT("Save screenshot"), wxDefaultPosition, wxDefaultSize, wxBU_EXACTFIT);
00330 
00331   m_chkb = new wxCheckBox(this, ID_CHECKBOX, wxT("Publish images"),
00332       wxPoint(20, 20));
00333   m_chkb->SetValue( true );
00334 
00335   // Create layout
00336   wxSizer *sizer = new wxBoxSizer(wxVERTICAL);
00337   this->SetSizer(sizer);
00338 
00339   wxSizer *hsizer = new wxBoxSizer(wxHORIZONTAL);
00340   hsizer->Add(m_chkb, ID_CHECKBOX, wxALIGN_LEFT);
00341   hsizer->Add(m_button, ID_SAVE_BUTTON, wxALIGN_RIGHT);
00342 
00343   sizer->Add(hsizer, 0, wxALIGN_LEFT);
00344 
00345   sizer->SetSizeHints(this);
00346 
00347 }
00348 
00350 
00354 void srs_ui_but::CButCamCast::CControllPane::OnChckToggle(wxCommandEvent& event)
00355 {
00356   // Call signal
00357   m_sigCheckBox( m_chkb->GetValue() );
00358 }
00359 
00361 
00365 void srs_ui_but::CButCamCast::CControllPane::OnSave(wxCommandEvent& event)
00366 {
00367   wxFileDialog fileDlg(this, _("Choose file to save"), wxEmptyString, _("screenshot.png"), _("*.*"), wxFD_SAVE | wxFD_OVERWRITE_PROMPT );
00368 
00369   wxString filename, directory;
00370 
00371   // Pause timer
00372   if(m_chkb->GetValue())
00373     m_sigCheckBox( false );
00374 
00375   if( fileDlg.ShowModal() == wxID_OK )
00376   {
00377     filename = fileDlg.GetFilename();
00378     directory = fileDlg.GetDirectory();
00379 
00380     std::string path(std::string( wxString(directory + wxFileName::GetPathSeparator() + filename).mb_str() ) );
00381 
00382      m_sigSave( path );
00383   }
00384 
00385   // Start timer
00386   if(m_chkb->GetValue())
00387     m_sigCheckBox( true );
00388 
00389 
00390 }
00391 
00393 BEGIN_EVENT_TABLE(srs_ui_but::CButCamCast::CControllPane, wxPanel)
00394 EVT_BUTTON(ID_SAVE_BUTTON,  srs_ui_but::CButCamCast::CControllPane::OnSave)
00395 EVT_CHECKBOX(ID_CHECKBOX, srs_ui_but::CButCamCast::CControllPane::OnChckToggle )
00396 END_EVENT_TABLE()


srs_ui_but
Author(s): Vit Stancl (stancl@fit.vutbr.cz), Michal Spanel (spanel@fit.vutbr.cz), Tomas Lokaj
autogenerated on Sun Jan 5 2014 12:12:49