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()
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()
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 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 ThreePointTracker(object): 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 handle_set_param_srv(self, req): """ Handles requests to set the blob finder's parameters. Currently this is just the threshold used for binarizing the image. """ with self.lock: self.blobFinder.threshold = req.threshold self.blobFinder.filter_by_area = req.filter_by_area self.blobFinder.min_area = req.min_area self.blobFinder.max_area = req.max_area return BlobFinderSetParamResponse(True, '') def handle_get_param_srv(self, req): """ Handles requests for the blob finders parameters """ with self.lock: threshold = self.blobFinder.threshold filter_by_area = self.blobFinder.filter_by_area min_area = self.blobFinder.min_area max_area = self.blobFinder.max_area resp_args = (threshold, filter_by_area, min_area, max_area) return BlobFinderGetParamResponse(*resp_args) def image_callback(self, data): """ Callback for image topic subscription - finds blobs in image. """ if not self.ready: return with self.lock: blobs_list, blobs_rosimage = self.blobFinder.findBlobs(data) # --------------------------------------------------------------------- # Create trakcing points image cv_image = self.bridge.imgmsg_to_cv(data, desired_encoding="passthrough") ipl_image = cv.GetImage(cv_image) tracking_pts_image = cv.CreateImage(cv.GetSize(ipl_image), cv.IPL_DEPTH_8U, 3) cv.CvtColor(ipl_image, tracking_pts_image, cv.CV_GRAY2BGR) #blobs_list = get_synthetic_blobs(self.tracking_pts_dist, self.camera_matrix) num_blobs = len(blobs_list) if num_blobs == 3: #print('3 pts found') uv_list = self.get_sorted_uv_points(blobs_list) t, w = get_object_pose(1.0, 2.2, uv_list, self.tracking_pts_dist, self.camera_matrix) uv_list_pred = get_uv_list_from_vects(t, w, self.tracking_pts_dist, self.camera_matrix) elev = math.asin(w[2]) head = math.atan2(w[1], w[0]) elev_deg = 180.0 * elev / math.pi head_deg = 180.0 * head / math.pi #print('w', ['{0:1.3f}'.format(x) for x in w]) #print('t', ['{0:1.3f}'.format(x) for x in t]) print('head: {0:1.3f}'.format(head_deg)) print('elev: {0:1.3f}'.format(elev_deg)) print('tx: {0:1.3f}'.format(t[0])) print('ty: {0:1.3f}'.format(t[1])) print('tz: {0:1.3f}'.format(t[2])) print() self.devel_pub.publish(t[2]) # Draw points on tracking points image for i, uv in enumerate(uv_list): color = self.tracking_pts_colors[i] u, v = uv #cv.Circle(tracking_pts_image, (int(u),int(v)),3, color) cv.Circle(tracking_pts_image, (int(u), int(v)), 3, (0, 255, 0)) for i, uv in enumerate(uv_list_pred): color = self.tracking_pts_colors[i] u, v = uv #cv.Circle(tracking_pts_image, (int(u),int(v)),3, color) cv.Circle(tracking_pts_image, (int(u), int(v)), 3, (0, 0, 255)) else: print('3 pts not found ', end='') if num_blobs > 3: print('too many blobs') else: print('too few blobs') # Publish calibration progress image ros_image = self.bridge.cv_to_imgmsg(tracking_pts_image, encoding="passthrough") self.image_tracking_pts_pub.publish(ros_image) # --------------------------------------------------------------------- ## Publish image of blobs #self.image_blobs_pub.publish(blobs_rosimage) ## Create the blob data message and publish #blob_data_msg = BlobData() #blob_data_msg.header = data.header #blob_data_msg.number_of_blobs = len(blobs_list) #for b in blobs_list: # blob_msg = Blob() # for k, v in b.iteritems(): # setattr(blob_msg,k,v) # blob_data_msg.blob.append(blob_msg) #self.blob_data_pub.publish(blob_data_msg) def get_object_pose(self, uv_list): """ Calculates pose of three point object """ pass def get_sorted_uv_points(self, blobs_list): """ For blob lists with three blobs finds the identities of the blobs based on colinearity and distance between the points. """ assert len(blobs_list) == 3, 'blobs list must contain only thre blobs' # Get x and y point lists u_list = [blob['centroid_x'] for blob in blobs_list] v_list = [blob['centroid_y'] for blob in blobs_list] # Determine x and y span u_min, u_max = min(u_list), max(u_list) v_min, v_max = min(v_list), max(v_list) u_span = u_max - u_min v_span = v_max - v_min # Use max span to sort points if u_span >= v_span: uv_list = zip(u_list, v_list) uv_list.sort() u_list = [u for u, v in uv_list] v_list = [v for u, v in uv_list] else: vu_list = zip(v_list, u_list) vu_list.sort() u_list = [u for v, u in vu_list] v_list = [v for v, u in vu_list] # ##################################################################### # Look at distances and sort so that the large gap occurs first # Note currently assuming the largest gap occurs first this should # really come from the tracking_pts_pos # ##################################################################### uv_list = zip(u_list, v_list) dist_0_to_1 = distance_2d(uv_list[0], uv_list[1]) dist_1_to_2 = distance_2d(uv_list[1], uv_list[2]) if dist_0_to_1 <= dist_1_to_2: uv_list.reverse() return uv_list def run(self): rospy.spin()
class ZoomToolNode(object): 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 handle_set_param_srv(self, req): """ Handles requests to set the blob finder's parameters. Currently this is just the threshold used for binarizing the image. """ with self.lock: self.blobFinder.threshold = req.threshold self.blobFinder.filter_by_area = req.filter_by_area self.blobFinder.min_area = req.min_area self.blobFinder.max_area = req.max_area return BlobFinderSetParamResponse(True,'') def handle_get_param_srv(self,req): """ Handles requests for the blob finders parameters """ with self.lock: threshold = self.blobFinder.threshold filter_by_area = self.blobFinder.filter_by_area min_area = self.blobFinder.min_area max_area = self.blobFinder.max_area resp_args = (threshold, filter_by_area, min_area, max_area) return BlobFinderGetParamResponse(*resp_args) def image_callback(self,data): """ Callback for image topic subscription - finds blobs in image. """ #t0 = rospy.Time.now() with self.lock: blobs_list, blobs_image = self.blobFinder.findBlobs(data, create_image=True) if len(blobs_list) == 2: # If two blobs are found computer the distance between them and # show it on the published image. x0 = blobs_list[0]['centroid_x'] y0 = blobs_list[0]['centroid_y'] x1 = blobs_list[1]['centroid_x'] y1 = blobs_list[1]['centroid_y'] point_list = [(x0,y0),(x1,y1)] dist = math.sqrt((x0-x1)**2 + (y0-y1)**2) message = 'dist = {0:1.1f} px'.format(dist) cv.PutText(blobs_image,message,(10,25),self.cv_text_font,self.text_color) ## Publish calibration progress image blobs_rosimage = self.bridge.cv_to_imgmsg(blobs_image,encoding="passthrough") self.image_pub.publish(blobs_rosimage) else: self.image_pub.publish(data) #t1 = rospy.Time.now() #dt = t1.to_sec() - t0.to_sec() #print(1.0/dt) #self.devel_pub.publish(dt) def run(self): rospy.spin()
time.sleep(0.5) bridge = CvBridge() got_pose_dict = {'flag': False, 'pose_fail': False} topic_name_cb = '/checkerdetector/ObjectDetection' rospy.Subscriber(topic_name_cb, ObjectDetection, got_pose_cb, got_pose_dict) failed_im_list = [] # list of filenames on which checkboard detection failed. n_images = len(im_name_list) for i in range(n_images): name = im_name_list[i] cv_im = cv.LoadImage(name) rosimage = bridge.cv_to_imgmsg(cv_im, "bgr8") rosimage.header.stamp = rospy.Time.from_sec(time_list[i]) image_pub.publish(rosimage) config_pub.publish(CameraInfo(P=intrinsic_list)) t_st = time.time() while got_pose_dict['flag'] == False: time.sleep(0.5) if (time.time()-t_st) > 10.: break if got_pose_dict['pose_fail'] == True: failed_im_list.append(name) time.sleep(0.5)
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." break time.sleep(1/100.0) cv.DestroyAllWindows()
class TimeStampWatchdog(object): """ Watchdog for hardware triggered cameras. Checks for correct sequence order and maximum allowed time stamp error. """ 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 def wait_for_camera_info_topics(self): """ Wait until the number of camera info topics is equal to the number of cameras in the current camera assignment. """ camera_assignment = file_tools.read_camera_assignment() number_of_cameras = len(camera_assignment) while 1: camera_info_topics = mct_introspection.find_camera_info_topics() if len(camera_info_topics) == number_of_cameras: break rospy.sleep(0.25) def reset_handler(self, req): """ Handles reset service """ with self.lock: self.seq_to_camera_stamps = {} self.stamp_last = {} self.max_error_by_camera = {} self.max_error = 0.0 self.most_recent_seq = None self.last_checked_seq = None self.ok = True self.seq_fail = 0 self.error_msg = '' self.camera_fail = '' return EmptyResponse() def camera_info_handler(self, camera, data): """ Handler for camera info messages. Stores the time stamps based on seqeunce number and camera. """ if not self.ready: return with self.lock: seq = data.header.seq stamp = data.header.stamp try: self.seq_to_camera_stamps[seq][camera] = stamp except KeyError: self.seq_to_camera_stamps[seq] = {camera: stamp} self.most_recent_seq = seq def check_camera_stamps(self, seq, camera_stamps): """ Checks the time stamps for a given sequence number. Examines the sequence number to ensure it is the next expected and exaimes the spread of the time stamps to ensure that is less than the allowed maximum. The results are published on the watchdog topic. """ cur_seq_ok = True error_list = [] # Check for correct sequence order if self.last_checked_seq is not None: if seq != self.last_checked_seq + 1: cur_seq_ok = False self.seq_fail = seq error_list.append('sequence out of order') self.last_checked_seq = seq # Get the max and min time stamps for cameras on the same machine stamp_delta = {} stamp_error = {} for camera, stamp in camera_stamps.iteritems(): # Get differenece between current and last time stamps and compute the stamp error try: stamp_delta[camera] = stamp.to_sec( ) - self.stamp_last[camera].to_sec() except KeyError: stamp_delta[camera] = 1.0 / self.frame_rate stamp_error[camera] = abs(1.0 / self.frame_rate - stamp_delta[camera]) # Find the maximum error so far for each camera try: self.max_error_by_camera[camera] = max( [self.max_error_by_camera[camera], stamp_error[camera]]) except KeyError: self.max_error_by_camera[camera] = stamp_error[camera] self.stamp_last[camera] = stamp # Compute the maximum error so far for all cameras self.max_error = max( [err for err in self.max_error_by_camera.values()]) if self.max_error > self.max_allowed_error: camera_fail_list = [ cam for cam, err in self.max_error_by_camera.items() if err > self.max_allowed_error ] self.camera_fail = str(camera_fail_list) cur_seq_ok = False error_list.append('time stamp outside of expected range') # If this is the first seq with an error set the error message if self.ok and not cur_seq_ok: self.ok = False self.seq_fail = seq self.error_msg = ', '.join(error_list) # Publish watchdog message watchdog_msg = TimeStampWatchDog() watchdog_msg.seq = seq watchdog_msg.ok = self.ok watchdog_msg.frame_rate = self.frame_rate watchdog_msg.max_allowed_error = self.max_allowed_error watchdog_msg.max_error = self.max_error watchdog_msg.seq_fail = self.seq_fail watchdog_msg.error_msg = self.error_msg watchdog_msg.camera_fail = self.camera_fail self.watchdog_pub.publish(watchdog_msg) # Publish watchdog image pil_info_image = PILImage.new('RGB', self.info_image_size, (255, 255, 255)) draw = PILImageDraw.Draw(pil_info_image) info_items = [('ok', 'ok', ''), ('seq', 'seq', ''), ('max_error', 'max error', 's')] text_x, text_y, step_y = 10, 10, 20 for i, item in enumerate(info_items): name, label, units = item value = getattr(watchdog_msg, name) label_text = '{0}:'.format(label) if type(value) == float: value_text = '{0:<1.6f}'.format(value) else: value_text = '{0}'.format(value) units_text = '{0}'.format(units) 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)) draw.text((text_x + 180, text_y + step_y * i), units_text, font=self.font, fill=(0, 0, 0)) if self.error_msg: error_text = 'Error: {0}'.format(self.error_msg) else: error_text = '' draw.text( (text_x, text_y + len(info_items) * step_y), error_text, font=self.font, fill=(255, 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 process_camera_stamps(self): """ Processes the camera time stamps. First, looks to see if all the images have been recieved for a given sequence number. Second, check if any of the sequences are older than the maximum allowed age and if so throws them away. """ for seq, camera_stamps in sorted(self.seq_to_camera_stamps.items()): if len(camera_stamps) == self.number_of_cameras: self.check_camera_stamps(seq, camera_stamps) del self.seq_to_camera_stamps[seq] else: if self.most_recent_seq is not None: seq_age = self.most_recent_seq - seq if seq_age > self.max_seq_age: del self.seq_to_camera_stamps[seq] def run(self): """ Node, main loop. While the node """ while not rospy.is_shutdown(): with self.lock: self.process_camera_stamps()
class Frame_Drop_Corrector(object): """ Frame drop corrector node. Subscribes to the given image topic and detects dropped frames. Dummy frames are inserted for any frames which are dropped. """ def __init__(self, topic, framerate, tol=0.005, max_stamp_age=1.5, publish_image_corr=True): self.ready = False self.topic = topic self.framerate = framerate self.tol = tol self.publish_image_corr = publish_image_corr self.lock = threading.Lock() self.camera_info_topic = get_camera_info_from_image_topic(self.topic) self.max_stamp_age = max_stamp_age self.bridge = CvBridge() self.last_pub_stamp = None self.latest_stamp = None self.dummy_image = None self.seq_offset = None self.frame_drop_list = [] # Data dictionaries for synchronizing tracking data with image seq number self.stamp_to_image = {} self.stamp_to_seq = {} self.seq_to_stamp_and_image = {} rospy.init_node('frame_drop_corrector') # Subscribe to image and camera info topics self.image_sub = rospy.Subscriber(self.topic, Image, self.image_callback) self.info_sub = rospy.Subscriber(self.camera_info_topic, CameraInfo, self.camera_info_callback) # Set up image publisher topic_split = self.topic.split('/') seq_and_image_topic = topic_split[:-1] seq_and_image_topic.append('seq_and_image_corr') seq_and_image_topic = '/'.join(seq_and_image_topic) self.seq_and_image_repub = rospy.Publisher(seq_and_image_topic, SeqAndImage) if self.publish_image_corr: image_topic = topic_split[:-1] image_topic.append('image_corr') image_topic = '/'.join(image_topic) self.image_repub = rospy.Publisher(image_topic, Image) # Set up dropped frame # publisher frames_dropped_topic = topic_split[:-1] frames_dropped_topic.append('frames_dropped') frames_dropped_topic = '/'.join(frames_dropped_topic) self.frames_dropped_pub = rospy.Publisher(frames_dropped_topic, FramesDropped) # Set up frame drop info service - returns list of seq numbers # corresponding to the dropped frames for image stream. frame_drop_srv_name = topic_split[:-1] frame_drop_srv_name.append('frame_drop_info') frame_drop_srv_name = '/'.join(frame_drop_srv_name) self.frame_drop_srv = rospy.Service(frame_drop_srv_name, FrameDropInfo, self.handle_frame_drop_srv) # Setup reset service - needs to be called anytime the camera trigger is # stopped. It resets the last_pub_stamp to none. reset_srv_name = topic_split[:-1] reset_srv_name.append('frame_drop_reset') reset_srv_name = '/'.join(reset_srv_name) self.reset_srv = rospy.Service(reset_srv_name, Empty, self.handle_reset_srv) self.ready = True def handle_reset_srv(self, req): """ Resets the frame not detection node. This prevents the addition of a huge number of 'false' dropped frames when the system is restarted. """ with self.lock: self.last_pub_stamp = None self.seq_offset = None self.frame_drop_list = [] return EmptyResponse() def handle_frame_drop_srv(self, req): """ Returns list of sequences numbers for all dropped frames. """ with self.lock: frame_drop_list = list(self.frame_drop_list) return FrameDropInfoResponse(frame_drop_list) def camera_info_callback(self, data): """ Callback for camera info topic subscription - used to associate stamp with the image seq number. """ if not self.ready: return stamp_tuple = data.header.stamp.secs, data.header.stamp.nsecs with self.lock: self.latest_stamp = stamp_tuple self.stamp_to_seq[stamp_tuple] = data.header.seq def image_callback(self, data): """ Callback for image topic subscription - used to associate the stamp tuple with the imcomming image. """ if not self.ready: return stamp_tuple = data.header.stamp.secs, data.header.stamp.nsecs with self.lock: self.stamp_to_image[stamp_tuple] = data if self.dummy_image is None: # Create dummy image for use when a dropped frame occurs. cv_image = self.bridge.imgmsg_to_cv( data, desired_encoding="passthrough") raw_image = cv.GetImage(cv_image) dummy_cv_image = cv.CreateImage(cv.GetSize(raw_image), raw_image.depth, raw_image.channels) cv.Zero(dummy_cv_image) self.dummy_image = self.bridge.cv_to_imgmsg( dummy_cv_image, encoding="passthrough") def associate_image_w_seq(self): """ Associate iamges with image seq numbers """ with self.lock: for stamp, image in self.stamp_to_image.items(): try: seq = self.stamp_to_seq[stamp] seq_found = True except KeyError: seq_found = False if seq_found: self.seq_to_stamp_and_image[seq] = stamp, image try: del self.stamp_to_image[stamp] except KeyError: pass try: del self.stamp_to_seq[stamp] except KeyError: pass else: # Throw away data greater than the maximum allowed age if self.latest_stamp is not None: latest_stamp_secs = stamp_tuple_to_secs( self.latest_stamp) stamp_secs = stamp_tuple_to_secs(stamp) stamp_age = latest_stamp_secs - stamp_secs if stamp_age > self.max_stamp_age: try: del self.stamp_to_image[stamp] except KeyError: pass def republish_seq_and_image(self): """ Republished the sequence numbers and images with black frames inserted for any dropped frames which are discovered by looking at the time stamps. """ for seq, stamp_and_image in sorted( self.seq_to_stamp_and_image.items()): stamp_tuple, image = stamp_and_image if self.seq_offset is None: # This is the first run or reset has been called. In the case reset # the seq offset so that the seq numbers begin with 1. self.seq_offset = 1 - seq if self.last_pub_stamp is not None: dt = stamp_dt_secs(stamp_tuple, self.last_pub_stamp) #framegap = int(round(dt*self.framerate)) framegap = approx_frame_number(dt, 1.0 / self.framerate, self.tol) for i in range(framegap - 1): # Note, framegap > 1 implies that we have drop frames. # Publish dummies frames until we are caught up. dummy_seq = seq + self.seq_offset dummy_stamp_tuple = incr_stamp_tuple( self.last_pub_stamp, (i + 1) / self.framerate) dummy_image = self.dummy_image dummy_image.header.seq = dummy_seq dummy_image.header.stamp = rospy.Time(*dummy_stamp_tuple) self.seq_and_image_repub.publish(dummy_seq, dummy_image) self.frames_dropped_pub.publish(dummy_seq, len(self.frame_drop_list)) if self.publish_image_corr: self.image_repub.publish(dummy_image) self.seq_offset += 1 self.frame_drop_list.append(dummy_seq) # Publish new frame with corrected sequence number - to account for dropped frames corrected_seq = seq + self.seq_offset image.header.seq = corrected_seq image.header.stamp = rospy.Time(*stamp_tuple) self.seq_and_image_repub.publish(corrected_seq, image) self.frames_dropped_pub.publish(corrected_seq, len(self.frame_drop_list)) if self.publish_image_corr: self.image_repub.publish(image) self.last_pub_stamp = stamp_tuple del self.seq_to_stamp_and_image[seq] def run(self): """ Main loop. Associates images and time stamps w/ image sequence numbers. Examines time interval between frames to detect dropped frames. Re-publishes sequences and images as combined SeqAndImage topic. When a dropped frame is detected a blank "dummy" frame is inserted in its place. """ while not rospy.is_shutdown(): self.associate_image_w_seq() self.republish_seq_and_image()
class Transform_2D_Calibrator(object): def __init__(self, topic_0, topic_1): self.ready = False self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED #self.state = WORKING self.topics = topic_0, topic_1 self.bridge = CvBridge() self.lock = threading.Lock() rospy.init_node('transform_2d_calibrator') self.node_name = rospy.get_name() # Initialize data lists self.blobs = { self.topics[0]: [], self.topics[1]: [], } self.index_to_image = { self.topics[0]: {}, self.topics[1]: {}, } self.overlap_indices = [] # 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 # Wait counter for image acquisition self.image_wait_cnt = { self.topics[0]: 0, self.topics[1]: 0, } # Sleep periods for idle and wait count loops self.idle_sleep_dt = 0.25 self.wait_sleep_dt = 0.005 # Led power setting params_namespace = '/transform_2d_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) # Initialize blob finder self.blobFinder = BlobFinder() self.blobFinder.threshold = rospy.get_param( '{0}/blob_finder/threshold'.format(params_namespace), 150) 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) # Number of points required self.num_points_required = rospy.get_param( '{0}/num_points_required'.format(params_namespace), 10) self.transform = None self.transform_error = None # Get homography matrices self.homography_dict = {} for topic in self.topics: try: # Get the data from the parameter server rows = rospy.get_param( '{0}/homography_matrix/rows'.format(topic)) cols = rospy.get_param( '{0}/homography_matrix/cols'.format(topic)) data = rospy.get_param( '{0}/homography_matrix/data'.format(topic)) except KeyError: # Data is not on the parameter server - try getting it by reading the file camera = get_camera_from_topic(topic) homography = file_tools.read_homography_calibration(camera) rows = homography['rows'] cols = homography['cols'] data = homography['data'] # Rearrange into numpy matrix homography_matrix = numpy.array(homography['data']) homography_matrix = homography_matrix.reshape((rows, cols)) self.homography_dict[topic] = homography_matrix # 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 image_callback_0 = functools.partial(self.image_callback, self.topics[0]) image_callback_1 = functools.partial(self.image_callback, self.topics[1]) self.image_sub = { self.topics[0]: rospy.Subscriber(self.topics[0], Image, image_callback_0), self.topics[1]: rospy.Subscriber(self.topics[1], Image, image_callback_1), } # Publications - bcalibration progress images for topics 0 and 1 self.image_pub = { self.topics[0]: rospy.Publisher('image_transform_calibration_0', Image), self.topics[1]: rospy.Publisher('image_transform_calibration_1', Image), } # Services self.start_srv = rospy.Service('{0}/start'.format(self.node_name), GetFlagAndMessage, self.handle_start_srv) self.is_calibrated_srv = rospy.Service( '{0}/is_calibrated'.format(self.node_name), GetBool, self.handle_is_calibrated_srv) self.get_transform_2d_srv = rospy.Service( '{0}/get_transform_2d'.format(self.node_name), GetTransform2d, self.handle_get_transform_2d_srv) self.ready = True def handle_get_transform_2d_srv(self, req): """ Handles requests to get the transform found by the """ if self.transform is None: rotation = 0.0 translation_x = 0.0 translation_y = 0.1 else: rotation = self.transform['rotation'] translation_x = self.transform['translation_x'] translation_y = self.transform['translation_y'] return GetTransform2dResponse(rotation, translation_x, translation_y) 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.image_info = '' self.state = WORKING self.led_n = 0 self.led_m = 0 self.blobs = {topic_0: [], topic_1: []} self.index_to_image = {topic_0: {}, topic_1: {}} self.overlap_indices = [] self.transform = None self.transform_error = None return GetFlagAndMessageResponse(flag, message) def handle_is_calibrated_srv(self, req): """ Handles requests for whether or not the transform calibration has been completed. """ if self.transform is not None: value = True else: value = False return GetBoolResponse(value) def image_callback(self, topic, data): """ Image topic callback function. Uses the blobFinder to find the blobs (leds) in the image. Publishes images showing the calibration progress. """ # Find blobs with self.lock: if not self.ready: return if self.state == WORKING: self.blobs[topic], blobs_image = self.blobFinder.findBlobs( data) blobs_rosimage = self.bridge.cv_to_imgmsg( blobs_image, encoding="passthrough") else: self.blobs[topic] = [] if self.image_wait_cnt[topic] < self.image_wait_number: self.image_wait_cnt[topic] += 1 # 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] if self.state == WORKING: for image_point in self.index_to_image[topic].values(): x, y = image_point cv.Circle(calib_image, (int(x), int(y)), 3, color) else: for index in self.overlap_indices: x, y = self.index_to_image[topic][index] 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: if self.state == WORKING: num_points_found = len(self.index_to_image[topic]) else: num_points_found = len(self.overlap_indices) message.append('{0}/{1} pts'.format(num_points_found, self.get_led_count())) if self.image_info: message.append('- {0}'.format(self.image_info)) if (self.state == FINISHED) and (self.transform_error is not None): message.append('- error {0:1.2f} mm'.format(1e3 * self.transform_error)) 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_pub[topic].publish(calib_rosimage) def wait_for_images(self): """ Wait for 'image_wait_number' of images to be acquired. """ with self.lock: for topic in self.topics: self.image_wait_cnt[topic] = 0 done = False while not done: with self.lock: image_wait_cnt = min( [self.image_wait_cnt[t] for t in self.topics]) 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. self.state = FINISHED self.overlap_indices = self.get_overlap_indices() if len(self.overlap_indices) >= self.num_points_required: self.state = FINISHED self.image_info = '' else: self.state = WAITING self.image_info = 'not enough data' self.transform = None def get_overlap_indices(self): """ Returns the overlaping points """ indices_0 = self.index_to_image[self.topics[0]] indices_1 = self.index_to_image[self.topics[1]] overlap_indices = set(indices_0) overlap_indices.intersection_update(indices_1) return list(overlap_indices) def get_led_count(self): return self.led_n + self.led_m * self.led_n_max + 1 def save_blob_data(self): """ Save blob data found in images """ with self.lock: for topic in self.topics: if len(self.blobs[topic]) == 1: blob = self.blobs[topic][0] image_x = blob['centroid_x'] image_y = blob['centroid_y'] self.index_to_image[topic][(self.led_n, self.led_m)] = (image_x, image_y) def find_transform(self): """ Find 2d transform (rotation + translation) from the world points found in topic[0] and topic[1] and thier respective homographies. """ # Get world points for both topics for all points in overlap world_points_dict = {} for topic in self.topics: # Get homography from image coordinates to world coordinates homography_matrix = self.homography_dict[topic] homography_matrix_t = homography_matrix.transpose() # Get image points for indices in overlap image_points = [] for index in self.overlap_indices: image_points.append(self.index_to_image[topic][index]) image_points = numpy.array(image_points) # Convert to homogenious coordinates and compute world coordinates image_points_hg = numpy.ones((image_points.shape[0], 3)) image_points_hg[:, :2] = image_points world_points_hg = numpy.dot(image_points_hg, homography_matrix_t) world_points = numpy.array(world_points_hg[:, :2]) denom = numpy.zeros((world_points.shape[0], 2)) denom[:, 0] = world_points_hg[:, 2] denom[:, 1] = world_points_hg[:, 2] world_points = world_points / denom world_points_dict[topic] = world_points # Estimate translate as mean difference in point positions points_0 = world_points_dict[self.topics[0]] points_1 = world_points_dict[self.topics[1]] rotation_est, translation_est = estimate_transform_2d( points_0, points_1) rotation, translation, error = fit_transform_2d( points_0, points_1, rotation_est, translation_est) self.transform = { 'rotation': rotation, 'translation_x': translation[0], 'translation_y': translation[1], } self.transform_error = error 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() self.save_blob_data() self.increment_led() elif self.state == FINISHED and self.transform is None: # Calculate 2d transforms self.find_transform() else: rospy.sleep(self.idle_sleep_dt)
class BlobFinderNode(object): 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 handle_set_param_srv(self, req): """ Handles requests to set the blob finder's parameters. Currently this is just the threshold used for binarizing the image. """ with self.lock: self.blobFinder.threshold = req.threshold self.blobFinder.filter_by_area = req.filter_by_area self.blobFinder.min_area = req.min_area self.blobFinder.max_area = req.max_area return BlobFinderSetParamResponse(True, '') def handle_get_param_srv(self, req): """ Handles requests for the blob finders parameters """ with self.lock: threshold = self.blobFinder.threshold filter_by_area = self.blobFinder.filter_by_area min_area = self.blobFinder.min_area max_area = self.blobFinder.max_area resp_args = (threshold, filter_by_area, min_area, max_area) return BlobFinderGetParamResponse(*resp_args) def publish_blob_data(self, blobs_list, blobs_image, image_header, image_seq): """ Publish image of blobs and blob data. """ blobs_rosimage = self.bridge.cv_to_imgmsg(blobs_image, encoding="passthrough") self.image_pub.publish(blobs_rosimage) # Create the blob data message and publish blob_data_msg = BlobData() blob_data_msg.header = image_header blob_data_msg.image_seq = image_seq blob_data_msg.number_of_blobs = len(blobs_list) for b in blobs_list: blob_msg = Blob() for k, v in b.iteritems(): setattr(blob_msg, k, v) blob_data_msg.blob.append(blob_msg) self.blob_data_pub.publish(blob_data_msg) def image_callback(self, data): """ Callback for image topic subscription. """ if not self.ready: return with self.lock: blobs_list, blobs_image = self.blobFinder.findBlobs(data) self.publish_blob_data(blobs_list, blobs_image, data.header, data.header.seq) def seq_and_image_callback(self, data): """ Callback for SeqAndImage topic subscription. """ if not self.ready: return with self.lock: blobs_list, blobs_image = self.blobFinder.findBlobs(data.image) self.publish_blob_data(blobs_list, blobs_image, data.image.header, data.seq) def run(self): rospy.spin()
class StitchedImageLabeler(object): 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 stitched_image_handler(self, data): """ Callback for handling the sequence and stitched image topics. """ if not self.ready: return with self.lock: self.latest_seq = data.seq self.seq_to_stitched_image[data.seq] = data.image def tracking_pts_handler(self, data): """ Callback for handling the tracking point data from the tracker synchronizer. """ if not self.ready: return with self.lock: if data.seq % self.stitching_params['frame_skip'] == 0: self.latest_seq = data.seq self.seq_to_tracking_pts[data.seq] = data def create_labeled_image(self, ros_image, tracking_pts): """ Create labeled version of stitched image using the tracking points data """ # Convert stitched image from rosimage to opencv image. cv_image = self.bridge.imgmsg_to_cv(ros_image, desired_encoding="passthrough") ipl_image = cv.GetImage(cv_image) # Create color version of stitched image. if self.labeled_image is None: labeled_image = cv.CreateImage(cv.GetSize(ipl_image), cv.IPL_DEPTH_8U, 3) cv.Zero(labeled_image) cv.CvtColor(ipl_image, labeled_image, cv.CV_GRAY2BGR) # Write sequence number on image message = '{0}'.format(tracking_pts.seq) cv.PutText(labeled_image, message, (10, 25), self.cv_text_font, self.magenta) if tracking_pts.found: # Draw boundry box around tracked object p_list = [(int(pt.x), int(pt.y)) for pt in tracking_pts.bndry_stitching_plane] q_list = p_list[1:] + [p_list[0]] for p, q in zip(p_list, q_list): cv.Line(labeled_image, p, q, self.magenta) # Draw circles around tracked object points for pt, color in zip(tracking_pts.pts_stitching_plane, self.tracking_pts_colors): cv.Circle(labeled_image, (int(pt.x), int(pt.y)), 3, color) # Convert labeled image to ros image and publish labeled_rosimage = self.bridge.cv_to_imgmsg(labeled_image, encoding="passthrough") self.labeled_image_pub.publish(labeled_rosimage) # DEBUG ################################################### #self.labeled_image_pub.publish(ros_image) ########################################################### def run(self): """ Node main loop. Pairs up the tracking points data with the stitched images by sequence number and passes them to the create labeled image function. Also, cleans out any old data from the seq_to_xxx dictionaries. """ while not rospy.is_shutdown(): with self.lock: # Match up tracking points data with stitched images using sequence number for seq, tracking_pts in sorted( self.seq_to_tracking_pts.items()): try: image = self.seq_to_stitched_image[seq] found = True except KeyError: found = False if found: # Reomve items from storage dictionaries del self.seq_to_stitched_image[seq] del self.seq_to_tracking_pts[seq] self.create_labeled_image(image, tracking_pts) else: # Remove any elements seq_to_tracking_pts dict older than maximum allowed age seq_age = self.latest_seq - seq if seq_age > self.max_seq_age: del self.seq_to_tracking_pts[seq] # Remove and elements form seq_to_stitched_image dict older than maximum allowed age for seq in self.seq_to_stitched_image.keys(): seq_age = self.latest_seq - seq if seq_age > self.max_seq_age: del self.seq_to_stitched_image[seq]
if not opt.headless: #cv.ShowImage('left', l) #cv.ShowImage('right', r) cv.ShowImage('stereo-anaglyph', red_blue) k = cv.WaitKey(10) print k if k == escape: break if k == left: anaglyph_cyan_image_distance_correction = anaglyph_cyan_image_distance_correction - 1 print anaglyph_cyan_image_distance_correction if k == right: anaglyph_cyan_image_distance_correction = anaglyph_cyan_image_distance_correction + 1 print anaglyph_cyan_image_distance_correction else: rosimage = bridge.cv_to_imgmsg(red_blue, "bgra8") image_pub.publish(rosimage)
class ImageStitcher(object): """ Subscribes to all rectified image topics in a 2d tracking region and uses the calibration data to stitch the images together. In turn it publishes image_stitched and image_stitched/seq. Note, in order to handle drop frames gracefully I've modified this node so that it can subscribe to the mct_msg_and_srv/SeqAndImage topics published by the frame_drop_corrector nodes. """ def __init__( self, region, topic_end='image_rect_skip', topic_type='sensor_msgs/Image', max_seq_age=150, max_stamp_age=1.5 ): self.topic_end = topic_end self.topic_type = topic_type self.max_seq_age = max_seq_age self.max_stamp_age = max_stamp_age self.warp_flags = cv.CV_INTER_LINEAR self.stitching_params = file_tools.read_tracking_2d_stitching_params() rospy.init_node('image_stitcher') self.ready = False self.is_first_write = True self.stitched_image = None self.seq_to_images = {} self.stamp_to_seq_pool= {} self.image_waiting_pool = {} # Information on the latest sequence received and stitched self.seq_newest = None self.stamp_newest = None self.lock = threading.Lock() self.bridge = CvBridge() # Get image and camera information regions_dict = file_tools.read_tracking_2d_regions() self.region = region self.camera_list = regions_dict[region] self.create_camera_to_image_dict() self.create_camera_to_info_dict() # Get transforms from cameras to stitched image and stitched image size self.tf2d = transform_2d.Transform2d() self.create_tf_data() self.get_stitched_image_size() # Subscribe to topics based on incoming topic type. if self.topic_type == 'sensor_msgs/Image': # Create pool dictionaries for incomming data. for camera in self.camera_list: self.stamp_to_seq_pool[camera] = {} self.image_waiting_pool[camera] = {} # Subscribe to camera info topics self.info_sub = {} for camera, topic in self.camera_to_info.iteritems(): info_handler = functools.partial(self.info_handler, camera) self.info_sub[camera] = rospy.Subscriber(topic, CameraInfo, info_handler) # Subscribe to rectified image topics self.image_sub = {} for camera, topic in self.camera_to_image.iteritems(): image_handler = functools.partial(self.image_handler, camera) self.image_sub[camera] = rospy.Subscriber(topic, Image, image_handler) elif self.topic_type == 'mct_msg_and_srv/SeqAndImage': # Subscribe to SeqAndImage topics self.seq_and_image_sub = {} for camera, topic in self.camera_to_image.iteritems(): seq_and_image_handler = functools.partial(self.seq_and_image_handler, camera) self.seq_and_image_sub[camera] = rospy.Subscriber(topic, SeqAndImage, seq_and_image_handler) else: err_msg = 'unable to handle topic type: {0}'.format(self.topic_type) raise ValueError, err_msg # Stitched image publisher and seq publisher self.image_pub = rospy.Publisher('image_stitched', Image) self.image_and_seq_pub = rospy.Publisher('seq_and_image_stitched', SeqAndImage) self.seq_pub = rospy.Publisher('image_stitched/seq', UInt32) self.stamp_pub = rospy.Publisher('image_stitched/stamp_info', StampInfo) self.processing_dt_pub = rospy.Publisher('image_stitched/processing_dt', ProcessingInfo) # Setup reset service - needs to be called anytime the camera trigger is # stopped - before it is restarted. Empties buffers of images and sequences. self.reset_srv = rospy.Service('reset_image_stitcher', Empty, self.handle_reset_srv) self.ready = True def handle_reset_srv(self, req): """ Handles the nodes reset service - which empties the image and sequence buffers. """ with self.lock: self.seq_to_images = {} self.stamp_to_seq_pool= {} self.image_waiting_pool = {} self.seq_newest = None self.stamp_newest = None return EmptyResponse() def create_camera_to_image_dict(self): """ Create camera to image topic dictionary """ self.camera_to_image = {} image_topics = mct_introspection.find_camera_image_topics(transport=self.topic_end) #for topic in rect_topics: for topic in image_topics: topic_split = topic.split('/') for camera in self.camera_list: if camera in topic_split: self.camera_to_image[camera] = topic def create_camera_to_info_dict(self): """ Create camera to image topic dictionary """ self.camera_to_info = {} info_topics = mct_introspection.find_camera_info_topics() for topic in info_topics: topic_split = topic.split('/') for camera in self.camera_list: if camera in topic_split: self.camera_to_info[camera] = topic def create_tf_data(self): """ Pre-computes transform matrices and roi for creating the stitched images. """ self.tf_data = {} for camera in self.camera_list: # Get modified transform which maps to ROI in stitched image bbox = self.tf2d.get_stitching_plane_bounding_box( region, camera_list=[camera] ) tf_matrix = self.tf2d.get_camera_to_stitching_plane_tf(camera) roi_x = int(math.floor(bbox['min_x'])) roi_y = int(math.floor(bbox['min_y'])) roi_w = int(math.floor(bbox['max_x']) - math.floor(bbox['min_x']+5)) roi_h = int(math.floor(bbox['max_y']) - math.floor(bbox['min_y']+5)) shift_matrix = numpy.array([ [1.0, 0.0, -bbox['min_x']], [0.0, 1.0, -bbox['min_y']], [0.0, 0.0, 1.0], ]) tf_matrix = numpy.dot(shift_matrix, tf_matrix) self.tf_data[camera] = { 'matrix': cv.fromarray(tf_matrix), 'roi': (roi_x, roi_y, roi_w, roi_h) } def get_stitched_image_size(self): """ Determines the size of the stitched image. """ # Get stitched image size from bounding box bbox = self.tf2d.get_stitching_plane_bounding_box(region) self.image_width = int(math.ceil(bbox['max_x'])) self.image_height = int(math.ceil(bbox['max_y'])) self.image_size = (self.image_width, self.image_height) def info_handler(self,camera,data): """ Handler for incoming camera info messages. In this callback we place image sequence number into stamp_to_seq_pool dictionay by camera name and timestamp Note, only used when imcoming topics are of type sensor_msgs/Image. In this case both the Image and camera_info topics are need to associate the acquisition sequence numbers with the images as the seq numbers in the image headers aren't reliable. """ if self.ready: with self.lock: stamp = data.header.stamp.secs, data.header.stamp.nsecs self.stamp_to_seq_pool[camera][stamp] = data.header.seq self.update_seq_newest(data.header.seq) self.update_stamp_newest(stamp) def image_handler(self, camera, data): """ Handler for incoming camera images. In this callback we place the image data into the image_waiting_pool by camera name and timestamp. Note, we don't want to use the seq information in data.header.seq as this seems to initialized in some random way - probably has to do with python not fully supporting ROS's image_transport. The seq's from the camera_info topics are correct. Note, only used when imcoming topics are of type sensor_msgs/Image. In this case both the Image and camera_info topics are need to associate the acquisition sequence numbers with the images as the seq numbers in the image headers aren't reliable. """ if self.ready: with self.lock: stamp = data.header.stamp.secs, data.header.stamp.nsecs self.image_waiting_pool[camera][stamp] = data def seq_and_image_handler(self,camera,data): """ Handler for incoming SeqAndImage messages which contain both the image data and the frame drop corrected acquisition sequence numbers. Note, onle used when the incomind topics are of type mct_msg_and_srv/SeqAndImage. """ if self.ready: with self.lock: try: self.seq_to_images[data.seq][camera] = data.image except KeyError: self.seq_to_images[data.seq] = {camera: data.image} self.update_seq_newest(data.seq) def update_seq_newest(self,seq): if self.seq_newest is None: self.seq_newest = seq else: self.seq_newest = max([self.seq_newest, seq]) def update_stamp_newest(self,stamp): if self.stamp_newest is None: self.stamp_newest = stamp else: self.stamp_newest = max([self.stamp_newest, stamp]) def process_waiting_images(self): """ Processes waiting images. Associates images in the waiting pool with their acquisition sequence number and places them in the seq_to_images buffer. """ with self.lock: # Associate images in the waiting pool with their seq numbers for camera in self.camera_list: for stamp, image_data in self.image_waiting_pool[camera].items(): try: seq = self.stamp_to_seq_pool[camera][stamp] del self.stamp_to_seq_pool[camera][stamp] del self.image_waiting_pool[camera][stamp] image_data.header.seq = seq deleted = True try: self.seq_to_images[seq][camera] = image_data except KeyError: self.seq_to_images[seq] = {camera: image_data} except KeyError: deleted = False # Clear out stale data from image waiting pool if not deleted: stamp_age = self.get_stamp_age(stamp) if stamp_age > self.max_stamp_age: del self.image_waiting_pool[camera][stamp] # Clear out any stale data from stamp-to-seq pool for stamp, seq in self.stamp_to_seq_pool[camera].items(): seq_age = self.get_seq_age(seq) if seq_age > self.max_seq_age: del self.stamp_to_seq_pool[camera][stamp] def get_stamp_age(self, stamp): """ Computes the age of a time stamp. """ stamp_secs = stamp_tuple_to_secs(stamp) stamp_newest_secs = stamp_tuple_to_secs(self.stamp_newest) return stamp_newest_secs - stamp_secs def get_seq_age(self, seq): """ Compute sequece age - handling 32bit uint rollover of seq counter. Note, at 30Hz this should roll over for something like 4yrs, but you never know. """ age0 = abs(self.seq_newest - seq) age1 = MAX_UINT32 - seq + self.seq_newest + 1 return min([age0, age1]) def publish_stitched_image(self): """ Checks to see if the sequences to images buffer contains all required frames and if so produces and publishes a stitched image. """ # Check to see if we have all images for a given sequence number for seq, image_dict in sorted(self.seq_to_images.items()): # If we have all images for the sequece - stitch into merged view if len(image_dict) == len(self.camera_list): t0 = rospy.Time.now() for i, camera in enumerate(self.camera_list): # Convert ros image to opencv image ros_image = image_dict[camera] cv_image = self.bridge.imgmsg_to_cv( ros_image, desired_encoding="passthrough" ) ipl_image = cv.GetImage(cv_image) # Create stitced image if it doesn't exist yet if self.stitched_image is None: self.stitched_image = cv.CreateImage( self.image_size, ipl_image.depth, ipl_image.channels ) if i==0: cv.Zero(self.stitched_image) # Set image warping flags - if first fill rest of image with zeros if self.is_first_write: warp_flags = self.warp_flags | cv.CV_WARP_FILL_OUTLIERS self.is_first_write = False else: warp_flags = self.warp_flags # Warp into stitched image cv.SetImageROI(self.stitched_image, self.tf_data[camera]['roi']) # Warp stitched image cv.WarpPerspective( ipl_image, self.stitched_image, self.tf_data[camera]['matrix'], flags=warp_flags, ) cv.ResetImageROI(self.stitched_image) # Get max and min time stamps for stamp_info stamp = ros_image.header.stamp if i == 0: stamp_max = stamp stamp_min = stamp else: stamp_max = stamp if stamp.to_sec() > stamp_max.to_sec() else stamp_max stamp_mim = stamp if stamp.to_sec() < stamp_min.to_sec() else stamp_min # Convert stitched image to ros image stitched_ros_image = self.bridge.cv_to_imgmsg( self.stitched_image, encoding="passthrough" ) stitched_ros_image.header.seq = seq self.image_pub.publish(stitched_ros_image) self.image_and_seq_pub.publish(seq, stitched_ros_image) self.seq_pub.publish(seq) # Compute time stamp spread stamp_diff = stamp_max.to_sec() - stamp_min.to_sec() self.stamp_pub.publish(stamp_max, stamp_min, stamp_diff) t1 = rospy.Time.now() dt = t1.to_sec() - t0.to_sec() self.processing_dt_pub.publish(dt,1.0/dt) # Remove data from buffer del self.seq_to_images[seq] # Throw away any stale data in seq to images buffer seq_age = self.get_seq_age(seq) if seq_age > self.max_seq_age: try: del self.seq_to_images[seq] except KeyError: pass def run(self): while not rospy.is_shutdown(): if self.seq_newest is None: continue if self.topic_type == 'sensor_msgs/Image': self.process_waiting_images() self.publish_stitched_image()
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]
if not opt.headless: #cv.ShowImage('left', l) #cv.ShowImage('right', r) cv.ShowImage('stereo-anaglyph', red_blue) k = cv.WaitKey(10) print k if k == escape: break if k == left: anaglyph_cyan_image_distance_correction = anaglyph_cyan_image_distance_correction - 1 print anaglyph_cyan_image_distance_correction if k == right: anaglyph_cyan_image_distance_correction = anaglyph_cyan_image_distance_correction + 1 print anaglyph_cyan_image_distance_correction else: rosimage = bridge.cv_to_imgmsg(red_blue, "bgra8") image_pub.publish(rosimage) #from opencv import cv #from opencv import highgui #from time import sleep # #def makeMagic(left, right, out): # chans=[] # for i in range(6): # chans.append(cv.cvCreateImage(cv.cvGetSize(left),8,1)) # cv.cvSplit(left, chans[0], chans[1], chans[2], None); # cv.cvSplit(right, chans[3], chans[4], chans[5], None); # cv.cvMerge(chans[3],chans[4],chans[2], None, out); # # #cv.cvMerge(None,chans[1],None, None, out);
class ThreePointTracker_Synchronizer: """ Synchronization node for all three point tracker in a given tracking region. """ def __init__(self, region, max_seq_age=200): self.lock = threading.Lock() self.region = region regions_dict = file_tools.read_tracking_2d_regions() self.camera_list = regions_dict[region] self.camera_list.sort() self.create_camera_to_tracking_dict() self.latest_seq = None self.max_seq_age = max_seq_age self.tracking_pts_pool = {} self.tracking_pts_roi_size = rospy.get_param( '/three_point_tracker_params/roi_size', (150, 150)) # Color and font for tracking points image self.magenta = (255, 255, 0) self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX, 0.6, 0.6, thickness=0) # Font for PIL tracking info image self.info_image_size = (180, 100) self.font = PILImageFont.truetype( "/usr/share/fonts/truetype/ubuntu-font-family/Ubuntu-B.ttf", 16) # Get transforms from cameras to tracking and stitched image planes self.tf2d = transform_2d.Transform2d() self.bridge = CvBridge() self.ready = False rospy.init_node('three_point_tracker_synchronizer', log_level=rospy.DEBUG) # Subscribe to raw tracking pts topics self.tracking_pts_sub = {} for camera, topic in self.camera_to_tracking.iteritems(): handler = functools.partial(self.tracking_pts_handler, camera) self.tracking_pts_sub[camera] = rospy.Subscriber( topic, ThreePointTrackerRaw, handler) # Create publishers self.tracking_pts_pub = rospy.Publisher('tracking_pts', ThreePointTracker) self.image_tracking_pts = None self.image_tracking_pts_pub = rospy.Publisher('image_tracking_pts', Image) self.image_tracking_info_pub = rospy.Publisher('image_tracking_info', Image) # Setup rand sync service rospy.wait_for_service('/get_rand_sync_signal') self.rand_sync_srv = rospy.ServiceProxy('/get_rand_sync_signal', GetRandSyncSignal) # Setup reset service - needs to be called anytime the camera trigger is # stopped - before it is restarted. Empties buffers of images and sequences. self.reset_srv = rospy.Service('reset_tracker_synchronizer', Empty, self.reset_handler) self.ready = True def create_camera_to_tracking_dict(self): """ Creates a dictionary relating the cameras in the tracking region to their corresponding three point tracker nodes. """ self.camera_to_tracking = {} for camera in self.camera_list: camera_fullpath_topic = mct_introspection.get_camera_fullpath_topic( camera) tracking_pts_topic = '{0}/tracking_pts'.format( camera_fullpath_topic) self.camera_to_tracking[camera] = tracking_pts_topic def reset_handler(self, req): """ Reset service handler. Empties the tracking_pts_pool. """ with self.lock: self.latest_seq = None self.tracking_pts_pool = {} return EmptyResponse() def tracking_pts_handler(self, camera, msg): """ Handler for messages from the individual tracker nodes. Sticks the tracking point data into a dictionary by sequence number and camera. """ if not self.ready: return with self.lock: self.latest_seq = msg.data.seq try: self.tracking_pts_pool[msg.data.seq][camera] = msg except KeyError: self.tracking_pts_pool[msg.data.seq] = {camera: msg} def process_tracking_pts(self, tracking_pts_dict): """ Determines whether the object has been found in any of the three point trackers for the region. If is has been found than best tracking point data is selected in a winner takes all fashion. The best tracking point data is that which is nearest to the center of the image on camera upon which is was captured. """ # Get time stamp (secs, nsecs) - always from the same camera to avoid jumps due to # possible system clock differences. time_camera = self.camera_list[0] time_camera_secs = tracking_pts_dict[time_camera].data.secs time_camera_nsecs = tracking_pts_dict[time_camera].data.nsecs # Get list of messages in which the object was found found_list = [ msg for msg in tracking_pts_dict.values() if msg.data.found ] tracking_pts_msg = ThreePointTracker() if found_list: # Object found - select object with largest ROI or if the ROIs are of equal size # select the object whose distance to center of the # image is the smallest found_list.sort(cmp=tracking_pts_sort_cmp) best = found_list[0] camera = best.data.camera # Get coordintates of points in tracking and stitching planes best_points_array = numpy.array([(p.x, p.y) for p in best.data.points]) pts_anchor_plane = self.tf2d.camera_pts_to_anchor_plane( camera, best_points_array) pts_stitching_plane = self.tf2d.camera_pts_to_stitching_plane( camera, best_points_array) pts_anchor_plane = [ Point2d(p[0], p[1]) for p in list(pts_anchor_plane) ] pts_stitching_plane = [ Point2d(p[0], p[1]) for p in list(pts_stitching_plane) ] # Get orientation angle and mid point of object in anchor and stitching planes angle = get_angle(pts_anchor_plane) midpt_anchor_plane = get_midpoint(pts_anchor_plane) midpt_stitching_plane = get_midpoint(pts_stitching_plane) #self.camera_fail = max([(err,cam) for cam, err in self.max_error_by_camera.items()])[1] # Get size of tracking points image in the anchor (tracking) plane roi = best.data.roi x0, x1 = roi[0], roi[0] + roi[2] y0, y1 = roi[1], roi[1] + roi[3] bndry_camera = [(x0, y0), (x1, y0), (x1, y1), (x0, y1)] bndry_camera_array = numpy.array(bndry_camera) bndry_anchor = self.tf2d.camera_pts_to_anchor_plane( camera, bndry_camera_array) bndry_stitching = self.tf2d.camera_pts_to_stitching_plane( camera, bndry_camera_array) bndry_anchor = [tuple(x) for x in list(bndry_anchor)] bndry_stitching = [tuple(x) for x in list(bndry_stitching)] dx1 = abs(bndry_anchor[1][0] - bndry_anchor[0][0]) dx2 = abs(bndry_anchor[3][0] - bndry_anchor[2][0]) dy1 = abs(bndry_anchor[2][1] - bndry_anchor[1][1]) dy2 = abs(bndry_anchor[3][1] - bndry_anchor[0][1]) dx_max = max([dx1, dx2]) dy_max = max([dy1, dy2]) dim_max = max([dx_max, dy_max]) # Convert tracking points image to opencv image. image_tracking_pts = self.bridge.imgmsg_to_cv( best.image, desired_encoding="passthrough") image_tracking_pts = cv.GetImage(image_tracking_pts) image_size = cv.GetSize(image_tracking_pts) image_dim_max = max(image_size) # Get matrix for homography from camera to anchor plane tf_matrix = self.tf2d.get_camera_to_anchor_plane_tf(camera) # Shift for local ROI tf_shift = numpy.array([ [1.0, 0.0, roi[0]], [0.0, 1.0, roi[1]], [0.0, 0.0, 1.0], ]) tf_matrix = numpy.dot(tf_matrix, tf_shift) # Get scaling factor shift_x = -min([x for x, y in bndry_anchor]) shift_y = -min([y for x, y in bndry_anchor]) scale_factor = float(image_dim_max) / dim_max # Scale and shift transform so that homography maps the tracking points # sub-image into a image_size image starting at coord. 0,0 tf_shift_and_scale = numpy.array([ [scale_factor, 0.0, scale_factor * shift_x], [0.0, scale_factor, scale_factor * shift_y], [0.0, 0.0, 1.0], ]) tf_matrix = numpy.dot(tf_shift_and_scale, tf_matrix) # Warp image using homography image_tracking_pts_mod = cv.CreateImage( image_size, image_tracking_pts.depth, image_tracking_pts.channels) cv.WarpPerspective( image_tracking_pts, image_tracking_pts_mod, cv.fromarray(tf_matrix), cv.CV_INTER_LINEAR | cv.CV_WARP_FILL_OUTLIERS, ) # Add sequence number to image message = '{0}'.format(best.data.seq) cv.PutText(image_tracking_pts_mod, message, (2, 15), self.cv_text_font, self.magenta) self.image_tracking_pts = self.bridge.cv_to_imgmsg( image_tracking_pts_mod, encoding="passthrough") # Create tracking points message tracking_pts_msg.seq = best.data.seq tracking_pts_msg.secs = time_camera_secs tracking_pts_msg.nsecs = time_camera_nsecs tracking_pts_msg.camera = camera tracking_pts_msg.found = True tracking_pts_msg.angle = angle tracking_pts_msg.angle_deg = (180.0 * angle) / math.pi tracking_pts_msg.midpt_anchor_plane = midpt_anchor_plane tracking_pts_msg.midpt_stitching_plane = midpt_stitching_plane tracking_pts_msg.pts_anchor_plane = pts_anchor_plane tracking_pts_msg.pts_stitching_plane = pts_stitching_plane tracking_pts_msg.bndry_anchor_plane = [ Point2d(x, y) for x, y in bndry_anchor ] tracking_pts_msg.bndry_stitching_plane = [ Point2d(x, y) for x, y in bndry_stitching ] else: sample = tracking_pts_dict.values()[0] tracking_pts_msg.seq = sample.data.seq tracking_pts_msg.secs = time_camera_secs tracking_pts_msg.nsecs = time_camera_nsecs tracking_pts_msg.camera = '' tracking_pts_msg.found = False # Create tracking info image pil_info_image = PILImage.new('RGB', self.info_image_size, (255, 255, 255)) draw = PILImageDraw.Draw(pil_info_image) text_list = [] label = 'Found:' value = '{0}'.format(tracking_pts_msg.found) unit = '' text_list.append((label, value, unit)) label = 'Angle:' value = '{0:+3.1f}'.format(180.0 * tracking_pts_msg.angle / math.pi) unit = 'deg' text_list.append((label, value, unit)) label = 'Pos X:' value = '{0:+1.3f}'.format(tracking_pts_msg.midpt_anchor_plane.x) unit = 'm' text_list.append((label, value, unit)) #self.camera_fail = max([(err,cam) for cam, err in self.max_error_by_camera.items()])[1] label = 'Pos Y:' value = '{0:+1.3f}'.format(tracking_pts_msg.midpt_anchor_plane.y) unit = 'm' text_list.append((label, value, unit)) for i, text in enumerate(text_list): label, value, unit = text draw.text((10, 10 + 20 * i), label, font=self.font, fill=(0, 0, 0)) draw.text((70, 10 + 20 * i), value, font=self.font, fill=(0, 0, 0)) draw.text((125, 10 + 20 * i), unit, 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_tracking_info_pub.publish(info_rosimage) # Get sync signal sync_rsp = self.rand_sync_srv(tracking_pts_msg.seq) if sync_rsp.status: tracking_pts_msg.sync_signal = sync_rsp.signal else: tracking_pts_msg.sync_signal = [0, 0, 0] # Publish messages self.tracking_pts_pub.publish(tracking_pts_msg) if self.image_tracking_pts is not None: self.image_tracking_pts_pub.publish(self.image_tracking_pts) else: zero_image_cv = cv.CreateImage(self.tracking_pts_roi_size, cv.IPL_DEPTH_8U, 3) cv.Zero(zero_image_cv) zero_image_ros = self.bridge.cv_to_imgmsg(zero_image_cv, encoding="passthrough") self.image_tracking_pts_pub.publish(zero_image_ros) def run(self): """ Node main loop - consolidates the tracking points messages by acquisition sequence number and proccess them by passing them to the process_tracking_pts function. """ while not rospy.is_shutdown(): with self.lock: #t0 = time.time() #print('len pool:', len(self.tracking_pts_pool)) for seq, tracking_pts_dict in sorted( self.tracking_pts_pool.items()): #print(' ', seq) # Check if we have all the tracking data for the given sequence number if len(tracking_pts_dict) == len(self.camera_list): self.process_tracking_pts(tracking_pts_dict) del self.tracking_pts_pool[seq] # Throw away tracking data greater than the maximum allowed age if self.latest_seq - seq > self.max_seq_age: #print('Thowing away: ', seq) del self.tracking_pts_pool[seq]
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)
class ThreePointTracker(object): """ Three point object tracker. Looks for objects in the image stream with three bright unequally spaced points. """ def __init__(self, topic=None, max_stamp_age=1.5): self.lock = threading.Lock() self.ready = False self.topic = topic self.tracking_pts_roi = None self.tracking_pts_roi_src = None self.tracking_pts_roi_dst = None self.seq_to_data = {} self.topic_type = mct_introspection.get_topic_type(self.topic) self.bridge = CvBridge() self.camera = get_camera_from_topic(self.topic) self.camera_info_topic = get_camera_info_from_image_topic(self.topic) rospy.init_node('blob_finder') # Get parameters from the parameter server params_ns = 'three_point_tracker_params' self.blobFinder = BlobFinder() self.tracking_pts_roi_size = rospy.get_param( '/{0}/roi_size'.format(params_ns), (200, 200), ) self.blobFinder.threshold = rospy.get_param( '/{0}/threshold'.format(params_ns), 200, ) self.blobFinder.filter_by_area = rospy.get_param( '/{0}/filter_by_area'.format(params_ns), False) self.blobFinder.min_area = rospy.get_param( '/{0}/min_area'.format(params_ns), 0, ) self.blobFinder.max_area = rospy.get_param( '/{0}/max_area'.format(params_ns), 200, ) self.tracking_pts_spacing = rospy.get_param( '/{0}/pts_spacing'.format(params_ns), (0.0, 0.04774, 0.07019), ) self.tracking_pts_colors = [(0, 0, 255), (0, 255, 0), (0, 255, 255)] # Determine target point spacing d0 = self.tracking_pts_spacing[1] - self.tracking_pts_spacing[0] d1 = self.tracking_pts_spacing[2] - self.tracking_pts_spacing[1] if d0 > d1: self.large_step_first = True else: self.large_step_first = False # Set up subscribers based on topic type. if self.topic_type == 'sensor_msgs/Image': # Data dictionareis for synchronizing tracking data with image seq number self.max_stamp_age = max_stamp_age self.latest_stamp = None self.stamp_to_seq = {} self.stamp_to_data = {} # Subscribe to image and camera info topics self.image_sub = rospy.Subscriber(self.topic, Image, self.image_callback) self.info_sub = rospy.Subscriber(self.camera_info_topic, CameraInfo, self.camera_info_callback) elif self.topic_type == 'mct_msg_and_srv/SeqAndImage': self.seq_and_image_sub = rospy.Subscriber( self.topic, SeqAndImage, self.seq_and_image_callback) else: err_msg = 'unable to handle topic type: {0}'.format( self.topic_type) raise ValueError, err_msg # Create tracking point and tracking points image publications self.tracking_pts_pub = rospy.Publisher('tracking_pts', ThreePointTrackerRaw) self.image_tracking_pts_pub = rospy.Publisher('image_tracking_pts', Image) # Set up services for getting and setting the tracking parameters. 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 handle_set_param_srv(self, req): """ Handles requests to set the blob finder's parameters. Currently this is just the threshold used for binarizing the image. """ with self.lock: self.blobFinder.threshold = req.threshold self.blobFinder.filter_by_area = req.filter_by_area self.blobFinder.min_area = req.min_area self.blobFinder.max_area = req.max_area return BlobFinderSetParamResponse(True, '') def handle_get_param_srv(self, req): """ Handles requests for the blob finders parameters """ with self.lock: threshold = self.blobFinder.threshold filter_by_area = self.blobFinder.filter_by_area min_area = self.blobFinder.min_area max_area = self.blobFinder.max_area resp_args = (threshold, filter_by_area, min_area, smax_area) return BlobFinderGetParamResponse(*resp_args) def camera_info_callback(self, camera_info): """ Callback for camera info topic subscription - used to get the image seq number when subscribing to sensor_msgs/Images topics as the seq numbers in the Image headers are not reliable. Note, only used when subscription is of type sensor_msgs/Image. """ stamp_tuple = camera_info.header.stamp.secs, camera_info.header.stamp.nsecs with self.lock: self.latest_stamp = stamp_tuple self.stamp_to_seq[stamp_tuple] = camera_info.header.seq def image_callback(self, image): """ Callback for image topic subscription - gets images and finds the tracking points. Finds tracking data in Note, only used when subscription is of type sensor_msgs/Image. """ if not self.ready: return with self.lock: blobs_list = self.blobFinder.findBlobs(image, create_image=False) tracking_data = self.get_tracking_data(blobs_list, image) with self.lock: self.stamp_to_data[tracking_data['stamp']] = tracking_data def seq_and_image_callback(self, msg_data): """ Callback for subscriptions of type mct_msg_and_srv/SeqAndImage type. Finds blobs in msg_data.image and extracts tracking data. """ if not self.ready: return with self.lock: blobs_list = self.blobFinder.findBlobs(msg_data.image, create_image=False) tracking_data = self.get_tracking_data(blobs_list, msg_data.image) with self.lock: self.seq_to_data[msg_data.seq] = tracking_data def get_tracking_data(self, blobs_list, rosimage): """ Gets tracking data from list of blobs and raw image. """ # Convert to opencv image cv_image = self.bridge.imgmsg_to_cv(rosimage, desired_encoding="passthrough") ipl_image = cv.GetImage(cv_image) # Create tracking points image image_tracking_pts = cv.CreateImage(self.tracking_pts_roi_size, cv.IPL_DEPTH_8U, 3) cv.Zero(image_tracking_pts) num_blobs = len(blobs_list) if num_blobs == 3: found = True uv_list = self.get_sorted_uv_points(blobs_list) self.tracking_pts_roi = self.get_tracking_pts_roi( uv_list, cv.GetSize(ipl_image)) dist_to_image_center = self.get_dist_to_image_center( uv_list, cv.GetSize(ipl_image)) else: found = False dist_to_image_center = 0.0 uv_list = [(0, 0), (0, 0), (0, 0)] # Create tracking pts image using ROI around tracking points if self.tracking_pts_roi is not None: src_roi, dst_roi = truncate_roi(self.tracking_pts_roi, cv.GetSize(ipl_image)) cv.SetImageROI(ipl_image, src_roi) cv.SetImageROI(image_tracking_pts, dst_roi) cv.CvtColor(ipl_image, image_tracking_pts, cv.CV_GRAY2BGR) cv.ResetImageROI(ipl_image) cv.ResetImageROI(image_tracking_pts) self.tracking_pts_roi_src = src_roi self.tracking_pts_roi_dst = dst_roi if found: for i, uv in enumerate(uv_list): color = self.tracking_pts_colors[i] u, v = uv u = u - self.tracking_pts_roi[0] v = v - self.tracking_pts_roi[1] cv.Circle(image_tracking_pts, (int(u), int(v)), 3, color) # Convert tracking points image to rosimage and publish rosimage_tracking_pts = self.bridge.cv_to_imgmsg( image_tracking_pts, encoding="passthrough") self.image_tracking_pts_pub.publish(rosimage_tracking_pts) # If tracking points roi doesn't exist yet just send zeros if self.tracking_pts_roi is None: tracking_pts_roi = (0, 0, 0, 0) tracking_pts_roi_src = (0, 0, 0, 0) tracking_pts_roi_dst = (0, 0, 0, 0) else: tracking_pts_roi = self.tracking_pts_roi tracking_pts_roi_src = self.tracking_pts_roi_src tracking_pts_roi_dst = self.tracking_pts_roi_dst stamp_tuple = rosimage.header.stamp.secs, rosimage.header.stamp.nsecs tracking_data = { 'found': found, 'stamp': stamp_tuple, 'tracking_pts': uv_list, 'image_tracking_pts': rosimage_tracking_pts, 'dist_to_image_center': dist_to_image_center, 'tracking_pts_roi': tracking_pts_roi, 'tracking_pts_roi_src': tracking_pts_roi_src, 'tracking_pts_roi_dst': tracking_pts_roi_dst, } return tracking_data def get_dist_to_image_center(self, uv_list, img_size): """ Computes the distance from the mid point of the tracking points to the middle of the image. """ img_mid = 0.5 * img_size[0], 0.5 * img_size[1] pts_mid = get_midpoint_uv_list(uv_list) return distance_2d(pts_mid, img_mid) def get_tracking_pts_roi(self, uv_list, img_size, trunc=False): """ Get the coordinates of region of interest about the tracking points """ img_width, img_height = img_size roi_width, roi_height = self.tracking_pts_roi_size u_mid, v_mid = get_midpoint_uv_list(uv_list) u_min = int(math.floor(u_mid - roi_width / 2)) v_min = int(math.floor(v_mid - roi_height / 2)) u_max = u_min + roi_width v_max = v_min + roi_height return u_min, v_min, u_max - u_min, v_max - v_min def get_sorted_uv_points(self, blobs_list): """ For blob lists with three blobs finds the identities of the blobs based on colinearity and distance between the points. """ assert len(blobs_list) == 3, 'blobs list must contain only thre blobs' # Get x and y point lists u_list = [blob['centroid_x'] for blob in blobs_list] v_list = [blob['centroid_y'] for blob in blobs_list] # Determine x and y span u_min, u_max = min(u_list), max(u_list) v_min, v_max = min(v_list), max(v_list) u_span = u_max - u_min v_span = v_max - v_min # Use max span to sort points if u_span >= v_span: uv_list = zip(u_list, v_list) uv_list.sort() u_list = [u for u, v in uv_list] v_list = [v for u, v in uv_list] else: vu_list = zip(v_list, u_list) vu_list.sort() u_list = [u for v, u in vu_list] v_list = [v for v, u in vu_list] # Look at distances and sort so that either the large gap occurs first # or last based on the spacing data. uv_list = zip(u_list, v_list) dist_0_to_1 = distance_2d(uv_list[0], uv_list[1]) dist_1_to_2 = distance_2d(uv_list[1], uv_list[2]) if self.large_step_first: if dist_0_to_1 <= dist_1_to_2: uv_list.reverse() else: if dist_0_to_1 >= dist_1_to_2: uv_list.reverse() return uv_list def associate_data_w_seq(self): """ Associate tracking data with acquisition sequence number. Note, this is only used when the subscription topic type is sensor_msgs/Image. """ with self.lock: for stamp, data in self.stamp_to_data.items(): try: seq = self.stamp_to_seq[stamp] seq_found = True except KeyError: seq_found = False if seq_found: self.seq_to_data[seq] = data del self.stamp_to_data[stamp] del self.stamp_to_seq[stamp] else: # Throw away data greater than the maximum allowed age if self.latest_stamp is not None: latest_stamp_secs = stamp_tuple_to_secs( self.latest_stamp) stamp_secs = stamp_tuple_to_secs(stamp) stamp_age = latest_stamp_secs - stamp_secs if stamp_age > self.max_stamp_age: del self.stamp_to_data[stamp] def publish_tracking_pts(self): """ Creates tracking_pts message and publishes it. """ for seq, data in sorted(self.seq_to_data.items()): # Create list of tracking points tracking_pts = [] for u, v in data['tracking_pts']: tracking_pts.append(Point2d(u, v)) # Create the tracking points message and publish tracking_pts_msg = ThreePointTrackerRaw() tracking_pts_msg.data.seq = seq tracking_pts_msg.data.secs = data['stamp'][0] tracking_pts_msg.data.nsecs = data['stamp'][1] tracking_pts_msg.data.camera = self.camera tracking_pts_msg.data.found = data['found'] tracking_pts_msg.data.distance = data['dist_to_image_center'] tracking_pts_msg.data.roi = data['tracking_pts_roi'] tracking_pts_msg.data.roi_src = data['tracking_pts_roi_src'] tracking_pts_msg.data.roi_dst = data['tracking_pts_roi_dst'] tracking_pts_msg.data.points = tracking_pts tracking_pts_msg.image = data['image_tracking_pts'] self.tracking_pts_pub.publish(tracking_pts_msg) #print('publish', seq) # Remove data for this sequence number. del self.seq_to_data[seq] def run(self): """ Main loop - associates tracking data and time stamps w/ image sequence numbers and publishes the tracking data. """ while not rospy.is_shutdown(): if self.topic_type == 'sensor_msgs/Image': self.associate_data_w_seq() self.publish_tracking_pts()