def __init__(self): self.reusing_bg_images = rospy.get_param('/camera/faa_image_processing/reusing_bg_images') self.image_p_pub = rospy.Publisher("image_processed",Image) self.image_d_pub = rospy.Publisher("data_image",Image) self.tracking_data_pub = rospy.Publisher("tracking_data",TrackingData) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("image_conditioned",Image,self.conditioned_image_callback) self.actuation_state_sub = rospy.Subscriber("/faa_actuation/actuation_state",ActuationState,self.actuation_state_callback) self.tracking = False self.drawing = False self.tracker = Tracker() self.drawer = Drawer() self.parameters = Parameters() self.tracker.setParameters(self.parameters) self.drawer.setParameters(self.parameters) self.display_images = False if self.display_images: # cv.NamedWindow("Image Processed", 1) # cv.NamedWindow("Image Tracked", 1) cv.NamedWindow("Image Data", 1) self.image_conditioned = None self.sbi = rospy.Service('/faa_image_processing/save_background_image', SaveImage, self.save_background_image_callback) self.st = rospy.Service('/faa_image_processing/set_tracking', SetTracking, self.set_tracking_callback) self.gp = rospy.Service('/faa_image_processing/get_parameters', GetParameters, self.get_parameters_callback) self.sp = rospy.Service('/faa_image_processing/set_parameter', SetParameter, self.set_parameter_callback) self.sap = rospy.Service('/faa_image_processing/set_array_parameter', SetArrayParameter, self.set_array_parameter_callback) self.ss = rospy.Service('/faa_image_processing/set_status', SetStatus, self.set_status_callback)
class ImageProcessor(object): def __init__(self): self.reusing_bg_images = rospy.get_param('/camera/faa_image_processing/reusing_bg_images') self.image_p_pub = rospy.Publisher("image_processed",Image) self.image_d_pub = rospy.Publisher("data_image",Image) self.tracking_data_pub = rospy.Publisher("tracking_data",TrackingData) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("image_conditioned",Image,self.conditioned_image_callback) self.actuation_state_sub = rospy.Subscriber("/faa_actuation/actuation_state",ActuationState,self.actuation_state_callback) self.tracking = False self.drawing = False self.tracker = Tracker() self.drawer = Drawer() self.parameters = Parameters() self.tracker.setParameters(self.parameters) self.drawer.setParameters(self.parameters) self.display_images = False if self.display_images: # cv.NamedWindow("Image Processed", 1) # cv.NamedWindow("Image Tracked", 1) cv.NamedWindow("Image Data", 1) self.image_conditioned = None self.sbi = rospy.Service('/faa_image_processing/save_background_image', SaveImage, self.save_background_image_callback) self.st = rospy.Service('/faa_image_processing/set_tracking', SetTracking, self.set_tracking_callback) self.gp = rospy.Service('/faa_image_processing/get_parameters', GetParameters, self.get_parameters_callback) self.sp = rospy.Service('/faa_image_processing/set_parameter', SetParameter, self.set_parameter_callback) self.sap = rospy.Service('/faa_image_processing/set_array_parameter', SetArrayParameter, self.set_array_parameter_callback) self.ss = rospy.Service('/faa_image_processing/set_status', SetStatus, self.set_status_callback) def conditioned_image_callback(self,data): self.parameters.increment_image_count() self.tracker.setParameters(self.parameters) self.drawer.setParameters(self.parameters) try: image_cv = self.bridge.imgmsg_to_cv(data, "passthrough") except CvBridgeError, e: print e image_np = numpy.asarray(image_cv) self.image_conditioned = numpy.copy(image_np) if self.tracking: # data_tracked,image_tracked = self.tracker.processImage(image_np) data_tracked,data_image = self.tracker.processImage(image_np) tracking_data = TrackingData() tracking_data.header = data.header tracking_data.image_count = self.parameters.image_count # rospy.logwarn(str(data_tracked)) # try: # data_tracked_tunnels = data_tracked['tunnels'] # except KeyError: # data_tracked_tunnels = [] # if 0 < len(data_tracked_tunnels): # for tunnel in range(len(data_tracked_tunnels)): tunnel_data_list = [] tunnels = range(self.parameters.tunnel_count) for tunnel in tunnels: tunnel_data = TunnelData() enabled = self.parameters.tunnels_enabled[tunnel] tunnel_data.tunnel = tunnel tunnel_data.enabled = enabled tunnel_data.gate0 = "" tunnel_data.gate1 = "" tunnel_data.gate2 = "" tunnel_data.fly_x = 0 tunnel_data.fly_y = 0 tunnel_data.fly_angle = 0 tunnel_data.chamber = "" tunnel_data.blob_x = 0 tunnel_data.blob_y = 0 tunnel_data.blob_area = 0 tunnel_data.blob_slope = 0 tunnel_data.blob_ecc = 0 if enabled: try: tunnel_data.gate0 = data_tracked[tunnel]['gate0'] tunnel_data.gate1 = data_tracked[tunnel]['gate1'] tunnel_data.gate2 = data_tracked[tunnel]['gate2'] except KeyError: pass try: tunnel_data.fly_x = data_tracked[tunnel]['fly_x'] tunnel_data.fly_y = data_tracked[tunnel]['fly_y'] tunnel_data.fly_angle = data_tracked[tunnel]['fly_angle'] tunnel_data.chamber = data_tracked[tunnel]['chamber'] tunnel_data.blob_x = data_tracked[tunnel]['blob_x'] tunnel_data.blob_y = data_tracked[tunnel]['blob_y'] tunnel_data.blob_area = data_tracked[tunnel]['blob_area'] tunnel_data.blob_slope = data_tracked[tunnel]['blob_slope'] tunnel_data.blob_ecc = data_tracked[tunnel]['blob_ecc'] except KeyError: pass tunnel_data_list.append(tunnel_data) tracking_data.tunnel_data = tunnel_data_list self.tracking_data_pub.publish(tracking_data) data_image_cv = cv.fromarray(data_image) try: data_img = self.bridge.cv_to_imgmsg(data_image_cv, "bgr8") data_img.header = data.header self.image_d_pub.publish(data_img) except CvBridgeError, e: print e