test_autosequence.py
Go to the documentation of this file.
00001 #!/usr/bin/env rosh
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 #  Copyright (c) 2010, UC Regents
00006 #  All rights reserved.
00007 #
00008 #  Redistribution and use in source and binary forms, with or without
00009 #  modification, are permitted provided that the following conditions
00010 #  are met:
00011 #
00012 #   * Redistributions of source code must retain the above copyright
00013 #     notice, this list of conditions and the following disclaimer.
00014 #   * Redistributions in binary form must reproduce the above
00015 #     copyright notice, this list of conditions and the following
00016 #     disclaimer in the documentation and/or other materials provided
00017 #     with the distribution.
00018 #   * Neither the name of the University of California nor the names of its
00019 #     contributors may be used to endorse or promote products derived
00020 #     from this software without specific prior written permission.
00021 #
00022 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #  POSSIBILITY OF SUCH DAMAGE.
00034 """
00035 patrol.py
00036 
00037 Set up an autosequence for the vehicle to follow as part of the obstacle avoidance demo.
00038 """
00039 
00040 import roslib
00041 roslib.load_manifest('starmac_controller_testing')
00042 
00043 from starmac_roshlib.topics import is_publishing, wait_for_topic
00044 from starmac_roshlib.commanding import cmd_wait
00045 
00046 def runscript(script):
00047     for line in script.split('\n'):
00048         line = line.strip()
00049         if len(line) > 0:
00050             dest, cmd = line.split(' ', 1)
00051             topic_obj = {'CTRL':ccmd, 'AUTO':acmd}[dest]
00052             cmd_wait(topic_obj,cmd)
00053 
00054 wait_for_topic('/simflyer1/manager','/simflyer1/controller/cmd')
00055 wait_for_topic('/simflyer1/manager','/simflyer1/control_mode_autosequence/cmd')
00056 sleep(10.0)
00057 
00058 ccmd = topics.simflyer1.controller.cmd
00059 acmd = topics.simflyer1.control_mode_autosequence.cmd
00060 
00061 cmd_wait(ccmd, 'mode operational', 3.0)
00062 script1 = """\
00063 CTRL control_mode to_active autosequence
00064 """
00065 script2 = """\
00066 AUTO foo
00067 AUTO bar
00068 AUTO baz
00069 AUTO define hover_point A_north 0.800000 0.900000 0 0 45
00070 AUTO define hover_point A_east 0.800000 0.900000 0 0 135
00071 AUTO define hover_point A_south 0.800000 0.900000 0 0 225
00072 AUTO define hover_point A_west 0.800000 0.900000 0 0 315
00073 AUTO define hover_point A_southwest 0.800000 0.900000 0 0 270
00074 AUTO define hover_point A_northeast 0.800000 0.900000 0 0 90
00075 
00076 AUTO define hover_point B_north 0.800000 -0.600000 0 0 45
00077 AUTO define hover_point B_east 0.800000 -0.600000 0 0 135
00078 AUTO define hover_point B_south 0.800000 -0.600000 0 0 225
00079 AUTO define hover_point B_west 0.800000 -0.600000 0 0 315
00080 AUTO define hover_point B_southeast 0.800000 -0.600000 0 0 180
00081 AUTO define hover_point B_northwest 0.800000 -0.600000 0 0 0
00082 
00083 AUTO define hover_point C_north -0.800000 -0.600000 0 0 45
00084 AUTO define hover_point C_east -0.800000 -0.600000 0 0 135
00085 AUTO define hover_point C_south -0.800000 -0.600000 0 0 225
00086 AUTO define hover_point C_west -0.800000 -0.600000 0 0 315
00087 AUTO define hover_point C_southwest -0.800000 -0.600000 0 0 270
00088 AUTO define hover_point C_northeast -0.800000 -0.600000 0 0 90
00089 
00090 AUTO define hover_point D_north -0.800000 0.900000 0 0 45
00091 AUTO define hover_point D_east -0.800000 0.900000 0 0 135
00092 AUTO define hover_point D_south -0.800000 0.900000 0 0 225
00093 AUTO define hover_point D_west -0.800000 0.900000 0 0 315
00094 AUTO define hover_point D_southeast -0.800000 0.900000 0 0 180
00095 AUTO define hover_point D_northwest -0.800000 0.900000 0 0 0
00096 
00097 AUTO define hover_point origin_north 0.0 0.0 0 0 45
00098 AUTO define hover_point origin_northeast 0.0 0.0 0 0 90
00099 AUTO define hover_point origin_east 0.0 0.0 0 0 135
00100 AUTO define hover_point origin_southeast 0.0 0.0 0 0 180
00101 AUTO define hover_point origin_south 0.0 0.0 0 0 225
00102 AUTO define hover_point origin_southwest 0.0 0.0 0 0 270
00103 AUTO define hover_point origin_west 0.0 0.0 0 0 315
00104 AUTO define hover_point origin_northwest 0.0 0.0 0 0 0
00105 
00106 AUTO define autosequence patrol_square A_west pause B_west B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north A_north A_west
00107 AUTO define autosequence patrol_cross A_west pause B_west B_southeast D_southeast D_north A_north A_southwest C_southwest C_north B_north B_east B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north D_northwest B_northwest B_east B_southeast origin_southeast origin_south origin_west origin_north origin_east origin_south origin_east origin_north origin_west origin_south
00108 AUTO execute autosequence patrol_cross
00109 """
00110 #AUTO define autosequence patrol A_north A_west pause B_west B_north B_east A_east A_north A_west B_west B_north B_east A_east A_north A_west B_west B_north B_east A_east A_north A_west B_west B_north B_east A_east A_north A_west B_west B_north B_east A_east A_north A_west B_west B_north B_east A_east A_north A_west
00111 #AUTO define autosequence patrol_square A_west pause B_west B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north A_north A_west B_west B_south C_south C_east D_east D_north A_north A_west
00112 
00113 runscript(script1)
00114 sleep(3.0)
00115 runscript(script2)
00116         
00117 loginfo('My work here is done..')


starmac_controller_testing
Author(s): bouffard
autogenerated on Sun Jan 5 2014 11:38:45