stdr_laser_visualization.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015    
00016    Authors : 
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com 
00020 ******************************************************************************/
00021 
00022 #include "stdr_gui/stdr_visualization/stdr_laser_visualization.h"
00023 
00024 namespace stdr_gui
00025 {
00026 
00033   CLaserVisualisation::CLaserVisualisation(QString name,float resolution):
00034     name_(name),
00035     resolution_(resolution)
00036   {
00037     setupUi(this);
00038     setWindowTitle(name_);
00039     active_ = true;
00040     
00041     ros::NodeHandle n;
00042     
00043     subscriber_ = n.subscribe(
00044       name_.toStdString().c_str(), 
00045       1, 
00046       &CLaserVisualisation::callback,
00047       this);
00048     
00049     void_image_ = QImage(
00050       laserImage->width(),
00051       laserImage->height(),
00052       QImage::Format_RGB32);
00053       
00054     void_image_.fill(QColor(255,255,255,255));
00055   }
00056   
00061   CLaserVisualisation::~CLaserVisualisation(void)
00062   {
00063     
00064   }
00065   
00070   void CLaserVisualisation::destruct(void)
00071   {
00072     active_ = false;
00073     subscriber_.shutdown();
00074     hide();
00075     delete laserMean;
00076     delete laserMax;
00077     delete laserMin;
00078     delete laserImage;
00079   }
00080   
00086   void CLaserVisualisation::closeEvent(QCloseEvent *event)
00087   {
00088     destruct();
00089     active_ = false;
00090     subscriber_.shutdown();
00091   }
00092   
00097   bool CLaserVisualisation::getActive(void)
00098   {
00099     return active_;
00100   }
00101   
00107   void CLaserVisualisation::setLaser(stdr_msgs::LaserSensorMsg msg)
00108   {
00109     msg_ = msg;
00110     laserMax->setText(QString().setNum(msg.maxRange) + QString(" m"));
00111     laserMin->setText(QString().setNum(msg.minRange) + QString(" m"));
00112   }
00113   
00119   void CLaserVisualisation::callback(const sensor_msgs::LaserScan& msg)
00120   {
00121     scan_ = msg;
00122   }
00123   
00128   void CLaserVisualisation::paint(void)
00129   {
00130     internal_image_ = void_image_;
00131     QPainter painter(&internal_image_);
00132     float mean = 0;
00133     for(unsigned int i = 0 ; i < scan_.ranges.size() ; i++)
00134     {
00135 
00136     float real_dist = scan_.ranges[i];
00137       if(real_dist > msg_.maxRange)
00138       {
00139         real_dist = msg_.maxRange;
00140         painter.setPen(QColor(255,0,0,100));
00141       }
00142       else if(real_dist < msg_.minRange)
00143       {
00144         real_dist = msg_.minRange;
00145         painter.setPen(QColor(100,100,100,100));
00146       }
00147       else
00148       {
00149         painter.setPen(QColor(255,0,0,100));
00150       }
00151       
00152       mean += real_dist;
00153       painter.drawLine(
00154         internal_image_.width() / 2,
00155         internal_image_.height() / 2,
00156         internal_image_.width() / 2 + real_dist / msg_.maxRange * 
00157             cos(scan_.angle_min + ((float)i) * scan_.angle_increment) *
00158             internal_image_.width() / 2,
00159         internal_image_.height() / 2 + real_dist / msg_.maxRange *
00160             sin(scan_.angle_min + ((float)i) * scan_.angle_increment) *
00161             internal_image_.width() / 2
00162       );        
00163     }
00164     laserMean->setText(
00165       QString().setNum(mean/scan_.ranges.size()) + QString(" m"));
00166     laserImage->setPixmap(
00167       QPixmap().fromImage(internal_image_.mirrored(false,true)));
00168   }
00169 }


stdr_gui
Author(s): Manos Tsardoulias
autogenerated on Wed Sep 2 2015 03:36:44