topics_bag.py
Go to the documentation of this file.
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 bagfile 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 itertools
00086 
00087 import os
00088 import sys, subprocess
00089 
00090 import record_topic
00091 
00092 import global_lock
00093 
00094 class topics_bag():
00095 
00096     def __init__(self):
00097     
00098         # this defines the variables according to the ones specified at the yaml
00099         # file. The triggers, the wanted tfs, the wanted topics and where they are going
00100         # to be written, more specifically at the file named as self.bag.
00101         self.trigger_record_translation = rospy.get_param('~trigger_record_translation')
00102         self.trigger_record_rotation = rospy.get_param('~trigger_record_rotation')
00103         self.trigger_timestep = rospy.get_param('~trigger_timestep')
00104         self.wanted_tfs = rospy.get_param('~wanted_tfs')
00105         self.trigger_topics = rospy.get_param("~trigger_topics")
00106         self.continuous_topics = rospy.get_param("~continuous_topics")
00107         self.bag_path = rospy.get_param("~bag_path")
00108         
00109         # this creates the bagfile
00110         localtime = time.localtime(time.time())
00111         filename = str(localtime[0]) + "-" + str(localtime[1]) + "-" + str(localtime[2]) + "_" + str(localtime[3]) + "-" + str(localtime[4]) + "-" + str(localtime[5]) + ".bag"
00112         filelocation = str(roslib.packages.get_pkg_dir(PKG) + "/data/" + self.bag_path )
00113         if(not os.path.exists(filelocation)):
00114             os.makedirs(filelocation)
00115         rospy.loginfo("Logging to " + filelocation + filename)
00116         self.bag = rosbag.Bag(filelocation + filename, 'w')
00117         
00118         while(rospy.rostime.get_time() == 0.0):
00119                         time.sleep(0.1)
00120         # necessary tf elements 
00121         self.tfL = tf.TransformListener()
00122         self.tfposed = TransformStamped()
00123         self.tfMsg = tfMessage()
00124         
00125         rospy.sleep(2)
00126         # waits for a tf transform before starting. This is important to check if
00127         # the system is fully functional.
00128         
00129         self.tfL.waitForTransform(self.wanted_tfs[0]["reference_frame"], self.wanted_tfs[0]["target_frame"], rospy.Time(0), rospy.Duration(4.0))
00130         
00131         # dictionaries for storing current translation and rotation for the specific
00132         # frames
00133         self.current_translation = {}
00134         self.current_rotation = {}
00135         
00136         for frame in self.wanted_tfs:
00137             self.current_translation[frame["target_frame"]] = [0,0,0]
00138             self.current_rotation[frame["target_frame"]] = [0,0,0,0]
00139             
00140 
00141         rospy.Service('~start', Trigger, self.trigger_callback_start)
00142         rospy.Service('~stop', Trigger, self.trigger_callback_stop)
00143 
00144     def active(self):
00145     
00146         return global_lock.active_bag           
00147         
00148     def trigger_callback_start(self, req):
00149         res = TriggerResponse()
00150         global_lock.active_bag = True
00151         res.success.data = True
00152         res.error_message.data = "Bagfile recording started"
00153         print res.error_message.data
00154         return res
00155                   
00156     def trigger_callback_stop(self, req):
00157         res = TriggerResponse()
00158         global_lock.active_bag = False
00159         res.success.data = True
00160         res.error_message.data = "Bagfile recording stopped"
00161         print res.error_message.data
00162         return res  
00163                   
00164     def tf_trigger(self, reference_frame, target_frame, tfs):
00165         #  this function is responsible for setting up the triggers for recording
00166         # on the bagfile.
00167         
00168         # sequence for calculating distance and yaw rotation for defining if a 
00169         # recording trigger is set according to the trigger value on the yaml file
00170         
00171         self.tfL.waitForTransform(reference_frame, target_frame, rospy.Time(0), rospy.Duration(3.0))
00172         trans, rot = self.tfL.lookupTransform(reference_frame, target_frame, rospy.Time(0))
00173         
00174         x = trans[0] - self.current_translation[target_frame][0]
00175         y = trans[1] - self.current_translation[target_frame][1]
00176         
00177         distance_trans = math.sqrt(x*x + y*y)
00178         distance_rot = abs(euler_from_quaternion(rot)[2] - euler_from_quaternion(self.current_rotation[target_frame])[2])
00179         
00180         if("trigger_record_translation" in tfs and distance_trans >= tfs["trigger_record_translation"]):
00181             rospy.loginfo("triggered for translation, trans = " + str(distance_trans))
00182             self.current_translation[target_frame] = trans
00183             self.current_rotation[target_frame] = rot
00184             return "triggered"
00185             
00186         if("trigger_record_rotation" in tfs and distance_rot >= tfs["trigger_record_rotation"]):
00187             rospy.loginfo("triggered for rotation, rot = " + str(distance_rot))
00188             self.current_translation[target_frame] = trans
00189             self.current_rotation[target_frame] = rot
00190             return "triggered"
00191         
00192         return "not_triggered"
00193         
00194     def bag_processor(self, tfs=None):
00195         
00196         trigger_position = self.tf_trigger(tfs["reference_frame"], tfs["target_frame"], tfs)
00197         
00198         return trigger_position
00199         
00200 if __name__ == "__main__":
00201     rospy.init_node('topics_bag')
00202     bagR = topics_bag()
00203     rospy.sleep(2)
00204     time_step = rospy.Duration.from_sec(bagR.trigger_timestep)
00205     start_time = rospy.Time.now()
00206     with bagR.bag as bagfile:
00207         rate = rospy.Rate(10) #Hz
00208         topics_t = []
00209         topics_c = []   
00210         for tfs in bagR.trigger_topics:
00211             topic_r = record_topic.record_topic(tfs, bagfile)
00212             topics_t.append(topic_r)
00213         for tfc in bagR.continuous_topics:
00214             topic_r = record_topic.record_topic(tfc, bagfile, continuous=True)
00215             topics_c.append(topic_r)
00216         rospy.sleep(2)
00217         while not rospy.is_shutdown():
00218                         
00219             if bagR.active()==1:
00220                 # listen to tf changes
00221                 for tfs in bagR.wanted_tfs:
00222                     triggers = bagR.bag_processor(tfs)
00223                     if(triggers == "triggered"):
00224                         rospy.loginfo("triggered by tf")
00225                         start_time = rospy.Time.now()
00226                         #Records the triggered topics
00227                         for tops_c, tfm in itertools.izip(topics_t, bagR.trigger_topics):
00228                             tops_c.record()
00229                     else:
00230                         rospy.logdebug("not triggered")
00231                 # listen to ellapsed time
00232                 time_msg = "time passed:" + (str)((rospy.Time.now() - start_time).to_sec())
00233                 rospy.logdebug(time_msg)
00234                 
00235                 if(rospy.Time.now() - start_time > time_step):
00236                     rospy.loginfo("triggered by time")
00237                     start_time = rospy.Time.now()
00238                     for tops_c, tfm in itertools.izip(topics_t, bagR.trigger_topics):
00239                         tops_c.record()
00240                 # sleep until next check
00241 
00242             rate.sleep()
00243                         
00244         # closing bag file
00245     rospy.loginfo("Closing bag file")
00246     bagR.bag.close()


srs_user_tests
Author(s): SRS team
autogenerated on Mon Oct 6 2014 08:53:11