Ejemplo n.º 1
0
  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)
Ejemplo n.º 2
0
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