sensor_base.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/sensor_base.h>
00023 
00024 namespace stdr_robot {
00025 
00036   Sensor::Sensor(
00037       const nav_msgs::OccupancyGrid& map,
00038       const std::string& name,
00039       ros::NodeHandle& n,
00040       const geometry_msgs::Pose2D& sensorPose,
00041       const std::string& sensorFrameId,
00042       float updateFrequency)
00043       : 
00044         _map(map), 
00045         _namespace(name),
00046         _sensorPose(sensorPose),
00047         _sensorFrameId(sensorFrameId),
00048         _updateFrequency(updateFrequency),
00049         _gotTransform(false) 
00050   {
00051     _timer = n.createTimer(
00052       ros::Duration(1/updateFrequency), &Sensor::checkAndUpdateSensor, this);
00053     _tfTimer = n.createTimer(
00054       ros::Duration(1/(2*updateFrequency)), &Sensor::updateTransform, this);
00055   }
00056 
00057   
00063   void Sensor::checkAndUpdateSensor(const ros::TimerEvent& ev)
00064   {
00065     if (!_gotTransform) { 
00066       return;
00067     }
00068     
00069     updateSensorCallback();
00070   }
00071   
00076   void Sensor::updateTransform(const ros::TimerEvent&)
00077   {
00078     try {
00079       _tfListener.waitForTransform("map_static",
00080                                   _namespace + "_" + _sensorFrameId,
00081                                   ros::Time(0),
00082                                   ros::Duration(0.2));
00083       _tfListener.lookupTransform("map_static",
00084                                   _namespace + "_" + _sensorFrameId,
00085                                   ros::Time(0), _sensorTransform);
00086       _gotTransform = true;
00087     }
00088     catch (tf::TransformException ex) {
00089       ROS_DEBUG("%s",ex.what());
00090     }
00091   }
00092 }  // namespace stdr_robot


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