def __init__(self, rob_self):
# this could be done extracting a property from the knowledge base
self._rob_self = rob_self
# start move_base client and wait for the server
self._client = actionlib.SimpleActionClient(
'move_base', move_base_msgs.msg.MoveBaseAction)
self.loginfo("Waiting for move_base server")
self._client.wait_for_server()
# get goals through this service
rospy.Service('move_coordinator/goal', Goal, self.setup_goal)
# get move_base updates
rospy.Subscriber("move_base/result", MoveBaseActionResult,
self.new_result)
# get robot knowledge updates
rospy.Service('move_coordinator/status_update', Empty,
self.check_and_run)
# publish successfull results
self._result = rospy.Publisher('move_coordinator/result',
MoveBaseActionResult)
self._status_pub = rospy.Publisher('/' + self._rob_self + '/status',
Status)
# interface to prolog
self.loginfo("Waiting for json_prolog server")
rospy.wait_for_service('json_prolog/simple_query')
self._prolog = json_prolog.Prolog('json_prolog')
self.loginfo("move_coordinator up and running")
def listener():
rospy.init_node('medicineTest', anonymous=False)
rospy.Subscriber('xfspeech', String, listenCallback)
prolog = json_prolog.Prolog()
query = prolog.query("owl_has(herb:'名称',P,O)")
for sol in query.solutions():
print sol
query.finish()
rospy.spin()
def listenCallback(data):
request = data.data
print "get voice"
rospy.loginfo(rospy.get_caller_id() + 'I heard %s', request)
prolog = json_prolog.Prolog()
query = prolog.query("owl_has(herb:'data.data',P,O)")
for sol in query.solutions():
print sol
query.finish()
rospy.spin()
def query_knowrob_properties(obj):
prolog = json_prolog.Prolog()
query = prolog.query("owl_has('%s', P, O)." % obj)
was_empty = True
for sol in query.solutions():
was_empty = False
rospy.loginfo('\tP = %s; O = %s' % (sol['P'], sol['O']))
query.finish()
if was_empty: rospy.loginfo('\tNo properties')
def query_knowrob(query):
prolog = json_prolog.Prolog()
obj = str(KNOWROB[query['theme']])
rospy.loginfo('Looking for %s in knowrob knowledge base' % obj)
query = prolog.query("owl_has(A, '%s', '%s')." % (str(RDF['type']), obj))
rospy.loginfo('Prolog query complete')
was_empty = True
for sol in query.solutions():
was_empty = False
rospy.loginfo('Found solution. A = %s' % sol['A'])
query_knowrob_properties(sol['A'])
query.finish()
if was_empty: rospy.logwarn('Could not find %s' % obj)
def __init__(self, rob_self, rob_list):
self._rob_self = rob_self
self._rob_list = rob_list
self.loginfo("Waiting for json_prolog server")
rospy.wait_for_service('json_prolog/simple_query')
self._prolog = json_prolog.Prolog("json_prolog")
# for each robot in the robot list
for robot in [x for x in self._rob_list if x != self._rob_self]:
self._rob_timing[robot] = 0.00
rospy.Subscriber("/" + robot + "/odom", Odometry,
self.position_callback(robot))
rospy.Subscriber("/" + robot + "/status", Status,
self.status_callback(robot))
self.loginfo("robot_logger up and running")
#!/usr/bin/env python
import roslib
roslib.load_manifest('json_prolog')
import rospy
import json_prolog
if __name__ == '__main__':
rospy.init_node('test_json_prolog')
prolog = json_prolog.Prolog()
query = prolog.query("member(A, [1, 2, 3, 4]), B = ['x', A]")
for solution in query.solutions():
print 'Found solution. A = %s, B = %s' % (solution['A'], solution['B'])
query.finish()
objects2type = {}
objects = []
for inst, issue in zip(frames[0]['instance_ids'], frames[0]['issue_ids']):
for inst_number in inst:
inst_number -= 1
for issue_number in issue:
issue_number -= 1
obj_name = '%s_%d' % (classes[issue_number], inst_number)
objects2type[obj_name] = classes[issue_number]
objects.append(obj_name)
print 'objects:'
print objects
# start json-prolog
rospy.init_node('tracking2knowrob', anonymous=True)
knowrob = json_prolog.Prolog()
# ask knowrob for object features
object_store = []
for o in objects:
o_type = objects2type[o]
obj = Object(o)
object_store.append(obj)
if not o_type in concept_map: continue
print 'Object: %s' % obj.name
query = knowrob.query("object_feature(%s, ITasCFeatureT, Pos, Dir)" %
concept_map[o_type])
counters = [1] * 3
for s in query.solutions():
featureT = s['ITasCFeatureT']
pos = numpy.matrix(s['Pos']).transpose()
