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

Add runPreAction and runPostAction

parent b77017ee
......@@ -6,11 +6,33 @@ from dynamic_graph_bridge_msgs.srv import RunCommand
class RosInterface(object):
def __init__ (self, supervisor = None):
rospy.Service('/sot/plug_sot', PlugSot, self.plugSot)
rospy.Service('/sot/run_post_action', PlugSot, self.runPostAction)
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)
self.runCommand = rospy.ServiceProxy ('/run_command', RunCommand)
self.supervisor = supervisor
def runPreAction (self, req):
rsp = PlugSotResponse()
if self.supervisor is not None:
try:
self.supervisor.runPreAction(req.transition_id)
except Exception as e:
rospy.logerr(str(e))
rsp.success = False
rsp.msg = str(e)
return rsp
else:
answer = self.runCommand ("supervisor.runPreAction({})".format(req.transition_id))
if len(answer.standarderror) != 0:
rospy.logerr(answer.standarderror)
rsp.success = False
rsp.msg = answer.standarderror
return rsp
rsp.success = True
return rsp
def plugSot (self, req):
rsp = PlugSotResponse()
if self.supervisor is not None:
......@@ -31,6 +53,26 @@ class RosInterface(object):
rsp.success = True
return rsp
def runPostAction (self, req):
rsp = PlugSotResponse()
if self.supervisor is not None:
try:
self.supervisor.runPostAction(req.transition_id)
except Exception as e:
rospy.logerr(str(e))
rsp.success = False
rsp.msg = str(e)
return rsp
else:
answer = self.runCommand ("supervisor.runPostAction({})".format(req.transition_id))
if len(answer.standarderror) != 0:
rospy.logerr(answer.standarderror)
rsp.success = False
rsp.msg = answer.standarderror
return rsp
rsp.success = True
return rsp
def setupHppJoints(self, prefix = ""):
if self.supervisor is not None:
names = self.supervisor.getJointList(prefix = prefix)
......
......@@ -69,6 +69,8 @@ class Supervisor(object):
"""
self.grasps = dict()
self.sots = dict()
self.postActions = dict()
self.preActions = dict()
self.transitions = transitions
for t in transitions:
......@@ -177,6 +179,30 @@ class Supervisor(object):
print sot.display()
self.currentSot = id
def runPreAction(self, idTransition):
if self.preActions.has_key(idTransition):
sot = self.preActions[idTransition]
print "Running pre action", idTransition
print sot.display()
t = self.sotrobot.device.control.time
sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print "No pre action", idTransition
def runPostAction(self, idStateTarget):
if self.postActions.has_key(self.currentSot):
d = self.postActions[self.currentSot]
if d.has_key(idStateTarget):
sot = d[idStateTarget]
print "Running post action", self.currentSot, idStateTarget
print sot.display()
t = self.sotrobot.device.control.time
sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print "No post action", self.currentSot, idStateTarget
def getJointList (self, prefix = ""):
return [ prefix + n for n in self.sotrobot.dynamic.model.names[2:] ]
......
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