visualize_objectdetection.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00004 
00005 import rospy
00006 from mongodb_store.message_store import MessageStoreProxy
00007 from posedetection_msgs.msg import Object6DPose
00008 from visualization_msgs.msg import MarkerArray
00009 from geometry_msgs.msg import TransformStamped, Pose
00010 from datetime import datetime, timedelta
00011 import tf
00012 
00013 from transform_utils import TransformationUtils as T
00014 from visualization_utils import VisualizationUtils as V
00015 
00016 class DBPlay(object):
00017     def __init__(self):
00018         self.db_name = rospy.get_param('~db_name','jsk_pr2_lifelog')
00019         self.col_name = rospy.get_param('~col_name', 'pr1012')
00020         self.duration = rospy.get_param('~duration', 30)
00021         self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name)
00022         rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name))
00023         self.pub = rospy.Publisher('/object_detection_marker_array', MarkerArray)
00024         self.marker_count = 0
00025 
00026         objs = self.msg_store.query(type=Object6DPose._type,
00027                                     meta_query={"inserted_at": {
00028                                         "$gt": datetime.now() - timedelta(days=self.duration)
00029                                     }},
00030                                     sort_query=[("$natural", -1)])
00031 
00032         first_obj_meta = objs[0][1]
00033 
00034         trans = [tuple(self.msg_store.query(type=TransformStamped._type,
00035                                             meta_query={"inserted_at": {
00036                                               "$lt": first_obj_meta["inserted_at"]
00037                                             }},
00038                                             sort_query=[("$natural", -1)],
00039                                             single=True))]
00040 
00041         trans += self.msg_store.query(type=TransformStamped._type,
00042                                       meta_query={"inserted_at": {
00043                                         "$gt": first_obj_meta["inserted_at"]
00044                                       }},
00045                                       sort_query=[("$natural", -1)])
00046 
00047         j = 0
00048         m_arr = MarkerArray()
00049         for i in range(len(objs)):
00050             o,o_meta = objs[i]
00051             t,t_meta = trans[j]
00052             if o_meta["inserted_at"] > t_meta["inserted_at"]:
00053                 j += 1
00054                 t,t_meta = trans[j]
00055             ps = T.transformPoseWithTransformStamped(o.pose, t)
00056 
00057             m_arr.markers += V.poseStampedToLabeledSphereMarker([ps, o_meta], o.type)
00058 
00059         while not rospy.is_shutdown():
00060             self.pub.publish(m_arr)
00061             rospy.sleep(1.0)
00062             rospy.logdebug("publishing objectdetection_marker_array")
00063 
00064 
00065 if __name__ == '__main__':
00066     rospy.init_node('visualize_objectdetection', log_level=rospy.DEBUG)
00067     o = DBPlay()


jsk_pr2_startup
Author(s):
autogenerated on Wed Sep 16 2015 10:32:17