Go to the documentation of this file.00001
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()