Commit df878ec7 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Add Supervisor.readQeue and add ros srv /sot/read_queue

parent 686bf229
import rospy
from std_srvs.srv import Trigger, TriggerResponse
from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse
from hpp_ros_interface.srv import PlugSot, PlugSotResponse, SetString, SetJointNames
from dynamic_graph_bridge_msgs.srv import RunCommand
......@@ -10,6 +10,7 @@ class RosInterface(object):
rospy.Service('/sot/run_pre_action', PlugSot, self.runPreAction)
rospy.Service('/sot/request_hpp_topics', Trigger, self.requestHppTopics)
rospy.Service('/sot/clear_queues', Trigger, self.clearQueues)
rospy.Service('/sot/read_queue', SetBool, self.readQueue)
self.runCommand = rospy.ServiceProxy ('/run_command', RunCommand)
self.supervisor = supervisor
......@@ -95,6 +96,16 @@ class RosInterface(object):
print answer
return TriggerResponse (True, "ok")
def readQueue(self, req):
if self.supervisor is not None:
self.supervisor.readQueue(req.data)
else:
cmd = "self.supervisor.readQueue(" + str(req.data) + ")"
answer = self.runCommand (cmd)
print cmd
print answer
return SetBoolResponse (True, "ok")
def requestHppTopics(self, req):
handlers = {
'hppcom': rospy.ServiceProxy ('/hpp/target/add_center_of_mass', SetString),
......
......@@ -221,6 +221,10 @@ class Supervisor(object):
for s in tmp:
self.rosexport.clearQueue(s)
def readQueue(self, read):
print "Read queues:", read
self.rosexport.readQueue (read)
def plugSot(self, id, check = False):
if check and not self.isSotConsistentWithCurrent (id):
# raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
......
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