Commit 6be905af authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add files to perform servo-off.

parent 0bc47a79
Pipeline #1880 failed with stage
in 1 second
#!/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)
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);
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