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_robot/sensors/sonar.h>
00023 
00024 namespace stdr_robot {
00025 
00034   Sonar::Sonar(const nav_msgs::OccupancyGrid& map,
00035       const stdr_msgs::SonarSensorMsg& msg, 
00036       const std::string& name,
00037       ros::NodeHandle& n)
00038   : 
00039     Sensor(map, name, n, msg.pose, msg.frame_id, msg.frequency)
00040   {
00041     _description = msg;
00042 
00043     _publisher = n.advertise<sensor_msgs::Range>
00044       ( _namespace + "/" + msg.frame_id, 1 );
00045   }
00046   
00051   Sonar::~Sonar(void)
00052   {
00053     
00054   }
00055 
00060   void Sonar::updateSensorCallback() 
00061   {
00062     float angle;
00063     int distance;
00064     int xMap, yMap;
00065     sensor_msgs::Range sonarRangeMsg;
00066 
00067     sonarRangeMsg.max_range = _description.maxRange;
00068     sonarRangeMsg.min_range = _description.minRange;
00069     sonarRangeMsg.radiation_type = 0;
00070     sonarRangeMsg.field_of_view = _description.coneAngle;
00071 
00072     if ( _map.info.height == 0 || _map.info.width == 0 )
00073     {
00074       ROS_DEBUG("Outside limits\n");
00075       return;
00076     }
00077 
00078     sonarRangeMsg.range = _description.maxRange;
00079 
00080     float angleStep = 3.14159 / 180.0;
00081     float angleMin = - ( _description.coneAngle / 2.0 ); 
00082     float angleMax = _description.coneAngle / 2.0 ; 
00084     //float angleMin = -( _description.coneAngle * 180.0 / 3.14159 / 2.0 ); 
00085     //float angleMax = _description.coneAngle * 180.0 / 3.14159 / 2.0 ; 
00086 
00087     for ( float sonarIter = angleMin; sonarIter < angleMax; 
00088       sonarIter += angleStep )
00089     {
00090 
00091       distance = 1;
00092 
00093       while ( distance <= _description.maxRange / _map.info.resolution )
00094       {
00095         xMap = _sensorTransform.getOrigin().x() / _map.info.resolution + 
00096           cos( sonarIter + tf::getYaw(_sensorTransform.getRotation()) ) 
00097             * distance;
00098         yMap = _sensorTransform.getOrigin().y() / _map.info.resolution + 
00099           sin( sonarIter + tf::getYaw(_sensorTransform.getRotation()) ) 
00100             * distance;
00101         
00102         if (yMap * _map.info.width + xMap > _map.info.height*_map.info.width ||
00103             yMap * _map.info.width + xMap < 0)
00104         {
00105           distance = _description.maxRange / _map.info.resolution - 1;
00106           break;
00107         }
00108         
00110         if ( _map.data[ yMap * _map.info.width + xMap ] > 70 )
00111         {
00112           break;
00113         }
00114         distance ++;
00115       }
00116 
00117       if ( distance * _map.info.resolution < sonarRangeMsg.range )
00118       {
00119         sonarRangeMsg.range = distance * _map.info.resolution;
00120       }
00121     }
00122     
00123     if ( sonarRangeMsg.range < _description.minRange )
00124     {
00125       sonarRangeMsg.range = -std::numeric_limits<float>::infinity();
00126     }
00127     else if ( sonarRangeMsg.range >= _description.maxRange )
00128     {
00129       sonarRangeMsg.range = std::numeric_limits<float>::infinity();
00130     }
00131 
00132     sonarRangeMsg.header.stamp = ros::Time::now();
00133     sonarRangeMsg.header.frame_id = _namespace + "_" + _description.frame_id;
00134     _publisher.publish( sonarRangeMsg );
00135   }
00136 
00137 }  // namespace stdr_robot


stdr_robot
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Thu Jun 6 2019 18:57:28