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
