$search
00001 #!/usr/bin/python 00002 00003 00004 00005 import roslib; roslib.load_manifest('camera_pose_calibration') 00006 import rospy 00007 import threading 00008 from calibration_msgs.msg import Interval, CalibrationPattern 00009 00010 00011 00012 def diff(f1, f2): 00013 if not f1 or not f2: 00014 return 999999 00015 return pow(f1.x - f2.x, 2) + pow(f1.y - f2.y, 2) 00016 00017 00018 class FilterIntervals: 00019 def __init__(self): 00020 self.min_duration = rospy.Duration(rospy.get_param('~min_duration', 0.5)) 00021 self.min_motion = rospy.get_param('~min_motion', 5.0) 00022 self.lock = threading.Lock() 00023 self.feature = None 00024 self.last_feature = None 00025 00026 self.pub = rospy.Publisher('interval_filtered', Interval) 00027 self.sub_intervals = rospy.Subscriber('interval', Interval, self.interval_cb) 00028 self.sub_features = rospy.Subscriber('features', CalibrationPattern, self.feature_cb) 00029 00030 00031 def interval_cb(self, msg): 00032 with self.lock: 00033 duration = msg.end - msg.start 00034 if self.feature and diff(self.feature, self.last_feature) > self.min_motion and duration > self.min_duration: 00035 self.last_feature = self.feature 00036 self.pub.publish(msg) 00037 00038 00039 def feature_cb(self, msg): 00040 with self.lock: 00041 if len(msg.image_points) > 0: 00042 self.feature = msg.image_points[0] 00043 00044 00045 def main(): 00046 rospy.init_node('filter_intervals') 00047 f = FilterIntervals() 00048 rospy.spin() 00049 00050 00051 00052 if __name__ == '__main__': 00053 main()