stdr_gui_laser.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_gui_sensors/stdr_gui_laser.h"
00023 
00024 namespace stdr_gui
00025 {
00032   CGuiLaser::CGuiLaser(stdr_msgs::LaserSensorMsg msg,std::string baseTopic):
00033     msg_(msg)
00034   {
00035     topic_ = baseTopic + "/" + msg_.frame_id;
00036     tf_frame_ = baseTopic + "_" + msg_.frame_id;
00037     ros::NodeHandle n;
00038     lock_ = false;
00039     subscriber_ = n.subscribe(topic_.c_str(), 1, &CGuiLaser::callback,this);
00040     visualization_status_ = 0;
00041   }
00042   
00047   CGuiLaser::~CGuiLaser(void)
00048   { 
00049   }
00050   
00055   char CGuiLaser::getVisualizationStatus(void)
00056   {
00057     return visualization_status_;
00058   }
00059   
00064   void CGuiLaser::toggleVisualizationStatus(void)
00065   {
00066     visualization_status_ = (visualization_status_ + 1) % 3;
00067   }
00068   
00074   void CGuiLaser::callback(const sensor_msgs::LaserScan& msg)
00075   {
00076     if(lock_)
00077     {
00078       return;
00079     }
00080     scan_ = msg;
00081   }
00082   
00090   void CGuiLaser::paint(
00091     QImage *m,
00092     float ocgd,
00093     tf::TransformListener *listener)
00094   {
00095     lock_ = true;
00096     QPainter painter(m);
00097     //~ painter.setRenderHint(QPainter::Antialiasing, true);
00098     
00100     tf::StampedTransform transform;
00101       
00102     try
00103     {
00104       listener->waitForTransform("map_static",
00105                                   tf_frame_.c_str(),
00106                                   ros::Time(0),
00107                                   ros::Duration(0.2));
00108       listener->lookupTransform("map_static", 
00109         tf_frame_.c_str(), ros::Time(0), transform);
00110     }
00111     catch (tf::TransformException ex)
00112     {
00113       ROS_DEBUG("%s",ex.what());
00114     }
00115     tfScalar roll,pitch,yaw;
00116     float pose_x = transform.getOrigin().x();
00117     float pose_y = transform.getOrigin().y();
00118     transform.getBasis().getRPY(roll,pitch,yaw);
00119     float pose_theta = yaw;
00120     
00122     
00123     for(unsigned int i = 0 ; i < scan_.ranges.size() ; i++)
00124     {
00125       float real_dist = scan_.ranges[i];
00126       if(real_dist > msg_.maxRange)
00127       {
00128         real_dist = msg_.maxRange;
00129         painter.setPen(QColor(255,0,0,75 * (2 - visualization_status_)));
00130       }
00131       else if(real_dist < msg_.minRange)
00132       {
00133         real_dist = msg_.minRange;
00134         painter.setPen(QColor(100,100,100,
00135           75 * (2 - visualization_status_)));
00136       }
00137       else
00138       {
00139         painter.setPen(QColor(255,0,0,75 * (2 - visualization_status_)));
00140       }
00141       painter.drawLine(
00142         pose_x / ocgd,
00143         
00144         pose_y / ocgd,
00145           
00146         pose_x / ocgd + real_dist * 
00147           cos(pose_theta + scan_.angle_min + i * scan_.angle_increment)
00148            / ocgd,
00149           
00150         pose_y / ocgd + real_dist * 
00151           sin(pose_theta + scan_.angle_min + i * scan_.angle_increment)
00152            / ocgd
00153       );
00154     }
00155     lock_ = false;
00156   }
00157   
00165   void CGuiLaser::visualizerPaint(
00166     QImage *m,
00167     float ocgd,
00168     float maxRange)
00169   {
00170     lock_ = true;
00171     QPainter painter(m);
00172     float size = m->width();
00173     float climax = size / maxRange * ocgd / 2.1;
00174 
00175     for(unsigned int i = 0 ; i < scan_.ranges.size() ; i++)
00176     {
00177           float real_dist = scan_.ranges[i];
00178       if(real_dist > msg_.maxRange)
00179       {
00180         real_dist = msg_.maxRange;
00181         painter.setPen(QColor(255,0,0,100));
00182       }
00183       else if(real_dist < msg_.minRange)
00184       {
00185         real_dist = msg_.minRange;
00186         painter.setPen(QColor(100,100,100,100));
00187       }
00188       else
00189       {
00190         painter.setPen(QColor(255,0,0,100));
00191       }
00192 
00193       painter.drawLine(
00194         size / 2 + (msg_.pose.x / ocgd) * climax,
00195         size / 2 + (msg_.pose.y / ocgd) * climax,
00196 
00197         size / 2 + ((msg_.pose.x / ocgd) + real_dist *
00198           cos(scan_.angle_min + i * scan_.angle_increment + msg_.pose.theta)
00199           / ocgd) * climax,
00200         size / 2 + ((msg_.pose.y / ocgd) + real_dist * 
00201           sin(scan_.angle_min + i * scan_.angle_increment + msg_.pose.theta) 
00202           / ocgd) * climax
00203       );
00204     }
00205     lock_ = false;
00206   }
00207   
00212   float CGuiLaser::getMaxRange(void)
00213   {
00214     return msg_.maxRange;
00215   }
00216   
00221   std::string CGuiLaser::getFrameId(void)
00222   {
00223     return msg_.frame_id;
00224   }
00225   
00231   void CGuiLaser::setVisualizationStatus(char vs)
00232   {
00233     visualization_status_ = vs;
00234   }
00235 }
00236 


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