def process_message(line):
trace('arduino_handler serial read: >%s<', line.replace('\n', ''))
# detect an empty and a line with only a \n char:
if len(line) <= 1:
return "No message"
try:
values = line[:-1].decode().split(',')
status_code = values[0]
# Expand status code to status dict
status = (STATUS_CODE_INDEX.get(status_code)
or expand_unknown_status(status_code))
# WARN/ERR format: "status_code, device_name, message"
if not status["is_ok"]:
error_device = values[1]
error_message = values[3] if len(values) >= 4 else values[2]
message = "arduino_handler {}> {}: {}".format(
status["message"], error_device, error_message)
rospy.logwarn(message)
return message
# else status: OK
# Zip values with the corresponding environmental variable
variable_values = values[1:]
pairs = tuple((headers, sensor_csv_headers[headers](value))
for headers, value in zip(sensor_csv_headers.keys()[1:],
variable_values))
return pairs
except ValueError:
message = "arduino_handler: Type conversion error, skipping."
rospy.logwarn(message)
return message
except IndexError:
message = "arduino_handler: Partial message: >{}<".format(line)
rospy.logwarn(message)
return message
# Occasionally, we get rotten bytes which couldn't decode
except UnicodeDecodeError:
message = "arduino_handler: Ignoring weird bits: >{}<".format(line)
rospy.logwarn(message)
return message
def interpret_simple_recipe(recipe, start_time, now_time):
"""
Produces a tuple of ``(variable, value)`` pairs by building up
a recipe state from walking through the recipe keyframes
"""
_id = recipe["_id"]
operations = recipe["operations"]
#rospy.logdebug(operations)
end_time_relative = operations[-1][0]
trace("recipe_handler: interpret_simple_recipe end_time_relative=%s",
end_time_relative)
end_time = start_time + end_time_relative
# If start time is at some point in the future beyond the threshold
if start_time - now_time > THRESHOLD:
raise ValueError("Recipes cannot be scheduled for the future")
# If there are no recipe operations, immediately start and stop
# The recipe.
if not len(operations):
return (
(RECIPE_START.name, _id),
(RECIPE_END.name, _id),
)
if now_time >= (end_time + THRESHOLD):
return ((RECIPE_END.name, _id),)
if abs(now_time - start_time) < THRESHOLD:
return ((RECIPE_START.name, _id),)
now_relative = (now_time - start_time)
# Create a state object to accrue recipe setpoint values.
state = {}
# Build up state up until now_time (inclusive).
trace("recipe_handler: interpret_simple_recipe now=%s", now_relative)
for timestamp, variable, value in operations:
if timestamp > now_relative:
break
state[variable] = value
trace("recipe_handler: interpret_simple_recipe: %s %s %s",
timestamp, variable, value)
#rospy.logdebug(state)
return tuple(
(variable, value)
for variable, value in state.iteritems()
)
def recover_any_previous_recipe(self):
"""
Attempt to resume any previous recipe that was started but
not completed.
"""
# Get the recipe that has been started most recently
start_view = self.env_data_db.view(
"openag/by_variable",
startkey=[self.environment, "desired", RECIPE_START.name],
endkey=[self.environment, "desired", RECIPE_START.name, {}],
stale="update_after",
group_level=3
)
if len(start_view) == 0:
trace("recover_any_previous_recipe: No previous recipe to recover.")
return
start_doc = start_view.rows[0].value
#trace("recover_any_previous_recipe: start_doc=%s", start_doc)
# If a recipe has been ended more recently than the most recent time a
# recipe was started, don't run the recipe
end_view = self.env_data_db.view(
"openag/by_variable",
startkey=[self.environment, "desired", RECIPE_END.name],
endkey=[self.environment, "desired", RECIPE_END.name, {}],
stale="update_after",
group_level=3
)
if len(end_view):
end_doc = end_view.rows[0].value
#trace("recover_any_previous_recipe: end_doc=%s", end_doc)
if (end_doc["timestamp"] > start_doc["timestamp"]):
trace("recover_any_previous_recipe: RETURNING: '\
'end_time=%s > start_time=%s",
end_doc["timestamp"], start_doc["timestamp"])
return
# Run the recipe
trace("recover_any_previous_recipe: restarting recipe=%s at time=%s",
start_doc["value"], start_doc["timestamp"])
self.start_recipe_service(
StartRecipe._request_class(start_doc["value"]),
start_doc["timestamp"]
)
def recover_any_previous_recipe(self):
"""
Attempt to resume any previous recipe that was started but
not completed.
"""
# Get the recipe that has been started most recently
start_view = self.env_data_db.view(
"openag/by_variable",
startkey=[self.environment, "desired", RECIPE_START.name],
endkey=[self.environment, "desired", RECIPE_START.name, {}],
stale="update_after",
group_level=3
)
if len(start_view) == 0:
trace("recover_any_previous_recipe: No previous recipe to recover.")
return
start_doc = start_view.rows[0].value
#trace("recover_any_previous_recipe: start_doc=%s", start_doc)
# If a recipe has been ended more recently than the most recent time a
# recipe was started, don't run the recipe
end_view = self.env_data_db.view(
"openag/by_variable",
startkey=[self.environment, "desired", RECIPE_END.name],
endkey=[self.environment, "desired", RECIPE_END.name, {}],
stale="update_after",
group_level=3
)
if len(end_view):
end_doc = end_view.rows[0].value
#trace("recover_any_previous_recipe: end_doc=%s", end_doc)
if (end_doc["timestamp"] > start_doc["timestamp"]):
trace("recover_any_previous_recipe: RETURNING: '\
'end_time=%s > start_time=%s",
end_doc["timestamp"], start_doc["timestamp"])
return
# Run the recipe
trace("recover_any_previous_recipe: restarting recipe=%s at time=%s",
start_doc["value"], start_doc["timestamp"])
self.start_recipe_service(
StartRecipe._request_class(start_doc["value"]),
start_doc["timestamp"]
)
def callback(data):
try:
recipe_handler.clear_recipe()
except RecipeIdleError:
pass
trace("recipe_handler.Subscriber: clearing current 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_doc:
try:
interpret_recipe = RECIPE_INTERPRETERS[recipe_doc["format"]]
except KeyError:
recipe_handler.clear_recipe()
rospy.logwarn("Invalid recipe format: '%s'",
recipe_doc.get("format"))
continue
# Get recipe state and publish it
setpoints = interpret_recipe(recipe_doc, start_time, now_time)
trace("Start_time: %s Now_time: %s", start_time, now_time)
for variable, value in setpoints:
try:
pub = PUBLISHERS[variable]
except KeyError:
msg = 'Recipe references invalid variable "{}"'
rospy.logwarn(msg.format(variable))
continue
if TRACE:
pub_debug.publish("{} : {}".format(variable, value))
# Publish any setpoints that we can
trace("recipe_handler publish: %s, %s", variable, value)
if variable == RECIPE_END.name:
trace("recipe_handler publish: END!")
def callback(data):
try:
recipe_handler.clear_recipe()
except RecipeIdleError:
pass
trace("recipe_handler.Subscriber: clearing current recipe.")
recipe_doc, start_time, now_time = recipe_handler.get_state()
# 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_doc:
try:
interpret_recipe = RECIPE_INTERPRETERS[recipe_doc["format"]]
except KeyError:
recipe_handler.clear_recipe()
rospy.logwarn("Invalid recipe format: '%s'",
recipe_doc.get("format"))
continue
# Get recipe state and publish it
setpoints = interpret_recipe(recipe_doc, start_time, now_time)
trace("Start_time: %s Now_time: %s", start_time, now_time)
for variable, value in setpoints:
try:
pub = PUBLISHERS[variable]
except KeyError:
msg = 'Recipe references invalid variable "{}"'
rospy.logwarn(msg.format(variable))
continue
if TRACE:
pub_debug.publish("{} : {}".format(variable, value))
# Publish any setpoints that we can
trace("recipe_handler publish: %s, %s", variable, value)
if variable == RECIPE_END.name:
trace("recipe_handler publish: END!")
actuator_state["light_intensity_blue"],
actuator_state["light_intensity_white"],
actuator_state["light_intensity_red"],
actuator_state["heater_core_1_1"],
actuator_state["chiller_compressor_1"]).encode('utf-8')
buf = ""
# Fix issue #328, sometimes serial_connection is None because of a
# serial port path / or error opening issue.
if serial_connection is None:
serial_connection = connect_serial()
try:
# Write
serial_connection.write(message) # Write message or timeout
trace('arduino_handler serial write %d bytes: >%s<', \
len(message), message.replace('\n',''))
serial_connection.flush() # Wait until all data is written
serial_connection.flushOutput() # Clear output buffer
# Read. Arduino sends both error messages and sensor data,
# in that order, and both may be in the buffer.
# Wait until Arduino data is stable
# (rospy.Rate will still try to keep the loop at serial_rate_hz)
rospy.sleep(arduino_delay_s)
# Blocks until one line is read or times out
buf = serial_connection.readline()
# Count consecutive empty reads, raise exception if threshold is exceeded
if buf == "":
empty_read_count += 1
else:
empty_read_count = 0
trace('arduino_handler empty read count: %d', empty_read_count)