def __init__(self, topic=None): self.topic = topic self.lock = threading.Lock() self.bridge = CvBridge() self.blobFinder = BlobFinder() self.blobFinder.threshold = 150 self.blobFinder.filter_by_area = True self.blobFinder.min_area = 0 self.blobFinder.max_area = 200 self.topic_type = mct_introspection.get_topic_type(topic) rospy.init_node('blob_finder') self.ready = False if self.topic_type == 'sensor_msgs/Image': self.image_sub = rospy.Subscriber(self.topic, Image, self.image_callback) else: self.image_sub = rospy.Subscriber(self.topic, SeqAndImage, self.seq_and_image_callback) self.image_pub = rospy.Publisher('image_blobs', Image) self.blob_data_pub = rospy.Publisher('blob_data', BlobData) node_name = rospy.get_name() self.set_param_srv = rospy.Service('{0}/set_param'.format(node_name), BlobFinderSetParam, self.handle_set_param_srv) self.get_param_srv = rospy.Service('{0}/get_param'.format(node_name), BlobFinderGetParam, self.handle_get_param_srv) self.ready = True
def __init__(self, topic, frame_rate=30.0): self.topic = topic self.frame_rate = frame_rate self.start_t = 0.0 self.current_t = 0.0 self.progress_t = 0.0 self.frame_count = 0 self.recording_message = 'stopped' self.writer = None self.done = True self.cv_image_size = None self.filename = os.path.join(os.environ['HOME'], 'default.avi') self.lock = threading.Lock() self.bridge = CvBridge() rospy.init_node('avi_writer') self.node_name = rospy.get_name() # Set up publications self.progress_msg = RecordingProgressMsg() self.progress_pub = rospy.Publisher('progress', RecordingProgressMsg) # Subscribe to messages self.image_sub = rospy.Subscriber(self.topic, Image, self.image_handler) # Set up services self.recording_srv = rospy.Service( '{0}/recording_cmd'.format(self.node_name), RecordingCmd, self.handle_recording_cmd)
def __init__(self,topic=None): self.topic = topic self.lock = threading.Lock() self.bridge = CvBridge() rospy.init_node('zoom_tool') node_name = rospy.get_name() self.blobFinder = BlobFinder() self.blobFinder.threshold = rospy.get_param('/zoom_tool_params/threshold', 200) self.blobFinder.filter_by_area = rospy.get_param('/zoom_tool_params/filter_by_area', False) self.blobFinder.min_area = rospy.get_param('/zoom_tool_params/min_area', 0) self.blobFinder.max_area = rospy.get_param('/zoom_tool_params/max_area', 200) self.circle_color = (0,0,255) self.text_color = (0,0,255) self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.8, 0.8,thickness=1) self.image_sub = rospy.Subscriber(self.topic,Image,self.image_callback) self.image_pub = rospy.Publisher('image_zoom_tool', Image) #self.devel_pub = rospy.Publisher('develop', Float32) self.set_param_srv = rospy.Service( '{0}/set_param'.format(node_name), BlobFinderSetParam, self.handle_set_param_srv ) self.get_param_srv = rospy.Service( '{0}/get_param'.format(node_name), BlobFinderGetParam, self.handle_get_param_srv )
def __init__(self, topic_name): self.bridge = CvBridge() def message_extractor(ros_img): try: cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8') return cv_image, ros_img except CvBridgeError, e: return None
def __init__(self): self.bridge = CvBridge() file_dir, file_name = os.path.split(__file__) font_path = '%s/../fonts/FreeMono.ttf'%(file_dir,) self.font = PILImageFont.truetype(font_path, 20) self.fill = (0,0,0) rospy.init_node('progress_info') self.pub = rospy.Publisher('image_progress_message', Image) self.sub = rospy.Subscriber('progress',ProgressMsg, self.handle_progress_msg)
def __init__(self, frame_rate, max_allowed_error=10.0e-3, max_seq_age=200): self.frame_rate = frame_rate self.lock = threading.Lock() self.max_allowed_error = max_allowed_error self.max_seq_age = max_seq_age self.seq_to_camera_stamps = {} self.stamp_last = {} self.max_error_by_camera = {} self.max_error = 0.0 self.camera_fail = '' self.most_recent_seq = None self.last_checked_seq = None self.ok = True self.seq_fail = 0 self.error_msg = '' self.bridge = CvBridge() self.info_image_size = (400, 90) self.font = PILImageFont.truetype( "/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16) self.ready = False rospy.init_node('time_stamp_watchdog') # Subscribe to camera info topics self.wait_for_camera_info_topics() camera_info_topics = mct_introspection.find_camera_info_topics() self.camera_info_sub = {} for topic in camera_info_topics: camera = get_camera_from_topic(topic) handler = functools.partial(self.camera_info_handler, camera) self.camera_info_sub[camera] = rospy.Subscriber( topic, CameraInfo, handler) self.number_of_cameras = len(camera_info_topics) # Create reset service self.reset_srv = rospy.Service('time_stamp_watchdog_reset', Empty, self.reset_handler) # Create time stamp watchdog publication self.watchdog_pub = rospy.Publisher('time_stamp_watchdog', TimeStampWatchDog) # Create watchdog info image self.image_watchdog_pub = rospy.Publisher('image_time_stamp_watchdog', Image) self.ready = True
class ROSStereoListener: def __init__(self, topics, rate=30.0, name='stereo_listener'): self.listener = ru.GenericListener(name, [Image, Image], topics, rate) self.lbridge = CvBridge() self.rbridge = CvBridge() def next(self): lros, rros = self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True) lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8')) rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8')) return lcv, rcv
def __init__(self): self.bridge = CvBridge() self.font = PILImageFont.truetype("/usr/share/fonts/truetype/freefont/FreeMono.ttf", 20) self.fill = (0,00,0) self.bg_color = (255,255,255) #self.bg_color = (200,200,255) self.image_size = (105,28) self.text_pos = (0,4) rospy.init_node('fps_info_image') self.pub = rospy.Publisher('image_fps', Image) self.sub = rospy.Subscriber('fps_info',FPSInfoMsg, self.handle_fps_msg)
def find_image_features(bagname, topic): features_list = [] bridge = CvBridge() i = 0 for topic, msg, t in ru.bag_iter(bagname, [topic]): t = msg.header.stamp.to_time() image = bridge.imgmsg_to_cv(msg, 'bgr8') image_gray = fea.grayscale(image) surf_keypoints, surf_descriptors = fea.surf(image_gray) features_list.append((t, (surf_keypoints, surf_descriptors))) rospy.loginfo("%.3f frame %d found %d points" % (t, i, len(surf_keypoints))) i = i + 1 return features_list
def __init__(self, threshold=200, filter_by_area=False, min_area=0, max_area=200): self.threshold = threshold self.filter_by_area = filter_by_area self.min_area = min_area self.max_area = max_area self.bridge = CvBridge() #self.blob_mask = cvblob.CV_BLOB_RENDER_CENTROID #self.blob_mask |= cvblob.CV_BLOB_RENDER_COLOR self.blobs_image = None self.label_image = None self.thresh_image = None
class FPS_Image(object): def __init__(self): self.bridge = CvBridge() self.font = PILImageFont.truetype("/usr/share/fonts/truetype/freefont/FreeMono.ttf", 20) self.fill = (0,00,0) self.bg_color = (255,255,255) #self.bg_color = (200,200,255) self.image_size = (105,28) self.text_pos = (0,4) rospy.init_node('fps_info_image') self.pub = rospy.Publisher('image_fps', Image) self.sub = rospy.Subscriber('fps_info',FPSInfoMsg, self.handle_fps_msg) def handle_fps_msg(self,data): # Extract keyword arguments fps = data.rate # Create PIL image and write text to it pil_image = PILImage.new('RGB',self.image_size,self.bg_color) draw = PILImageDraw.Draw(pil_image) fps_text = 'fps %2.1f'%(fps,) draw.text(self.text_pos,fps_text,font=self.font,fill=self.fill) # Convert to opencv image, then to ROS image and publish cv_image = cv.CreateImageHeader(pil_image.size, cv.IPL_DEPTH_8U, 3) cv.SetData(cv_image, pil_image.tostring()) rosimage = self.bridge.cv_to_imgmsg(cv_image,'rgb8') self.pub.publish(rosimage) def run(self): rospy.spin()
def __init__(self,topic,frame_rate=30.0): self.topic = topic self.frame_rate = frame_rate self.start_t = 0.0 self.current_t = 0.0 self.progress_t = 0.0 self.frame_count = 0 self.recording_message = 'stopped' self.writer = None self.done = True self.cv_image_size = None self.filename = os.path.join(os.environ['HOME'],'default.avi') self.lock = threading.Lock() self.bridge = CvBridge() rospy.init_node('avi_writer') self.node_name = rospy.get_name() # Set up publications self.progress_msg = RecordingProgressMsg() self.progress_pub = rospy.Publisher('progress',RecordingProgressMsg) # Subscribe to messages self.image_sub = rospy.Subscriber(self.topic,Image,self.image_handler) # Set up services self.recording_srv = rospy.Service( '{0}/recording_cmd'.format(self.node_name), RecordingCmd, self.handle_recording_cmd )
def __init__(self): self.image_shape = (15, 640, 3) self.empty_color = (230, 230, 230) self.fill_color = (0, 0, 200) self.base_array = 255 * numpy.ones(self.image_shape, dtype=numpy.uint8) for i in range(0, 3): self.base_array[:, :, i] = self.empty_color[i] self.bridge = CvBridge() rospy.init_node('progress_bar') # Pulications self.pub = rospy.Publisher('image_progress_bar', Image) # Subscriptions self.sub = rospy.Subscriber('progress', ProgressMsg, self.handle_progress_msg)
class Progress_Bar(object): def __init__(self): self.image_shape = (15, 640, 3) self.empty_color = (230, 230, 230) self.fill_color = (0, 0, 200) self.base_array = 255 * numpy.ones(self.image_shape, dtype=numpy.uint8) for i in range(0, 3): self.base_array[:, :, i] = self.empty_color[i] self.bridge = CvBridge() rospy.init_node('progress_bar') # Pulications self.pub = rospy.Publisher('image_progress_bar', Image) # Subscriptions self.sub = rospy.Subscriber('progress', ProgressMsg, self.handle_progress_msg) def handle_progress_msg(self, data): frame_count = data.frame_count progress_t = data.progress_t record_t = data.record_t image_array = numpy.array(self.base_array) if record_t > 0: fill_ind = int(self.image_shape[1] * progress_t / record_t) else: fill_ind = self.image_shape[1] for i in range(0, 3): image_array[:, :fill_ind, i] = self.fill_color[i] cv_image = cv.fromarray(image_array) rosimage = self.bridge.cv_to_imgmsg(cv_image, 'rgb8') self.pub.publish(rosimage) def run(self): rospy.spin()
def find_contact_images(bag_name, contact_times, all_times, topic_name): print 'finding closest images for ', topic_name times_tree = sp.KDTree(np.matrix(all_times).T) closest_times = [all_times[times_tree.query([a_time])[1]] for a_time in contact_times] pdb.set_trace() print 'getting & saving images, expecting', len(set(closest_times)), 'images' bridge = CvBridge() cleaned_topic_name = topic_name.replace('/', '') i = 0 for ros_msg in get_closest_msgs(bag_name, [topic_name], closest_times): i = i + 1 msg_time = ros_msg.header.stamp.to_time() - all_times[0] cv_image = bridge.imgmsg_to_cv(ros_msg, 'bgr8') img_name = "%s_%.3f_touched.png" % (cleaned_topic_name, msg_time) print 'writing', img_name cv.SaveImage(img_name, cv_image) print 'got', i, 'images'
def __init__(self, image_topic, filename): self.image_topic = image_topic self.filename = filename self.frame_count = 0 self.recording = False self.writer = None self.cv_image_size = None self.framerate = None self.last_stamp = None self.lock = threading.Lock() self.bridge = CvBridge() rospy.init_node('avi_writer') rospy.on_shutdown(self.stop_video_writer) # Subscribe to image topic self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_handler)
def __init__(self, topic): # Video recording parameters self.topic = topic self.record_t = 10.0 self.start_t = 0.0 self.current_t = 0.0 self.progress_t = 0.0 self.frame_count = 0 self.recording_message = 'stopped' self.frame_rate = 24.0 # This is a kludge - get from image topic self.writer = None self.done = True self.cv_image_size = None self.filename = os.path.join(os.environ['HOME'], 'default.avi') # Current pulse parameters #self.pulse_channel = 'a' self.pulse_channel = 'c' # Should probalby move this to a configuration file self.pulse_controller = None self.timing_fid = None self.lock = threading.Lock() self.redis_db = redis.Redis('localhost', db=lia_config.redis_db) self.bridge = CvBridge() rospy.init_node('avi_writer') # Set up publications self.progress_msg = ProgressMsg() self.progress_pub = rospy.Publisher('progress', ProgressMsg) # Subscribe to messages self.image_sub = rospy.Subscriber(self.topic, Image, self.image_handler) # Set up services self.recording_srv = rospy.Service('recording_cmd', RecordingCmd, self.handle_recording_cmd) # Set up proxy for set current service rospy.wait_for_service('set_current') self.set_current_proxy = rospy.ServiceProxy('set_current', SetCurrentCmd)
class ROSImageClient: def __init__(self, topic_name): self.bridge = CvBridge() def message_extractor(ros_img): try: cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8') return cv_image, ros_img except CvBridgeError, e: return None self.listener = ru.GenericListener('ROSImageClient', Image, topic_name, .1, message_extractor)
def find_contact_images(bag_name, contact_times, all_times, topic_name): print 'finding closest images for ', topic_name times_tree = sp.KDTree(np.matrix(all_times).T) closest_times = [ all_times[times_tree.query([a_time])[1]] for a_time in contact_times ] pdb.set_trace() print 'getting & saving images, expecting', len( set(closest_times)), 'images' bridge = CvBridge() cleaned_topic_name = topic_name.replace('/', '') i = 0 for ros_msg in get_closest_msgs(bag_name, [topic_name], closest_times): i = i + 1 msg_time = ros_msg.header.stamp.to_time() - all_times[0] cv_image = bridge.imgmsg_to_cv(ros_msg, 'bgr8') img_name = "%s_%.3f_touched.png" % (cleaned_topic_name, msg_time) print 'writing', img_name cv.SaveImage(img_name, cv_image) print 'got', i, 'images'
def __init__(self, surf_name, contacts_name): forearm_cam_l = '/l_forearm_cam/image_rect_color' self.bridge = CvBridge() rospy.loginfo('loading %s' % surf_name) self.surf_data = ut.load_pickle(surf_name) rospy.loginfo('loading %s' % contacts_name) self.scene, self.contact_points = ut.load_pickle(contacts_name) self.surf_idx = None cv.NamedWindow('surf', 1) self.tflistener = tf.TransformListener() self.camera_geo = ROSCameraCalibration('/l_forearm_cam/camera_info') self.lmat0 = None self.rmat0 = None self.contact = False self.contact_stopped = False #rospy.Subscriber('/pressure/l_gripper_motor', pm.PressureState, self.lpress_cb) rospy.Subscriber(forearm_cam_l, sm.Image, self.image_cb) print 'ready'
def __init__(self, topic=None): self.topic = topic self.lock = threading.Lock() self.bridge = CvBridge() self.camera = get_camera_from_topic(self.topic) camera_calibration = file_tools.read_camera_calibration(self.camera) self.camera_matrix = get_camera_matrix(camera_calibration) # Blob finder parameters self.blobFinder = BlobFinder() self.blobFinder.threshold = 150 self.blobFinder.filter_by_area = False self.blobFinder.min_area = 0 self.blobFinder.max_area = 200 # Tracking parameters self.tracking_pts_dist = (0.0, 0.04774, 0.07019) self.tracking_pts_colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255)] rospy.init_node('blob_finder') self.ready = False self.image_sub = rospy.Subscriber(self.topic, Image, self.image_callback) self.image_tracking_pts_pub = rospy.Publisher('image_tracking_pts', Image) self.image_blobs_pub = rospy.Publisher('image_blobs', Image) self.blob_data_pub = rospy.Publisher('blob_data', BlobData) self.devel_pub = rospy.Publisher('devel_data', Float32) node_name = rospy.get_name() self.set_param_srv = rospy.Service('{0}/set_param'.format(node_name), BlobFinderSetParam, self.handle_set_param_srv) self.get_param_srv = rospy.Service('{0}/get_param'.format(node_name), BlobFinderGetParam, self.handle_get_param_srv) self.ready = True
def __init__(self, max_seq_age=150): rospy.init_node('frame_drop_watchdog') self.max_seq_age = max_seq_age self.lock = threading.Lock() self.frames_dropped = {} self.latest_seq = None self.ready = False camera_assignment = file_tools.read_camera_assignment() self.number_of_cameras = len(camera_assignment) self.bridge = CvBridge() self.info_image_size = (400, 90) self.font = PILImageFont.truetype( "/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16) # Subscribe to camera info topics self.frames_dropped_sub = {} frames_dropped_topics = self.wait_for_topics() for topic in frames_dropped_topics: camera = get_camera_from_topic(topic) handler = functools.partial(self.frames_dropped_handler, camera) self.frames_dropped_sub[camera] = rospy.Subscriber( topic, FramesDropped, handler) # Setup total frames dropped service self.total_dropped_pub = rospy.Publisher('total_frames_dropped', FramesDropped) # Setup reset service self.reset_srv = rospy.Service('frame_drop_watchdog_reset', Empty, self.reset_handler) # Create watchdog info image self.image_watchdog_pub = rospy.Publisher('image_frame_drop_watchdog', Image) self.ready = True
def __init__(self, stitched_image_topic, tracking_pts_topic, max_seq_age=150): self.lock = threading.Lock() self.bridge = CvBridge() self.stitching_params = file_tools.read_tracking_2d_stitching_params() self.stitched_image_topic = stitched_image_topic self.tracking_pts_topic = tracking_pts_topic self.max_seq_age = max_seq_age self.latest_seq = None self.seq_to_stitched_image = {} self.seq_to_tracking_pts = {} self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.8, 0.8, thickness=1) self.magenta = (255, 255, 0) self.tracking_pts_colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255)] self.labeled_image = None self.ready = False rospy.init_node('stitched_image_labeler') # Subscribe to stitched image and tracking pts topics self.image_sub = rospy.Subscriber(self.stitched_image_topic, SeqAndImage, self.stitched_image_handler) self.tracking_pts_sub = rospy.Subscriber(self.tracking_pts_topic, ThreePointTracker, self.tracking_pts_handler) # Create labeled image publication self.labeled_image_pub = rospy.Publisher('image_stitched_labeled', Image) self.ready = True
def __init__(self, *args, **kwargs): self.lock = threading.Lock() self.calibrate = False super(MCT_CalibrationNode, self).__init__(*args, **kwargs) self.bridge = CvBridge() self.cal_img_pub = rospy.Publisher('image_calibrator', sensor_msgs.msg.Image) self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2) self.font_color_red = cv.CV_RGB(255, 0, 0) self.font_color_green = cv.CV_RGB(0, 255, 0) self.font_color_blue = cv.CV_RGB(80, 80, 255) self.x_start = 10 self.y_start = 30 self.x_step = 140 self.y_step = 32 # Set up sevices node_name = rospy.get_name() self.good_enough_srv = rospy.Service( '{0}/good_enough'.format(node_name), GetBool, self.handle_good_enough_srv, ) self.calibrate_srv = rospy.Service( '{0}/calibrate'.format(node_name), GetBool, self.handle_calibrate_srv, ) self.get_calibration_srv = rospy.Service( '{0}/get_calibration'.format(node_name), GetString, self.handle_get_calibration_srv, )
def __init__(self,topic): self.topic = topic self.record_t = 10.0 self.start_t = 0.0 self.current_t = 0.0 self.progress_t = 0.0 self.frame_count = 0 self.recording_message = 'stopped' self.frame_rate = 24.0 # This is a kludge - get from image topic self.writer = None self.done = True self.cv_image_size = None self.filename = os.path.join(os.environ['HOME'],'default.avi') #self.progress_message = Progress_Message() self.lock = threading.Lock() self.redis_db = redis.Redis('localhost', db=lia_config.redis_db) self.bridge = CvBridge() rospy.init_node('avi_writer') # Set up publications self.progress_msg = ProgressMsg() self.progress_pub = rospy.Publisher('progress',ProgressMsg) # Subscribe to messages self.image_sub = rospy.Subscriber(self.topic,Image,self.image_handler) # Set up services self.recording_srv = rospy.Service( 'recording_cmd', RecordingCmd, self.handle_recording_cmd )
class Progress_Info(object): def __init__(self): self.bridge = CvBridge() file_dir, file_name = os.path.split(__file__) font_path = '%s/../fonts/FreeMono.ttf'%(file_dir,) self.font = PILImageFont.truetype(font_path, 20) self.fill = (0,0,0) rospy.init_node('progress_info') self.pub = rospy.Publisher('image_progress_message', Image) self.sub = rospy.Subscriber('progress',ProgressMsg, self.handle_progress_msg) def handle_progress_msg(self,data): # Extract keyword arguments message = data.recording_message frame_count = data.frame_count record_t = data.record_t progress_t = data.progress_t # Get minutes and seconds record_hr, record_min, record_sec = get_hr_min_sec(record_t) progress_hr, progress_min, progress_sec = get_hr_min_sec(progress_t) # Create PIL image and write text to it pil_image = PILImage.new('RGB',(640,30),(255,255,255)) draw = PILImageDraw.Draw(pil_image) message_text = '%s,'%(message,) draw.text((0,10),message_text,font=self.font,fill=self.fill) frame_text = 'frame %d'%(frame_count,) draw.text((160,10),frame_text,font=self.font,fill=self.fill) time_data = (progress_hr,progress_min,progress_sec,record_hr,record_min,record_sec) time_text = '%02d:%02d:%02d/%02d:%02d:%02d'%time_data draw.text((430,10),time_text,font=self.font,fill=self.fill) # Convert to opencv image, then to ROS image and publish cv_image = cv.CreateImageHeader(pil_image.size, cv.IPL_DEPTH_8U, 3) cv.SetData(cv_image, pil_image.tostring()) rosimage = self.bridge.cv_to_imgmsg(cv_image,'rgb8') self.pub.publish(rosimage) def run(self): rospy.spin()
class ImageProcess: def __init__(self, surf_name, contacts_name): forearm_cam_l = '/l_forearm_cam/image_rect_color' self.bridge = CvBridge() rospy.loginfo('loading %s' % surf_name) self.surf_data = ut.load_pickle(surf_name) rospy.loginfo('loading %s' % contacts_name) self.scene, self.contact_points = ut.load_pickle(contacts_name) self.surf_idx = None cv.NamedWindow('surf', 1) self.tflistener = tf.TransformListener() self.camera_geo = ROSCameraCalibration('/l_forearm_cam/camera_info') self.lmat0 = None self.rmat0 = None self.contact = False self.contact_stopped = False #rospy.Subscriber('/pressure/l_gripper_motor', pm.PressureState, self.lpress_cb) rospy.Subscriber(forearm_cam_l, sm.Image, self.image_cb) print 'ready' #def lpress_cb(self, msg): #def lpress_cb(self, pmsg): # #conv to mat # lmat = np.matrix((pmsg.l_finger_tip)).T # rmat = np.matrix((pmsg.r_finger_tip)).T # if self.lmat0 == None: # self.lmat0 = lmat # self.rmat0 = rmat # return # #zero # lmat = lmat - self.lmat0 # rmat = rmat - self.rmat0 # #touch detected # if np.any(np.abs(lmat) > 250) or np.any(np.abs(rmat) > 250): #TODO: replace this with something more sound # self.contact = True # else: # if self.contact == True: # self.contact_stopped = True # self.contact = False # #Contact has been made!! look up gripper tip location # #to_frame = 'base_link' # #def frame_loc(from_frame): # # p_base = tfu.transform('base_footprint', from_frame, self.tflistener) \ # # * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0))) # # #* tfu.tf_as_matrix((p.A1.tolist(), tr.quaternion_from_euler(0,0,0))) # # return tfu.matrix_as_tf(p_base) # #tip_locs = [frame_loc(n)[0] for n in self.ftip_frames] # #t = pmsg.header.stamp.to_time() # #rospy.loginfo("contact detected at %.3f" % t) # #self.contact_locs.append([t, tip_locs]) # #self.last_msg = time.time() # rospy.loginfo('lpress_cb ' + str(np.max(rmat)) + ' ' + str(np.max(lmat))) def image_cb(self, msg): image_time = msg.header.stamp.to_time() image = self.bridge.imgmsg_to_cv(msg, 'bgr8') if self.surf_idx == None: for i, d in enumerate(self.surf_data): t, sdata = d if image_time == t: self.surf_idx = i break else: self.surf_idx = self.surf_idx + 1 stime, sdata = self.surf_data[self.surf_idx] if stime != image_time: print 'surf time != image_time' surf_keypoints, surf_descriptors = sdata nimage = fea.draw_surf(image, surf_keypoints, (255,0,0)) cv.ShowImage('surf', nimage) cv.WaitKey(10) # Track and give 3D location to features. ## Project 3D points into this frame (need access to tf => must do online or from cache) ## camera_T_3dframe at ti scene2d = self.camera_geo.project(self.scene, self.tflistener, 'base_footprint') scene2d_tree = sp.KDTree(np.array(scene2d.T)) ## Find features close to these 2d points for loc, lap, size, d, hess in surf_keypoints: idx = scene2d_tree.query(np.array(loc))[1] orig3d = self.scene[:, idx] ## Get features closest to the contact point ## stop running if contact has stopped if self.contact:
dest='headless', help='headless mode') opt, args = p.parse_args() cameras = [ opt.cam + '/left/image_rect_color', opt.cam + '/right/image_rect_color' ] stereo_listener = rc.ROSStereoListener(cameras) if not opt.headless: #cv.NamedWindow('left', 0) #cv.NamedWindow('right', 0) cv.NamedWindow('stereo-anaglyph', 0) cv.ResizeWindow('stereo-anaglyph', 640, 480) cv.WaitKey(10) else: bridge = CvBridge() image_pub = rospy.Publisher('stereo_anaglyph', Image) anaglyph_cyan_image_distance_correction = rospy.get_param( 'anaglyph_dist', opt.dist) left = 1113937 # 65361 right = 1113939 #65363 escape = 1048603 #27 while not rospy.is_shutdown(): l, r = stereo_listener.next() red_blue = anaglyph(l, r, anaglyph_cyan_image_distance_correction) if not opt.headless: #cv.ShowImage('left', l) #cv.ShowImage('right', r) cv.ShowImage('stereo-anaglyph', red_blue)
class FrameDropWatchdog(object): """ Frame drop watchdog monitors the number of frames dropped by the system. """ def __init__(self, max_seq_age=150): rospy.init_node('frame_drop_watchdog') self.max_seq_age = max_seq_age self.lock = threading.Lock() self.frames_dropped = {} self.latest_seq = None self.ready = False camera_assignment = file_tools.read_camera_assignment() self.number_of_cameras = len(camera_assignment) self.bridge = CvBridge() self.info_image_size = (400, 90) self.font = PILImageFont.truetype( "/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16) # Subscribe to camera info topics self.frames_dropped_sub = {} frames_dropped_topics = self.wait_for_topics() for topic in frames_dropped_topics: camera = get_camera_from_topic(topic) handler = functools.partial(self.frames_dropped_handler, camera) self.frames_dropped_sub[camera] = rospy.Subscriber( topic, FramesDropped, handler) # Setup total frames dropped service self.total_dropped_pub = rospy.Publisher('total_frames_dropped', FramesDropped) # Setup reset service self.reset_srv = rospy.Service('frame_drop_watchdog_reset', Empty, self.reset_handler) # Create watchdog info image self.image_watchdog_pub = rospy.Publisher('image_frame_drop_watchdog', Image) self.ready = True def wait_for_topics(self): """ Wait for the frames_dropped topics to be published. """ while 1: frames_dropped_topics = mct_introspection.find_topics_w_ending( 'frames_dropped') if len(frames_dropped_topics) == self.number_of_cameras: break rospy.sleep(0.25) return frames_dropped_topics def reset_handler(self, req): """ Handler for the nodes reset service - empties the frames_dropped buffer. """ with self.lock: self.frames_dropped = {} self.latest_seq = None return EmptyResponse() def frames_dropped_handler(self, camera, data): if not self.ready: return with self.lock: try: self.frames_dropped[data.seq][camera] = data.frames_dropped except KeyError: self.frames_dropped[data.seq] = {camera: data.frames_dropped} self.update_latest_seq(data.seq) def update_latest_seq(self, seq): if self.latest_seq is None: self.latest_seq = seq else: self.latest_seq = max([seq, self.latest_seq]) def publish_watchdog_image(self, seq, total_frames_dropped, cameras_w_drops): """ Publish image for GUI w/ seq #, total frames dropped, other info? """ pil_info_image = PILImage.new('RGB', self.info_image_size, (255, 255, 255)) draw = PILImageDraw.Draw(pil_info_image) info_items = [ ('seq', seq), ('dropped', total_frames_dropped), ('cameras', cameras_w_drops), ] text_x, text_y, step_y = 10, 10, 20 for i, item in enumerate(info_items): label, value = item label_text = '{0}:'.format(label) if type(value) == float: value_text = '{0:<1.6f}'.format(value) else: value_text = '{0}'.format(value) draw.text((text_x, text_y + step_y * i), label_text, font=self.font, fill=(0, 0, 0)) draw.text((text_x + 100, text_y + step_y * i), value_text, font=self.font, fill=(0, 0, 0)) cv_info_image = cv.CreateImageHeader(pil_info_image.size, cv.IPL_DEPTH_8U, 3) cv.SetData(cv_info_image, pil_info_image.tostring()) # Convert to a rosimage and publish info_rosimage = self.bridge.cv_to_imgmsg(cv_info_image, 'rgb8') self.image_watchdog_pub.publish(info_rosimage) def run(self): """ Node, main loop. While the node """ while not rospy.is_shutdown(): with self.lock: for seq, data in sorted(self.frames_dropped.items()): if len(data) == self.number_of_cameras: total_frames_dropped = sum(data.values()) self.total_dropped_pub.publish(seq, total_frames_dropped) cameras_w_drops = [ c for c, n in data.iteritems() if n > 0 ] cameras_w_drops = [ int(c.split('_')[1]) for c in cameras_w_drops ] del self.frames_dropped[seq] self.publish_watchdog_image(seq, total_frames_dropped, cameras_w_drops) else: if self.latest_seq - seq > self.max_seq_age: del self.frames_dropped[seq]
class MCT_CalibrationNode(CalibrationNode): def __init__(self, *args, **kwargs): self.lock = threading.Lock() self.calibrate = False super(MCT_CalibrationNode, self).__init__(*args, **kwargs) self.bridge = CvBridge() self.cal_img_pub = rospy.Publisher('image_calibrator', sensor_msgs.msg.Image) self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2) self.font_color_red = cv.CV_RGB(255, 0, 0) self.font_color_green = cv.CV_RGB(0, 255, 0) self.font_color_blue = cv.CV_RGB(80, 80, 255) self.x_start = 10 self.y_start = 30 self.x_step = 140 self.y_step = 32 # Set up sevices node_name = rospy.get_name() self.good_enough_srv = rospy.Service( '{0}/good_enough'.format(node_name), GetBool, self.handle_good_enough_srv, ) self.calibrate_srv = rospy.Service( '{0}/calibrate'.format(node_name), GetBool, self.handle_calibrate_srv, ) self.get_calibration_srv = rospy.Service( '{0}/get_calibration'.format(node_name), GetString, self.handle_get_calibration_srv, ) def handle_good_enough_srv(self, req): """ Handles requests for the calibrators good_enough flag which indicates whether or not sufficient data has been aquired for calibrating the camera. """ with self.lock: good_enough = self.c.goodenough return GetBoolResponse(good_enough) def handle_calibrate_srv(self, req): """ Handles requests for calibrating the cameras associated with the calibrator. """ with self.lock: if self.c.goodenough: self.calibrate = True flag = True else: flag = False return GetBoolResponse(flag) def handle_get_calibration_srv(self, req): """ Handles requests for the camera calibration parameters. """ with self.lock: if self.c.calibrated: ost_txt = self.c.ost() else: ost_txt = '' if ost_txt: # Add actuall camera name to calibration camera_topic = rospy.remap_name('camera') camera_name = camera_topic.split('/')[2] ost_txt = ost_txt.replace('narrow_stereo/left', camera_name) return GetStringResponse(ost_txt) def redraw_monocular(self, drawable): """ Redraw calibratoin image callback. """ if not self.c.calibrated: with self.lock: if self.calibrate and self.c.goodenough: self.c.do_calibration() if self.c.goodenough: text_data = [('Qty', '{0}'.format(len(self.c.db))), ('Good Enough', )] self.add_progress_text(drawable.scrib, text_data, self.font_color_green) else: if drawable.params: text_data = [('Qty', '{0}'.format(len(self.c.db)))] + list( drawable.params) self.add_progress_text(drawable.scrib, text_data, self.font_color_red) else: text_data = [('No Data', )] self.add_progress_text(drawable.scrib, text_data, self.font_color_red) else: text_data = [('Calibrated', )] self.add_progress_text(drawable.scrib, text_data, self.font_color_green) #print self.c.ost() rosimage = self.bridge.cv_to_imgmsg(drawable.scrib, 'bgr8') self.cal_img_pub.publish(rosimage) def add_progress_text(self, img, text_data, font_color): """ Adds progress text to calibration image, img. The text to be added consists of a list of tuples where each tuple is a row of text to be added. """ for i, values in enumerate(text_data): for j, item in enumerate(values): if type(item) == float: msg = '{0:1.2f}'.format(item) else: msg = '{0}'.format(item) pos = (self.x_start + j * self.x_step, self.y_start + i * self.y_step) cv.PutText(img, msg, pos, self.font, font_color)
class AVI_Writer(object): def __init__(self,topic,frame_rate=30.0): self.topic = topic self.frame_rate = frame_rate self.start_t = 0.0 self.current_t = 0.0 self.progress_t = 0.0 self.frame_count = 0 self.recording_message = 'stopped' self.writer = None self.done = True self.cv_image_size = None self.filename = os.path.join(os.environ['HOME'],'default.avi') self.lock = threading.Lock() self.bridge = CvBridge() rospy.init_node('avi_writer') self.node_name = rospy.get_name() # Set up publications self.progress_msg = RecordingProgressMsg() self.progress_pub = rospy.Publisher('progress',RecordingProgressMsg) # Subscribe to messages self.image_sub = rospy.Subscriber(self.topic,Image,self.image_handler) # Set up services self.recording_srv = rospy.Service( '{0}/recording_cmd'.format(self.node_name), RecordingCmd, self.handle_recording_cmd ) def run(self): rospy.spin() def handle_recording_cmd(self,req): """ Handles avi recording commands - starts and stops avi recording. """ with self.lock: if self.cv_image_size is None: # If we don't have and image yet - we can't get started, return fail. return RecordingCmdResponse(False) self.filename = req.filename self.frame_rate = req.frame_rate command = req.command.lower() if command == 'start': # Get start time and create video writer self.writer = cv.CreateVideoWriter( self.filename, cv.CV_FOURCC('D','I','V','X'), self.frame_rate, self.cv_image_size, ) if self.writer is None: response = False else: response = True self.start_t = rospy.get_time() self.frame_count = 0 self.done = False self.recording_message = 'recording' elif command == 'stop': self.done = True del self.writer self.writer = None self.recording_message = 'stopped' response = True return RecordingCmdResponse(response) def image_handler(self,data): """ Writes frames to avi file. """ self.current_t = rospy.get_time() # Convert to opencv image and then to ipl_image cv_image = self.bridge.imgmsg_to_cv(data,'bgr8') ipl_image = cv.GetImage(cv_image) with self.lock: if self.cv_image_size == None: self.cv_image_size = cv.GetSize(cv_image) if not self.done: # Write video frame cv.WriteFrame(self.writer,ipl_image) # Update times and frame count - these are used elsewhere so we # need the lock with self.lock: self.frame_count += 1 self.progress_t = self.current_t - self.start_t # Publish progress message with self.lock: self.progress_msg.frame_count = self.frame_count self.progress_msg.progress_t = self.progress_t self.progress_msg.recording_message = self.recording_message self.progress_pub.publish(self.progress_msg)
print 'This utility is used for publishing' print 'OpenCV accessible camera images over' print 'ROS.\n' print 'Usage ./ros_camera OPENCV_ID' print 'where OPENCV_ID is a number >= 0' print 'representing opencv\'s index for ' print 'the particular device' camera_id = int(sys.argv[1]) topic_name = 'cvcamera' + str(camera_id) image_pub = rospy.Publisher(topic_name, Image) rospy.init_node('cvcamera', anonymous=True) capture = cv.CaptureFromCAM(camera_id) bridge = CvBridge() print 'Opening OpenCV camera with ID', camera_id print 'Publishing on topic', topic_name while not rospy.is_shutdown(): try: cv_image = cv.CloneImage(cv.QueryFrame(capture)) rosimage = bridge.cv_to_imgmsg(cv_image, "bgr8") image_pub.publish(rosimage) except rospy.exceptions.ROSSerializationException, e: print 'serialization exception' except CvBridgeError, e: print e break except KeyboardInterrupt: print "Shutting down."
class HomographyCalibratorNode(object): def __init__(self, topic): self.ready = False self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED self.topic = topic self.bridge = CvBridge() self.lock = threading.Lock() rospy.init_node('homography_calibrator') self.node_name = rospy.get_name() # Initialize data lists self.blobs_list = [] self.image_points = [] self.world_points = [] # Set active target information and turn off leds target_info = mct_active_target.active_target_info() self.led_n_max = target_info[0] self.led_m_max = target_info[1] self.number_of_leds = self.led_n_max * self.led_m_max self.led_max_power = target_info[2] self.led_space_x = target_info[3] self.led_space_y = target_info[3] mct_active_target.off() # Current led indices self.led_n = 0 self.led_m = 0 # Led power setting params_namespace = '/homography_calibrator_params' self.led_power = rospy.get_param( '{0}/target/led_power'.format(params_namespace), 10) # Wait count for image acquisition self.image_wait_number = rospy.get_param( '{0}/image_wait_number'.format(params_namespace), 4) self.image_wait_cnt = 0 # Sleep periods for idle and wait count loops self.idle_sleep_dt = 0.25 self.wait_sleep_dt = 0.005 # Initialize blob finder self.blobFinder = BlobFinder() self.blobFinder.threshold = rospy.get_param( '{0}/blob_finder/threshold'.format(params_namespace), 200) self.blobFinder.filter_by_area = rospy.get_param( '{0}/blob_finder/filter_by_area'.format(params_namespace), False) self.blobFinder.min_area = rospy.get_param( '{0}/blob_finder/min_area'.format(params_namespace), 0) self.blobFinder.max_area = rospy.get_param( '{0}/blob_finder/max_area'.format(params_namespace), 200) # Initialize homography matrix and number of points required to solve for it self.homography_matrix = None self.num_points_required = rospy.get_param( '{0}/num_points_required'.format(params_namespace), 10) # Set font and initial image information self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.8, 0.8, thickness=1) self.image_info = 'no data' # Subscription to image topic self.image_sub = rospy.Subscriber(self.topic, Image, self.image_callback) # Publications - blobs image and calibration progress image self.image_blobs_pub = rospy.Publisher('image_homography_blobs', Image) self.image_calib_pub = rospy.Publisher('image_homography_calibration', Image) # Services self.start_srv = rospy.Service('{0}/start'.format(self.node_name), GetFlagAndMessage, self.handle_start_srv) self.get_matrix_srv = rospy.Service( '{0}/get_matrix'.format(self.node_name), GetMatrix, self.handle_get_matrix_srv) self.is_calibrated_srv = rospy.Service( '{0}/is_calibrated'.format(self.node_name), GetBool, self.handle_is_calibrated_srv) self.ready = True def handle_start_srv(self, req): """ Handles to start/restart the homography calibration procedure. """ flag = True message = '' with self.lock: flag, message = mct_active_target.lock(self.node_name) if flag: self.homography_matrix = None self.image_info = '' self.image_points = [] self.world_points = [] self.blobs_list = [] self.state = WORKING self.led_n = 0 self.led_m = 0 return GetFlagAndMessageResponse(flag, message) def handle_get_matrix_srv(self, req): """ Returns the homography matrix. If the homography matrix has not yet be computed the identity matirx is returned. """ if self.homography_matrix is None: data = [1, 0, 0, 0, 1, 0, 0, 0, 1] else: data = self.homography_matrix.reshape((9, )) return GetMatrixResponse(3, 3, data) def handle_is_calibrated_srv(self, req): """ Handles requests for whether or not the homography calibration has been completed. """ if self.homography_matrix is not None: value = True else: value = False return GetBoolResponse(value) def image_callback(self, data): """ Image topic callback function. Uses the blobFinder to find the blobs (leds) in the image. Publishes images showing the blobs and the calibration progress. """ # Find blobs with self.lock: if not self.ready: return if self.state == WORKING: self.blobs_list, blobs_image = self.blobFinder.findBlobs(data) blobs_rosimage = self.bridge.cv_to_imgmsg( blobs_image, encoding="passthrough") else: self.blobs_list = [] blobs_rosimage = data if self.image_wait_cnt < self.image_wait_number: self.image_wait_cnt += 1 self.image_blobs_pub.publish(blobs_rosimage) # Create calibration data image cv_image = self.bridge.imgmsg_to_cv(data, desired_encoding="passthrough") ipl_image = cv.GetImage(cv_image) calib_image = cv.CreateImage(cv.GetSize(ipl_image), cv.IPL_DEPTH_8U, 3) cv.CvtColor(ipl_image, calib_image, cv.CV_GRAY2BGR) # Add calibration markers to image color = STATE_COLOR[self.state] for x, y in self.image_points: cv.Circle(calib_image, (int(x), int(y)), 3, color) ## Add text to image message = [STATE_MESSAGE[self.state]] if self.state == WORKING or self.state == FINISHED: #message.append('{0}/{1} pts'.format(len(self.image_points),self.number_of_leds)) message.append('{0}/{1} pts'.format(len(self.image_points), self.get_led_count())) if self.image_info: message.append('- {0}'.format(self.image_info)) message = ' '.join(message) cv.PutText(calib_image, message, (10, 25), self.cv_text_font, color) # Publish calibration progress image calib_rosimage = self.bridge.cv_to_imgmsg(calib_image, encoding="passthrough") self.image_calib_pub.publish(calib_rosimage) def wait_for_images(self): """ Wait for 'image_wait_number' of images to be acquired. """ with self.lock: self.image_wait_cnt = 0 done = False while not done: with self.lock: image_wait_cnt = self.image_wait_cnt if image_wait_cnt >= self.image_wait_number: done = True rospy.sleep(self.wait_sleep_dt) def increment_led(self): """ Increments the led array indices. When the last led is reached check to see if sufficient points have been captured for the homography calibratoin. If so, then the state is changed to FINISHED. If insufficient points have been gathered then the state is changed back to WAITING. """ if self.led_m < self.led_m_max - 1 or self.led_n < self.led_n_max - 1: if self.led_n < self.led_n_max - 1: self.led_n += 1 else: self.led_n = 0 self.led_m += 1 else: # Turn off led and unlock active target mct_active_target.off() mct_active_target.unlock(self.node_name) # Need to check that we got enough done otherwise return to idle. if len(self.image_points) >= self.num_points_required: self.state = FINISHED self.image_info = '' else: self.state = WAITING self.image_info = 'not enough data' self.homography_matrix = None def get_led_count(self): return self.led_n + self.led_m * self.led_n_max + 1 def run(self): """ Node main function. When the state is WORKING this function activates the leds on the active calibration target one at a time. When all of the leds have been activated and if sufficient number of points have been collected the homography matrix is calculated. """ while not rospy.is_shutdown(): if self.state == WORKING: # Turn on current led and wait for images mct_active_target.set_led(self.led_n, self.led_m, self.led_power) self.wait_for_images() if len(self.blobs_list) == 1: # Get centroid of blob and add to image point list blob = self.blobs_list[0] image_x = blob['centroid_x'] image_y = blob['centroid_y'] self.image_points.append((image_x, image_y)) # Compute coordinates of the led in the world plane world_x = self.led_n * self.led_space_x world_y = self.led_m * self.led_space_y self.world_points.append((world_x, world_y)) self.increment_led() elif self.state == FINISHED and self.homography_matrix is None: # Find the homography transformation image_points = numpy.array(self.image_points) world_points = numpy.array(self.world_points) result = cv2.findHomography(image_points, world_points, cv.CV_RANSAC) self.homography_matrix, mask = result # Compute the mean reprojection error image_points_hg = numpy.ones((image_points.shape[0], 3)) image_points_hg[:, :2] = image_points world_points_hg = numpy.ones((world_points.shape[0], 3)) world_points_hg[:, :2] = world_points homography_matrix_t = self.homography_matrix.transpose() world_points_hg_pred = numpy.dot(image_points_hg, homography_matrix_t) denom = numpy.zeros((world_points.shape[0], 2)) denom[:, 0] = world_points_hg_pred[:, 2] denom[:, 1] = world_points_hg_pred[:, 2] world_points_pred = world_points_hg_pred[:, :2] / denom error = (world_points - world_points_pred)**2 error = error.sum(axis=1) error = numpy.sqrt(error) error = error.mean() self.image_info = 'error {0:1.2f} mm'.format(1e3 * error) else: rospy.sleep(self.idle_sleep_dt)
initial_centers = fmat[:, center_indices] kresults = vq.kmeans(np.array(fmat.T), np.array(initial_centers.T)) return np.matrix(kresults[0]).T if __name__ == '__main__': import sys import pdb features_file = sys.argv[1] images_file = sys.argv[2] features_db = np.column_stack([ut.load_pickle(p[0]) for p in csv_bag_names(features_file)]) features_db_reduced = features_mat_compress(features_db, 500) #Generate a random color for each feature colors = np.matrix(np.random.randint(0, 255, (3, features_db_reduced.shape[1]))) features_tree = sp.KDTree(np.array(features_db_reduced.T)) bridge = CvBridge() forearm_cam_l = '/l_forearm_cam/image_rect_color' cv.NamedWindow('surf', 1) #import pdb #while not rospy.is_shutdown(): i = 0 for topic, msg, t in ru.bag_iter(images_file, [forearm_cam_l]): image = bridge.imgmsg_to_cv(msg, 'bgr8') #image = camera.get_frame() image_gray = fea.grayscale(image) surf_keypoints, surf_descriptors = fea.surf(image_gray) #print len(surf_keypoints) #pdb.set_trace()
p.add_option('-c', action='store', default='/wide_stereo', type='string', dest='cam', help='which camera to listen to') p.add_option('-d', action='store', default=30, type='int', dest='dist', help='separation distance') p.add_option('-s', action='store_true', dest='headless', help='headless mode') opt, args = p.parse_args() cameras = [opt.cam + '/left/image_rect_color', opt.cam + '/right/image_rect_color'] stereo_listener = rc.ROSStereoListener(cameras) if not opt.headless: #cv.NamedWindow('left', 0) #cv.NamedWindow('right', 0) cv.NamedWindow('stereo-anaglyph', 0) cv.ResizeWindow('stereo-anaglyph', 640, 480) cv.WaitKey(10) else: bridge = CvBridge() image_pub = rospy.Publisher('stereo_anaglyph', Image) anaglyph_cyan_image_distance_correction = rospy.get_param('anaglyph_dist', opt.dist) left = 1113937# 65361 right = 1113939#65363 escape = 1048603#27 while not rospy.is_shutdown(): l, r = stereo_listener.next() red_blue = anaglyph(l, r, anaglyph_cyan_image_distance_correction) if not opt.headless: #cv.ShowImage('left', l) #cv.ShowImage('right', r) cv.ShowImage('stereo-anaglyph', red_blue) k = cv.WaitKey(10)
def __init__(self, topic): self.ready = False self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED self.topic = topic self.bridge = CvBridge() self.lock = threading.Lock() rospy.init_node('homography_calibrator') self.node_name = rospy.get_name() # Initialize data lists self.blobs_list = [] self.image_points = [] self.world_points = [] # Set active target information and turn off leds target_info = mct_active_target.active_target_info() self.led_n_max = target_info[0] self.led_m_max = target_info[1] self.number_of_leds = self.led_n_max * self.led_m_max self.led_max_power = target_info[2] self.led_space_x = target_info[3] self.led_space_y = target_info[3] mct_active_target.off() # Current led indices self.led_n = 0 self.led_m = 0 # Led power setting params_namespace = '/homography_calibrator_params' self.led_power = rospy.get_param( '{0}/target/led_power'.format(params_namespace), 10) # Wait count for image acquisition self.image_wait_number = rospy.get_param( '{0}/image_wait_number'.format(params_namespace), 4) self.image_wait_cnt = 0 # Sleep periods for idle and wait count loops self.idle_sleep_dt = 0.25 self.wait_sleep_dt = 0.005 # Initialize blob finder self.blobFinder = BlobFinder() self.blobFinder.threshold = rospy.get_param( '{0}/blob_finder/threshold'.format(params_namespace), 200) self.blobFinder.filter_by_area = rospy.get_param( '{0}/blob_finder/filter_by_area'.format(params_namespace), False) self.blobFinder.min_area = rospy.get_param( '{0}/blob_finder/min_area'.format(params_namespace), 0) self.blobFinder.max_area = rospy.get_param( '{0}/blob_finder/max_area'.format(params_namespace), 200) # Initialize homography matrix and number of points required to solve for it self.homography_matrix = None self.num_points_required = rospy.get_param( '{0}/num_points_required'.format(params_namespace), 10) # Set font and initial image information self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.8, 0.8, thickness=1) self.image_info = 'no data' # Subscription to image topic self.image_sub = rospy.Subscriber(self.topic, Image, self.image_callback) # Publications - blobs image and calibration progress image self.image_blobs_pub = rospy.Publisher('image_homography_blobs', Image) self.image_calib_pub = rospy.Publisher('image_homography_calibration', Image) # Services self.start_srv = rospy.Service('{0}/start'.format(self.node_name), GetFlagAndMessage, self.handle_start_srv) self.get_matrix_srv = rospy.Service( '{0}/get_matrix'.format(self.node_name), GetMatrix, self.handle_get_matrix_srv) self.is_calibrated_srv = rospy.Service( '{0}/is_calibrated'.format(self.node_name), GetBool, self.handle_is_calibrated_srv) self.ready = True
def __init__(self, topics, rate=30.0, name='stereo_listener'): self.listener = ru.GenericListener(name, [Image, Image], topics, rate) self.lbridge = CvBridge() self.rbridge = CvBridge()