Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
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 }