stdr_gui_sound.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_sound.h"
00023 
00024 namespace stdr_gui
00025 {
00029   CGuiSound::CGuiSound(stdr_msgs::SoundSensorMsg msg,std::string baseTopic):
00030     msg_(msg)
00031   {
00032     topic_ = baseTopic + "/" + msg_.frame_id;
00033     tf_frame_ = baseTopic + "_" + msg_.frame_id;
00034     ros::NodeHandle n;
00035     lock_ = false;
00036     subscriber_ = n.subscribe(topic_.c_str(), 1, &CGuiSound::callback,this);
00037     visualization_status_ = 0;
00038   }
00039   
00043   void CGuiSound::callback(const stdr_msgs::SoundSensorMeasurementMsg& msg)
00044   {
00045     if(lock_)
00046     {
00047       return;
00048     }
00049     sound_sources_ = msg;
00050   }
00051   
00055   void CGuiSound::paint(
00056     QImage *m,
00057     float ocgd,
00058     tf::TransformListener *listener)
00059   {
00060     lock_ = true;
00061     QPainter painter(m);
00062     
00064     tf::StampedTransform transform;
00065       
00066     try
00067     {
00068       listener->waitForTransform("map_static",
00069                                   tf_frame_.c_str(),
00070                                   ros::Time(0),
00071                                   ros::Duration(0.2));
00072       listener->lookupTransform("map_static", 
00073         tf_frame_.c_str(), ros::Time(0), transform);
00074     }
00075     catch (tf::TransformException ex)
00076     {
00077       ROS_DEBUG("%s",ex.what());
00078     }
00079     tfScalar roll,pitch,yaw;
00080     float pose_x = transform.getOrigin().x();
00081     float pose_y = transform.getOrigin().y();
00082     transform.getBasis().getRPY(roll,pitch,yaw);
00083     float pose_theta = yaw;
00084     
00086     QBrush brush(QColor(50,100,50,75 * (2 - visualization_status_)));
00087     painter.setBrush(brush);
00088     
00089     
00090     // What to draw?
00091     
00092     //~ for(unsigned int j = 0 ; j < sound_sources_.sound_source_ids.size() ; j++)
00093     //~ {
00094       //~ for(unsigned int i = 0 ; i < env_sound_sources_.sound_sources.size() ; i++)
00095       //~ {
00096         //~ if(sound_sources_.sound_sources_ids[j] == env_sound_sources_.sound_sources[i].id)
00097         //~ {
00098           //~ int x1 = pose_x / ocgd;
00099           //~ int y1 = pose_y / ocgd;
00100           //~ int x2 = env_sound_sources_.sound_sources[i].pose.x / ocgd;
00101           //~ int y2 = env_sound_sources_.sound_sources[i].pose.y / ocgd;
00102           //~ painter.drawLine(x1, y1, x2, y2);
00103           //~ break;
00104         //~ }
00105       //~ }
00106     //~ }
00107     
00108     QBrush brush_cone(QColor(50,100,50, 20 * (2 - visualization_status_)));
00109     painter.setBrush(brush_cone);
00110     QPen pen(QColor(0,0,0,0));
00111     painter.setPen(pen);
00112 
00113     painter.drawPie(
00114       pose_x / ocgd - msg_.maxRange / ocgd,
00115         
00116       pose_y / ocgd - msg_.maxRange / ocgd,
00117       
00118       msg_.maxRange / ocgd * 2,
00119       msg_.maxRange / ocgd * 2,
00120       
00121       - (pose_theta - msg_.angleSpan / 2.0)
00122         * 180.0 / STDR_PI * 16,
00123         
00124       - msg_.angleSpan * 180.0 / STDR_PI * 16);
00125 
00126     lock_ = false;
00127   }
00128   
00132   CGuiSound::~CGuiSound(void)
00133   {
00134 
00135   }
00136   
00140   char CGuiSound::getVisualizationStatus(void)
00141   {
00142     return visualization_status_;
00143   }
00144   
00148   void CGuiSound::toggleVisualizationStatus(void)
00149   {
00150     visualization_status_ = (visualization_status_ + 1) % 3;
00151   }
00152   
00156   void CGuiSound::setVisualizationStatus(char vs)
00157   {
00158     visualization_status_ = vs;
00159   }
00160   
00165   std::string CGuiSound::getFrameId(void)
00166   {
00167     return msg_.frame_id;
00168   }
00169   
00175   void CGuiSound::setEnvironmentalSoundSources(
00176     stdr_msgs::SoundSourceVector env_sound_sources)
00177   {
00178     while(lock_)
00179     {
00180       usleep(100);
00181     }
00182     lock_ = true;
00183     env_sound_sources_ = env_sound_sources;
00184     lock_ = false;
00185   }
00186 }


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