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