def solutionToInfo(self, solution):
actions = []
for action in solution[self._actions]:
actions.append({"identifier": IRI(action[0]), "title": action[1]})
return {
"identifier": IRI(solution[self._identifier]),
"type": IRI(solution[self._type]),
"label": solution[self._label],
"tf": ObjectPose(pose=self._pose).solutionToTf(solution),
"handle": solution[self._handlePath],
"actions": actions
}
def getIndividual(self):
solution = self.solution
if solution:
return IRI(solution[self._individual])
return None
def getChildren(self):
solution = self.solution
if solution:
return [IRI(child) for child in solution[self._children]]
return []
def getType(self):
solution = self.solution
if solution:
return IRI(solution[self._type])
return None
def solutionToInfo(self, solution):
return {
"identifier":
IRI(solution[self._identifier]),
"type":
IRI(solution[self._type]),
"label":
solution[self._label],
"tf":
ObjectPose(pose=self._pose).solutionToTf(solution),
"dimensions":
ObjectDimensions(width=self._width,
height=self._height,
depth=self._depth).solutionToDimensions(solution),
"model":
solution[self._modelPath]
}
def solutionToInfo(self, solution):
return {
"identifier":
IRI(solution[self._identifier]),
"type":
IRI(solution[self._type]),
"label":
solution[self._label],
"frame":
solution[self._frame],
"stamp":
MapObjectStamp(stamp=self._stamp).solutionToStamp(solution),
"pose":
ObjectPose(pose=self._pose).solutionToMessage(solution),
"dimensions":
ObjectDimensions(width=self._width,
height=self._height,
depth=self._depth).solutionToDimensions(solution),
"parent":
IRI(solution[self._parent])
}
def addAction(self, action, object):
msg = SemMapAction()
if "type" in action:
msg.type = action["type"]
else:
msg.type = "knowrob:Action"
if "id" in action:
msg.id = str(action["id"])
else:
msg.id = "%s_%s" % (IRI(msg.type).shortName, str(uuid.uuid1()))
if "asserted" in action:
msg.asserted = action["asserted"]
msg.object_acted_on = object
if "actions" in action:
for subaction in action["actions"]:
msg.subactions.append(self.addAction(subaction, object).id)
if "quantification" in action:
if action["quantification"] == "intersection_of":
msg.quantification = SemMapAction.INTERSECTION_OF
elif action["quantification"] == "union_of":
msg.quantification = SemMapAction.UNION_OF
else:
raise Exception("Invalid quantification for action [%s]: %s" %
(msg.id, str(action["quantification"])))
if "unordered" in action:
msg.unordered = action["unordered"]
self.map.actions.append(msg)
self.addObjectProperties(action, msg.id)
self.addDataProperties(action, msg.id)
return msg
def getObjects(self):
for solution in self.solutions:
yield IRI(solution[self._object])
def getObject(self):
solution = self.solution
if solution:
return IRI(solution[self._object])
def getSubject(self):
solution = self.solution
if solution:
return IRI(solution[self._subject])
def getIndividuals(self):
for solution in self.solutions:
yield IRI(solution[self._individual])
def addObject(self, object):
msg = SemMapObject()
msg.type = object["type"]
if "id" in object:
msg.id = str(object["id"])
else:
msg.id = "%s_%s" % (IRI(msg.type).shortName, str(uuid.uuid1()))
if "frame_id" in object:
msg.header.frame_id = object["frame_id"]
if "stamp" in object:
msg.header.stamp = Time.from_sec(
time.mktime(
time.strptime(object["stamp"].value, "%Y%m%dT%H:%M:%S")))
else:
msg.header.stamp = self.map.header.stamp
if "size" in object:
if "x" in object["size"]:
msg.size.x = object["size"]["x"]
if "y" in object["size"]:
msg.size.y = object["size"]["y"]
if "z" in object["size"]:
msg.size.z = object["size"]["z"]
if "position" in object:
if "x" in object["position"]:
msg.pose.position.x = object["position"]["x"]
if "y" in object["position"]:
msg.pose.position.y = object["position"]["y"]
if "z" in object["position"]:
msg.pose.position.z = object["position"]["z"]
if "orientation" in object:
if "w" in object["orientation"]:
msg.pose.orientation.w = object["orientation"]["w"]
if "x" in object["orientation"]:
msg.pose.orientation.x = object["orientation"]["x"]
if "y" in object["orientation"]:
msg.pose.orientation.y = object["orientation"]["y"]
if "z" in object["orientation"]:
msg.pose.orientation.z = object["orientation"]["z"]
if "actions" in object:
for action in object["actions"]:
self.addAction(action, msg.id)
if "part_of" in object:
msg.part_of = str(object["part_of"])
self.map.objects.append(msg)
self.addObjectProperties(object, msg.id)
self.addDataProperties(object, msg.id)
if "parts" in object:
for part in object["parts"]:
self.addObject(part).part_of = msg.id
return msg
def getActions(self):
for solution in self.solutions:
yield IRI(solution[self._action])
def getAction(self):
solution = self.solution
if solution:
return IRI(solution[self._action])