$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: but_display.cpp 810 2012-05-19 21:47:51Z stancl $ 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_display.h" 00029 #include <OGRE/OgreSceneManager.h> 00030 #include <rviz/render_panel.h> 00031 #include <rviz/visualization_manager.h> 00032 #include <rviz/window_manager_interface.h> 00033 #include <sstream> 00034 #include <srs_ui_but/topics_list.h> 00035 #include <tf/transform_broadcaster.h> 00036 00037 /* 00038 * Constructor 00039 */ 00040 srs_ui_but::CButDisplay::CButDisplay(const std::string & name, rviz::VisualizationManager * manager) 00041 : Display( name, manager ) 00042 , m_manualObject( 0 ) 00043 , m_child_window( 0 ) 00044 , m_dialog_window( 0 ) 00045 , m_controls_window( 0 ) 00046 { 00047 // Get node handle 00048 ros::NodeHandle private_nh("~"); 00049 00050 // Connect to the controller changed signal 00051 vis_manager_->getViewControllerTypeChangedSignal().connect( boost::bind(&CButDisplay::onViewControllerChange, this, _1 ) ); 00052 00053 // Get scene node 00054 m_sceneNode = scene_manager_->getRootSceneNode()->createChildSceneNode(); 00055 00056 // Try to create, add and show example window 00057 00058 rviz::WindowManagerInterface * wi( manager->getWindowManager() ); 00059 00060 if( wi != 0 ) 00061 { 00062 // Create camera control pane 00063 std::cerr << "Creating camera pane" << std::endl; 00064 00065 m_camera_window = new CCameraControlPane( wi->getParentWindow(), _T("Camera control"), wi ); 00066 00067 if( m_camera_window != 0 ) 00068 { 00069 m_camera_window->fixedFrameChanged( fixed_frame_ ); 00070 m_camera_window->targetFrameChanged( target_frame_ ); 00071 00072 wi->addPane( "Camera control", m_camera_window ); 00073 wi->showPane( m_camera_window ); 00074 ROS_DEBUG("Camera control pane added..."); 00075 } 00076 00077 } 00078 else 00079 { 00080 std::cerr << "No window manager, no panes :( " << std::endl; 00081 } 00082 } 00083 00084 /* 00085 * Destructor 00086 */ 00087 srs_ui_but::CButDisplay::~CButDisplay() 00088 { 00089 // Destroy all geometry 00090 destroyGeometry(); 00091 } 00092 00096 void srs_ui_but::CButDisplay::targetFrameChanged() 00097 { 00098 } 00099 00103 void srs_ui_but::CButDisplay::fixedFrameChanged() 00104 { 00105 } 00106 00110 void srs_ui_but::CButDisplay::createProperties() 00111 { 00112 // Create some properties 00113 00114 // Get camera. 00115 rviz::RenderPanel * panel = vis_manager_->getRenderPanel(); 00116 if( panel != 0 ) 00117 { 00118 Ogre::Camera * camera = panel->getCamera(); 00119 00120 // Set camera to window 00121 m_camera_window->setCamera( camera ); 00122 } 00123 else 00124 ROS_DEBUG( "No render panel... "); 00125 } 00126 00127 /* 00128 * Display enabled callback 00129 */ 00130 void srs_ui_but::CButDisplay::onEnable() 00131 { 00132 m_sceneNode->setVisible( true ); 00133 } 00134 00135 /* 00136 * Display disabled callback 00137 */ 00138 void srs_ui_but::CButDisplay::onDisable() 00139 { 00140 m_sceneNode->setVisible( false ); 00141 } 00142 00143 /* 00144 * Create geometry 00145 */ 00146 bool srs_ui_but::CButDisplay::createGeometry() 00147 { 00148 // Create manual object 00149 m_manualObject = scene_manager_->createManualObject( "manual" ); 00150 00151 // Create some geometry 00152 m_manualObject->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); 00153 { 00154 float lSize( 1.0f ); 00155 float cp( 1.0f * lSize ); 00156 float cm( -1.0f * lSize ); 00157 00158 // Insert some vertices 00159 m_manualObject->position(cm, cp, cm);// a vertex 00160 m_manualObject->colour(Ogre::ColourValue(0.0f,1.0f,0.0f,1.0f)); 00161 m_manualObject->position(cp, cp, cm);// a vertex 00162 m_manualObject->colour(Ogre::ColourValue(1.0f,1.0f,0.0f,1.0f)); 00163 m_manualObject->position(cp, cm, cm);// a vertex 00164 m_manualObject->colour(Ogre::ColourValue(1.0f,0.0f,0.0f,1.0f)); 00165 m_manualObject->position(cm, cm, cm);// a vertex 00166 m_manualObject->colour(Ogre::ColourValue(0.0f,0.0f,0.0f,1.0f)); 00167 00168 m_manualObject->position(cm, cp, cp);// a vertex 00169 m_manualObject->colour(Ogre::ColourValue(0.0f,1.0f,1.0f,1.0f)); 00170 m_manualObject->position(cp, cp, cp);// a vertex 00171 m_manualObject->colour(Ogre::ColourValue(1.0f,1.0f,1.0f,1.0f)); 00172 m_manualObject->position(cp, cm, cp);// a vertex 00173 m_manualObject->colour(Ogre::ColourValue(1.0f,0.0f,1.0f,1.0f)); 00174 m_manualObject->position(cm, cm, cp);// a vertex 00175 m_manualObject->colour(Ogre::ColourValue(0.0f,0.0f,1.0f,1.0f)); 00176 00177 // Create triangles - behind and front 00178 m_manualObject->triangle(0,1,2); 00179 m_manualObject->triangle(2,3,0); 00180 m_manualObject->triangle(4,6,5); 00181 m_manualObject->triangle(6,4,7); 00182 00183 // Top and bottom 00184 m_manualObject->triangle(0,4,5); 00185 m_manualObject->triangle(5,1,0); 00186 m_manualObject->triangle(2,6,7); 00187 m_manualObject->triangle(7,3,2); 00188 00189 // Left and right face 00190 m_manualObject->triangle(0,7,4); 00191 m_manualObject->triangle(7,0,3); 00192 m_manualObject->triangle(1,5,6); 00193 m_manualObject->triangle(6,2,1); 00194 } 00195 m_manualObject->end(); 00196 00197 m_manualObject->setDynamic( false ); 00198 00199 // Attach manual object to the scene 00200 m_sceneNode->attachObject( m_manualObject ); 00201 00202 return true; 00203 } 00204 00205 void srs_ui_but::CButDisplay::destroyGeometry() 00206 { 00207 // Destroy manual object 00208 if( m_manualObject != 0 ) 00209 scene_manager_->destroyManualObject( m_manualObject ); 00210 00211 // Destroy scene 00212 if( m_sceneNode != 0 ) 00213 scene_manager_->destroySceneNode(m_sceneNode->getName()); 00214 } 00215 00216 void srs_ui_but::CButDisplay::update (float wall_dt, float ros_dt) 00217 { 00218 rviz::RenderPanel * panel = vis_manager_->getRenderPanel(); 00219 if( panel == 0 ) 00220 { 00221 ROS_DEBUG( "No render panel... "); 00222 } 00223 00224 /* Ogre::Camera * camera = panel->getCamera(); 00225 00226 if( camera == 0 ) 00227 { 00228 ROS_DEBUG( "No camera "); 00229 }*/ 00230 } 00231 00235 void srs_ui_but::CButDisplay::onViewControllerChange( rviz::ViewController * c ) 00236 { 00237 } 00238