$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 is part of the SRS training package 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 import roslib; roslib.load_manifest('srs_training') 00060 import rospy 00061 from geometry_msgs.msg import Twist 00062 import tf 00063 from math import * 00064 00065 00066 class move_box(): 00067 00068 def __init__(self): 00069 self.tf_broadcaster = tf.TransformBroadcaster() 00070 self.tf_listener = tf.TransformListener() 00071 self.reference_frame = "/base_link" 00072 self.target_frame = "/map" 00073 rospy.Subscriber("/base_controller/command", Twist, self.callback) 00074 00075 self.x = 0. 00076 self.y = 0. 00077 self.z = 0.2 00078 self.r = 0. 00079 self.p = 0. 00080 self.t = 0. 00081 self.twist = Twist() 00082 00083 rospy.sleep(2) 00084 self.tf_broadcaster.sendTransform((self.x, self.y, self.z), 00085 tf.transformations.quaternion_from_euler(self.r, self.p, self.t), 00086 rospy.Time.now(), 00087 self.reference_frame, 00088 self.target_frame) 00089 00090 self.current_time = rospy.get_time() 00091 self.last_time = rospy.get_time() 00092 00093 def callback(self,msg): 00094 self.twist = msg 00095 00096 if __name__ == '__main__': 00097 rospy.init_node('move_box') 00098 mb = move_box() 00099 rate = rospy.Rate(100) 00100 while not rospy.is_shutdown(): 00101 mb.current_time = rospy.get_time() 00102 00103 try: 00104 (trans,rot) = mb.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0)) 00105 euler = tf.transformations.euler_from_quaternion(rot) 00106 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 00107 continue 00108 00109 x = (mb.current_time - mb.last_time) * (mb.twist.linear.x * cos(euler[2]) - mb.twist.linear.y * sin(euler[2])) + trans[0] 00110 y = (mb.current_time - mb.last_time) * (mb.twist.linear.x * sin(euler[2]) + mb.twist.linear.y * cos(euler[2])) + trans[1] 00111 t = mb.twist.angular.z*(mb.current_time - mb.last_time) + euler[2] 00112 00113 mb.tf_broadcaster.sendTransform((x , y, 0), 00114 tf.transformations.quaternion_from_euler(0, 0, t), 00115 rospy.Time.now(), 00116 mb.reference_frame, 00117 mb.target_frame) 00118 mb.last_time = mb.current_time 00119 rate.sleep()