jsk_pr2_startup: objectdetection

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # Store the ObjectDetection message
00004 #
00005 
00006 try:
00007     import roslib; roslib.load_manifest('jsk_pr2_startup')
00008 except:
00009     pass
00010 
00011 import rospy
00012 import tf
00013 
00014 from geometry_msgs.msg import PoseStamped, TransformStamped
00015 from posedetection_msgs.msg import ObjectDetection
00016 
00017 from mongodb_store.message_store import MessageStoreProxy
00018 
00019 class ObjectDetectionDB(object):
00020     def __init__(self):
00021         self.db_name = rospy.get_param('robot/database','jsk_robot_lifelog')
00022         try:
00023             self.col_name = rospy.get_param('robot/name')
00024         except KeyError as e:
00025             rospy.logerr("please specify param \"robot/name\" (e.g. pr1012, olive)")
00026             exit(1)
00027         self.update_cycle = rospy.get_param('update_cycle', 1)
00028         self.map_frame = rospy.get_param('~map_frmae', 'map')
00029         self.robot_frame = rospy.get_param('~robot_frame','base_footprint')
00030         self.tf_listener = tf.TransformListener()
00031         self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name)
00032         rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name))
00033         rospy.loginfo("map->robot: %s -> %s" % (self.map_frame, self.robot_frame))
00034 
00035         self.subscribers = []
00036 
00037     # DB Insertion function
00038     def _insert_pose_to_db(self, map_to_robot, robot_to_obj):
00039         try:
00040             self.msg_store.insert(map_to_robot)
00041             self.msg_store.insert(robot_to_obj)
00042             rospy.loginfo('inserted map2robot: %s, robot2obj: %s' % (map_to_robot, robot_to_obj))
00043         except Exception as e:
00044             rospy.logwarn('failed to insert to db' + e)
00045 
00046     def _lookup_transform(self, target_frame, source_frame, time=rospy.Time(0), timeout=rospy.Duration(0.0)):
00047         self.tf_listener.waitForTransform(target_frame, source_frame, time, timeout)
00048         res = self.tf_listener.lookupTransform(target_frame, source_frame, time)
00049         ret = TransformStamped()
00050         ret.header.frame_id = target_frame
00051         ret.header.stamp = time
00052         ret.child_frame_id = source_frame
00053         ret.transform.translation.x = res[0][0]
00054         ret.transform.translation.y = res[0][1]
00055         ret.transform.translation.z = res[0][2]
00056         ret.transform.rotation.x = res[1][0]
00057         ret.transform.rotation.y = res[1][1]
00058         ret.transform.rotation.z = res[1][2]
00059         ret.transform.rotation.w = res[1][3]
00060         return ret
00061 
00062     def _objectdetection_cb(self, msg):
00063         try:
00064             self.tf_listener.waitForTransform(self.robot_frame, self.map_frame, msg.header.stamp, rospy.Duration(1.0))
00065             map_to_robot = self._lookup_transform(self.robot_frame, self.map_frame, msg.header.stamp)
00066         except Exception as e:
00067             rospy.logwarn("failed to lookup tf: %s", e)
00068             return
00069 
00070         try:
00071             for obj in msg.objects:
00072                 spose = PoseStamped(header=msg.header,pose=obj.pose)
00073                 tpose = self.tf_listener.transformPose(self.robot_frame, spose)
00074                 obj.pose = tpose.pose
00075                 self._insert_pose_to_db(map_to_robot, obj)
00076         except Exception as e:
00077             rospy.logwarn("failed to object pose transform: %s", e)
00078 
00079     def _update_subscribers(self):
00080         current_subscribers = rospy.client.get_published_topics()
00081         targets = [x for x in current_subscribers if x[1]=='posedetection_msgs/ObjectDetection' and ('_agg' in x[0])]
00082         for sub in self.subscribers:
00083             if sub.get_num_connections() == 0:
00084                 sub.unregister()
00085                 self.subscribers.remove(sub)
00086                 rospy.loginfo('unsubscribe (%s)',sub.name)
00087         for topic_info in targets:
00088             if topic_info[0] in [x.name for x in self.subscribers]:
00089                 continue
00090             sub = rospy.Subscriber(topic_info[0], ObjectDetection,
00091                                    self._objectdetection_cb)
00092             self.subscribers += [sub]
00093             rospy.loginfo('start subscribe (%s)',sub.name)
00094 
00095     def sleep_one_cycle(self):
00096         self._update_subscribers()
00097         rospy.sleep(self.update_cycle)
00098 
00099 if __name__ == "__main__":
00100     rospy.init_node('objectdetecton_db')
00101     obj = ObjectDetectionDB()
00102     while not rospy.is_shutdown():
00103         obj.sleep_one_cycle()