class TrajectoryQueryService():
def __init__(self):
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
self.ms = MessageStoreProxy(collection="people_trajectory")
# setting up the service
self.ser = rospy.Service('/trajectory_query', TrajectoryQuery,
self.service_cb)
self.topic = 'trajectory_query'
self.vis = TrajectoryVisualizer(self.topic)
def retrieve_msg(self, uuid):
res = self.ms.query(Trajectory._type, message_query={"uuid": uuid})
if len(res) < 1:
rospy.logerr("Trajectory not found: %s" % uuid)
return None
elif len(res) > 1:
rospy.logerr("Multiple trajectories found: %s" % uuid)
t = res[0][0]
return t
t = res[0][0]
return t
def service_cb(self, req):
rospy.loginfo("Request received: %s" % req)
if req.visualize:
self.vis.clear()
res = TrajectoryQueryResponse()
res.trajectories = Trajectories()
try:
json_query = json.loads(req.query)
trajectories = self.gs.find(json_query)
except:
rospy.logerr("Invalid json => re-check syntax")
res.error = True
return res
count = 0
for t in trajectories:
if t.has_key('uuid'):
count += 1
#rospy.loginfo("retrieve msg for uuid: %s" % t['uuid'])
# otherwise result is not a trajectory => ignore
msg = self.retrieve_msg(t['uuid'])
if msg:
res.trajectories.trajectories.append(msg)
rospy.loginfo("Query result: %s trajectories" % count)
if req.visualize:
rospy.loginfo("Visualize result on topic: %s" % self.topic)
self.vis.visualize_trajectories(res.trajectories)
rospy.loginfo("Response returned")
res.error = False
return res
def main(self):
rospy.spin()