def on_image(self, item):
# Rate limit
curr_time = time.time()
if (curr_time - self.last_update) < self.min_update_interval:
return
self.last_update = curr_time
rospy.loginfo("Posting image")
image_format = self.image_format_mapping.get(item.encoding, None)
if image_format is None:
raise ValueError()
img = Image.fromstring(image_format, (item.width, item.height),
item.data)
point = EnvironmentalDataPoint({
"environment": self.environment,
"variable": self.variable.name,
"is_desired": False,
"value": None,
"timestamp": time.time()
})
point_id, point_rev = self.db.save(point)
url = "{db_url}/{point_id}/image?rev={rev}".format(
db_url=self.db.resource.url, point_id=point_id, rev=point_rev)
buf = StringIO()
img.save(buf, "PNG")
buf.seek(0)
headers = {"Content-Type": "image/png"}
res = requests.put(url, data=buf, headers=headers)
if res.status_code != 201:
raise RuntimeError("Failed to post image to database: {}".format(
res.content))
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 = self.gen_doc_id(curr_time)
self.db[point_id] = point
self.last_value = value
self.last_time = curr_time
def run(self):
while True:
while not self.recipe_flag.is_set():
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
rospy.sleep(1)
self.current_set_points = {}
for timestamp, variable, value in self.current_recipe.set_points():
if variable in self.valid_variables:
self.current_set_points[variable] = value
self.publishers[variable].publish(value)
else:
rospy.logwarn(
'Recipe references invalid variable "{}"'.format(
variable))
curr_time = time.time()
point = EnvironmentalDataPoint({
"environment": self.environment,
"variable": RECIPE_END.name,
"is_desired": True,
"value": self.current_recipe._id,
"timestamp": curr_time
})
point_id = self.gen_doc_id(curr_time)
self.env_data_db[point_id] = point
rospy.set_param(params.CURRENT_RECIPE, "")
rospy.set_param(params.CURRENT_RECIPE_START, 0)
self.recipe_flag.clear()
def start_recipe(self, data, start_time=None):
recipe_id = data.recipe_id
if not recipe_id:
return False, "No recipe id was specified"
if self.recipe_flag.is_set():
return False, "There is already a recipe running. Please stop it "\
"before attempting to start a new one"
try:
recipe = self.recipe_db[recipe_id]
except Exception as e:
return False, "\"{}\" does not reference a valid "\
"recipe".format(recipe_id)
start_time = start_time or time.time()
self.current_recipe = self.recipe_class_map[recipe.get(
"format", "simple")](recipe_id, recipe["operations"], start_time)
point = EnvironmentalDataPoint({
"environment": self.environment,
"variable": RECIPE_START.name,
"is_desired": True,
"value": recipe_id,
"timestamp": start_time
})
point_id = self.gen_doc_id(start_time)
self.env_data_db[point_id] = point
rospy.set_param(params.CURRENT_RECIPE, recipe_id)
rospy.set_param(params.CURRENT_RECIPE_START, start_time)
self.recipe_flag.set()
rospy.loginfo('Starting recipe "{}"'.format(recipe_id))
return True, "Success"
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