co2.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/co2.h>
00023 
00024 namespace stdr_robot {
00025   
00034   CO2Sensor::CO2Sensor(
00035     const nav_msgs::OccupancyGrid& map,
00036     const stdr_msgs::CO2SensorMsg& msg, 
00037     const std::string& name,
00038     ros::NodeHandle& n)
00039     : Sensor(map, name, n, msg.pose, msg.frame_id, msg.frequency)
00040   {
00041     _description = msg;
00042 
00043     _publisher = n.advertise<stdr_msgs::CO2SensorMeasurementMsg>
00044       ( _namespace + "/" + msg.frame_id, 1 );
00045       
00046     co2_sources_subscriber_ = n.subscribe(
00047       "stdr_server/co2_sources_list", 
00048       1, 
00049       &CO2Sensor::receiveCO2Sources,
00050       this);
00051   }
00052   
00057   CO2Sensor::~CO2Sensor(void)
00058   {
00059     
00060   }
00061 
00066   void CO2Sensor::updateSensorCallback() 
00067   {
00068     if (co2_sources_.co2_sources.size() == 0) return;    
00069 
00070     stdr_msgs::CO2SensorMeasurementMsg measuredSourcesMsg;
00071 
00072     measuredSourcesMsg.header.frame_id = _description.frame_id;
00073 
00074     float max_range = _description.maxRange;
00076     for(unsigned int i = 0 ; i < co2_sources_.co2_sources.size() ; i++)
00077     {
00079       float sensor_x = _sensorTransform.getOrigin().x();
00080       float sensor_y = _sensorTransform.getOrigin().y();
00081       float dist = sqrt(
00082         pow(sensor_x - co2_sources_.co2_sources[i].pose.x, 2) +
00083         pow(sensor_y - co2_sources_.co2_sources[i].pose.y, 2)
00084       );
00085       if(dist > max_range)
00086       {
00087         continue;
00088       }
00089       if(dist > 0.5)
00090       {
00091         measuredSourcesMsg.co2_ppm += co2_sources_.co2_sources[i].ppm *
00092           pow(0.5, 2) / pow(dist, 2);
00093       }
00094       else
00095       {
00096         measuredSourcesMsg.co2_ppm += co2_sources_.co2_sources[i].ppm;
00097       }
00098     }
00099     
00100     measuredSourcesMsg.header.stamp = ros::Time::now();
00101     measuredSourcesMsg.header.frame_id = _namespace + "_" + _description.frame_id;
00102     _publisher.publish( measuredSourcesMsg );
00103   }
00104   
00108   void CO2Sensor::receiveCO2Sources(const stdr_msgs::CO2SourceVector& msg)
00109   {
00110     co2_sources_ = msg;
00111   }
00112 
00113 }  // namespace stdr_robot


stdr_robot
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Wed Sep 2 2015 03:36:25