prepare_robot_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_nav_test_node')
00039     rospy.sleep(2)
00040     
00041     sim = rospy.get_param('/use_sim_time')
00042     
00043     if sim is True:
00044     
00045       rospy.loginfo('Waiting until end of spawning...')
00046       rospy.wait_for_message('/sim_spawned',EmptyMsg)
00047       rospy.sleep(5)
00048     
00049     sss.init('head', True)
00050     sss.init('tray', True)
00051     sss.init('arm', True)
00052     sss.init('torso', True)
00053     
00054     rospy.loginfo('Moving camera...')
00055     sss.move('head', 'front', True)
00056     
00057     rospy.loginfo('Moving tray...')
00058     sss.move('tray', 'down', True)
00059     
00060     rospy.loginfo('Moving arm...')
00061     sss.move('arm', 'folded', True)
00062     
00063     rospy.loginfo('Moving torso...')
00064     sss.move('torso', 'front_extreme', True)
00065     
00066     rospy.loginfo('Ready!')
00067     
00068     rospy.loginfo('Ready!')
00069     
00070     
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 Mon Oct 6 2014 08:53:11