tf_aggregator.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #################################################################
00004 ##\file
00005 #
00006 # \note
00007 # Copyright (c) 2012 \n
00008 # Fraunhofer Institute for Manufacturing Engineering
00009 # and Automation (IPA) \n\n
00010 #
00011 #################################################################
00012 #
00013 # \note
00014 # Project name: Care-O-bot Research
00015 # \note
00016 # ROS package name: 
00017 #
00018 # \author
00019 # Author: Thiago de Freitas Oliveira Araujo, 
00020 # email:thiago.de.freitas.oliveira.araujo@ipa.fhg.de
00021 # \author
00022 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00023 #
00024 # \date Date of creation: December 2012
00025 #
00026 # \brief
00027 # This module aggregates tf messages
00028 #
00029 #################################################################
00030 #
00031 # Redistribution and use in source and binary forms, with or without
00032 # modification, are permitted provided that the following conditions are met:
00033 #
00034 # - Redistributions of source code must retain the above copyright
00035 # notice, this list of conditions and the following disclaimer. \n
00036 # - Redistributions in binary form must reproduce the above copyright
00037 # notice, this list of conditions and the following disclaimer in the
00038 # documentation and/or other materials provided with the distribution. \n
00039 # - Neither the name of the Fraunhofer Institute for Manufacturing
00040 # Engineering and Automation (IPA) nor the names of its
00041 # contributors may be used to endorse or promote products derived from
00042 # this software without specific prior written permission. \n
00043 #
00044 # This program is free software: you can redistribute it and/or modify
00045 # it under the terms of the GNU Lesser General Public License LGPL as
00046 # published by the Free Software Foundation, either version 3 of the
00047 # License, or (at your option) any later version.
00048 #
00049 # This program is distributed in the hope that it will be useful,
00050 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00051 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00052 # GNU Lesser General Public License LGPL for more details.
00053 #
00054 # You should have received a copy of the GNU Lesser General Public
00055 # License LGPL along with this program.
00056 # If not, see < http://www.gnu.org/licenses/>.
00057 #
00058 #################################################################
00059 
00060 import roslib
00061 roslib.load_manifest('srs_scenarios')
00062 import rospy
00063 
00064 import rosbag
00065 from std_msgs.msg import Int32, String
00066 
00067 import tf
00068 from tf.msg import *
00069 from tf.transformations import euler_from_quaternion
00070 
00071 from simple_script_server import *
00072 sss = simple_script_server()
00073 
00074 from kinematics_msgs.srv import *
00075 from std_msgs.msg import *
00076 from sensor_msgs.msg import *
00077 from move_base_msgs.msg import *
00078 
00079 import math
00080 import rostopic
00081 
00082 import os
00083 import sys, subprocess
00084 
00085 import itertools
00086 
00087 class tf_aggregator():
00088 
00089     def __init__(self):
00090         
00091         self.transforms = {}
00092         rospy.Subscriber('/tf', tfMessage, self.process_tfs)
00093         self.locked = False
00094     def lock(self):
00095         self.locked = True
00096         
00097     def unlock(self):
00098         self.locked = False 
00099     
00100     def process_tfs(self,msg):
00101         timeout = 1.0
00102         #msg =  rospy.wait_for_message("/tf", tfMessage, timeout)
00103         # iterates through the message items to update the transforms dictionary
00104         # with the specified reference and child frame
00105         if not self.locked:
00106                     for trans in msg.transforms:
00107                         frame_id = trans.header.frame_id
00108                         child_frame_id = trans.child_frame_id
00109                     
00110                         if(frame_id not in self.transforms): 
00111                             self.transforms[frame_id] = {}
00112                         if(child_frame_id not in self.transforms[frame_id]):
00113                             self.transforms[frame_id][child_frame_id] = True
00114         
00115 if __name__=="__main__":
00116 
00117     rospy.init_node('tf_test')
00118     tfa = tf_aggregator()
00119     
00120     while not rospy.is_shutdown():
00121         
00122         tfa.process_tfs()
00123         
00124         rospy.sleep(2)


srs_user_tests
Author(s): SRS team
autogenerated on Sun Jan 5 2014 12:14:05