Commit 7143807d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Make this a ROS package and add ROS node start_supervisor.py

parent 5f2f3176
......@@ -23,7 +23,7 @@ SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME sot-hpp)
SET(PROJECT_NAME sot_hpp)
SET(PROJECT_DESCRIPTION "Bridge between hpp and sot")
SET(PROJECT_URL "")
......@@ -38,6 +38,13 @@ ADD_REQUIRED_DEPENDENCY ("sot-core >= 3")
ADD_REQUIRED_DEPENDENCY ("dynamic-graph-python >= 2")
ADD_REQUIRED_DEPENDENCY ("sot_hpp_msgs")
INSTALL(FILES
package.xml
DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
INSTALL(PROGRAMS
scripts/start_supervisor.py
DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}/scripts)
ADD_SUBDIRECTORY(src)
SETUP_PROJECT_FINALIZE()
<package>
<name>sot_hpp</name>
<version>0.0.0</version>
<description> SoT - HPP package. </description>
<maintainer email="jmirabel@laas.fr">Joseph Mirabel</maintainer>
<author>Joseph Mirabel</author>
<license>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>sot_hpp_msgs</run_depend>
</package>
#!/usr/bin/python
import sys
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')
def _runCommandPrint (ans):
if ans.result != "None":
rospy.loginfo (ans.result)
if len(ans.standardoutput) > 0:
rospy.loginfo (ans.standardoutput)
if len(ans.standarderror) > 0:
rospy.logerr (ans.standarderror)
def launchScript(code,title,description = ""):
# raw_input(title+': '+description)
rospy.loginfo(title)
# rospy.loginfo(code)
indent = ' '
indenting = False
for line in code:
if indenting:
if line == '' or line.startswith(indent):
rospy.loginfo (".. " + line)
codeblock += '\n' + line
continue
else:
answer = runCommandClient(str(codeblock))
_runCommandPrint (answer)
indenting = False
if line != '' and line[0] != '#':
rospy.loginfo (">> " + line)
if line.endswith(':'):
indenting = True
codeblock = line
else:
answer = runCommandClient(str(line))
_runCommandPrint (answer)
rospy.loginfo("...done with "+title)
def makeRosInterface():
from sot_hpp.ros_interface import RosInterface
import rospy
ri = RosInterface ()
ri.setupHppJoints (prefix = "talos/")
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")
rospy.wait_for_service('/run_command')
rospy.loginfo("...ok")
rospy.loginfo("Waiting for start_dynamic_graph")
rospy.wait_for_service('/start_dynamic_graph')
rospy.loginfo("...ok")
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:
rospy.logerr("Service call failed: %s" % e)
rospy.spin()
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