Add files to perform servo-off.

tests/publish-robot-state.py

+#!/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 *
+
+def launchScript(code,title,description = ""):
+    raw_input(title+':   '+description)
+    rospy.loginfo(title)
+    rospy.loginfo(code)
+    for line in code:
+        if line != '' and line[0] != '#':
+            print line
+            answer = runCommandClient(str(line))
+            rospy.logdebug(answer)
+            print answer
+    rospy.loginfo("...done with "+title)
+
+
+# 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( "servooff.py", "r").read().split("\n")
+    
+    rospy.loginfo("Stack of Tasks launched")
+
+
+    launchScript(initCode,'Publishing robotState_ros')
+
+except rospy.ServiceException, e:
+    rospy.logerr("Service call failed: %s" % e)
+

tests/servooff.py

+from dynamic_graph import plug
+from dynamic_graph.ros import RosPublish
+
+robot.ros=RosPublish('rosPublish')
+robot.ros.add('vector','robotState_ros','robotState')
+plug(robot.device.robotState,robot.ros.signal('robotState_ros'))
+robot.device.after.addDownsampledSignal('rosPublish.trigger',1);