def runscript(script):
for line in script.split('\n'):
line = line.strip()
if len(line) > 0:
dest, cmd = line.split(' ', 1)
topic_obj = {'CTRL':ccmd, 'AUTO':acmd}[dest]
cmd_wait(topic_obj,cmd)
from rospy import myargv, get_namespace, loginfo
import rospy
from starmac_roshlib.topics import wait_for_topic, wait_for_topic_sub
from starmac_roshlib.commanding import cmd_wait
#rospy.init_node('modes_to_standby')
loginfo('modes_to_standby script starting')
ns = get_namespace()
print "ns = ", ns
modes = rospy.get_param('~modes').split() #parameters['~'].modes() #myargv(argv=sys.argv)
loginfo('modes: %s' % modes)
wait_for_topic_sub(ns + 'manager', ns + 'controller/cmd')
for mode in modes:
wait_for_topic(ns + 'manager', ns + 'control_mode_%s/cmd' % mode)
sleep(5.0)
ccmd = topics[ns + 'controller/cmd']
cmd_wait(ccmd, "mode operational", 3.0)
for mode in modes:
cmd_wait(ccmd, "control_mode to_standby %s" % mode, 2.0)
# TODO: add some verification that the modes actually end up in standby
loginfo('modes_to_standby script finished')
def runscript(script):
for line in script.split('\n'):
line = line.strip()
if len(line) > 0:
dest, cmd = line.split(' ', 1)
topic_obj = {'CTRL':ccmd, 'AUTO':acmd}[dest]
cmd_wait(topic_obj,cmd)
wait_for_topic('/pelican1/manager','/pelican1/controller/cmd')
wait_for_topic('/pelican1/manager','/pelican1/control_mode_autosequence/cmd')
sleep(10.0)
ccmd = topics.pelican1.controller.cmd
acmd = topics.pelican1.control_mode_autosequence.cmd
cmd_wait(ccmd, 'mode operational', 3.0)
script1 = """\
CTRL control_mode to_active autosequence
"""
script2 = """\
AUTO foo
AUTO bar
AUTO baz
AUTO define hover_point A_north 0.800000 0.900000 0 0 45
AUTO define hover_point A_east 0.800000 0.900000 0 0 135
AUTO define hover_point A_south 0.800000 0.900000 0 0 225
AUTO define hover_point A_west 0.800000 0.900000 0 0 315
AUTO define hover_point A_southwest 0.800000 0.900000 0 0 270
AUTO define hover_point A_northeast 0.800000 0.900000 0 0 90
AUTO define hover_point B_north 0.800000 -0.600000 0 0 45
