$search
00001 #!/usr/bin/env python 00002 00003 ################################################################# 00004 ##\file 00005 # 00006 # \note 00007 # Copyright (c) 2013 \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: January 2013 00025 # 00026 # \brief 00027 # This module records topics according to triggering events 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 PKG = "srs_user_tests" 00062 roslib.load_manifest(PKG) 00063 import rospy 00064 00065 import rosbag 00066 from std_msgs.msg import Int32, String 00067 00068 import tf 00069 from tf.msg import * 00070 from tf.transformations import euler_from_quaternion 00071 00072 from simple_script_server import * 00073 sss = simple_script_server() 00074 00075 from kinematics_msgs.srv import * 00076 from std_msgs.msg import * 00077 from sensor_msgs.msg import * 00078 from move_base_msgs.msg import * 00079 from visualization_msgs.msg import * 00080 00081 import math 00082 import rostopic 00083 import time 00084 00085 import os 00086 import sys, subprocess 00087 00088 import joint_states_aggregator 00089 import tf_aggregator 00090 00091 import global_lock 00092 00093 class record_topic(): 00094 00095 def __init__(self, topic_name, bagfile, continuous=False): 00096 # this gets the topics type using the roslib 00097 the_type = rostopic.get_topic_type(topic_name, blocking=True)[0] 00098 00099 the_type = the_type.split("/") 00100 the_type[0] += ".msg" 00101 00102 # this imports the necessary modules and instantiates the necessary object 00103 mod = __import__(the_type[0], fromlist=[the_type[1]]) 00104 cls = getattr( mod , the_type[1] ) 00105 00106 self.msg = None 00107 self.topic_name = topic_name 00108 self.topic_type = cls 00109 self.continuous = continuous 00110 self.bagfile = bagfile 00111 00112 # this creates the subscriber for the specific topic 00113 rospy.Subscriber(self.topic_name, self.topic_type, self.callback) 00114 global_lock.locked = False 00115 00116 def lock(self): 00117 global_lock.locked = True 00118 00119 def unlock(self): 00120 global_lock.locked = False 00121 00122 def callback(self, msg): 00123 self.msg = msg 00124 if self.continuous: 00125 self.record() 00126 00127 def record(self): 00128 if(self.msg!=None): 00129 if not global_lock.locked: 00130 try: 00131 if(global_lock.active_bag): 00132 self.lock() 00133 self.bagfile.write(self.topic_name, self.msg) 00134 self.unlock() 00135 except KeyError, e: 00136 rospy.loginfo(self.msg) 00137 00138 if __name__ == "__main__": 00139 00140 rospy.init_node('record_topic') 00141 topics_list = ["/tf", "/collision_velocity_filter/velocity_limited_marker", "/joint_states"] 00142 topics_c = [] 00143 00144 for top in topics_list: 00145 topic_r = record_topic(top) 00146 00147 topics_c.append(topic_r) 00148 00149 while not rospy.is_shutdown(): 00150 00151 for tops_c in topics_c: 00152 rospy.loginfo(tops_c.msg) 00153 rospy.sleep(2) 00154