def begin_collision_detection(self, dynamic_detection=False, callback=None, static_collision_type=None, debug=False): if not self.perception_started: err("Perception hasn't been started!") return False # if not self.detect_ready: # err("ready_collision_detection hasn't been called") # return False self.is_dynamic = dynamic_detection self.callback = callback if not dynamic_detection: self._setup_collision_type(static_collision_type) self.collision_detected = False self.collision_finished = False self.debug = debug self.collision_type = "No collision" self.cur_iter = 0 self.cur_inds = {} for k in self.perceptions: self.cur_inds[k] = 0 self.i_data_buffers = {} self.t_data_buffers = {} for k in self.perceptions: self.i_data_buffers[k] = [ np.zeros( (self.impact_fgc.filter_len, self.impact_fgc.filter_len)) for i in range(self.perception_lengths[k]) ] self.t_data_buffers[k] = [ np.zeros((self.type_fgc.filter_len, self.type_fgc.filter_len)) for i in range(self.perception_lengths[k]) ] self.end_monitor_lock = threading.Lock() self.data_thread_spawner_lock1 = threading.Lock() self.data_thread_spawner_lock2 = threading.Lock() self.data_thread_spawner = RateCaller(self._data_spawn_thread, self.SAMPLING_RATE) self.data_thread_spawner.run() self.beg_time = rospy.Time.now().to_sec() self.t_sum, self.t_num = 0., 0. self.normal_dict_counts = [] self.temp_dict = {} for k in self.perceptions: self.temp_dict[k] = [[] for i in range(self.perception_lengths[k])] return True
def get_zeros(self, time=4.): self._zeros = {} for k in self.perceptions: self._zeros[k] = None self._n = 0 monitor = RateCaller(self._sum_values, self.rate) monitor.run() rospy.sleep(time) monitor.stop() for k in self.perceptions: self._zeros[k] /= self._n return self._zeros
def start_data_capture(self, duration=None): if not self.perception_started: err("Perception hasn't been started!") return False self.logger = RateCaller(self._gather_perception, self.SAMPLING_RATE) for k in self.perceptions: self.datasets[k] += [[]] self.logger.run() if not duration is None: threading.Timer(self.stop_data_capture, duration) return True
def begin_monitoring(self, models=None, model_zeros=None, std_dev_dict=None, noise_dev_dict=None, duration=None, std_dev_default=2.0, noise_dev_default=0.25, tol_thresh_dict=None, contingency=None, window_size=70, current_zeros=None, sampling_rate = 1, only_pressure=False, transform_dict=None, verbose=True, collide=True): if self.active: log("Perception already active.") return self.active = True self.setup_monitoring(models, std_dev_dict=std_dev_dict, noise_dev_dict=noise_dev_dict, duration=duration, std_dev_default=std_dev_default, noise_dev_default=noise_dev_default, tol_thresh_dict=tol_thresh_dict, contingency=contingency, window_size=window_size, sampling_rate=sampling_rate, current_zeros=current_zeros, transform_dict=transform_dict, verbose=verbose, collide=collide) # if models is not None: # self.models = models if model_zeros is not None: self.model_zeros = model_zeros self.cur_pt = 0 self.collision_times = {} self.collision_sums = {} self.cur_col_time = 0 self.current_zeros = {} self.z_sum = {} self.z_rsum = {} self.z_list = {} self.min_prob = 100000.0 self.cur_mals = {} self.mals = None self.only_pressure = only_pressure self.monitor = RateCaller(self._monitor_data, self.rate * self.sampling_rate) self.monitor.run() # self.sumcalls = {} # self.sumsave = {} if not duration is None: self.dur_timer = threading.Timer(self.end_monitoring, duration) self.dur_timer.start()
def begin_collision_detection(self, dynamic_detection=False, callback=None, static_collision_type=None, debug=False): if not self.perception_started: err("Perception hasn't been started!") return False # if not self.detect_ready: # err("ready_collision_detection hasn't been called") # return False self.is_dynamic = dynamic_detection self.callback = callback if not dynamic_detection: self._setup_collision_type(static_collision_type) self.collision_detected = False self.collision_finished = False self.debug = debug self.collision_type = "No collision" self.cur_iter = 0 self.cur_inds = {} for k in self.perceptions: self.cur_inds[k] = 0 self.i_data_buffers = {} self.t_data_buffers = {} for k in self.perceptions: self.i_data_buffers[k] = [ np.zeros((self.impact_fgc.filter_len, self.impact_fgc.filter_len)) for i in range(self.perception_lengths[k])] self.t_data_buffers[k] = [ np.zeros((self.type_fgc.filter_len, self.type_fgc.filter_len)) for i in range(self.perception_lengths[k])] self.end_monitor_lock = threading.Lock() self.data_thread_spawner_lock1 = threading.Lock() self.data_thread_spawner_lock2 = threading.Lock() self.data_thread_spawner = RateCaller(self._data_spawn_thread, self.SAMPLING_RATE ) self.data_thread_spawner.run() self.beg_time = rospy.Time.now().to_sec() self.t_sum, self.t_num = 0., 0. self.normal_dict_counts = [] self.temp_dict = {} for k in self.perceptions: self.temp_dict[k] = [[] for i in range(self.perception_lengths[k])] return True
class ArmPerceptionMonitor( ): ## # Initializes internal variables # # @param arm 0 if right, 1 if left # @param percept_mon_list list of perceptions to monitor; if None, do all def __init__(self, arm): self.load_parameters() ############## Classifiers ################# self.impact_classifier = cfiers.classifiers_dict["small_refined_random_forest"] self.coll_type_classifier = cfiers.classifiers_dict["large_refined_random_forest"] log("Loading impact classifier") self.impact_classifier.load(self.I_RTREE_CLASSIFIER) log("Loading collision type classifier") self.coll_type_classifier.load(self.T_RTREE_CLASSIFIER) if arm == 0: self.armc = "r" self.is_right_arm = True else: self.armc = "l" self.is_right_arm = False self.perceptions = {} self.perception_names = [ "accelerometer", "joint_angles", "joint_velocities", "joint_efforts", "r_finger_periph_pressure", "r_finger_pad_pressure", "l_finger_periph_pressure", "l_finger_pad_pressure", "gripper_pose"] self.perception_lengths = { "accelerometer" : 3, "joint_angles" : 7, "joint_velocities" : 7, "joint_efforts" : 7, "r_finger_periph_pressure" : 1, "r_finger_pad_pressure" : 1, "l_finger_periph_pressure" : 1, "l_finger_pad_pressure" : 1, "gripper_pose" : 7} self.percept_mon_list = self.perception_names self.impact_fgc = FastGaussConvolve(self.SAMPLING_RATE, self.I_FILTER_BEG_DELAY, self.I_FILTER_CEN_DELAY, self.i_sigma_list) self.type_fgc = FastGaussConvolve(self.SAMPLING_RATE, self.T_FILTER_BEG_DELAY, self.T_FILTER_CEN_DELAY, self.t_sigma_list) self.perception_started = False # self.detect_ready = False self.fos = FileOperations() def load_parameters(self): self.SAMPLING_RATE = rospy.get_param("/overhead_grasping/data_sampling_rate") self.PERCEPTION_ORDER= rospy.get_param("/overhead_grasping/perception_order") # impact parameters self.I_FILTER_BEG_DELAY = rospy.get_param("/overhead_grasping/i_filter_beg_delay") self.I_FILTER_CEN_DELAY = rospy.get_param("/overhead_grasping/i_filter_cen_delay") self.I_MIN_SIGMA = rospy.get_param("/overhead_grasping/i_min_sigma") self.I_MAX_SIGMA = rospy.get_param("/overhead_grasping/i_max_sigma") self.I_NUM_SIGMA = rospy.get_param("/overhead_grasping/i_num_sigma") self.i_sigma_list = np.linspace(self.I_MIN_SIGMA, self.I_MAX_SIGMA, self.I_NUM_SIGMA) self.I_DEGREE_DICT = rospy.get_param("/overhead_grasping/i_degree_dict") self.i_instance_len = 0 for k in self.PERCEPTION_ORDER: for coord in self.I_DEGREE_DICT[k]: for deg in coord: self.i_instance_len += self.I_NUM_SIGMA self.I_RTREE_CLASSIFIER = rospy.get_param("/overhead_grasping/i_rtree_classifier") # collision type parameters self.T_FILTER_BEG_DELAY = rospy.get_param("/overhead_grasping/t_filter_beg_delay") self.T_FILTER_CEN_DELAY = rospy.get_param("/overhead_grasping/t_filter_cen_delay") self.T_MIN_SIGMA = rospy.get_param("/overhead_grasping/t_min_sigma") self.T_MAX_SIGMA = rospy.get_param("/overhead_grasping/t_max_sigma") self.T_NUM_SIGMA = rospy.get_param("/overhead_grasping/t_num_sigma") self.t_sigma_list = np.linspace(self.T_MIN_SIGMA, self.T_MAX_SIGMA, self.T_NUM_SIGMA) self.T_DEGREE_DICT = rospy.get_param("/overhead_grasping/t_degree_dict") self.t_instance_len = 0 for k in self.PERCEPTION_ORDER: for coord in self.T_DEGREE_DICT[k]: for deg in coord: self.t_instance_len += self.T_NUM_SIGMA self.T_RTREE_CLASSIFIER = rospy.get_param("/overhead_grasping/t_rtree_classifier") self.STATIC_SIGMA_INDEX = rospy.get_param("/overhead_grasping/static_sigma_index") self.STATIC_DERIV_THRESH = rospy.get_param("/overhead_grasping/static_deriv_thresh") ## # Initializes the listeners on the sensor topics. This must # be started before any data can be collected from the arms. def activate_sensing(self, tf_listener=None): log("Initializing arm perception listeners") self.tf_listener = tf_listener if "accelerometer" in self.percept_mon_list: accel_listener = GenericListener("accel_mon_node", AccelerometerState, "accelerometer/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, accel_state_processor) self.perceptions["accelerometer"] = accel_listener.read if "joint_angles" in self.percept_mon_list: joint_angles_processor = ft.partial(joints_state_processor, right_arm=self.is_right_arm, angles_velocities_efforts=0) joint_angles_listener = GenericListener("joint_angles_mon_node", JointState, "joint_states", self.SAMPLING_RATE, joint_angles_processor) self.perceptions["joint_angles"] = joint_angles_listener.read if "joint_velocities" in self.percept_mon_list: joint_velocities_processor = ft.partial(joints_state_processor, right_arm=self.is_right_arm, angles_velocities_efforts=1) joint_velocities_listener = GenericListener("joint_vels_mon_node", JointState, "joint_states", self.SAMPLING_RATE, joint_velocities_processor) self.perceptions["joint_velocities"] = joint_velocities_listener.read if "joint_efforts" in self.percept_mon_list: joint_efforts_processor = ft.partial(joints_state_processor, right_arm=self.is_right_arm, angles_velocities_efforts=2) joint_efforts_listener = GenericListener("joint_efforts_mon_node", JointState, "joint_states", self.SAMPLING_RATE, joint_efforts_processor) self.perceptions["joint_efforts"] = joint_efforts_listener.read if "r_finger_periph_pressure" in self.percept_mon_list: r_finger_periph_pressure_processor = ft.partial(pressure_state_sum_processor, right_finger_tip=True, indicies=range(1,7)) r_finger_periph_pressure_listener = GenericListener(self.armc + "_pressure_r_periph_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, r_finger_periph_pressure_processor) self.perceptions["r_finger_periph_pressure"] = r_finger_periph_pressure_listener.read if "r_finger_pad_pressure" in self.percept_mon_list: r_finger_pad_pressure_processor = ft.partial(pressure_state_sum_processor, right_finger_tip=True, indicies=range(7,22)) r_finger_pad_pressure_listener = GenericListener(self.armc + "_pressure_r_pad_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, r_finger_pad_pressure_processor) self.perceptions["r_finger_pad_pressure"] = r_finger_pad_pressure_listener.read if "l_finger_periph_pressure" in self.percept_mon_list: l_finger_periph_pressure_processor = ft.partial(pressure_state_sum_processor, right_finger_tip=False, indicies=range(1,7)) l_finger_periph_pressure_listener = GenericListener(self.armc + "_pressure_l_periph_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, l_finger_periph_pressure_processor) self.perceptions["l_finger_periph_pressure"] = l_finger_periph_pressure_listener.read if "l_finger_pad_pressure" in self.percept_mon_list: l_finger_pad_pressure_processor = ft.partial(pressure_state_sum_processor, right_finger_tip=False, indicies=range(7,22)) l_finger_pad_pressure_listener = GenericListener(self.armc + "_pressure_l_pad_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, l_finger_pad_pressure_processor) self.perceptions["l_finger_pad_pressure"] = l_finger_pad_pressure_listener.read for k in self.perceptions: # Make sure we have recieved a message first self.perceptions[k](allow_duplication=False, willing_to_wait=True) self.perceptions[k] = ft.partial(self.perceptions[k], willing_to_wait=False, quiet=True, warn=False, allow_duplication=True) if "gripper_pose" in self.percept_mon_list: def gripper_pose_lookup(): if self.tf_listener is not None: lct = self.tf_listener.getLatestCommonTime("/torso_lift_link", "/" + self.armc + "_wrist_roll_link") pos, quat = self.tf_listener.lookupTransform("/torso_lift_link", "/" + self.armc + "_wrist_roll_link", lct) return (lct.to_sec(), np.array(pos + quat)) else: return (0., np.array([0.]*7)) self.perceptions["gripper_pose"] = gripper_pose_lookup # all callbacks should return data in this format: # (t, np.array([x1, x2, ...])) # t is time in seconds self.clear_vars() self.perception_started = True log("Finished initialization") ## # Initialize variables def clear_vars(self): self.datasets = {} self.models = {} for k in self.perceptions: self.datasets[k] = [] self.models[k] = {"mean" : None, "variance" : None} ## # Begin capturing peception data for all of the listeners # # @param duration If None, continue capturing until stop is called. # Else, stop capturing after duration seconds have passed. def start_data_capture(self, duration=None): if not self.perception_started: err("Perception hasn't been started!") return False self.logger = RateCaller(self._gather_perception, self.SAMPLING_RATE) for k in self.perceptions: self.datasets[k] += [[]] self.logger.run() if not duration is None: threading.Timer(self.stop_data_capture, duration) return True def _gather_perception(self): for k in self.perceptions: self.datasets[k][-1].append(self.perceptions[k]()) ## # Stop capturing perception data. Store output data in datasets list for # later statistics. def stop_data_capture(self): self.logger.stop() num_datapts = len(self.datasets[self.datasets.keys()[0]][-1]) log("%d datapoints collected", num_datapts) return num_datapts ## # do processing of signals in current coord group # sigs_proc contains a list of three signal sets # each corresponds to a list of convolutions of the signals # with a degree derivative of a gaussian with various # scales def process_signals(self, datasets): models = {} sum, num = 0., 0. for p in datasets: models[p] = {} i_sigs_proc = [ [[] for x in range(self.I_NUM_SIGMA)] for y in range(3)] t_sigs_proc = [ [[] for x in range(self.T_NUM_SIGMA)] for y in range(3)] data_list = datasets[p] num_coords = len(data_list[0][0][1]) times = None for coord in range(num_coords): stream = data_list[0] def conv(v): if type(v) == type([]) or type(v) == type(np.array([])): return v[0] return v signal = [conv(v) for v in zip(*zip(*stream)[1])[coord]] time_ind = np.array(zip(*stream)[0], dtype=np.float64) # normalize time indicies by offsetting to first value time_ind_beg = time_ind[0] for i, v in enumerate(time_ind): time_ind[i] = float(time_ind[i] - time_ind_beg) times = time_ind for deg in range(3): for sigma_ind, sigma in enumerate(self.i_sigma_list): s = self.impact_fgc.convolve_signal(signal, deg, sigma_ind) i_sigs_proc[deg][sigma_ind].append(s) for sigma_ind, sigma in enumerate(self.t_sigma_list): s = self.type_fgc.convolve_signal(signal, deg, sigma_ind) t_sigs_proc[deg][sigma_ind].append(s) if rospy.is_shutdown(): sys.exit() models[p]["impact_features"] = [ [[] for x in range(self.I_NUM_SIGMA)] for y in range(3)] models[p]["type_features"] = [ [[] for x in range(self.T_NUM_SIGMA)] for y in range(3)] for deg in range(3): for sigma in range(self.I_NUM_SIGMA): models[p]["impact_features"][deg][sigma] = zip(*i_sigs_proc[deg][sigma]) for sigma in range(self.T_NUM_SIGMA): models[p]["type_features"][deg][sigma] = zip(*t_sigs_proc[deg][sigma]) models[p]["time"] = times models[p]["i_sigma_list"] = self.i_sigma_list models[p]["t_sigma_list"] = self.t_sigma_list return models # def ready_collision_detection(self): # # wait for the classifiers to finish loading # self.impact_classifier.finish_loading() # # self.coll_type_classifier.finish_loading() # self.detect_ready = True ## # Begin monitoring peception data to make sure it doesn't deviate from # the model provided. # # TODO DOCS # @param duration If None, continue capturing until stop is called. # Else, stop capturing after duration seconds have passed. def begin_collision_detection(self, dynamic_detection=False, callback=None, static_collision_type=None, debug=False): if not self.perception_started: err("Perception hasn't been started!") return False # if not self.detect_ready: # err("ready_collision_detection hasn't been called") # return False self.is_dynamic = dynamic_detection self.callback = callback if not dynamic_detection: self._setup_collision_type(static_collision_type) self.collision_detected = False self.collision_finished = False self.debug = debug self.collision_type = "No collision" self.cur_iter = 0 self.cur_inds = {} for k in self.perceptions: self.cur_inds[k] = 0 self.i_data_buffers = {} self.t_data_buffers = {} for k in self.perceptions: self.i_data_buffers[k] = [ np.zeros((self.impact_fgc.filter_len, self.impact_fgc.filter_len)) for i in range(self.perception_lengths[k])] self.t_data_buffers[k] = [ np.zeros((self.type_fgc.filter_len, self.type_fgc.filter_len)) for i in range(self.perception_lengths[k])] self.end_monitor_lock = threading.Lock() self.data_thread_spawner_lock1 = threading.Lock() self.data_thread_spawner_lock2 = threading.Lock() self.data_thread_spawner = RateCaller(self._data_spawn_thread, self.SAMPLING_RATE ) self.data_thread_spawner.run() self.beg_time = rospy.Time.now().to_sec() self.t_sum, self.t_num = 0., 0. self.normal_dict_counts = [] self.temp_dict = {} for k in self.perceptions: self.temp_dict[k] = [[] for i in range(self.perception_lengths[k])] return True def _setup_collision_type(self, static_collision_type): if static_collision_type == "all": static_dict = {"accelerometer" : range(3), "joint_angles" : range(7), "joint_velocities" : range(7), "joint_efforts" : range(7), "r_finger_periph_pressure" : range(1), "r_finger_pad_pressure" : range(1), "l_finger_periph_pressure" : range(1), "l_finger_pad_pressure" : range(1), "gripper_pose" : range(7)} elif static_collision_type in self.perception_names: static_dict = {static_collision_type : range( self.perception_lengths[static_collision_type])} elif static_collision_type == "pressure": static_dict = {"r_finger_periph_pressure" : range(1), "r_finger_pad_pressure" : range(1), "l_finger_periph_pressure" : range(1), "l_finger_pad_pressure" : range(1)} elif type(static_collision_type) == type({}): static_dict = static_collision_type else: err("Bad static_collision_type") self.static_dict = static_dict def _data_spawn_thread(self): # only one thread allowed to wait at a time # all other threads will pass through if self.data_thread_spawner_lock1.acquire(False): if self.data_thread_spawner_lock2.acquire(True): self.data_thread_spawner_lock1.acquire(False) self.data_thread_spawner_lock1.release() mt = self.DataThread(self) mt.start() class DataThread(threading.Thread): def __init__(self, apm): threading.Thread.__init__(self) self.apm = apm def run(self): self.apm._monitor_data_collector() self.apm.data_thread_spawner_lock2.acquire(False) self.apm.data_thread_spawner_lock2.release() def _monitor_data_collector(self): st_time = rospy.Time.now().to_sec() ######################################### if rospy.is_shutdown(): self._trigger_collision() self._finish_collision("rospy shutdown") add_data = [] for k in self.perceptions: p_vals = self.perceptions[k]()[1] for i, v in enumerate(p_vals): # populate impact collision buffers for b in range(self.impact_fgc.filter_len - 1, -1, -1): self.i_data_buffers[k][i][( b - self.cur_iter) % self.impact_fgc.filter_len, b] = v if self.debug: if b == 0 and k == "joint_angles": # and ((i == 0) or (i == 1)): print v, # populate collision type detection buffers for b in range(self.type_fgc.filter_len - 1, -1, -1): self.t_data_buffers[k][i][( b - self.cur_iter) % self.type_fgc.filter_len, b] = v if self.debug: print if self.is_dynamic: if not self.collision_detected: self._dynamic_collision_detection() else: self._dynamic_collision_classifying() else: self._static_collision_detection() self.cur_iter += 1 ######################################### end_time = rospy.Time.now().to_sec() self.t_sum += end_time - st_time self.t_num += 1. def _dynamic_collision_detection(self): if self.cur_iter < self.impact_fgc.filter_len: return instance = np.zeros((self.i_instance_len, 1)) i_place = 0 for k in self.PERCEPTION_ORDER: for coord in range(len(self.i_data_buffers[k])): for deg in range(3): if deg not in self.I_DEGREE_DICT[k][coord]: continue for sigma_i, sigma_t in enumerate(self.i_sigma_list): val = self.impact_fgc.convolve_pt( self.i_data_buffers[k][coord][( self.cur_iter % self.impact_fgc.filter_len)], deg, sigma_i) instance[i_place, 0] = val i_place += 1 has_collided = self.impact_classifier.predict(instance) if has_collided and not self.debug: if not self.collision_detected: self._trigger_collision() def _dynamic_collision_classifying(self): if self.cur_iter < self.type_fgc.filter_len: # TODO HANDLE THIS CASE return if ((self.cur_iter - self.coll_iter) > self.type_fgc.filter_len / 2): return instance = np.zeros((self.t_instance_len, 1)) i_place = 0 for k in self.PERCEPTION_ORDER: for coord in range(len(self.t_data_buffers[k])): for deg in range(3): if deg not in self.T_DEGREE_DICT[k][coord]: continue for sigma_i, sigma_t in enumerate(self.t_sigma_list): val = self.type_fgc.convolve_pt( self.t_data_buffers[k][coord][( self.cur_iter % self.type_fgc.filter_len)], deg, sigma_i) instance[i_place, 0] = val i_place += 1 collision_class = self.coll_type_classifier.predict(instance) if collision_class == 1.: type = "Environmental collision" else: type = "Table collision" self._finish_collision(type) def _static_collision_detection(self): if self.cur_iter < self.impact_fgc.filter_len: return for k in self.PERCEPTION_ORDER: if k not in self.static_dict: continue for coord in self.static_dict[k]: if k == "gripper_pose" and coord >= 3: # Quaternions are unreliable, this should be fixed # at some point if self.debug: self.temp_dict[k][coord].append(0.) continue val = self.impact_fgc.convolve_pt(self.i_data_buffers[k][coord][self.cur_iter % self.impact_fgc.filter_len], 1, self.STATIC_SIGMA_INDEX) if self.debug: self.temp_dict[k][coord].append(val) if (np.fabs(val) >= self.STATIC_DERIV_THRESH[k][coord] and not self.debug): print "Collision:", val, "Thresh:", self.STATIC_DERIV_THRESH[k][coord] self._trigger_collision() self._finish_collision(k + " collision", coord) # if k == "r_finger_pad_pressure" and random.randint(0,40) == 0: # print val def _trigger_collision(self): self.collision_detected = True self.coll_iter = self.cur_iter - self.impact_fgc.filter_len / 2 if not self.callback is None: self.callback() def _finish_collision(self, type = "Unlabled", coord = 0): if not self.collision_finished: print '-' * 60 print print ' %s detected' % type print print '-' * 60 log("%s reported" % type) self.collision_type = type self.collision_coord = coord if not self.callback is None: self.callback() self.collision_finished = True self.end_collision_detection() ## # Stop capturing perception data. Store output data in datasets list for # later statistics. def end_collision_detection(self): while (not self.collision_finished and self.collision_detected and not rospy.is_shutdown()): rospy.sleep(0.01) with self.end_monitor_lock: if not self.data_thread_spawner is None: print "Avg cvpt: %1.9f" % (self.t_sum / self.t_num) print "cvpt sum: %1.5f" % (self.t_sum) print self.t_num self.data_thread_spawner.stop() self.data_thread_spawner_lock1.acquire(False) self.data_thread_spawner_lock1.release() self.data_thread_spawner_lock2.acquire(False) self.data_thread_spawner_lock2.release() self.data_thread_spawner = None return
def __init__(self, arm, tf_listener=None, rate=0.001, percept_mon_list=None, model_zeros=None): log("Initializing arm perception listeners") self.rate = rate if arm == 0: armc = "r" is_right_arm = True else: armc = "l" is_right_arm = False self.model_zeros = model_zeros self.perceptions = {} self.perception_names = [ "accelerometer", "joint_angles", "joint_velocities", "joint_efforts", "r_finger_periph_pressure", "r_finger_pad_pressure", "l_finger_periph_pressure", "l_finger_pad_pressure", "gripper_pose"] if percept_mon_list is None: percept_mon_list = self.perception_names if "accelerometer" in percept_mon_list: accel_listener = GenericListener("accel_mon_node", AccelerometerState, "accelerometer/" + armc + "_gripper_motor", rate, accel_state_processor) self.perceptions["accelerometer"] = accel_listener.read if "joint_angles" in percept_mon_list: joint_angles_processor = ft.partial(joints_state_processor, right_arm=is_right_arm, angles_velocities_efforts=0) joint_angles_listener = GenericListener("joint_angles_mon_node", JointState, "joint_states", rate, joint_angles_processor) self.perceptions["joint_angles"] = joint_angles_listener.read if "joint_velocities" in percept_mon_list: joint_velocities_processor = ft.partial(joints_state_processor, right_arm=is_right_arm, angles_velocities_efforts=1) joint_velocities_listener = GenericListener("joint_efforts_mon_node", JointState, "joint_states", rate, joint_velocities_processor) self.perceptions["joint_velocities"] = joint_velocities_listener.read if "joint_efforts" in percept_mon_list: joint_efforts_processor = ft.partial(joints_state_processor, right_arm=is_right_arm, angles_velocities_efforts=2) joint_efforts_listener = GenericListener("joint_efforts_mon_node", JointState, "joint_states", rate, joint_efforts_processor) self.perceptions["joint_efforts"] = joint_efforts_listener.read if "r_finger_periph_pressure" in percept_mon_list: r_finger_periph_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=True, indicies=range(1,7)) r_finger_periph_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, r_finger_periph_pressure_processor) self.perceptions["r_finger_periph_pressure"] = r_finger_periph_pressure_listener.read if "r_finger_pad_pressure" in percept_mon_list: r_finger_pad_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=True, indicies=range(7,22)) r_finger_pad_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, r_finger_pad_pressure_processor) self.perceptions["r_finger_pad_pressure"] = r_finger_pad_pressure_listener.read if "l_finger_periph_pressure" in percept_mon_list: l_finger_periph_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=False, indicies=range(1,7)) l_finger_periph_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, l_finger_periph_pressure_processor) self.perceptions["l_finger_periph_pressure"] = l_finger_periph_pressure_listener.read if "l_finger_pad_pressure" in percept_mon_list: l_finger_pad_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=False, indicies=range(7,22)) l_finger_pad_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, l_finger_pad_pressure_processor) self.perceptions["l_finger_pad_pressure"] = l_finger_pad_pressure_listener.read for k in self.perceptions: self.perceptions[k] = ft.partial(self.perceptions[k], willing_to_wait=False, quiet=True, warn=False, allow_duplication=True) if "gripper_pose" in percept_mon_list: def gripper_pose_lookup(): lct = tf_listener.getLatestCommonTime("/torso_lift_link", "/" + armc + "_wrist_roll_link") pos, quat = tf_listener.lookupTransform("/torso_lift_link", "/" + armc + "_wrist_roll_link", lct) return (lct.to_nsec(), np.array(pos + quat)) self.perceptions["gripper_pose"] = gripper_pose_lookup # all callbacks should return data in this format: # (t, np.array([x1, x2, ...])) # t is time in nanoseconds self.clear_vars() self.logger = RateCaller(self._gather_perception, self.rate) log("Finished initialization")
class ArmPerceptionMonitor( ): ## # Initializes the listeners on the perception topics # # @param arm 0 if right, 1 if left # @param rate the rate at which the perception should capture states # @param percept_mon_list list of perceptions to monitor; if None, do all def __init__(self, arm, tf_listener=None, rate=0.001, percept_mon_list=None, model_zeros=None): log("Initializing arm perception listeners") self.rate = rate if arm == 0: armc = "r" is_right_arm = True else: armc = "l" is_right_arm = False self.model_zeros = model_zeros self.perceptions = {} self.perception_names = [ "accelerometer", "joint_angles", "joint_velocities", "joint_efforts", "r_finger_periph_pressure", "r_finger_pad_pressure", "l_finger_periph_pressure", "l_finger_pad_pressure", "gripper_pose"] if percept_mon_list is None: percept_mon_list = self.perception_names if "accelerometer" in percept_mon_list: accel_listener = GenericListener("accel_mon_node", AccelerometerState, "accelerometer/" + armc + "_gripper_motor", rate, accel_state_processor) self.perceptions["accelerometer"] = accel_listener.read if "joint_angles" in percept_mon_list: joint_angles_processor = ft.partial(joints_state_processor, right_arm=is_right_arm, angles_velocities_efforts=0) joint_angles_listener = GenericListener("joint_angles_mon_node", JointState, "joint_states", rate, joint_angles_processor) self.perceptions["joint_angles"] = joint_angles_listener.read if "joint_velocities" in percept_mon_list: joint_velocities_processor = ft.partial(joints_state_processor, right_arm=is_right_arm, angles_velocities_efforts=1) joint_velocities_listener = GenericListener("joint_efforts_mon_node", JointState, "joint_states", rate, joint_velocities_processor) self.perceptions["joint_velocities"] = joint_velocities_listener.read if "joint_efforts" in percept_mon_list: joint_efforts_processor = ft.partial(joints_state_processor, right_arm=is_right_arm, angles_velocities_efforts=2) joint_efforts_listener = GenericListener("joint_efforts_mon_node", JointState, "joint_states", rate, joint_efforts_processor) self.perceptions["joint_efforts"] = joint_efforts_listener.read if "r_finger_periph_pressure" in percept_mon_list: r_finger_periph_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=True, indicies=range(1,7)) r_finger_periph_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, r_finger_periph_pressure_processor) self.perceptions["r_finger_periph_pressure"] = r_finger_periph_pressure_listener.read if "r_finger_pad_pressure" in percept_mon_list: r_finger_pad_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=True, indicies=range(7,22)) r_finger_pad_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, r_finger_pad_pressure_processor) self.perceptions["r_finger_pad_pressure"] = r_finger_pad_pressure_listener.read if "l_finger_periph_pressure" in percept_mon_list: l_finger_periph_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=False, indicies=range(1,7)) l_finger_periph_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, l_finger_periph_pressure_processor) self.perceptions["l_finger_periph_pressure"] = l_finger_periph_pressure_listener.read if "l_finger_pad_pressure" in percept_mon_list: l_finger_pad_pressure_processor = ft.partial(pressure_state_processor, right_finger_tip=False, indicies=range(7,22)) l_finger_pad_pressure_listener = GenericListener("pressure_mon_node", PressureState, "pressure/" + armc + "_gripper_motor", rate, l_finger_pad_pressure_processor) self.perceptions["l_finger_pad_pressure"] = l_finger_pad_pressure_listener.read for k in self.perceptions: self.perceptions[k] = ft.partial(self.perceptions[k], willing_to_wait=False, quiet=True, warn=False, allow_duplication=True) if "gripper_pose" in percept_mon_list: def gripper_pose_lookup(): lct = tf_listener.getLatestCommonTime("/torso_lift_link", "/" + armc + "_wrist_roll_link") pos, quat = tf_listener.lookupTransform("/torso_lift_link", "/" + armc + "_wrist_roll_link", lct) return (lct.to_nsec(), np.array(pos + quat)) self.perceptions["gripper_pose"] = gripper_pose_lookup # all callbacks should return data in this format: # (t, np.array([x1, x2, ...])) # t is time in nanoseconds self.clear_vars() self.logger = RateCaller(self._gather_perception, self.rate) log("Finished initialization") def _gather_perception(self): t1 = rospy.Time.now().to_sec() for k in self.perceptions: self.datasets[k][-1] += [self.perceptions[k]()] t2 = rospy.Time.now().to_sec() import random if random.randint(0,100)==0: print "Time:", t1-t2 ## # Initialize variables def clear_vars(self): self.datasets = {} self.models = {} for k in self.perceptions: self.datasets[k] = [] self.models[k] = {"mean" : None, "variance" : None} self.active = False ## # Begin capturing peception data for all of the listeners # # @param duration If None, continue capturing until stop is called. # Else, stop capturing after duration seconds have passed. def start_training(self, duration=None): if self.active: log("Perception already active.") return self.active = True for k in self.perceptions: self.datasets[k] += [[]] self.logger.run() if not duration is None: threading.Timer(self.stop_training, duration) ## # Stop capturing perception data. Store output data in datasets list for # later statistics. def stop_training(self): if not self.active: log("Nothing to stop.") return self.logger.stop() self.active = False return len(self.datasets[self.datasets.keys()[0]][-1]) ## # Save training data as a pickle with given filename # # @param filename name of the pickle def save(self, filename): save_pickle((self.datasets, self.models), filename) ## # Load training data as a pickle with given filename # # @param filename name of the pickle def load(self, filename): self.datasets, self.models = load_pickle(filename) ## # Generates model functions of all the perceptions over several # identical trajectories. Each of the parameters is a dictionary # directing perceptions to their parameters. # # @param smooth_wind_dict the window size of the smoothing function # @param var_wind_dict window size of the variance function # @param var_smooth_wind_dict window size of the smoothing function on the variance # @return mean function, variance function # def generate_models(self, smooth_wind_dict=None, var_wind_dict=None): # smooth_wind_default = 234 # var_wind_default = 400 # var_smooth_wind = 143 # for perception in self.perceptions: # data_list = self.datasets[perception] # # if data_list is None: # log("No data to generate model for") # return None # if self.active: # log("Perception already active.") # return None # # get the minimum model length # lens = [len(m) for m in data_list] # max_len = np.max(lens) # # dynamic finding of parameters # if smooth_wind_dict is None or smooth_wind_dict[perception] is None: # smooth_wind = smooth_wind_default # else: # smooth_wind = smooth_wind_dict[perception] # ret_means, ret_vars, ret_mean_models, ret_noise_vars = [], [], [], [] # ret_times, noise_vars = [], [] # # find the number of coordinates from the first element # num_coords = len(data_list[0][0][1]) # for coord in range(num_coords): # mean_models, variance_models = [], [] # times = None # for stream in data_list: # # extract only the data stream for a single coordinate (all x values) # stream_coord = np.array(zip(*zip(*stream)[1])[coord]) # cur_mean_model = signal_smooth(stream_coord, smooth_wind) # mean_models += [cur_mean_model] # # # sum up the squared difference over the whole model # noise_vars += [ ( sum([(x - y) ** 2 for x,y in zip(cur_mean_model,stream_coord)]) / # len(cur_mean_model) ) ] # # find the average case over the several runs # avg_means_model = np.array([0.] * max_len) # for i in range(max_len): # n = 0 # for j in range(len(mean_models)): # if i < len(mean_models[j]): # avg_means_model[i] += mean_models[j][i] # n += 1 # avg_means_model[i] /= n # if var_wind_dict is None or var_wind_dict[perception] is None: # var_wind = var_wind_default # else: # var_wind = var_wind_dict[perception] # # find the variance of the signal but use var_wind points around the centers # # to increase the sample size # vars_model = signal_list_variance(mean_models, avg_means_model, var_wind) # vars_model = signal_smooth(vars_model, var_smooth_wind) # vars_model = signal_smooth(vars_model, var_smooth_wind + 23) # ret_times += [times] # ret_means += [avg_means_model] # ret_vars += [vars_model] # ret_mean_models += [mean_models] # ret_noise_vars += [np.average(noise_vars)] # # TODO deal with timestamp data in some way? # # self.models[perception]["time"] = ret_times # self.models[perception]["mean"] = np.array(zip(*ret_means)) # self.models[perception]["variance"] = np.array(zip(*ret_vars)) # a = ret_mean_models # b = [] # for stream in range(len(a[0])): # t1 = [] # for val in range(len(a[0][0])): # t2 = [] # for coord in range(len(a)): # if val < len(a[coord][stream]): # t2 += [a[coord][stream][val]] # t1 += [np.array(t2)] # b += [t1] # self.models[perception]["smoothed_signals"] = b # self.models[perception]["noise_variance"] = np.array(ret_noise_vars) # return self.models def get_zeros(self, time=4.): self._zeros = {} for k in self.perceptions: self._zeros[k] = None self._n = 0 monitor = RateCaller(self._sum_values, self.rate) monitor.run() rospy.sleep(time) monitor.stop() for k in self.perceptions: self._zeros[k] /= self._n return self._zeros def _sum_values(self): for k in self.perceptions: add_data = np.array(self.perceptions[k]()[1]) if self._zeros[k] is None: self._zeros[k] = np.copy(add_data) else: self._zeros[k] += add_data self._n += 1 ## # Sets up monitoring parameters def setup_monitoring(self, models, std_dev_dict=None, noise_dev_dict=None, duration=None, std_dev_default=2.0, noise_dev_default=0.25, tol_thresh_dict=None, contingency=None, window_size=70, current_zeros=None, sampling_rate = 1, transform_dict=None, verbose=True, collide=True): self.std_dev_default = std_dev_default self.noise_dev_default = noise_dev_default self.models = models self.current_data = {} self.std_dev_dict = std_dev_dict self.noise_dev_dict = {} self.contingency = contingency self.window_size = window_size self.current_zeros = {} self.transform_dict = transform_dict self.tol_thresh_dict = tol_thresh_dict self.sampling_rate = sampling_rate self.verbose = verbose self.collide = collide self.sum_data = {} self.cur_pt = 0 self.failure = False self.dur_timer = None self.avg_list = {} self.cum_avg = {} self.c = {} self.locks = {} for k in self.perceptions: self.current_data[k] = [None] * window_size self.sum_data[k] = None self.avg_list[k] = [] self.c[k] = None self.locks[k] = threading.Lock() self.create_max_min() # Checks percept to see if the model indicates a collision with the current # smoothed perception avg # Returns index of perception and difference if collision, -1, 0 else def create_max_min(self): for k in self.models: deviation = self.models[k]["variance"] # noise_deviation = np.sqrt(self.models[k]["noise_variance"]) if self.std_dev_dict is not None and k in self.std_dev_dict: std_dev = np.array(self.std_dev_dict[k]) else: std_dev = self.std_dev_default # if self.noise_dev_dict is not None and k in self.noise_dev_dict: # noise_dev = np.array(self.noise_dev_dict[k]) # else: # noise_dev = self.noise_dev_default if self.tol_thresh_dict is not None and k in self.tol_thresh_dict: tol_thresh = np.array(self.tol_thresh_dict[k]) else: tol_thresh = 0. self.models[k]["dev"] = (std_dev * deviation + tol_thresh) # self.models[k]["dev"] = (std_dev * deviation + noise_dev * noise_deviation + tol_thresh) # This is the monitoring equation self.models[k]["max"] = self.models[k]["mean"] + self.models[k]["dev"] self.models[k]["min"] = self.models[k]["mean"] - self.models[k]["dev"] ## # Begin monitoring peception data to make sure it doesn't deviate from # the model provided. # # TODO DOCS # @param duration If None, continue capturing until stop is called. # Else, stop capturing after duration seconds have passed. def begin_monitoring(self, models=None, model_zeros=None, std_dev_dict=None, noise_dev_dict=None, duration=None, std_dev_default=2.0, noise_dev_default=0.25, tol_thresh_dict=None, contingency=None, window_size=70, current_zeros=None, sampling_rate = 1, only_pressure=False, transform_dict=None, verbose=True, collide=True): if self.active: log("Perception already active.") return self.active = True self.setup_monitoring(models, std_dev_dict=std_dev_dict, noise_dev_dict=noise_dev_dict, duration=duration, std_dev_default=std_dev_default, noise_dev_default=noise_dev_default, tol_thresh_dict=tol_thresh_dict, contingency=contingency, window_size=window_size, sampling_rate=sampling_rate, current_zeros=current_zeros, transform_dict=transform_dict, verbose=verbose, collide=collide) # if models is not None: # self.models = models if model_zeros is not None: self.model_zeros = model_zeros self.cur_pt = 0 self.collision_times = {} self.collision_sums = {} self.cur_col_time = 0 self.current_zeros = {} self.z_sum = {} self.z_rsum = {} self.z_list = {} self.min_prob = 100000.0 self.cur_mals = {} self.mals = None self.only_pressure = only_pressure self.monitor = RateCaller(self._monitor_data, self.rate * self.sampling_rate) self.monitor.run() # self.sumcalls = {} # self.sumsave = {} if not duration is None: self.dur_timer = threading.Timer(self.end_monitoring, duration) self.dur_timer.start() def _stable_summer(self, percept, data): # kahanSum k = percept # if percept not in self.sumcalls: # self.sumcalls[percept] = 1 # self.sumsave[percept] = [] # else: # self.sumcalls[percept] += 1 if self.c[k] is None: self.c[k] = np.array([0.]*len(data)) if self.sum_data[k] is None: self.sum_data[k] = np.copy(data) else: y = data - self.c[k] t = self.sum_data[k] + y self.c[k] = (t - self.sum_data[k]) - y self.sum_data[k] = t # self.sumsave[percept] += [self.sum_data[k]] def _monitor_data(self): avg_dict = {} sttime = rospy.Time.now().to_sec() for k in self.perceptions: # update the running sum add_data = self.perceptions[k]()[1] # apply necessary transforms # if self.transform_dict is not None: # if k in self.transform_dict: # add_data = self.transform_dict[k](add_data) self._stable_summer(k, add_data) # If we have enough data to monitor, we check to see if the values are in range if self.cur_pt >= self.window_size: #self.current_data[k].full(): # self.prev_sum = copy.deepcopy(self.sum_data) avg_dict[k] = self.sum_data[k] / self.window_size if not k in self.current_zeros: if not self.only_pressure: mean = np.array(self.models[k]["mean"][self.cur_pt * self.sampling_rate]) else: mean = np.array(self.models[k]["mean"][0]) self.current_zeros[k] = mean - avg_dict[k] # self.current_zeros[k] = - avg_dict[k] avg_dict[k] += self.current_zeros[k] self.avg_list[k] += [avg_dict[k]] * self.sampling_rate # if self.cur_pt == self.window_size: # print self.avg_list[k] # self.prev_avg = copy.deepcopy(avg) # offset zeros into original perception frame # if self.current_zeros is not None: # if False and self.model_zeros is not None: # avg_dict[k] += self.model_zeros[k] - self.current_zeros[k] # else: # # this is hacky, need to use zeros during training instead of first pt # # print "NOOOOOOOOOOOO!\n"*10 # avg_dict[k] += np.array(self.models[k]["mean"][self.window_size]) - self.current_zeros[k] # if collision_detected: # log("Value %d of the perception %s failed with difference %f" # % (index, k, diff)) # # if diff > 5. and k == "accelerometer": # # print "avg_list", self.avg_list # # print "avg", avg # # for i in range(self.window_size+1): # # d = self.current_data[k].get() # # print d # # self.current_data[k].put(d) # self.failure = True # self.contingency() # self.monitor.stop() rem_data = self.current_data[k][self.cur_pt % self.window_size] self._stable_summer(k, -rem_data) self.current_data[k][self.cur_pt % self.window_size] = add_data if self.cur_pt >= self.window_size: #self.current_data[k].full(): collision_detected = self.collision_detect(avg_dict) if self.collide and collision_detected: print "collision reported" self.failure = True if not self.contingency is None: self.contingency() self.monitor.stop() return self.cur_pt += 1 if not self.only_pressure: if self.cur_pt * self.sampling_rate >= 1.00 * len(self.models[self.models.keys()[0]]["mean"]): print "ending early:", self.cur_pt * self.sampling_rate self.end_monitoring() endtime = rospy.Time.now().to_sec() # if self.cur_pt % 10 == 0: # print "dur:", sttime - endtime ## # Stop capturing perception data. Store output data in datasets list for # later statistics. # TODO DOCS def end_monitoring(self): if not self.active: log("Nothing to stop.") return self.avg_list print "-------------------------" print "z averages:" self.z_avg = {} for k in self.z_sum: self.z_avg[k] = self.z_sum[k] / self.cur_pt print k, ":", self.z_avg[k] print "-------------------------" print "MINIMUM PROB" print self.min_prob print "-------------------------" # print "Sum calls", self.sumcalls # if self.sumcalls["r_finger_periph_pressure"] == 202: # print self.sumsave self.monitor.stop() if self.dur_timer is not None: self.dur_timer.cancel() self.dur_timer = None self.active = False return self.avg_list # Checks percept to see if the model indicates a collision with the current # smoothed perception avg # Returns index of perception and difference if collision, -1, 0 else def collision_detect(self, avg_dict): # is_outside_range_dict = {} z = {} mal_sum = 0. for k in avg_dict: # This is the monitoring equation # is_outside_range_dict[k] = ((avg_dict[k] > self.models[k]["max"][self.cur_pt * self.sampling_rate]) + # (avg_dict[k] < self.models[k]["min"][self.cur_pt * self.sampling_rate])) # Uses both variance from various grasp tries and general noise variance if not self.only_pressure: mean = self.models[k]["mean"][self.cur_pt * self.sampling_rate] dev = self.models[k]["dev"][self.cur_pt * self.sampling_rate] else: mean = self.models[k]["mean"][0] dev = self.models[k]["dev"][0] cur_mal = ((avg_dict[k] - mean) / dev) ** 2 self.cur_mals[k] = cur_mal mal_sum += np.add.reduce( ( (avg_dict[k] - mean) / dev) ** 2 ) z[k] = np.fabs(avg_dict[k] - mean) / dev if not k in self.z_sum: self.z_sum[k] = np.copy(z[k]) else: self.z_sum[k] += z[k] # print "incoldet" # collision_detected = self.collision_filtering(is_outside_range_dict) # collision_detected = self.collision_filtering_mal(np.sqrt(mal_sum)) # return collision_detected collision_detected = self.collision_filtering(z) # print "coldete", collision_detected return collision_detected def collision_filtering_mal(self, mal): prob = 1. TIME_WINDOW = 4 if self.mals is None: self.mals = np.array([0.] * TIME_WINDOW) self.mals[self.cur_pt % TIME_WINDOW] = mal if self.cur_pt < TIME_WINDOW: return False mal_avg = np.add.reduce(self.mals / TIME_WINDOW) MAL_THRESH = 8.0 if mal_avg > MAL_THRESH: print "Collision with mal dist:", mal_avg for k in self.cur_mals: print k, self.cur_mals[k] return True return False def collision_filtering(self, z): prob = 1. TIME_WINDOW = 4 for k in z: if not k in self.z_rsum: self.z_rsum[k] = np.copy(z[k]) self.z_list[k] = [np.array([0.]*len(z[k]))] * TIME_WINDOW else: self.z_rsum[k] += z[k] self.z_rsum[k] -= self.z_list[k][self.cur_pt % TIME_WINDOW] self.z_list[k][self.cur_pt % TIME_WINDOW] = z[k] # print z[k] prob *= np.multiply.reduce(2 * stats.norm.cdf(-self.z_rsum[k] / TIME_WINDOW)) # print prob if self.cur_pt < TIME_WINDOW: return False if prob < self.min_prob: self.min_prob = prob # CONFIDENCE = 1e-6 if not self.only_pressure: # 4e-7 50/50 CONFIDENCE = 2.0e-6 else: # pressure only confidence CONFIDENCE = 1.e-5 if self.cur_pt % 20 == 0: print "Current pt:", self.cur_pt, ", probability:", prob if prob < CONFIDENCE: print "probability:", prob print "collision returned" return True # print "-------------------------" # print num_percep_col # for k in sig_counts: # if sig_counts[k] >= PERCEPT_SIG_REQ[k]: # print k, ":", sig_counts[k] # print "-------------------------" # return True return False # def collision_filtering(self, is_outside_range_dict): # isord = is_outside_range_dict # TIME_WINDOW = 10 # NUM_PERCEPT_COL_REQ = 2 # PERCEPT_SIG_REQ = { "accelerometer" : 2, # "joint_angles" : 2, # "joint_velocities" : 3, # "joint_efforts" : 2, # "r_finger_periph_pressure" : 2, # "r_finger_pad_pressure" : 5, # "l_finger_periph_pressure" : 2, # "l_finger_pad_pressure" : 5 } # num_percep_col = 0 # sig_counts = {} # num_periph = 0 # for k in isord: # # if any(isord[k]): # # print k, isord # if not k in self.collision_times: # self.collision_times[k] = [np.array([0] * len(isord[k]))] * TIME_WINDOW # self.collision_sums[k] = np.array([0] * len(isord[k])) # # def queue_sum(q): # # sum = None # # for i in range(TIME_WINDOW): # # val = q.get() # # if sum is None: # # sum = np.copy(val) # # sum += val # # q.put(val) # # return sum # # c_sum = queue_sum(self.collision_times[k]) # self.collision_sums[k] -= self.collision_times[k][self.cur_col_time] # self.cur_col_time = (self.cur_col_time + 1) % TIME_WINDOW # self.collision_times[k][self.cur_col_time] = np.int32(np.array(isord[k])) # self.collision_sums[k] += self.collision_times[k][self.cur_col_time] # def g0(x): # if x > 0: # return 1 # return 0 # sig_count = np.sum(map(g0, self.collision_sums[k])) # sig_counts[k] = sig_count # if sig_count >= PERCEPT_SIG_REQ[k]: # # print "cols", k, isord[k] # if k == "r_finger_periph_pressure" or k == "l_finger_periph_pressure": # num_periph += 1 # num_percep_col += 1 # # num_periph != 1 eliminates side object collision possibilities # if num_percep_col >= NUM_PERCEPT_COL_REQ and num_periph != 1: # print "collision returned" # print "-------------------------" # print num_percep_col # for k in sig_counts: # if sig_counts[k] >= PERCEPT_SIG_REQ[k]: # print k, ":", sig_counts[k] # print "-------------------------" # return True # return False # if self.collide and any(is_outside_range): # # sensor has fallen outside acceptable range, trigger # for i, x in enumerate(is_outside_range): # if x: # # print "Average:", avg # # print "Last avg:", self.prev_avg # # print "Prev sum:", self.prev_sum # # print "MOD:", self.model_zeros[k] # # print "MON:", self.current_zeros[k] # return i, diff[i] # return -1, 0. # @TODO add sampling_rate stuff def simulate_monitoring(self, monitor_data, models=None, model_zeros=None, std_dev_dict=None, noise_dev_dict=None, duration=None, std_dev_default=2.0, noise_dev_default=0.25, tol_thresh_dict=None, contingency=None, window_size=70, current_zeros=None, transform_dict=None, verbose=True, collide=True): self.setup_monitoring(std_dev_dict=std_dev_dict, noise_dev_dict=noise_dev_dict, duration=duration, std_dev_default=std_dev_default, tol_thresh_dict=tol_thresh_dict, noise_dev_default=noise_dev_default, contingency=contingency, window_size=window_size, current_zeros=current_zeros, transform_dict=transform_dict, verbose=verbose, collide=collide) if models is not None: self.models = models if model_zeros is not None: self.model_zeros = model_zeros self.cur_pt = self.window_size collision = None # i is the number of samples in the monitor data for i in range(len(monitor_data[monitor_data.keys()[0]])): for k in monitor_data: index, diff = self.collision_detect(k, monitor_data[k][i]) if index != -1: collision = (k, index, diff) self.cur_pt += 1 return collision def resample_and_thin_data(self, monitor_data, sample_rate): ret_data = {} for k in monitor_data: ret_data[k] = {} ret_data[k]["mean"] = monitor_data[k]["mean"][::sample_rate] ret_data[k]["variance"] = monitor_data[k]["variance"][::sample_rate] return ret_data
class ArmPerceptionMonitor(): ## # Initializes internal variables # # @param arm 0 if right, 1 if left # @param percept_mon_list list of perceptions to monitor; if None, do all def __init__(self, arm): self.load_parameters() ############## Classifiers ################# self.impact_classifier = cfiers.classifiers_dict[ "small_refined_random_forest"] self.coll_type_classifier = cfiers.classifiers_dict[ "large_refined_random_forest"] log("Loading impact classifier") self.impact_classifier.load(self.I_RTREE_CLASSIFIER) log("Loading collision type classifier") self.coll_type_classifier.load(self.T_RTREE_CLASSIFIER) if arm == 0: self.armc = "r" self.is_right_arm = True else: self.armc = "l" self.is_right_arm = False self.perceptions = {} self.perception_names = [ "accelerometer", "joint_angles", "joint_velocities", "joint_efforts", "r_finger_periph_pressure", "r_finger_pad_pressure", "l_finger_periph_pressure", "l_finger_pad_pressure", "gripper_pose" ] self.perception_lengths = { "accelerometer": 3, "joint_angles": 7, "joint_velocities": 7, "joint_efforts": 7, "r_finger_periph_pressure": 1, "r_finger_pad_pressure": 1, "l_finger_periph_pressure": 1, "l_finger_pad_pressure": 1, "gripper_pose": 7 } self.percept_mon_list = self.perception_names self.impact_fgc = FastGaussConvolve(self.SAMPLING_RATE, self.I_FILTER_BEG_DELAY, self.I_FILTER_CEN_DELAY, self.i_sigma_list) self.type_fgc = FastGaussConvolve(self.SAMPLING_RATE, self.T_FILTER_BEG_DELAY, self.T_FILTER_CEN_DELAY, self.t_sigma_list) self.perception_started = False # self.detect_ready = False self.fos = FileOperations() def load_parameters(self): self.SAMPLING_RATE = rospy.get_param( "/overhead_grasping/data_sampling_rate") self.PERCEPTION_ORDER = rospy.get_param( "/overhead_grasping/perception_order") # impact parameters self.I_FILTER_BEG_DELAY = rospy.get_param( "/overhead_grasping/i_filter_beg_delay") self.I_FILTER_CEN_DELAY = rospy.get_param( "/overhead_grasping/i_filter_cen_delay") self.I_MIN_SIGMA = rospy.get_param("/overhead_grasping/i_min_sigma") self.I_MAX_SIGMA = rospy.get_param("/overhead_grasping/i_max_sigma") self.I_NUM_SIGMA = rospy.get_param("/overhead_grasping/i_num_sigma") self.i_sigma_list = np.linspace(self.I_MIN_SIGMA, self.I_MAX_SIGMA, self.I_NUM_SIGMA) self.I_DEGREE_DICT = rospy.get_param( "/overhead_grasping/i_degree_dict") self.i_instance_len = 0 for k in self.PERCEPTION_ORDER: for coord in self.I_DEGREE_DICT[k]: for deg in coord: self.i_instance_len += self.I_NUM_SIGMA self.I_RTREE_CLASSIFIER = rospy.get_param( "/overhead_grasping/i_rtree_classifier") # collision type parameters self.T_FILTER_BEG_DELAY = rospy.get_param( "/overhead_grasping/t_filter_beg_delay") self.T_FILTER_CEN_DELAY = rospy.get_param( "/overhead_grasping/t_filter_cen_delay") self.T_MIN_SIGMA = rospy.get_param("/overhead_grasping/t_min_sigma") self.T_MAX_SIGMA = rospy.get_param("/overhead_grasping/t_max_sigma") self.T_NUM_SIGMA = rospy.get_param("/overhead_grasping/t_num_sigma") self.t_sigma_list = np.linspace(self.T_MIN_SIGMA, self.T_MAX_SIGMA, self.T_NUM_SIGMA) self.T_DEGREE_DICT = rospy.get_param( "/overhead_grasping/t_degree_dict") self.t_instance_len = 0 for k in self.PERCEPTION_ORDER: for coord in self.T_DEGREE_DICT[k]: for deg in coord: self.t_instance_len += self.T_NUM_SIGMA self.T_RTREE_CLASSIFIER = rospy.get_param( "/overhead_grasping/t_rtree_classifier") self.STATIC_SIGMA_INDEX = rospy.get_param( "/overhead_grasping/static_sigma_index") self.STATIC_DERIV_THRESH = rospy.get_param( "/overhead_grasping/static_deriv_thresh") ## # Initializes the listeners on the sensor topics. This must # be started before any data can be collected from the arms. def activate_sensing(self, tf_listener=None): log("Initializing arm perception listeners") self.tf_listener = tf_listener if "accelerometer" in self.percept_mon_list: accel_listener = GenericListener( "accel_mon_node", AccelerometerState, "accelerometer/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, accel_state_processor) self.perceptions["accelerometer"] = accel_listener.read if "joint_angles" in self.percept_mon_list: joint_angles_processor = ft.partial(joints_state_processor, right_arm=self.is_right_arm, angles_velocities_efforts=0) joint_angles_listener = GenericListener("joint_angles_mon_node", JointState, "joint_states", self.SAMPLING_RATE, joint_angles_processor) self.perceptions["joint_angles"] = joint_angles_listener.read if "joint_velocities" in self.percept_mon_list: joint_velocities_processor = ft.partial( joints_state_processor, right_arm=self.is_right_arm, angles_velocities_efforts=1) joint_velocities_listener = GenericListener( "joint_vels_mon_node", JointState, "joint_states", self.SAMPLING_RATE, joint_velocities_processor) self.perceptions[ "joint_velocities"] = joint_velocities_listener.read if "joint_efforts" in self.percept_mon_list: joint_efforts_processor = ft.partial(joints_state_processor, right_arm=self.is_right_arm, angles_velocities_efforts=2) joint_efforts_listener = GenericListener("joint_efforts_mon_node", JointState, "joint_states", self.SAMPLING_RATE, joint_efforts_processor) self.perceptions["joint_efforts"] = joint_efforts_listener.read if "r_finger_periph_pressure" in self.percept_mon_list: r_finger_periph_pressure_processor = ft.partial( pressure_state_sum_processor, right_finger_tip=True, indicies=range(1, 7)) r_finger_periph_pressure_listener = GenericListener( self.armc + "_pressure_r_periph_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, r_finger_periph_pressure_processor) self.perceptions[ "r_finger_periph_pressure"] = r_finger_periph_pressure_listener.read if "r_finger_pad_pressure" in self.percept_mon_list: r_finger_pad_pressure_processor = ft.partial( pressure_state_sum_processor, right_finger_tip=True, indicies=range(7, 22)) r_finger_pad_pressure_listener = GenericListener( self.armc + "_pressure_r_pad_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, r_finger_pad_pressure_processor) self.perceptions[ "r_finger_pad_pressure"] = r_finger_pad_pressure_listener.read if "l_finger_periph_pressure" in self.percept_mon_list: l_finger_periph_pressure_processor = ft.partial( pressure_state_sum_processor, right_finger_tip=False, indicies=range(1, 7)) l_finger_periph_pressure_listener = GenericListener( self.armc + "_pressure_l_periph_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, l_finger_periph_pressure_processor) self.perceptions[ "l_finger_periph_pressure"] = l_finger_periph_pressure_listener.read if "l_finger_pad_pressure" in self.percept_mon_list: l_finger_pad_pressure_processor = ft.partial( pressure_state_sum_processor, right_finger_tip=False, indicies=range(7, 22)) l_finger_pad_pressure_listener = GenericListener( self.armc + "_pressure_l_pad_mon_node", PressureState, "pressure/" + self.armc + "_gripper_motor", self.SAMPLING_RATE, l_finger_pad_pressure_processor) self.perceptions[ "l_finger_pad_pressure"] = l_finger_pad_pressure_listener.read for k in self.perceptions: # Make sure we have recieved a message first self.perceptions[k](allow_duplication=False, willing_to_wait=True) self.perceptions[k] = ft.partial(self.perceptions[k], willing_to_wait=False, quiet=True, warn=False, allow_duplication=True) if "gripper_pose" in self.percept_mon_list: def gripper_pose_lookup(): if self.tf_listener is not None: lct = self.tf_listener.getLatestCommonTime( "/torso_lift_link", "/" + self.armc + "_wrist_roll_link") pos, quat = self.tf_listener.lookupTransform( "/torso_lift_link", "/" + self.armc + "_wrist_roll_link", lct) return (lct.to_sec(), np.array(pos + quat)) else: return (0., np.array([0.] * 7)) self.perceptions["gripper_pose"] = gripper_pose_lookup # all callbacks should return data in this format: # (t, np.array([x1, x2, ...])) # t is time in seconds self.clear_vars() self.perception_started = True log("Finished initialization") ## # Initialize variables def clear_vars(self): self.datasets = {} self.models = {} for k in self.perceptions: self.datasets[k] = [] self.models[k] = {"mean": None, "variance": None} ## # Begin capturing peception data for all of the listeners # # @param duration If None, continue capturing until stop is called. # Else, stop capturing after duration seconds have passed. def start_data_capture(self, duration=None): if not self.perception_started: err("Perception hasn't been started!") return False self.logger = RateCaller(self._gather_perception, self.SAMPLING_RATE) for k in self.perceptions: self.datasets[k] += [[]] self.logger.run() if not duration is None: threading.Timer(self.stop_data_capture, duration) return True def _gather_perception(self): for k in self.perceptions: self.datasets[k][-1].append(self.perceptions[k]()) ## # Stop capturing perception data. Store output data in datasets list for # later statistics. def stop_data_capture(self): self.logger.stop() num_datapts = len(self.datasets[self.datasets.keys()[0]][-1]) log("%d datapoints collected", num_datapts) return num_datapts ## # do processing of signals in current coord group # sigs_proc contains a list of three signal sets # each corresponds to a list of convolutions of the signals # with a degree derivative of a gaussian with various # scales def process_signals(self, datasets): models = {} sum, num = 0., 0. for p in datasets: models[p] = {} i_sigs_proc = [[[] for x in range(self.I_NUM_SIGMA)] for y in range(3)] t_sigs_proc = [[[] for x in range(self.T_NUM_SIGMA)] for y in range(3)] data_list = datasets[p] num_coords = len(data_list[0][0][1]) times = None for coord in range(num_coords): stream = data_list[0] def conv(v): if type(v) == type([]) or type(v) == type(np.array([])): return v[0] return v signal = [conv(v) for v in zip(*zip(*stream)[1])[coord]] time_ind = np.array(zip(*stream)[0], dtype=np.float64) # normalize time indicies by offsetting to first value time_ind_beg = time_ind[0] for i, v in enumerate(time_ind): time_ind[i] = float(time_ind[i] - time_ind_beg) times = time_ind for deg in range(3): for sigma_ind, sigma in enumerate(self.i_sigma_list): s = self.impact_fgc.convolve_signal( signal, deg, sigma_ind) i_sigs_proc[deg][sigma_ind].append(s) for sigma_ind, sigma in enumerate(self.t_sigma_list): s = self.type_fgc.convolve_signal( signal, deg, sigma_ind) t_sigs_proc[deg][sigma_ind].append(s) if rospy.is_shutdown(): sys.exit() models[p]["impact_features"] = [[[] for x in range(self.I_NUM_SIGMA)] for y in range(3)] models[p]["type_features"] = [[[] for x in range(self.T_NUM_SIGMA)] for y in range(3)] for deg in range(3): for sigma in range(self.I_NUM_SIGMA): models[p]["impact_features"][deg][sigma] = zip( *i_sigs_proc[deg][sigma]) for sigma in range(self.T_NUM_SIGMA): models[p]["type_features"][deg][sigma] = zip( *t_sigs_proc[deg][sigma]) models[p]["time"] = times models[p]["i_sigma_list"] = self.i_sigma_list models[p]["t_sigma_list"] = self.t_sigma_list return models # def ready_collision_detection(self): # # wait for the classifiers to finish loading # self.impact_classifier.finish_loading() # # self.coll_type_classifier.finish_loading() # self.detect_ready = True ## # Begin monitoring peception data to make sure it doesn't deviate from # the model provided. # # TODO DOCS # @param duration If None, continue capturing until stop is called. # Else, stop capturing after duration seconds have passed. def begin_collision_detection(self, dynamic_detection=False, callback=None, static_collision_type=None, debug=False): if not self.perception_started: err("Perception hasn't been started!") return False # if not self.detect_ready: # err("ready_collision_detection hasn't been called") # return False self.is_dynamic = dynamic_detection self.callback = callback if not dynamic_detection: self._setup_collision_type(static_collision_type) self.collision_detected = False self.collision_finished = False self.debug = debug self.collision_type = "No collision" self.cur_iter = 0 self.cur_inds = {} for k in self.perceptions: self.cur_inds[k] = 0 self.i_data_buffers = {} self.t_data_buffers = {} for k in self.perceptions: self.i_data_buffers[k] = [ np.zeros( (self.impact_fgc.filter_len, self.impact_fgc.filter_len)) for i in range(self.perception_lengths[k]) ] self.t_data_buffers[k] = [ np.zeros((self.type_fgc.filter_len, self.type_fgc.filter_len)) for i in range(self.perception_lengths[k]) ] self.end_monitor_lock = threading.Lock() self.data_thread_spawner_lock1 = threading.Lock() self.data_thread_spawner_lock2 = threading.Lock() self.data_thread_spawner = RateCaller(self._data_spawn_thread, self.SAMPLING_RATE) self.data_thread_spawner.run() self.beg_time = rospy.Time.now().to_sec() self.t_sum, self.t_num = 0., 0. self.normal_dict_counts = [] self.temp_dict = {} for k in self.perceptions: self.temp_dict[k] = [[] for i in range(self.perception_lengths[k])] return True def _setup_collision_type(self, static_collision_type): if static_collision_type == "all": static_dict = { "accelerometer": range(3), "joint_angles": range(7), "joint_velocities": range(7), "joint_efforts": range(7), "r_finger_periph_pressure": range(1), "r_finger_pad_pressure": range(1), "l_finger_periph_pressure": range(1), "l_finger_pad_pressure": range(1), "gripper_pose": range(7) } elif static_collision_type in self.perception_names: static_dict = { static_collision_type: range(self.perception_lengths[static_collision_type]) } elif static_collision_type == "pressure": static_dict = { "r_finger_periph_pressure": range(1), "r_finger_pad_pressure": range(1), "l_finger_periph_pressure": range(1), "l_finger_pad_pressure": range(1) } elif type(static_collision_type) == type({}): static_dict = static_collision_type else: err("Bad static_collision_type") self.static_dict = static_dict def _data_spawn_thread(self): # only one thread allowed to wait at a time # all other threads will pass through if self.data_thread_spawner_lock1.acquire(False): if self.data_thread_spawner_lock2.acquire(True): self.data_thread_spawner_lock1.acquire(False) self.data_thread_spawner_lock1.release() mt = self.DataThread(self) mt.start() class DataThread(threading.Thread): def __init__(self, apm): threading.Thread.__init__(self) self.apm = apm def run(self): self.apm._monitor_data_collector() self.apm.data_thread_spawner_lock2.acquire(False) self.apm.data_thread_spawner_lock2.release() def _monitor_data_collector(self): st_time = rospy.Time.now().to_sec() ######################################### if rospy.is_shutdown(): self._trigger_collision() self._finish_collision("rospy shutdown") add_data = [] for k in self.perceptions: p_vals = self.perceptions[k]()[1] for i, v in enumerate(p_vals): # populate impact collision buffers for b in range(self.impact_fgc.filter_len - 1, -1, -1): self.i_data_buffers[k][i][(b - self.cur_iter) % self.impact_fgc.filter_len, b] = v if self.debug: if b == 0 and k == "joint_angles": # and ((i == 0) or (i == 1)): print v, # populate collision type detection buffers for b in range(self.type_fgc.filter_len - 1, -1, -1): self.t_data_buffers[k][i][(b - self.cur_iter) % self.type_fgc.filter_len, b] = v if self.debug: print if self.is_dynamic: if not self.collision_detected: self._dynamic_collision_detection() else: self._dynamic_collision_classifying() else: self._static_collision_detection() self.cur_iter += 1 ######################################### end_time = rospy.Time.now().to_sec() self.t_sum += end_time - st_time self.t_num += 1. def _dynamic_collision_detection(self): if self.cur_iter < self.impact_fgc.filter_len: return instance = np.zeros((self.i_instance_len, 1)) i_place = 0 for k in self.PERCEPTION_ORDER: for coord in range(len(self.i_data_buffers[k])): for deg in range(3): if deg not in self.I_DEGREE_DICT[k][coord]: continue for sigma_i, sigma_t in enumerate(self.i_sigma_list): val = self.impact_fgc.convolve_pt( self.i_data_buffers[k][coord][( self.cur_iter % self.impact_fgc.filter_len)], deg, sigma_i) instance[i_place, 0] = val i_place += 1 has_collided = self.impact_classifier.predict(instance) if has_collided and not self.debug: if not self.collision_detected: self._trigger_collision() def _dynamic_collision_classifying(self): if self.cur_iter < self.type_fgc.filter_len: # TODO HANDLE THIS CASE return if ((self.cur_iter - self.coll_iter) > self.type_fgc.filter_len / 2): return instance = np.zeros((self.t_instance_len, 1)) i_place = 0 for k in self.PERCEPTION_ORDER: for coord in range(len(self.t_data_buffers[k])): for deg in range(3): if deg not in self.T_DEGREE_DICT[k][coord]: continue for sigma_i, sigma_t in enumerate(self.t_sigma_list): val = self.type_fgc.convolve_pt( self.t_data_buffers[k][coord][( self.cur_iter % self.type_fgc.filter_len)], deg, sigma_i) instance[i_place, 0] = val i_place += 1 collision_class = self.coll_type_classifier.predict(instance) if collision_class == 1.: type = "Environmental collision" else: type = "Table collision" self._finish_collision(type) def _static_collision_detection(self): if self.cur_iter < self.impact_fgc.filter_len: return for k in self.PERCEPTION_ORDER: if k not in self.static_dict: continue for coord in self.static_dict[k]: if k == "gripper_pose" and coord >= 3: # Quaternions are unreliable, this should be fixed # at some point if self.debug: self.temp_dict[k][coord].append(0.) continue val = self.impact_fgc.convolve_pt( self.i_data_buffers[k][coord][self.cur_iter % self.impact_fgc.filter_len], 1, self.STATIC_SIGMA_INDEX) if self.debug: self.temp_dict[k][coord].append(val) if (np.fabs(val) >= self.STATIC_DERIV_THRESH[k][coord] and not self.debug): print "Collision:", val, "Thresh:", self.STATIC_DERIV_THRESH[ k][coord] self._trigger_collision() self._finish_collision(k + " collision", coord) # if k == "r_finger_pad_pressure" and random.randint(0,40) == 0: # print val def _trigger_collision(self): self.collision_detected = True self.coll_iter = self.cur_iter - self.impact_fgc.filter_len / 2 if not self.callback is None: self.callback() def _finish_collision(self, type="Unlabled", coord=0): if not self.collision_finished: print '-' * 60 print print ' %s detected' % type print print '-' * 60 log("%s reported" % type) self.collision_type = type self.collision_coord = coord if not self.callback is None: self.callback() self.collision_finished = True self.end_collision_detection() ## # Stop capturing perception data. Store output data in datasets list for # later statistics. def end_collision_detection(self): while (not self.collision_finished and self.collision_detected and not rospy.is_shutdown()): rospy.sleep(0.01) with self.end_monitor_lock: if not self.data_thread_spawner is None: print "Avg cvpt: %1.9f" % (self.t_sum / self.t_num) print "cvpt sum: %1.5f" % (self.t_sum) print self.t_num self.data_thread_spawner.stop() self.data_thread_spawner_lock1.acquire(False) self.data_thread_spawner_lock1.release() self.data_thread_spawner_lock2.acquire(False) self.data_thread_spawner_lock2.release() self.data_thread_spawner = None return