def __init__(self, bdf_file): self.bdf_file = bdf_file self.pulse_sequence = 0 self.signal = None self.peaks = None self.frequency = None self.heart_rates = None self.total_average_heart_rate = None self.publisher = PulsePublisher("ecg")
def __init__(self, show_processed_image): self.show_processed_image = show_processed_image self.count = 0 self.levels = 2 self.low = 0.9 self.high = 1.7 self.amplification = 30 self.publisher = PulsePublisher("eulerian_motion_magnification") self.fps = 30 self.video_array = [] self.buffer_size = 0 self.time_array = [] self.calculating_at = 0 self.calculating_boarder = 50 self.recording_time = 10 self.isFirst = True self.arrayLength = 0
def __init__(self, is_video): self.is_video = is_video self.fps = 0 self.buffer_size = 250 self.data_buffer = [] self.times = [] self.ttimes = [] self.samples = [] self.freqs = [] self.fft = [] self.slices = [[0]] self.bpms = [] self.bpm = 0 self.MAX_BPM = 150 self.MIN_BPM = 40 self.pulse_sequence = 0 self.publisher = PulsePublisher("legacy_measurement") self.publish_count = 0
def __init__(self): """ Constructor. """ # set up publisher self.publisher = PulsePublisher("pulse_head_movement") # sequence of published pulse values, published with each pulse message self.published_pulse_value_sequence = 0 # previous image is needed for lucas kanade optical flow tracker (see calculate_optical_flow method) self.previous_image = None # the refresh rate specifies how many frames each calculated feature points should be tracked self.refresh_rate = 300 # the publish rate specifies how many frames are between each published pulse value self.publish_rate = 50 self.frame_index = 0 # current positions of the tracking point for each buffer position # three dimensional array of the form # [buffer_position][point][x/y], so e.g. # [[[x1_b1,y1_b1], [x2_b1,y2_b1],...], # [[x1_b2,y1_b2], [x2_b2,y2_b2],...], # ...] # for each point self.buffer_points = [] # time for each buffer meaning the times associated with the frames of this buffer # two dimensional array of the form # [buffer_position][y_points # [[t1_b1,t2_b1,t3_b1,...] # [t1_b2,t2_b2,t3_b2,...], # ...] self.buffered_time_arrays = [] # y positions over the time for each tracking point for each buffer position # three dimensional array of the form # [[[y1_t1_b1,y1_t2_b1,y1_t3_b1,...],[y2_t1_b1,y2_t2_b1,y2_t3_b1,...],...] # [y1_t1_b2,y1_t2_b2,y1_t3_b2,...],[y2_t1_b2,y2_t2_b2,y2_t3_b2,...],...], # ...] self.buffered_y_tracking_signal = [] self.fps = 0
class LegacyMeasurement(object): def __init__(self, is_video): self.is_video = is_video self.fps = 0 self.buffer_size = 250 self.data_buffer = [] self.times = [] self.ttimes = [] self.samples = [] self.freqs = [] self.fft = [] self.slices = [[0]] self.bpms = [] self.bpm = 0 self.MAX_BPM = 150 self.MIN_BPM = 40 self.pulse_sequence = 0 self.publisher = PulsePublisher("legacy_measurement") self.publish_count = 0 def on_image_frame(self, roi, timestamp): self.publish_count += 1 self.times.append(timestamp.to_sec()) # calculate mean green from roi green_mean = np.mean(self.extractGreenColorChannel(roi)) self.data_buffer.append(green_mean) # get number of frames processed L = len(self.data_buffer) # remove sudden changes, if the avg value change is over 10, use the previous green mean instead if abs(green_mean - np.mean(self.data_buffer)) > 10 and L > 99: self.data_buffer[-1] = self.data_buffer[-2] # only use a max amount of frames. Determined by buffer_size if L > self.buffer_size: self.data_buffer = self.data_buffer[-self.buffer_size:] self.times = self.times[-self.buffer_size:] L = self.buffer_size # create array from average green values of all processed frames processed = np.array(self.data_buffer) # calculate heart rate every 30 frames if L == self.buffer_size and self.publish_count % 30 == 0: # remove linear trend on processed data to avoid interference of light change processed = signal.detrend(processed) # calculate fps self.fps = float(L) / (self.times[-1] - self.times[0]) if self.is_video: interpolated = processed else: # calculate equidistant frame times even_times = np.linspace(self.times[0], self.times[-1], L) # interpolate the values for the even times interpolated = np.interp(x=even_times, xp=self.times, fp=processed) # apply hamming window to make the signal become more periodic interpolated = np.hamming(L) * interpolated # normalize the interpolation norm = interpolated / np.linalg.norm(interpolated) # do a fast fourier transformation on the (real) interpolated values raw = np.fft.rfft(norm) # get amplitude spectrum self.fft = np.abs(raw) ** 2 # create a list for mapping the fft frequencies to the correct bpm self.freqs = (float(self.fps) / L) * np.arange(L / 2 + 1) freqs = 60. * self.freqs # find indeces where the frequencey is within the expected heart rate range idx = np.where((freqs > self.MIN_BPM) & (freqs < self.MAX_BPM)) # reduce fft to "interesting" frequencies self.fft = self.fft[idx] # reduce frequency list to "interesting" frequencies self.freqs = freqs[idx] # find the frequency with the highest amplitude if len(self.fft) > 0: idx2 = np.argmax(self.fft) self.bpm = self.freqs[idx2] self.bpms.append(self.bpm) self.publisher.publish(self.bpm, timestamp) rospy.loginfo("[LegacyMeasurement] BPM: " + str(self.bpm)) self.samples = processed # plot fourrier transform if L == self.buffer_size and self.publish_count % 30 == 0: index = np.arange(len(self.data_buffer)) data = self.data_buffer - np.mean(self.data_buffer) plt.clf() plt.subplot(2, 1, 1) plt.plot(index, data, '.-') plt.title('Green value over time') plt.ylabel('Green value') plt.xlabel('last x frames') index = np.arange(len(self.freqs)) plt.subplot(2, 1, 2) plt.bar(index, self.fft) plt.xlabel('Frequencies (bpm)', fontsize=10) plt.ylabel('Amplitude', fontsize=10) plt.xticks(index, [round(x, 2) for x in self.freqs], fontsize=10, rotation=30) plt.title('Fourier Transformation') plt.draw() plt.pause(0.001) def extractGreenColorChannel(self, frame): return frame[:, :, 1]
class PulseHeadMovement: def __init__(self): """ Constructor. """ # set up publisher self.publisher = PulsePublisher("pulse_head_movement") # sequence of published pulse values, published with each pulse message self.published_pulse_value_sequence = 0 # previous image is needed for lucas kanade optical flow tracker (see calculate_optical_flow method) self.previous_image = None # the refresh rate specifies how many frames each calculated feature points should be tracked self.refresh_rate = 300 # the publish rate specifies how many frames are between each published pulse value self.publish_rate = 50 self.frame_index = 0 # current positions of the tracking point for each buffer position # three dimensional array of the form # [buffer_position][point][x/y], so e.g. # [[[x1_b1,y1_b1], [x2_b1,y2_b1],...], # [[x1_b2,y1_b2], [x2_b2,y2_b2],...], # ...] # for each point self.buffer_points = [] # time for each buffer meaning the times associated with the frames of this buffer # two dimensional array of the form # [buffer_position][y_points # [[t1_b1,t2_b1,t3_b1,...] # [t1_b2,t2_b2,t3_b2,...], # ...] self.buffered_time_arrays = [] # y positions over the time for each tracking point for each buffer position # three dimensional array of the form # [[[y1_t1_b1,y1_t2_b1,y1_t3_b1,...],[y2_t1_b1,y2_t2_b1,y2_t3_b1,...],...] # [y1_t1_b2,y1_t2_b2,y1_t3_b2,...],[y2_t1_b2,y2_t2_b2,y2_t3_b2,...],...], # ...] self.buffered_y_tracking_signal = [] self.fps = 0 def pulse_callback(self, original_image, forehead_mask, bottom_mask, time): """ Callback method for incoming video frames :param mask: the message published to the topic (contains gray-image, mask from bottom part of the face and mask for forehead) """ # rospy.loginfo("Capture frame: " + str(self.frame_index)) self.get_current_tracking_points_position(original_image, time.to_sec()) if self.frame_index % self.publish_rate == 0: self.add_new_points_to_buffer(original_image, forehead_mask, bottom_mask, time) self.frame_index += 1 self.previous_image = original_image def get_current_tracking_points_position(self, current_image, time): """ Get the current position for each tracking point in each buffer position :param current_image: the current image to calculate the optical flow on :param time: the corresponding time for the image frame """ for buffer_index, points in enumerate(self.buffer_points): # calculate new tracking points position for each buffer position new_points = self.calculate_optical_flow(current_image, points) # replace old [x,y] positions of tracking points in buffer_points array self.buffer_points[buffer_index] = new_points # time position of each point meaning the sequential position of the points for each buffer position time_position = ( (self.frame_index - 1) % self.publish_rate) + buffer_index * self.publish_rate # add y point in the time movement for the points for tracking_point_index, point in enumerate(new_points): self.buffered_y_tracking_signal[buffer_index][ tracking_point_index][time_position] = point[0][1] self.buffered_time_arrays[buffer_index][time_position] = time def calculate_optical_flow(self, image, previous_points): """ Calculate the optical flow (track the points) with the lucas kanade tracker See https://docs.opencv.org/3.4/d4/dee/tutorial_optical_flow.html for more information on optical flow :param image: the current image to calculate difference to previous image :param previous_points: the previous position of the points """ # make a copy for visualization vis = image.copy() lk_params = dict(winSize=(35, 35), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 100, 0.03)) new_points, _, _ = cv2.calcOpticalFlowPyrLK(self.previous_image, image, previous_points, None, **lk_params) # visualization of tracking points for p in new_points: cv2.circle(vis, (p[0][0], p[0][1]), 2, (0, 255, 0), -1) cv2.imshow('lk_track', vis) cv2.waitKey(3) return new_points def add_new_points_to_buffer(self, current_image, forehead_mask, bottom_mask, timestamp): """ Add new points to the buffer in the frequency of self.publish_rate and add them to the buffer """ current_points_to_track = self.get_points_to_track( current_image, forehead_mask, bottom_mask) self.edit_buffer(current_points_to_track, timestamp) def get_points_to_track(self, image, forehead_mask, bottom_mask): """ get new tracking points using OpenCV goodFeaturesToTrack. New tracking points are added in the frequency of self.publish_rate For more information on the goodFeaturesToTrack method see https://docs.opencv.org/master/d4/d8c/tutorial_py_shi_tomasi.html and https://docs.opencv.org/2.4/modules/imgproc/doc/feature_detection.html """ # get the tracking points in the bottom face region bottom_feature_params = dict(maxCorners=100, qualityLevel=0.01, minDistance=7, blockSize=7) bottom_points = cv2.goodFeaturesToTrack(image, mask=bottom_mask, **bottom_feature_params) feature_points = np.array(bottom_points, dtype=np.float32) # get the tracking points in the forehead region forehead_feature_params = dict(maxCorners=100, qualityLevel=0.01, minDistance=7, blockSize=7) forehead_points = cv2.goodFeaturesToTrack(image, mask=forehead_mask, **forehead_feature_params) forehead_points = np.array(forehead_points, dtype=np.float32) # put tracking points of both regions in one array and return feature points if feature_points.ndim == forehead_points.ndim: feature_points = np.append(feature_points, forehead_points, axis=0) elif feature_points.size == 0 and forehead_points.size > 0: feature_points = forehead_points return feature_points def edit_buffer(self, current_points_to_track, publish_time): """ If new points are added, the buffer has to be edited. If the buffer is full, points in the last position are removed from the array and are further processed to calculate pulse. The new points are added in the front of the buffer. :param current_points_to_track: the new points to add. [x,y] coordinates are pushed into self.buffer_points. :param publish_time: the timestamp for the ros message to publish the pulse value """ if len(self.buffer_points) < self.refresh_rate / self.publish_rate: # as the buffer is not full, there are no points yet to process. rospy.loginfo("[PulseHeadMovement] array not full yet ") else: # process points on the last array position self.buffer_points.pop() points_calculate_pulse = self.buffered_y_tracking_signal.pop() current_time_array = self.buffered_time_arrays.pop() self.process_saved_points(points_calculate_pulse, current_time_array, publish_time) # push new points in buffer self.buffer_points.insert(0, current_points_to_track) # initialize empty array for the y tracking signal of the new points and the time # and push it to self.buffered_y_tracking_signal and self.buffered_time_array self.buffered_y_tracking_signal.insert( 0, np.empty([len(current_points_to_track), self.refresh_rate], dtype=np.float32)) self.buffered_time_arrays.insert(0, np.empty(self.refresh_rate)) return def process_saved_points(self, y_tracking_signal, time_array, publish_time): """ central method for processing the points, to calculate pulse in the end. :param y_tracking_signal: the tracking signal to calculate pulse on :param time_array: the time of the single positions of the points tracked :param publish_time: the timestamp for the ros message to publish the pulse value """ self.calculate_fps(time_array) stable_signal = self.remove_erratic_trajectories(y_tracking_signal) interpolated_points = self.interpolate_points(stable_signal, time_array) filtered_signal = self.apply_butterworth_filter( interpolated_points, time_array) less_movement = self.discard_much_movement(filtered_signal) pca_array = self.process_PCA(less_movement, time_array) signal, frequency = self.find_most_periodic_signal( pca_array, time_array) pulse = self.calculate_pulse(signal, frequency, time_array) self.publish_pulse(pulse, publish_time) return def calculate_fps(self, time_array): """ Helper method to calculate fps for performance measuring. """ timespan = time_array[-1] - time_array[0] fps = self.refresh_rate / timespan self.fps = fps rospy.loginfo("[PulseHeadMovement] Estimated FPS: " + str(fps) + " (Measured timespan: " + str(timespan) + "s)") def remove_erratic_trajectories(self, y_tracking_signal): """ Some feature points behave unstable. This method removes outliers. """ stable_signal = [] rounded_signal = np.rint(y_tracking_signal) y_point_distance = np.diff(rounded_signal) y_point_distance = np.absolute(y_point_distance) # get the maximum distance for each point max_distances = map(lambda diff: np.amax(diff), y_point_distance) mode, _ = stats.mode(max_distances) # only keep the points with max_distance equal or lower than the mode for point_index, point in enumerate(y_tracking_signal): if max_distances[point_index] <= mode[0]: stable_signal.append(point) stable_signal = np.array(stable_signal) # uncomment the following lines to see the maximum distances each point has moved. For debugging. # xs = np.arange(len(max_distances)) # filename = "/home/studienarbeit/Dokumente/" + str(self.seq) + "max_distance" # plt.figure(figsize=(6.5, 4)) # plt.plot(xs, max_distances, label="S") # plt.savefig(filename) return stable_signal def interpolate_points(self, y_tracking_signal, time_array): """ As the movement of the points is measured with max. 30 fps (normally lower) we interpolate the signal to 250Hz which is the normal sample rate of an ECG. For this, cubic spline interpolation is applied. :param y_tracking_signal: the signal resulting from remove_erratic_trajectories :param time_array: """ # uncomment the following lines to see the signal before interpolation # for a random point (i.e. at position 6). For debugging. # filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step1_before_interp_move_signal_" # plt.figure(figsize=(6.5, 4)) # plt.plot(time_array, y_tracking_signal[6], label="S") # plt.savefig(filename) # plt.close() sample_rate = 250 stepsize = 1. / sample_rate interpolated_time = np.arange(time_array[0], time_array[-1], stepsize) interpolated_points = np.empty( [np.size(y_tracking_signal, 0), np.size(interpolated_time)]) for point_index, row in enumerate(y_tracking_signal): interpolation = CubicSpline(time_array, row) array_interpolated = interpolation(interpolated_time) for interpolated_point_index, point in enumerate( array_interpolated): interpolated_points[point_index][ interpolated_point_index] = point # uncomment the following lines to see the interpolated signal # for a random point (i.e. at position 6). For debugging. # filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step2_move_signal_" # plt.figure(figsize=(6.5, 4)) # plt.plot(interpolated_time, interpolated_points[6], label="S") # plt.savefig(filename) return interpolated_points def apply_butterworth_filter(self, input_signal, time_array): """ Remove the movements which are not in the frequency of the pulse. :param input_signal: the signal resulting from interpolate_points :param time_array: """ sample_rate = len(input_signal[0]) / (time_array[-1] - time_array[0]) rospy.loginfo("[PulseHeadMovement] sample rate: " + str(sample_rate)) lowcut = 0.75 highcut = 5 filtered_signal = np.empty( [np.size(input_signal, 0), np.size(input_signal, 1)]) for point_index, point in enumerate(input_signal): filtered_points = butter_bandpass_filter(point, lowcut, highcut, sample_rate, order=5) for filtered_point_index, filtered_point in enumerate( filtered_points): filtered_signal[point_index][ filtered_point_index] = filtered_point # uncomment the following lines to see the filtered signal # for a random point (i.e. at position 6). For debugging. # filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step3_filter" # stepsize = 1. / sample_rate # xs = np.arange(time_array[0], time_array[-1], stepsize) # plt.figure(figsize=(6.5, 4)) # plt.plot(xs, filtered_signal[6], label="S") # plt.savefig(filename) # plt.close() return filtered_signal def discard_much_movement(self, signal): """ Discard the tracking point with the highest movements. The parameter alpha determines, how much percent of the points should be removed. :param signal: the signal to filter resulting from apply_butterworth_filter """ alpha = 0.25 number_of_rows_to_discard = int(alpha * np.size(signal, 0)) number_of_rows_to_keep = np.size(signal, 0) - number_of_rows_to_discard filtered_signal = np.empty( [number_of_rows_to_keep, np.size(signal, 1)]) square_signal = np.square(signal) square_sum = np.sum(square_signal, axis=1) square_sum = np.squeeze(square_sum) square_root = np.sqrt(square_sum) indices_to_discard = square_root.argsort( )[-number_of_rows_to_discard:][::-1] filtered_signal_index = 0 for row_index, row in enumerate(signal): if row_index not in indices_to_discard: filtered_signal[filtered_signal_index] = row filtered_signal_index += 1 return filtered_signal def process_PCA(self, filtered_signal, time_array): """ process PCA to get the 5 main movement directions of the signal. :param filtered_signal: the signal resulting from discard_much_movement :param time_array: """ filtered_signal_transposed = filtered_signal.transpose() pca = PCA(n_components=5) pca_array = pca.fit_transform(filtered_signal_transposed) pca_array = pca_array.transpose() # uncomment the following lines to see the signal after PCA. For debugging. # filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step4_pca_signal_" # sample_rate = len(filtered_signal[0]) / (time_array[-1] - time_array[0]) # stepsize = 1. / sample_rate # xs = np.arange(time_array[0], time_array[-1], stepsize) # plt.figure(figsize=(6.5, 4)) # for row in pca_array: # plt.plot(xs, row, label="S") # plt.savefig(filename) return pca_array def find_most_periodic_signal(self, pca_array, time_array): """ Find the most periodic signal to use for pulse calculation. Uses the highest value of the autocorrelation funciton :param pca_array: the array resulting from process_PCA :param time_array: """ sample_rate = len(pca_array[0]) / (time_array[-1] - time_array[0]) best_signal = None best_frequency = 0 best_correlation = 0 for ind, signal in enumerate(pca_array): spectrum, frequencies, _ = plt.magnitude_spectrum(signal, Fs=sample_rate) max_index = np.argmax(spectrum) strongest_frequency = frequencies[max_index] # strongest frequency is far too small if strongest_frequency < 0.1: continue T_i = int(sample_rate / strongest_frequency) s_i_new = np.roll(signal, T_i) correlation = stats.pearsonr(s_i_new, signal)[0] if correlation > best_correlation: best_frequency = strongest_frequency best_correlation = correlation best_signal = signal # uncomment the following lines to see the magnitude spectrum of the PCA singal. For debugging. # filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step5_periodic_pca_signal_" + str(ind) # plt.xlim(0, 10) # plt.savefig(filename) # plt.close() # uncomment the following lines to see the most periodic signal of the PCA. For debugging. # filename = "/home/studienarbeit/Dokumente/" + str(self.seq) + "_periodic_pca_signal_" # sample_rate = len(pca_array[0]) / (time_array[-1] - time_array[0]) # stepsize = 1. / sample_rate # pca_array = None # xs = np.arange(time_array[0], time_array[-1], stepsize) # plt.figure(figsize=(6.5, 4)) # plt.plot(xs, best_signal, label="S") # plt.show() # print(best_frequency) return best_signal, best_frequency def calculate_pulse(self, signal, frequency, time_array): """ calculates the pulse value from the processed signal. Uses automatic peak detection and divides number of peaks through measured this is multiplied by 60 to get bpm. :param signal: the finally processed signal """ sample_rate = len(signal) / (time_array[-1] - time_array[0]) distance = 0.5 * (sample_rate / frequency) peaks, _ = find_peaks(signal, distance=distance) measured_time = time_array[-1] - time_array[0] pulse = (len(peaks) / measured_time) * 60 # pulse = np.int16(pulse) rospy.loginfo("[PulseHeadMovement] Pulse: " + str(pulse)) # uncomment the following lines to see the final singal with the detected peaks. For debugging. # stepsize = 1. / sample_rate # xs = np.arange(time_array[0], time_array[-1], stepsize) # filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step6_final_signal_" # plt.figure(figsize=(6.5, 4)) # plt.plot(xs, signal, label="S") # plt.plot(xs[peaks], signal[peaks], "x") # plt.savefig(filename) return pulse def publish_pulse(self, pulse_value, publish_time): """ Publish the calculated pulse to ROS. Message is of type pulse() from pulse_chest_strap package. :param pulse_value: the calculated pulse value :param publish_time: the timestamp of the last incoming frame """ self.publisher.publish(pulse_value, publish_time) self.published_pulse_value_sequence += 1 def show_image_with_mask(self, image, forehead_mask, bottom_mask): """ Helper function to check if ROI is selected correctly """ bottom_dst = cv2.bitwise_and(image, bottom_mask) top_dst = cv2.bitwise_and(image, forehead_mask) dst = cv2.bitwise_or(bottom_dst, top_dst) cv2.imshow("Bottom", dst) cv2.waitKey(3)
def __init__(self): self.publisher = PulsePublisher("pulse_chest_strap") self.start_time = rospy.Time.now()
class PulseChestStrap: def __init__(self): self.publisher = PulsePublisher("pulse_chest_strap") self.start_time = rospy.Time.now() def run(self, addr=None, gatttool="gatttool"): """ main routine to which orchestrates everything """ # number of measured pulse values. Increments for every measured value seq = 0 if addr is None: # A mac address has to be provided as command line argument rospy.logerr( "[PulseChestStrap] MAC address of polar H7 has not been provided" ) return hr_handle = None hr_ctl_handle = None retry = True while retry: while 1: rospy.loginfo("[PulseChestStrap] Establishing connection to " + addr) gt = pexpect.spawn(gatttool + " -b " + addr + " -I") gt.expect(r"\[LE\]>") gt.sendline("connect") try: i = gt.expect(["Connection successful.", r"\[CON\]"], timeout=30) if i == 0: gt.expect(r"\[LE\]>", timeout=30) except pexpect.TIMEOUT: rospy.loginfo( "[PulseChestStrap] Connection timeout. Retrying.") continue except KeyboardInterrupt: rospy.loginfo( "[PulseChestStrap] Received keyboard interrupt. Quitting cleanly." ) retry = False break break if not retry: break rospy.loginfo("[PulseChestStrap] Connected to " + addr) # We determine which handle we should read for getting the heart rate # measurement characteristic. gt.sendline("char-desc") while 1: try: gt.expect(r"handle: (0x[0-9a-f]+), uuid: ([0-9a-f]{8})", timeout=10) except pexpect.TIMEOUT: break handle = gt.match.group(1) uuid = gt.match.group(2) if uuid == b"00002902" and hr_handle: hr_ctl_handle = handle break elif uuid == b"00002a37": hr_handle = handle if hr_handle is None: rospy.logerr( "[PulseChestStrap] Couldn't find the heart rate measurement handle?!" ) return if hr_ctl_handle: # We send the request to get HRM notifications gt.sendline("char-write-req " + hr_ctl_handle.decode("utf-8") + " 0100") # Time period between two measures. This will be updated automatically. period = 1. last_measure = time.time() - period hr_expect = "Notification handle = " + hr_handle.decode( "utf-8") + " value: ([0-9a-f ]+)" while 1: try: gt.expect(hr_expect, timeout=10) except pexpect.TIMEOUT: # If the timer expires, it means that we have lost the # connection with the HR monitor rospy.logwarn("[PulseChestStrap] Connection lost with " + addr + ". Reconnecting.") gt.sendline("quit") try: gt.wait() except: pass time.sleep(1) break except KeyboardInterrupt: rospy.loginfo( "[PulseChestStrap] Received keyboard interrupt. Quitting cleanly." ) retry = False break # We measure here the time between two measures. As the sensor # sometimes sends a small burst, we have a simple low-pass filter # to smooth the measure. tmeasure = time.time() period = period + 1 / 16. * ( (tmeasure - last_measure) - period) last_measure = tmeasure # Get data from gatttool datahex = gt.match.group(1).strip() data = map(lambda x: int(x, 16), datahex.split(b' ')) data = list(data) res = self.interpret(data) rospy.loginfo("[PulseChestStrap] Heart rate: " + str(res["hr"])) self.publisher.publish(res["hr"], rospy.Time.now() - self.start_time) seq += 1 # We quit close the BLE connection properly gt.sendline("quit") try: gt.wait() except: pass def interpret(self, data): """ data is a list of integers corresponding to readings from the BLE HR monitor """ byte0 = data[0] res = {} res["hrv_uint8"] = (byte0 & 1) == 0 sensor_contact = (byte0 >> 1) & 3 if sensor_contact == 2: res["sensor_contact"] = "No contact detected" elif sensor_contact == 3: res["sensor_contact"] = "Contact detected" else: res["sensor_contact"] = "Sensor contact not supported" res["ee_status"] = ((byte0 >> 3) & 1) == 1 res["rr_interval"] = ((byte0 >> 4) & 1) == 1 if res["hrv_uint8"]: res["hr"] = data[1] i = 2 else: res["hr"] = (data[2] << 8) | data[1] i = 3 if res["ee_status"]: res["ee"] = (data[i + 1] << 8) | data[i] i += 2 if res["rr_interval"]: res["rr"] = [] while i < len(data): # Note: Need to divide the value by 1024 to get in seconds res["rr"].append((data[i + 1] << 8) | data[i]) i += 2 return res
class PulseMeasurement: def __init__(self, show_processed_image): self.show_processed_image = show_processed_image self.count = 0 self.levels = 2 self.low = 0.9 self.high = 1.7 self.amplification = 30 self.publisher = PulsePublisher("eulerian_motion_magnification") self.fps = 30 self.video_array = [] self.buffer_size = 0 self.time_array = [] self.calculating_at = 0 self.calculating_boarder = 50 self.recording_time = 10 self.isFirst = True self.arrayLength = 0 def calculate_fps(self): """ calculate fps of incoming frames by calculating time-difference of the first timestamp (first image in array) and last timestamp (last image in array) """ time_difference = self.time_array[-1] - self.time_array[0] time_difference_in_seconds = time_difference.to_sec() if time_difference_in_seconds == 0: pass self.fps = self.buffer_size / time_difference_in_seconds rospy.loginfo("[EulerianMotionMagnification] Estimated FPS: " + str(self.fps) + " (Measured timespan: " + str(time_difference_in_seconds) + "s)") rospy.loginfo("[EulerianMotionMagnification] Video array length: " + str(len(self.video_array))) # calculate pulse after certain amount of images taken, calculation based on a larger amount of time def start_calulation(self, roi, timestamp): # append timestamp to array self.time_array.append(timestamp) # normalize, resize and downsample image normalized = cv2.normalize(roi.astype('float'), None, 0.0, 1.0, cv2.NORM_MINMAX) cropped = cv2.resize(normalized, (200, 200)) gaussian_frame = build_gaussian_frame(cropped, self.levels) if self.show_processed_image: self.video_array.append(gaussian_frame) else: red_values_images = extract_red_values(gaussian_frame, self.show_processed_image) self.video_array.append(red_values_images) # check if recording images took longer than certain amount of time time_difference = self.time_array[-1] - self.time_array[0] time_difference_in_seconds = time_difference.to_sec() if time_difference_in_seconds >= self.recording_time: self.buffer_size = (len(self.time_array)) # determine how many pictures got buffered during time interval # release first image and timestamp self.video_array.pop(0) self.time_array.pop(0) self.calculating_at = self.calculating_at + 1 # calculate again after certain amount of images if self.calculating_at >= self.calculating_boarder: rospy.loginfo("[EulerianMotionMagnification] Length final " + str(len(self.video_array))) self.calculate_fps() copy_video_array = np.copy(self.video_array) copy_video_array = np.asarray(copy_video_array, dtype=np.float32) copy_video_array = temporal_bandpass_filter( copy_video_array, self.low, self.high, self.fps) if self.show_processed_image: self.arrayLength = len(self.video_array) processed_video = amplify_video(copy_video_array, amplify=self.amplification) show_images(processed_video, self.video_array, self.arrayLength, self.isFirst, self.levels, self.calculating_boarder, self.fps) else: copy_video_array = amplify_video( copy_video_array, amplify=self.amplification) pulse, red_values = calculate_pulse(copy_video_array, self.recording_time, self.show_processed_image) self.publisher.publish(pulse, timestamp) self.calculating_at = 0 self.isFirst = False
class BdfProcessor: def __init__(self, bdf_file): self.bdf_file = bdf_file self.pulse_sequence = 0 self.signal = None self.peaks = None self.frequency = None self.heart_rates = None self.total_average_heart_rate = None self.publisher = PulsePublisher("ecg") def run(self): self.signal, self.frequency = self.get_signal(name='EXG2') self.total_average_heart_rate, self.peaks = self.estimate_average_heartrate( self.signal, self.frequency) rospy.loginfo("[BdfProcessor] Total average heart rate: " + str(self.total_average_heart_rate)) self.heart_rates = self.calculate_heart_rates(self.peaks, self.frequency) # Uncomment to plot the ECG signal # self.plot_signal(self.signal, self.frequency, 'EXG2') # plt.tight_layout() # plt.show() def process_frame(self, frame_count, video_fps, timestamp): """ Synchronizes the processing of the video frames and the publishing of the ECG pulse values. :param frame_count: The index of the current processed frame. :param video_fps: The fps of the provided video :param timestamp: The timestamp of the current processed frame. """ if self.pulse_sequence > len(self.heart_rates) - 1: return signal_position = frame_count / float(video_fps) * self.frequency if signal_position >= self.heart_rates[self.pulse_sequence][1]: self.publisher.publish(self.heart_rates[self.pulse_sequence][0], timestamp) self.pulse_sequence += 1 def get_signal(self, name='EXG2'): """ Extracts the ECG signal out of the BDF file. :param name: The name of the channel to extract the signal from. """ reader = pyedflib.EdfReader(self.bdf_file) # Get index of ECG channel index = reader.getSignalLabels().index(name) # Get sample frequency of ECG frequency = reader.getSampleFrequency(index) # Get index of status channel status_index = reader.getSignalLabels().index('Status') # Read status signal status = reader.readSignal(status_index, 0).round().astype('int').nonzero()[0] # Determine start of the video file in signal with status bits video_start = status[0] # Determine the end of the video file. It seems like the status bits are set to 0 before the videos finishing # Therefore ignore the end of the signal and just cut the signal at the beginning. # You can ignore if the signal is longer than the video, the remaining values will not be published. # video_end = status[-1] # video_length = video_end - video_start # Read ECG signal and return as tuple with sample frequency return reader.readSignal(index, video_start), frequency def calculate_heart_rates(self, peaks, sampling_frequency): """ Calculates multiple heart rates out of the signal in equidistant time slots. :param peaks: The detected peaks in the signal. :param sampling_frequency: The sampling frequency of the signal. :return: The calculated heart rates. """ rates = (sampling_frequency * 60) / np.diff(peaks) # Remove instantaneous rates which are lower than 30, higher than 240 selector = (rates > 30) & (rates < 240) rates = rates[selector] heart_rates = [] hr = rates[:10] heart_rates.append((hr.mean(), peaks[10])) for index, rate in enumerate(rates[10:]): hr[:-1] = hr[1:] hr[-1] = rate heart_rates.append((hr.mean(), peaks[index + 11])) return heart_rates def estimate_average_heartrate(self, signal, sampling_frequency): """ Calculates the total average heart rate of the signal. :param peaks: The detected peaks in the signal. :param sampling_frequency: The sampling frequency of the signal. :return: The calculated average heart rate. """ peaks = qrs_detector(sampling_frequency, signal) instantaneous_rates = (sampling_frequency * 60) / np.diff(peaks) # remove instantaneous rates which are lower than 30, higher than 240 selector = (instantaneous_rates > 30) & (instantaneous_rates < 240) return float(np.nan_to_num( instantaneous_rates[selector].mean())), peaks def plot_signal(self, signal, sampling_frequency, channel_name): """ Plots the ECG signal. :param signal: The signal to plot. :param sampling_frequency: The sampling frequency of the signal. :param channel_name: The channel name of the signal in the BDF file. """ avg, peaks = self.estimate_average_heartrate(signal, sampling_frequency) ax = plt.gca() ax.plot(np.arange(0, len(signal) / sampling_frequency, 1 / sampling_frequency), signal, label='Raw signal') xmin, xmax, ymin, ymax = plt.axis() ax.vlines(peaks / sampling_frequency, ymin, ymax, colors='r', label='P-T QRS detector') plt.xlim(0, len(signal) / sampling_frequency) plt.ylabel('uV') plt.xlabel('time (s)') plt.title('Channel %s - Average heart-rate = %d bpm' % (channel_name, avg)) ax.grid(True) ax.legend(loc='best', fancybox=True, framealpha=0.5)