stdr_gui: stdr_tools.cpp Source File

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_tools.h"
00023 
00024 namespace stdr_gui_tools
00025 {
00031   std::string getRosPackagePath(std::string package)
00032   {
00033     return ros::package::getPath(package.c_str());
00034   }
00035   
00041   float angleRadToDegrees(float angle)
00042   {
00043     return angle * 180.0 / STDR_PI;
00044   }
00045   
00051   float angleDegreesToRad(float angle)
00052   {
00053     return angle / 180.0 * STDR_PI;
00054   }
00055   
00061   QString getLiteralTime(int ms)
00062   {
00063     QString str;
00064     int h = ms / (1000 * 60 * 60);
00065     int m = ms / (1000 * 60) - h * 60;
00066     int s = ms / 1000 - h * 60 * 60 - m * 60;
00067     int ms_ = ms - h * 60 * 60 * 1000 - m * 1000 * 60 - s * 1000;
00068     if(h)
00069     {
00070       str += QString().setNum(h) + QString(" h ");
00071     }
00072     if(m || h)
00073     {
00074       str += QString().setNum(m) + QString(" min ");
00075     }
00076     if(s || h || m)
00077     {
00078       str += QString().setNum(s) + QString(" sec ");
00079     }
00080     if(ms_ || s || h || m)
00081     {
00082       str += QString().setNum(ms_) + QString(" ms");
00083     }
00084     return str;
00085   }
00086   
00092   void printSonarMsg(stdr_msgs::SonarSensorMsg &msg)
00093   {
00094     ROS_ERROR("Sonar sensor msg :");
00095     ROS_ERROR("\tMax range : %f",msg.maxRange);
00096     ROS_ERROR("\tMin range : %f",msg.minRange);
00097     ROS_ERROR("\tCone angle : %f",msg.coneAngle);
00098     ROS_ERROR("\tFrequency : %f",msg.frequency);
00099     ROS_ERROR("\tNoise :");
00100     ROS_ERROR("\t\tMean : %f",msg.noise.noiseMean);
00101     ROS_ERROR("\t\tStd : %f",msg.noise.noiseStd);
00102     ROS_ERROR("\tFrame id : %s",msg.frame_id.c_str());
00103     ROS_ERROR("\tRelative pose :");
00104     ROS_ERROR("\t\tx : %f",msg.pose.x);
00105     ROS_ERROR("\t\ty : %f",msg.pose.y);
00106     ROS_ERROR("\t\ttheta : %f",msg.pose.theta);
00107   }
00108   
00114   void printLaserMsg(stdr_msgs::LaserSensorMsg &msg)
00115   {
00116     ROS_ERROR("Laser sensor msg :");
00117     ROS_ERROR("\tMax range : %f",msg.maxRange);
00118     ROS_ERROR("\tMin range : %f",msg.minRange);
00119     ROS_ERROR("\tMax angle : %f",msg.maxAngle);
00120     ROS_ERROR("\tMin angle : %f",msg.minAngle);
00121     ROS_ERROR("\tFrequency : %f",msg.frequency);
00122     ROS_ERROR("\tNoise :");
00123     ROS_ERROR("\t\tMean : %f",msg.noise.noiseMean);
00124     ROS_ERROR("\t\tStd : %f",msg.noise.noiseStd);
00125     ROS_ERROR("\tFrame id : %s",msg.frame_id.c_str());
00126     ROS_ERROR("\tRelative pose :");
00127     ROS_ERROR("\t\tx : %f",msg.pose.x);
00128     ROS_ERROR("\t\ty : %f",msg.pose.y);
00129     ROS_ERROR("\t\ttheta : %f",msg.pose.theta);
00130   }
00131   
00137   void printPose2D(geometry_msgs::Pose2D &msg)
00138   {
00139     ROS_ERROR("Pose 2D :");
00140     ROS_ERROR("\tx : %f",msg.x);
00141     ROS_ERROR("\ty : %f",msg.y);
00142     ROS_ERROR("\ttheta : %f",msg.theta);
00143   }
00144   
00150   stdr_msgs::RobotMsg fixRobotAnglesToRad(stdr_msgs::RobotMsg rmsg)
00151   {
00152     rmsg.initialPose.theta = 
00153       rmsg.initialPose.theta / 180.0 * STDR_PI;
00154     for(unsigned int i = 0 ; i < rmsg.laserSensors.size() ; i++)
00155     {
00156       rmsg.laserSensors[i].maxAngle = 
00157         rmsg.laserSensors[i].maxAngle / 180.0 * STDR_PI;
00158       rmsg.laserSensors[i].minAngle = 
00159         rmsg.laserSensors[i].minAngle / 180.0 * STDR_PI;
00160       rmsg.laserSensors[i].pose.theta = 
00161         rmsg.laserSensors[i].pose.theta / 180.0 * STDR_PI;
00162     }
00163     for(unsigned int i = 0 ; i < rmsg.sonarSensors.size() ; i++)
00164     {
00165       rmsg.sonarSensors[i].coneAngle = 
00166         rmsg.sonarSensors[i].coneAngle / 180.0 * STDR_PI;
00167       rmsg.sonarSensors[i].pose.theta = 
00168         rmsg.sonarSensors[i].pose.theta / 180.0 * STDR_PI;
00169     }
00170     for(unsigned int i = 0 ; i < rmsg.rfidSensors.size() ; i++)
00171     {
00172       rmsg.rfidSensors[i].angleSpan = 
00173         rmsg.rfidSensors[i].angleSpan / 180.0 * STDR_PI;
00174       rmsg.rfidSensors[i].pose.theta = 
00175         rmsg.rfidSensors[i].pose.theta / 180.0 * STDR_PI;
00176     }
00177     for(unsigned int i = 0 ; i < rmsg.co2Sensors.size() ; i++)
00178     {
00179       //~ rmsg.co2Sensors[i].angleSpan = 
00180         //~ rmsg.co2Sensors[i].angleSpan / 180.0 * STDR_PI;
00181       rmsg.co2Sensors[i].pose.theta = 
00182         rmsg.co2Sensors[i].pose.theta / 180.0 * STDR_PI;
00183     }
00184     for(unsigned int i = 0 ; i < rmsg.thermalSensors.size() ; i++)
00185     {
00186       rmsg.thermalSensors[i].angleSpan = 
00187         rmsg.thermalSensors[i].angleSpan / 180.0 * STDR_PI;
00188       rmsg.thermalSensors[i].pose.theta = 
00189         rmsg.thermalSensors[i].pose.theta / 180.0 * STDR_PI;
00190     }
00191     for(unsigned int i = 0 ; i < rmsg.soundSensors.size() ; i++)
00192     {
00193       rmsg.soundSensors[i].angleSpan = 
00194         rmsg.soundSensors[i].angleSpan / 180.0 * STDR_PI;
00195       rmsg.soundSensors[i].pose.theta = 
00196         rmsg.soundSensors[i].pose.theta / 180.0 * STDR_PI;
00197     }
00198     return rmsg;
00199   }
00200   
00206   stdr_msgs::RobotMsg fixRobotAnglesToDegrees(stdr_msgs::RobotMsg rmsg)
00207   {
00208     rmsg.initialPose.theta = 
00209       rmsg.initialPose.theta * 180.0 / STDR_PI;
00210     for(unsigned int i = 0 ; i < rmsg.laserSensors.size() ; i++)
00211     {
00212       rmsg.laserSensors[i].maxAngle = 
00213         rmsg.laserSensors[i].maxAngle * 180.0 / STDR_PI;
00214       rmsg.laserSensors[i].minAngle = 
00215         rmsg.laserSensors[i].minAngle * 180.0 / STDR_PI;
00216       rmsg.laserSensors[i].pose.theta = 
00217         rmsg.laserSensors[i].pose.theta * 180.0 / STDR_PI;
00218     }
00219     for(unsigned int i = 0 ; i < rmsg.sonarSensors.size() ; i++)
00220     {
00221       rmsg.sonarSensors[i].coneAngle = 
00222         rmsg.sonarSensors[i].coneAngle * 180.0 / STDR_PI;
00223       rmsg.sonarSensors[i].pose.theta = 
00224         rmsg.sonarSensors[i].pose.theta * 180.0 / STDR_PI;
00225     }
00226     for(unsigned int i = 0 ; i < rmsg.rfidSensors.size() ; i++)
00227     {
00228       rmsg.rfidSensors[i].angleSpan = 
00229         rmsg.rfidSensors[i].angleSpan * 180.0 / STDR_PI;
00230       rmsg.rfidSensors[i].pose.theta = 
00231         rmsg.rfidSensors[i].pose.theta * 180.0 / STDR_PI;
00232     }
00233     for(unsigned int i = 0 ; i < rmsg.co2Sensors.size() ; i++)
00234     {
00235       //~ rmsg.rfidSensors[i].angleSpan = 
00236         //~ rmsg.rfidSensors[i].angleSpan * 180.0 / STDR_PI;
00237       rmsg.co2Sensors[i].pose.theta = 
00238         rmsg.co2Sensors[i].pose.theta * 180.0 / STDR_PI;
00239     }
00240     for(unsigned int i = 0 ; i < rmsg.thermalSensors.size() ; i++)
00241     {
00242       rmsg.thermalSensors[i].angleSpan = 
00243         rmsg.thermalSensors[i].angleSpan * 180.0 / STDR_PI;
00244       rmsg.thermalSensors[i].pose.theta = 
00245         rmsg.thermalSensors[i].pose.theta * 180.0 / STDR_PI;
00246     }
00247     for(unsigned int i = 0 ; i < rmsg.soundSensors.size() ; i++)
00248     {
00249       rmsg.soundSensors[i].angleSpan = 
00250         rmsg.soundSensors[i].angleSpan * 180.0 / STDR_PI;
00251       rmsg.soundSensors[i].pose.theta = 
00252         rmsg.soundSensors[i].pose.theta * 180.0 / STDR_PI;
00253     }
00254     return rmsg;
00255   }
00256   
00262   stdr_msgs::LaserSensorMsg fixLaserAnglesToRad(stdr_msgs::LaserSensorMsg rmsg)
00263   {
00264     rmsg.maxAngle = rmsg.maxAngle / 180.0 * STDR_PI;
00265     rmsg.minAngle = rmsg.minAngle / 180.0 * STDR_PI;
00266     rmsg.pose.theta = rmsg.pose.theta / 180.0 * STDR_PI;
00267     return rmsg;
00268   }
00269   
00275   stdr_msgs::LaserSensorMsg fixLaserAnglesToDegrees(stdr_msgs::LaserSensorMsg rmsg)
00276   {
00277     rmsg.maxAngle = rmsg.maxAngle * 180.0 / STDR_PI;
00278     rmsg.minAngle = rmsg.minAngle * 180.0 / STDR_PI;
00279     rmsg.pose.theta = rmsg.pose.theta * 180.0 / STDR_PI;
00280     return rmsg;
00281   }
00282   
00288   stdr_msgs::SonarSensorMsg fixSonarAnglesToRad(stdr_msgs::SonarSensorMsg rmsg)
00289   {
00290     rmsg.coneAngle = rmsg.coneAngle / 180.0 * STDR_PI;
00291     rmsg.pose.theta = rmsg.pose.theta / 180.0 * STDR_PI;
00292     return rmsg;
00293   }
00294   
00300   stdr_msgs::SonarSensorMsg fixSonarAnglesToDegrees(
00301     stdr_msgs::SonarSensorMsg rmsg)
00302   {
00303     rmsg.coneAngle = rmsg.coneAngle * 180.0 / STDR_PI;
00304     rmsg.pose.theta = rmsg.pose.theta * 180.0 / STDR_PI;
00305     return rmsg;
00306   }
00307   
00313   stdr_msgs::RfidSensorMsg fixRfidAnglesToRad(stdr_msgs::RfidSensorMsg rmsg)
00314   {
00315     rmsg.angleSpan = rmsg.angleSpan / 180.0 * STDR_PI;
00316     rmsg.pose.theta = rmsg.pose.theta / 180.0 * STDR_PI;
00317     return rmsg;
00318   }
00319   
00325   stdr_msgs::RfidSensorMsg fixRfidAnglesToDegrees(
00326     stdr_msgs::RfidSensorMsg rmsg)
00327   {
00328     rmsg.angleSpan = rmsg.angleSpan * 180.0 / STDR_PI;
00329     rmsg.pose.theta = rmsg.pose.theta * 180.0 / STDR_PI;
00330     return rmsg;
00331   }
00332   
00336   stdr_msgs::CO2SensorMsg fixCO2AnglesToRad(stdr_msgs::CO2SensorMsg rmsg)
00337   {
00338     //~ rmsg.angleSpan = rmsg.angleSpan / 180.0 * STDR_PI;
00339     rmsg.pose.theta = rmsg.pose.theta / 180.0 * STDR_PI;
00340     return rmsg;
00341   }
00342   
00346   stdr_msgs::CO2SensorMsg fixCO2AnglesToDegrees(
00347     stdr_msgs::CO2SensorMsg rmsg)
00348   {
00349     //~ rmsg.angleSpan = rmsg.angleSpan * 180.0 / STDR_PI;
00350     rmsg.pose.theta = rmsg.pose.theta * 180.0 / STDR_PI;
00351     return rmsg;
00352   }
00353   
00357   stdr_msgs::ThermalSensorMsg fixThermalAnglesToRad(stdr_msgs::ThermalSensorMsg rmsg)
00358   {
00359     rmsg.angleSpan = rmsg.angleSpan / 180.0 * STDR_PI;
00360     rmsg.pose.theta = rmsg.pose.theta / 180.0 * STDR_PI;
00361     return rmsg;
00362   }
00363   
00367   stdr_msgs::ThermalSensorMsg fixThermalAnglesToDegrees(
00368     stdr_msgs::ThermalSensorMsg rmsg)
00369   {
00370     rmsg.angleSpan = rmsg.angleSpan * 180.0 / STDR_PI;
00371     rmsg.pose.theta = rmsg.pose.theta * 180.0 / STDR_PI;
00372     return rmsg;
00373   }
00374   
00378   stdr_msgs::SoundSensorMsg fixSoundAnglesToRad(stdr_msgs::SoundSensorMsg rmsg)
00379   {
00380     rmsg.angleSpan = rmsg.angleSpan / 180.0 * STDR_PI;
00381     rmsg.pose.theta = rmsg.pose.theta / 180.0 * STDR_PI;
00382     return rmsg;
00383   }
00384   
00388   stdr_msgs::SoundSensorMsg fixSoundAnglesToDegrees(
00389     stdr_msgs::SoundSensorMsg rmsg)
00390   {
00391     rmsg.angleSpan = rmsg.angleSpan * 180.0 / STDR_PI;
00392     rmsg.pose.theta = rmsg.pose.theta * 180.0 / STDR_PI;
00393     return rmsg;
00394   }
00395 }