Commit 308212d0 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Clean script start_supervisor

parent 7143807d
#!/usr/bin/python
import sys
import rospy
import sys, getopt
def usage():
print ("Usage: " + sys.argv[0] + " --input script.py [--prefix prefix]")
sys.exit(1)
opts, args = getopt.getopt (sys.argv[1:], "i:p:", ["input=", "prefix="])
f = None
prefix = ""
for opt, arg in opts:
if opt == "-i" or opt == "--input":
from os.path import isfile
if not isfile(arg):
raise ValueError ("File not found: " + arg)
f = arg
elif opt == "-p" or opt == "--prefix":
prefix = arg
else:
usage()
if f is None:
usage()
import rospy
from std_srvs.srv import *
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
rospy.init_node ('start_supervisor')
rospy.loginfo ("Will read file: " + f)
def _runCommandPrint (ans):
if ans.result != "None":
......@@ -16,10 +39,8 @@ def _runCommandPrint (ans):
if len(ans.standarderror) > 0:
rospy.logerr (ans.standarderror)
def launchScript(code,title,description = ""):
# raw_input(title+': '+description)
def launchScript(code,title):
rospy.loginfo(title)
# rospy.loginfo(code)
indent = ' '
indenting = False
for line in code:
......@@ -46,15 +67,9 @@ def makeRosInterface():
from sot_hpp.ros_interface import RosInterface
import rospy
ri = RosInterface ()
ri.setupHppJoints (prefix = "talos/")
ri.setupHppJoints (prefix = prefix)
return ri
if len(sys.argv) > 1:
f = sys.argv[1]
else:
print ("Usage: " + sys.argv[0] + " script.py")
sys.exit(1)
# Waiting for services
try:
rospy.loginfo("Waiting for run_command")
......@@ -65,17 +80,15 @@ try:
rospy.wait_for_service('/start_dynamic_graph')
rospy.loginfo("...ok")
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
runCommandClient = rospy.ServiceProxy('/run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('/start_dynamic_graph', Empty)
initCode = open( f, "r").read().split("\n")
rospy.loginfo("Stack of Tasks launched")
launchScript(initCode,'initialize SoT')
# raw_input("Wait before starting ros interface")
ri = makeRosInterface ()
# raw_input("Wait before starting the dynamic graph")
runCommandStartDynamicGraph()
except rospy.ServiceException, e:
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment