Skip to content
GitLab
Commit e961230f authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Use transition names instead of transition id.

parent bae0523d
Hide whitespace changes
Inline Side-by-side
sot_hpp/factory.py
View file @ e961230f
......@@ -126,16 +126,14 @@ class Factory(GraphFactoryAbstract):
ids = { n.name: n.id for n in elmts.edges }
nids = { n.name: n.id for n in elmts.nodes }
self.transitionIds = { n.name: n.id for n in elmts.edges }
self.supervisor.sots = { ids[n]: sot for n, sot in self.sots.items() if ids.has_key(n) }
self.supervisor.sots = self.sots
self.supervisor.grasps = { (gh, w): t for gh, ts in self.tasks._grasp.items() for w, t in ts.items() }
self.supervisor.hpTasks = self.hpTasks
self.supervisor.lpTasks = self.lpTasks
self.supervisor.postActions = {
ids[trans] : {
nids[state]: sot for state, sot in values.items() if nids.has_key(state)
} for trans, values in self.postActions.items() if ids.has_key(trans)
}
self.supervisor.preActions = { ids[trans] : sot for trans, sot in self.preActions.items() if ids.has_key(trans) }
self.supervisor.postActions = self.postActions
self.supervisor.preActions = self.preActions
def setupFrames (self, hppclient, sotrobot):
self.sotrobot = sotrobot
......
sot_hpp/ros_interface.py
View file @ e961230f
......@@ -28,14 +28,14 @@ class RosInterface(object):
rsp = PlugSotResponse()
if self.supervisor is not None:
try:
self.supervisor.runPreAction(req.transition_id)
self.supervisor.runPreAction(req.transition_name)
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))
answer = self.runCommand ("supervisor.runPreAction({})".format(req.transition_name))
if len(answer.standarderror) != 0:
rospy.logerr(answer.standarderror)
rsp.success = False
......@@ -48,14 +48,14 @@ class RosInterface(object):
rsp = PlugSotResponse()
if self.supervisor is not None:
try:
self.supervisor.plugSot(req.transition_id, False)
self.supervisor.plugSot(req.transition_name, False)
except Exception as e:
rospy.logerr(str(e))
rsp.success = False
rsp.msg = str(e)
return rsp
else:
answer = self.runCommand ("supervisor.plugSot({}, False)".format(req.transition_id))
answer = self.runCommand ("supervisor.plugSot({}, False)".format(req.
if len(answer.standarderror) != 0:
rospy.logerr(answer.standarderror)
rsp.success = False
......@@ -68,14 +68,14 @@ class RosInterface(object):
rsp = PlugSotResponse()
if self.supervisor is not None:
try:
self.supervisor.runPostAction(req.transition_id)
self.supervisor.runPostAction(req.transition_name)
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))
answer = self.runCommand ("supervisor.runPostAction({})".format(req.transition_name))
if len(answer.standarderror) != 0:
rospy.logerr(answer.standarderror)
rsp.success = False
......
sot_hpp/supervisor.py
View file @ e961230f
......@@ -88,11 +88,11 @@ class Supervisor(object):
plug (rosexport.signal(n), s())
print topic, "plugged to", n, ', ', len(t['signalGetters']), 'times'
def isSotConsistentWithCurrent(self, id, thr = 1e-3):
if self.currentSot is None or id == self.currentSot:
def isSotConsistentWithCurrent(self, transitionName, thr = 1e-3):
if self.currentSot is None or transitionName == self.currentSot:
return True
csot = self.sots[self.currentSot]
nsot = self.sots[id]
nsot = self.sots[transitionName]
t = self.sotrobot.device.control.time
csot.control.recompute(t)
nsot.control.recompute(t)
......@@ -120,47 +120,47 @@ class Supervisor(object):
def stopReadingQueue(self):
self.rosexport.readQueue (-1)
def plugSot(self, id, check = False):
if check and not self.isSotConsistentWithCurrent (id):
def plugSot(self, transitionName, check = False):
if check and not self.isSotConsistentWithCurrent (transitionName):
# raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
print "Sot %d not consistent with sot %d" % (self.currentSot, id)
if id == -1:
print("Sot {0} not consistent with sot {1}".format(self.currentSot, transitionName))
if transitionName == "":
# TODO : Explanation and linked TODO in the function makeInitialSot
if self.sotrobot.dynamic.position.value[0] > -0.5:
self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:])
else:
self.keep_posture._signalPositionRef().value = self.sotrobot.dynamic.position.value
sot = self.sots[id]
sot = self.sots[transitionName]
# Start reading queues
self.readQueue(10)
plug(sot.control, self.sotrobot.device.control)
print "Current sot:", id
print "Current sot:", transitionName
print sot.display()
self.currentSot = id
self.currentSot = transitionName
def runPreAction(self, idTransition):
if self.preActions.has_key(idTransition):
sot = self.preActions[idTransition]
print "Running pre action", idTransition
def runPreAction(self, transitionName):
if self.preActions.has_key(transitionName):
sot = self.preActions[transitionName]
print "Running pre action", transitionName
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
print "No pre action", transitionName
def runPostAction(self, idStateTarget):
def runPostAction(self, targetStateName):
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
if d.has_key(targetStateName):
sot = d[targetStateName]
print "Running post action", self.currentSot, targetStateName
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
print "No post action", self.currentSot, targetStateName
def getJointList (self, prefix = ""):
return [ prefix + n for n in self.sotrobot.dynamic.model.names[1:] ]
......
Supports Markdown
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