def run(self): while not rospy.is_shutdown(): rospy.loginfo("$gt: %d" % (rospy.Time.now().secs - self.duration * 60 * 60 * 24)) if self.use_ros_time or self.use_sim_time: rospy.loginfo("fetching data") trans = self.msg_store.query(type=TransformStamped._type, message_query={"header.stamp.secs": { "$lte": rospy.Time.now().secs, "$gt": rospy.Time.now().secs - self.duration * 60 * 60 * 24 }}, sort_query=[("$natural", 1)], limit=self.limit) else: trans = self.msg_store.query(type=TransformStamped._type, meta_query={"inserted_at": { "$gt": datetime.utcnow() - timedelta(days=self.duration) }}, sort_query=[("$natural", 1)]) rospy.loginfo("found %d msgs" % len(trans)) m_arr = MarkerArray() m_arr.markers = V.transformStampedArrayToLabeledLineStripMarker(trans[::self.downsample], label_downsample=self.label_downsample, discrete=True) m_arr.markers = V.transformStampedArrayToLabeledArrayMarker(trans[::self.downsample], label_downsample=self.label_downsample, discrete=True) self.pub.publish(m_arr) rospy.sleep(1.0) rospy.loginfo("publishing move_base_marker_array")
def run(self): while not rospy.is_shutdown(): rospy.loginfo( "$gt: %d" % (rospy.Time.now().secs - self.duration * 60 * 60 * 24)) if self.use_ros_time or self.use_sim_time: rospy.loginfo("fetching data") trans = self.msg_store.query( type=TransformStamped._type, message_query={ "header.stamp.secs": { "$lte": rospy.Time.now().secs, "$gt": rospy.Time.now().secs - self.duration * 60 * 60 * 24 } }, sort_query=[("$natural", 1)], limit=self.limit) else: trans = self.msg_store.query( type=TransformStamped._type, meta_query={ "inserted_at": { "$gt": datetime.utcnow() - timedelta(days=self.duration) } }, sort_query=[("$natural", 1)]) rospy.loginfo("found %d msgs" % len(trans)) m_arr = MarkerArray() m_arr.markers = V.transformStampedArrayToLabeledLineStripMarker( trans[::self.downsample], label_downsample=self.label_downsample, discrete=True) m_arr.markers = V.transformStampedArrayToLabeledArrayMarker( trans[::self.downsample], label_downsample=self.label_downsample, discrete=True) self.pub.publish(m_arr) rospy.sleep(1.0) rospy.loginfo("publishing move_base_marker_array")
def __init__(self): self.db_name = rospy.get_param('~db_name','jsk_pr2_lifelog') self.col_name = rospy.get_param('~col_name', 'pr1012') self.duration = rospy.get_param('~duration', 30) self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) self.pub = rospy.Publisher('/object_detection_marker_array', MarkerArray) self.marker_count = 0 objs = self.msg_store.query(type=Object6DPose._type, meta_query={"inserted_at": { "$gt": datetime.now() - timedelta(days=self.duration) }}, sort_query=[("$natural", -1)]) first_obj_meta = objs[0][1] trans = [tuple(self.msg_store.query(type=TransformStamped._type, meta_query={"inserted_at": { "$lt": first_obj_meta["inserted_at"] }}, sort_query=[("$natural", -1)], single=True))] trans += self.msg_store.query(type=TransformStamped._type, meta_query={"inserted_at": { "$gt": first_obj_meta["inserted_at"] }}, sort_query=[("$natural", -1)]) j = 0 m_arr = MarkerArray() for i in range(len(objs)): o,o_meta = objs[i] t,t_meta = trans[j] if o_meta["inserted_at"] > t_meta["inserted_at"]: j += 1 t,t_meta = trans[j] ps = T.transformPoseWithTransformStamped(o.pose, t) m_arr.markers += V.poseStampedToLabeledSphereMarker([ps, o_meta], o.type) while not rospy.is_shutdown(): self.pub.publish(m_arr) rospy.sleep(1.0) rospy.logdebug("publishing objectdetection_marker_array")