thermal.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/thermal.h>
00023 
00024 namespace stdr_robot {
00025   
00029   ThermalSensor::ThermalSensor(
00030     const nav_msgs::OccupancyGrid& map,
00031     const stdr_msgs::ThermalSensorMsg& msg, 
00032     const std::string& name,
00033     ros::NodeHandle& n)
00034     : Sensor(map, name, n, msg.pose, msg.frame_id, msg.frequency)
00035   {
00036     _description = msg;
00037 
00038     _publisher = n.advertise<stdr_msgs::ThermalSensorMeasurementMsg>
00039       ( _namespace + "/" + msg.frame_id, 1 );
00040       
00041     thermal_sources_subscriber_ = n.subscribe(
00042       "stdr_server/thermal_sources_list", 
00043       1, 
00044       &ThermalSensor::receiveThermalSources,
00045       this);
00046   }
00047   
00052   ThermalSensor::~ThermalSensor(void)
00053   {
00054     
00055   }
00056 
00061   void ThermalSensor::updateSensorCallback() 
00062   {
00063     if (thermal_sources_.thermal_sources.size() == 0) return;    
00064 
00065     stdr_msgs::ThermalSensorMeasurementMsg measuredSourcesMsg;
00066 
00067     measuredSourcesMsg.header.frame_id = _description.frame_id;
00068 
00069     
00070     float max_range = _description.maxRange;
00071     float sensor_th = tf::getYaw(_sensorTransform.getRotation());
00072     float min_angle = sensor_th - _description.angleSpan / 2.0;
00073     float max_angle = sensor_th + _description.angleSpan / 2.0;
00074     
00075     measuredSourcesMsg.thermal_source_degrees.push_back(0);
00077     for(unsigned int i = 0 ; i < thermal_sources_.thermal_sources.size() ; i++)
00078     {
00080       float sensor_x = _sensorTransform.getOrigin().x();
00081       float sensor_y = _sensorTransform.getOrigin().y();
00082       float dist = sqrt(
00083         pow(sensor_x - thermal_sources_.thermal_sources[i].pose.x, 2) +
00084         pow(sensor_y - thermal_sources_.thermal_sources[i].pose.y, 2)
00085       );
00086       if(dist > max_range)
00087       {
00088         continue;
00089       }
00090       
00092       float ang = atan2( 
00093         thermal_sources_.thermal_sources[i].pose.y - sensor_y,
00094         thermal_sources_.thermal_sources[i].pose.x - sensor_x);
00095       
00096       if(!stdr_robot::angCheck(ang, min_angle, max_angle))
00097       {
00098         continue;
00099       }
00100       
00101       // Returns the larger temperature found in its range
00102       if( thermal_sources_.thermal_sources[i].degrees >
00103         measuredSourcesMsg.thermal_source_degrees[0])
00104       {
00105         measuredSourcesMsg.thermal_source_degrees[0] = 
00106           thermal_sources_.thermal_sources[i].degrees;
00107       }
00108     }
00109     
00110     measuredSourcesMsg.header.stamp = ros::Time::now();
00111     measuredSourcesMsg.header.frame_id = _namespace + "_" + _description.frame_id;
00112     _publisher.publish( measuredSourcesMsg );
00113   }
00114   
00118   void ThermalSensor::receiveThermalSources(
00119     const stdr_msgs::ThermalSourceVector& msg)
00120   {
00121     thermal_sources_ = msg;
00122   }
00123 
00124 }  // namespace stdr_robot


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