stdr_gui_sonar.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_sonar.h"
00023 
00024 namespace stdr_gui{
00025   
00032   CGuiSonar::CGuiSonar(stdr_msgs::SonarSensorMsg 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, &CGuiSonar::callback,this);
00040     visualization_status_ = 0;
00041   }
00042   
00047   CGuiSonar::~CGuiSonar(void)
00048   {
00049 
00050   }
00051   
00057   void CGuiSonar::callback(const sensor_msgs::Range& msg)
00058   {
00059     if(lock_)
00060     {
00061       return;
00062     }
00063     range_ = msg;
00064   }
00065   
00073   void CGuiSonar::paint(
00074     QImage *m,
00075     float ocgd,
00076     tf::TransformListener *listener)
00077   {
00078     lock_ = true;
00079     QPainter painter(m);
00080     
00082     tf::StampedTransform transform;
00083       
00084     try
00085     {
00086       listener->waitForTransform("map_static",
00087                                   tf_frame_.c_str(),
00088                                   ros::Time(0),
00089                                   ros::Duration(0.2));
00090       listener->lookupTransform("map_static", 
00091         tf_frame_.c_str(), ros::Time(0), transform);
00092     }
00093     catch (tf::TransformException ex)
00094     {
00095       ROS_DEBUG("%s",ex.what());
00096     }
00097     tfScalar roll,pitch,yaw;
00098     float pose_x = transform.getOrigin().x();
00099     float pose_y = transform.getOrigin().y();
00100     transform.getBasis().getRPY(roll,pitch,yaw);
00101     float pose_theta = yaw;
00102     
00104     
00105     float real_dist = range_.range;
00106     if(real_dist > msg_.maxRange)
00107     {
00108       real_dist = msg_.maxRange;
00109       QBrush brush(QColor(100,100,100,75 * (2 - visualization_status_)));
00110       painter.setBrush(brush);
00111       QPen pen(QColor(0,0,0,0));
00112       painter.setPen(pen);
00113     }
00114     else if(real_dist < msg_.minRange)
00115     {
00116       real_dist = msg_.minRange;
00117       QBrush brush(QColor(100,100,100,75 * (2 - visualization_status_)));
00118       painter.setBrush(brush);
00119       QPen pen(QColor(0,0,0,0));
00120       painter.setPen(pen);
00121     }
00122     else
00123     {
00124       QBrush brush(QColor(0,200,0,75 * (2 - visualization_status_)));
00125       painter.setBrush(brush);
00126       QPen pen(QColor(0,0,0,0));
00127       painter.setPen(pen);
00128     }
00129     
00130     painter.drawPie(
00131       pose_x / ocgd - real_dist / ocgd,
00132         
00133       pose_y / ocgd - real_dist / ocgd,
00134       
00135       real_dist / ocgd * 2,
00136       real_dist / ocgd * 2,
00137       
00138       - (pose_theta - msg_.coneAngle / 2.0)
00139         * 180.0 / STDR_PI * 16,
00140         
00141       - msg_.coneAngle * 180.0 / STDR_PI * 16);
00142 
00143     lock_ = false;
00144   }
00145   
00153   void CGuiSonar::visualizerPaint(
00154     QImage *m,
00155     float ocgd,
00156     float maxRange)
00157   {
00158     float size = m->width();
00159     float climax = size / maxRange * ocgd / 2.1;
00160     lock_ = true;
00161     QPainter painter(m);
00162     
00163     float real_dist = range_.range;
00164     if(real_dist > msg_.maxRange)
00165     {
00166       real_dist = msg_.maxRange;
00167       QBrush brush(QColor(100,100,100,100));
00168       painter.setBrush(brush);
00169     }
00170     else if(real_dist < msg_.minRange)
00171     {
00172       real_dist = msg_.minRange;
00173       QBrush brush(QColor(100,100,100,100));
00174       painter.setBrush(brush);
00175     }
00176     else
00177     {
00178       QBrush brush(QColor(0,200,0,100));
00179       painter.setBrush(brush);
00180     }
00181     
00182     painter.drawPie(
00183       size / 2 + (msg_.pose.x / ocgd - real_dist / ocgd) * climax,
00184       size / 2 + (msg_.pose.y / ocgd - real_dist / ocgd) * climax,
00185       
00186       real_dist / ocgd * 2 * climax,
00187       real_dist / ocgd * 2 * climax,
00188       
00189       -(msg_.pose.theta - msg_.coneAngle / 2.0) * 180.0 / STDR_PI * 16,
00190       
00191       -(msg_.coneAngle * 180.0 / STDR_PI) * 16);
00192 
00193     lock_ = false;
00194   }
00195   
00200   float CGuiSonar::getMaxRange(void)
00201   {
00202     return msg_.maxRange;
00203   }
00204   
00209   char CGuiSonar::getVisualizationStatus(void)
00210   {
00211     return visualization_status_;
00212   }
00213   
00218   void CGuiSonar::toggleVisualizationStatus(void)
00219   {
00220     visualization_status_ = (visualization_status_ + 1) % 3;
00221   }
00222   
00227   std::string CGuiSonar::getFrameId(void)
00228   {
00229     return msg_.frame_id;
00230   }
00231   
00237   void CGuiSonar::setVisualizationStatus(char vs)
00238   {
00239     visualization_status_ = vs;
00240   }
00241 }
00242 


stdr_gui
Author(s): Manos Tsardoulias
autogenerated on Tue Feb 7 2017 03:46:43