class InitializePath(smach.State):
hppTargetPubDict = {"read_subpath": [ReadSubPath, 1]}
serviceProxiesDict = {
"agimus": {
"sot": {
"clear_queues": [std_srvs.srv.Trigger]
}
},
"hpp": {
"target": {
"publish_first": [std_srvs.srv.Trigger]
}
},
}
def __init__(self):
super(InitializePath, self).__init__(
outcomes=_outcomes,
input_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
],
output_keys=[
"transitionId", "endStateId", "currentSection", "duration"
],
)
self.targetPub = ros_tools.createPublishers("/hpp/target",
self.hppTargetPubDict)
self.serviceProxies = ros_tools.createServiceProxies(
"", InitializePath.serviceProxiesDict)
self.hppclient = HppClient(context="corbaserver")
def execute(self, userdata):
userdata.currentSection += 1
if userdata.currentSection + 1 >= len(userdata.times):
return _outcomes[0]
start = userdata.times[userdata.currentSection]
length = userdata.times[userdata.currentSection + 1] - start
transitionId = userdata.transitionIds[userdata.currentSection]
userdata.transitionId = transitionId
userdata.endStateId = userdata.endStateIds[userdata.currentSection]
userdata.duration = length
wait_if_step_by_step("Preparing to read subpath.", 3)
manip = self.hppclient._manip()
manip.graph.selectGraph(transitionId[1])
self.targetPub["read_subpath"].publish(
ReadSubPath(userdata.pathId, start, length))
rospy.loginfo("Start reading subpath.")
return _outcomes[2]
class WaitForInput(smach.State):
# Accessed services
serviceProxiesDict = {
"agimus": {
"sot": {
"request_hpp_topics": [std_srvs.srv.Trigger],
"plug_sot": [PlugSot],
"set_base_pose": [SetPose],
}
},
"hpp": {
"target": {
"reset_topics": [std_srvs.srv.Empty]
}
},
}
def __init__(self):
super(WaitForInput, self).__init__(
outcomes=["succeeded", "aborted"],
input_keys=[],
output_keys=[
"pathId",
"times",
"transitionIds",
"endStateIds",
"currentSection",
"queue_initialized",
],
)
rospy.logwarn("Create service WaitForInput")
self.services = ros_tools.createServiceProxies("",
self.serviceProxiesDict)
rospy.logwarn("f**k")
self.status_srv = rospy.Service("status", std_srvs.srv.Trigger,
self.getStatus)
rospy.logwarn("you")
self.hppclient = HppClient(context="corbaserver")
self.ready = False
self.status = "not started"
def getStatus(self, empty):
return self.ready, self.status
def execute(self, userdata):
self.status = "waiting"
self.ready = True
res = rospy.wait_for_message("start_path", UInt32)
self.ready = False
pid = res.data
self.status = "playing path " + str(pid)
rospy.loginfo("Requested to start path " + str(pid))
userdata.pathId = pid
userdata.queue_initialized = False
try:
hpp = self.hppclient._hpp()
manip = self.hppclient._manip()
qs, ts = hpp.problem.getWaypoints(pid)
# Add a first section to force going to init pose.
# ts: list of transition times. The first one is repeated to make the robot
# move to the initial configuration before any other motion.
ts = ts[0:1] + ts
# tids: list of pair (transitionName, graphName)
tids = [
manip.problem.edgeAtParam(pid, (t0 + t1) / 2)
for t0, t1 in zip(ts[:-1], ts[1:])
]
for i, tid in enumerate(tids):
manip.graph.selectGraph(tid[1])
tids[i] = (manip.graph.getName(tid[0]), tid[1])
# print len(qs), len(ts), len(tids)
# Remove fake transitions (i.e. when id is the same for two consecutive transitions)
tts = ts[0:3]
ttids = tids[0:2]
tqs = qs[0:2]
for t, id, q in zip(ts[2:], tids[1:], qs[1:]):
if ttids[-1] == id:
tts[-1] = t
tqs[-1] = q
else:
ttids.append(id)
tts.append(t)
tqs.append(q)
userdata.times = tuple(tts)
userdata.transitionIds = tuple(ttids)
endStateIds = []
for q, tid in zip(tqs, ttids):
manip.graph.selectGraph(tid[1])
nid = manip.graph.getNode(q)
endStateIds.append((manip.graph.getName(nid), tid[1]))
userdata.endStateIds = tuple(endStateIds)
userdata.currentSection = -1
# TODO reset_topics should not be necessary
self.services["hpp"]["target"]["reset_topics"]()
self.services["agimus"]["sot"]["request_hpp_topics"]()
# Set base pose
# TODO shouldn't this be done by the estimation node ?
# moreover, this assumes that HPP has a free-floating base.
from agimus_hpp.tools import hppPoseToSotTransRPY
self.services["agimus"]["sot"]["set_base_pose"](
*hppPoseToSotTransRPY(tqs[0][:7]))
rospy.sleep(0.001)
# TODO check that qs[0] and the current robot configuration are
# close
except Exception as e:
rospy.logerr("Failed " + str(e))
return "aborted"
return "succeeded"