def on_data(self, item): curr_time = time.time() value = item.data # This is kind of a hack to correctly interpret UInt8MultiArray # messages. There should be a better way to do this if item._slot_types[item.__slots__.index('data')] == "uint8[]": value = [ord(x) for x in value] # Throttle updates delta_time = curr_time - self.last_time if delta_time < self.min_update_interval: return if delta_time < self.max_update_interval and self.last_value: delta_val = value - self.last_value if abs(delta_val / self.last_value) <= 0.01: return # Save the data point point = EnvironmentalDataPoint({ "environment": self.environment, "variable": self.variable, "is_desired": self.is_desired, "value": value, "timestamp": curr_time }) point_id = gen_doc_id(curr_time) self.db[point_id] = point self.last_value = value self.last_time = curr_time
def on_data(self, item): curr_time = time.time() value = item.data if value is None or value == self.last_value: return # This is kind of a hack to correctly interpret UInt8MultiArray # messages. There should be a better way to do this if item._slot_types[item.__slots__.index('data')] == "uint8[]": value = [ord(x) for x in value] # Throttle updates by value only (not time) if self.topic_type == Float64 and \ self.last_value is not None and \ self.last_value != 0.0: delta_val = value - self.last_value if abs(delta_val / self.last_value) <= 0.01: return # Save the data point point = EnvironmentalDataPoint({ "environment": self.environment, "variable": self.variable, "is_desired": self.is_desired, "value": value, "timestamp": curr_time }) point_id = gen_doc_id(curr_time) self.db[point_id] = point self.last_value = value
def loop(self): while not rospy.is_shutdown(): # Check for a recipe recipe = self.get_recipe() # If we have a recipe, process it. Running a recipe is a blocking # operation, so the recipe will stay in this turn of the loop # until it is finished. if recipe: rospy.loginfo('Starting recipe "{}"'.format(recipe.id)) state = {} for timestamp, variable, value in recipe: # If recipe was canceled or changed, or ROS stopped, # break setpoint iteration if self.get_recipe() != recipe or rospy.is_shutdown(): break # Skip invalid variable types if variable not in VALID_VARIABLES: msg = 'Recipe references invalid variable "{}"' rospy.logwarn(msg.format(variable)) continue # Publish any setpoints that coerce to float try: float_value = float(value) topic_name = "{}/desired".format(variable) pub = publisher_memo(topic_name, Float64, 10) pub.publish(float_value) except ValueError: pass # Advance state prev = state.get(variable, None) state[variable] = value # Store unique datapoints if prev != value: # @TODO ideally, this should be handled in a separate # desired_persistence ros node and we should only publish to # topic endpoint. doc = EnvironmentalDataPoint({ "environment": self.environment, "variable": variable, "is_desired": True, "value": value, "timestamp": timestamp }) doc_id = gen_doc_id(time.time()) self.env_data_db[doc_id] = doc # Clear running recipe if we exited by finishing iteration. # If there is a new recipe or recipe was already cleared, # we do nothing, and allow the loop to turn again and pick # up new recipe. if self.get_recipe() == recipe: try: self.clear_recipe() except RecipeIdleError: pass rospy.sleep(1)
def save_recipe_dp(self, variable): """ Save the recipe start/end to the env. data pt. DB, so we can restart the recipe if necessary. """ doc = EnvironmentalDataPoint({ "environment": self.environment, "variable": variable, "is_desired": True, "value": rospy.get_param(params.CURRENT_RECIPE), "timestamp": rospy.get_time() }) doc_id = gen_doc_id(rospy.get_time()) self.env_data_db[doc_id] = doc