prepare_robot_manip_sim.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ###############################################################################
00003 # \file
00004 #
00005 # $Id:$
00006 #
00007 # Copyright (C) Brno University of Technology
00008 #
00009 # This file is part of software developed by dcgm-robotics@FIT group.
00010 # 
00011 # Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00012 # Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00013 # Date: dd/mm/2012
00014 #
00015 # This file is free software: you can redistribute it and/or modify
00016 # it under the terms of the GNU Lesser General Public License as published by
00017 # the Free Software Foundation, either version 3 of the License, or
00018 # (at your option) any later version.
00019 # 
00020 # This file is distributed in the hope that it will be useful,
00021 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00022 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023 # GNU Lesser General Public License for more details.
00024 # 
00025 # You should have received a copy of the GNU Lesser General Public License
00026 # along with this file.  If not, see <http://www.gnu.org/licenses/>.
00027 #
00028 import roslib; roslib.load_manifest('srs_user_tests')
00029 import rospy
00030 from simple_script_server import *
00031 from std_msgs.msg import Empty as EmptyMsg
00032 
00033 sss = simple_script_server()
00034 
00035 
00036 def main():
00037     
00038     rospy.init_node('prepare_robot_for_manip_test_node')
00039     rospy.sleep(2)
00040     
00041     sim = rospy.get_param('/use_sim_time')
00042     
00043     torso_pos = rospy.get_param('~torso','home')
00044     
00045     if sim is True:
00046     
00047       rospy.loginfo('Waiting until end of spawning...')
00048       rospy.wait_for_message('/sim_spawned',EmptyMsg)
00049       rospy.sleep(5)
00050     
00051     sss.init('head', True)
00052     sss.init('tray', True)
00053     sss.init('arm', True)
00054     sss.init('torso', True)
00055     
00056     rospy.loginfo('Moving tray...')
00057     sss.move('tray', 'up', False)
00058     
00059     rospy.loginfo('Moving camera...')
00060     sss.move('head', 'back', False)
00061     
00062     
00063     rospy.loginfo('Moving arm...')
00064     #sss.move('arm', 'folded', True)
00065     sss.move('arm', 'look_at_table', True)
00066     
00067     
00068     rospy.loginfo('Moving torso...')
00069     sss.move('torso', torso_pos, True)
00070     #sss.move('torso', 'home', True)
00071     
00072     if sim is True:
00073     
00074       rospy.loginfo('Running in simulation, publishing to /sim_robot_init topic')
00075       
00076       pub = rospy.Publisher('/sim_robot_init', EmptyMsg,latch=True)
00077       pub.publish(EmptyMsg())
00078       pub.publish(EmptyMsg())
00079       pub.publish(EmptyMsg())
00080       
00081       rospy.spin()
00082 
00083 
00084 if __name__ == '__main__':
00085   try:
00086     main()
00087   except rospy.ROSInterruptException: pass


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