Commit 4290b807 authored by Alexis Nicolin's avatar Alexis Nicolin

Moved modifications on readQueue from ros_interface to supervisor to avoid a...

Moved modifications on readQueue from ros_interface to supervisor to avoid a possible delay issue between ros services calls
parent fc6bc64b
......@@ -99,15 +99,9 @@ class RosInterface(object):
def readQueue(self, req):
if self.supervisor is not None:
SoTTime = self.supervisor.sotrobot.device.control.time
self.supervisor.readQueue( SoTTime + req.data)
self.supervisor.readQueue( req.data)
else:
cmd = "supervisor.sotrobot.device.control.time"
answer = self.runCommand (cmd)
print cmd
print answer
SoTTime = int(answer.result)
cmd = "supervisor.readQueue(" + str(SoTTime + req.data) + ")"
cmd = "supervisor.readQueue(" + str(req.data) + ")"
answer = self.runCommand (cmd)
print cmd
print answer
......@@ -115,9 +109,9 @@ class RosInterface(object):
def stopReadingQueue(self, req):
if self.supervisor is not None:
self.supervisor.readQueue( -1 )
self.supervisor.stopReadingQueue ()
else:
cmd = "supervisor.readQueue(" + str(-1) + ")"
cmd = "supervisor.stopReadingQueue()"
answer = self.runCommand (cmd)
print cmd
print answer
......
......@@ -222,8 +222,14 @@ class Supervisor(object):
self.rosexport.clearQueue(s)
def readQueue(self, read):
print "Read queues:", read
self.rosexport.readQueue (read)
if read < 0:
print "ReadQueue argument should be >= 0"
return
t = self.sotrobot.device.control.time
self.rosexport.readQueue (t + read)
def stopReadingQueue(self):
self.rosexport.readQueue (-1)
def plugSot(self, id, check = False):
if check and not self.isSotConsistentWithCurrent (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