def update_trace(self,
event_name,
fault_status,
context=None,
force=False):
"""
Update the trace. If the fault_status is unchanged, do nothing,
unless the force flag is set.
fault_status can be a boolean value. If so, then a value of 'True'
indicates the presence of a fault, while a value of 'False' indicates
nominal operation.
"""
if type(fault_status) == bool:
fault_status = MonitorMetadata.NOMINAL if not fault_status else MonitorMetadata.FAULT
if fault_status == self.fault_status and not force:
return
# Update the fault_status
self.fault_status = fault_status
trace_event = ExecutionEvent(
stamp=rospy.Time.now(),
name=event_name,
type=ExecutionEvent.MONITOR_EVENT,
monitor_metadata=MonitorMetadata(
fault_status=self.fault_status,
context=(pickle.dumps(context) if context is not None else ''),
topics=self.topics,
services=self.services,
actions=self.actions,
nodes=self.nodes))
self._trace.publish(trace_event)
def _update_monitor_trace(self,
event_subtype,
context,
topics=[],
services=[],
actions=[]):
# Context must be a dictionary
context['step_name'] = self.name
context['step_uuid'] = self.uuid
# We always publish a monitoring event, because these are manually
# triggered event notifications in the code
event = ExecutionEvent(
stamp=rospy.Time.now(),
name=
event_subtype, # ACTION_SEND_GOAL_EVENT, ACTION_RECV_RESULT_EVENT, etc.
type=ExecutionEvent.MONITOR_EVENT,
monitor_metadata=MonitorMetadata(
fault_status=MonitorMetadata.
NOMINAL, # We never try to detect faults here
context=pickle.dumps(context),
topics=topics,
services=services,
actions=actions))
self._trace.publish(event)
def update_beliefs(self, beliefs, context=None, force=False):
"""Take in a dictionary of beliefs and update them according to force"""
# Keep a record of the event messages that were sent
events_sent = []
# Update the beliefs as necessary
for belief, value in beliefs.iteritems():
value = float(value)
assert 0 <= value <= 1, "Invalid value for belief, {}: {}".format(
belief, value)
if force or self.beliefs.get(belief, -1) != value:
trace_event = ExecutionEvent(
stamp=rospy.Time.now(),
name=belief,
type=ExecutionEvent.BELIEF_EVENT,
belief_metadata=BeliefMetadata(
value=value, context=pickle.dumps(context)))
self._trace.publish(trace_event)
events_sent.append(trace_event)
# Cache the belief
self.beliefs[belief] = value
# Finally return the messages that were sent
return events_sent
def initialize_trace(self, start_time):
"""Initialize the first trace event"""
event = ExecutionEvent(stamp=start_time)
self.full_trace.append(event)
if self._create_parsed_events:
self.parsed_trace.append(self._get_parsed_trace_from_event(event))
self._trace[0, 0] = start_time.to_time()
for idx, trace_spec in enumerate(ExecutionTracer.trace_types):
if trace_spec[0] == ExecutionEvent.MONITOR_EVENT:
self._trace[idx, 0] = MonitorMetadata.NOMINAL
def update_beliefs(self, beliefs, context={}):
"""Updates the event trace with belief updates. Expects lists/tuples"""
for belief, value in beliefs.iteritems():
value = float(value)
assert 0 <= value <= 1, "Invalid value for belief, {}: {}".format(
belief, value)
event = ExecutionEvent(stamp=rospy.Time.now(),
name=belief,
type=ExecutionEvent.BELIEF_EVENT,
belief_metadata=BeliefMetadata(
value=value,
context=pickle.dumps(context)))
self._trace.publish(event)
def _update_task_trace(self, context):
# Check to see if this is a trivial update
if (self._last_event is not None and
self._last_event[0].task_step_metadata.status == self.status
and self._last_event[1] == context):
return
# Publish the event
event = ExecutionEvent(stamp=rospy.Time.now(),
name=self.name,
type=ExecutionEvent.TASK_STEP_EVENT,
task_step_metadata=TaskStepMetadata(
uuid=self.uuid,
status=self.status,
context=pickle.dumps(context)))
self._trace.publish(event)
self._last_event = (
event,
context,
)
def _on_topology(self, graph_msg):
# First parse out the nodes from the graph message
nodes = {}
topic_users = {}
action_users = {}
service_users = {} # At this time, this is only /task_executor
# Iterate through the message
for node_msg in graph_msg.nodes:
node = Node(node_msg.name)
# Parse through the topics
publishes = set(node_msg.publishes)
subscribes = set(node_msg.subscribes)
candidate_actions = {
} # action: [hypothesis-of-provide/use, set-of-seen-suffixes]
for topic_name in publishes | subscribes:
action_name, suffix = get_action_name_from_topic(topic_name)
if suffix: # This is potentially an action
if action_name not in candidate_actions.keys():
candidate_actions[action_name] = [
Connection.BOTH, set()
]
if suffix == "/goal" and topic_name in publishes:
candidate_actions[action_name][0] = Connection.OUT
elif suffix == "/goal" and topic_name in subscribes:
candidate_actions[action_name][0] = Connection.IN
candidate_actions[action_name][1].add(suffix)
elif topic_name not in ROSGraphMonitor.TOPICS_OF_NO_INTEREST: # This is a topic
if topic_name in publishes:
node.provides_topic(topic_name)
if topic_name in subscribes:
node.uses_topic(topic_name)
if topic_name not in topic_users:
topic_users[topic_name] = set()
topic_users[topic_name].add(node)
# Now iterate through the potential actions. If they are actions,
# add them as such. Otherwise, add to the topics
for action_name, action_hyp in candidate_actions.iteritems():
if action_hyp[0] != Connection.BOTH and action_hyp[
1] == ROSGraphMonitor.ACTION_SUFFIXES:
if action_hyp[0] == Connection.OUT:
node.uses_action(action_name)
if action_name not in action_users:
action_users[action_name] = set()
action_users[action_name].add(node)
else:
node.provides_action(action_name)
else:
for suffix in action_hyp[1]:
topic_name = action_name + suffix
if topic_name in publishes:
node.provides_topic(topic_name)
else:
node.uses_topic(topic_name)
if topic_name not in topic_users:
topic_users[topic_name] = set()
topic_users[topic_name].add(node)
# If this is the task executor, then add in the services
if node.name == ROSGraphMonitor.TASK_EXECUTOR_SERVER_NAME:
for service_name in ROSGraphMonitor.SERVICES_OF_INTEREST:
node.uses_service(service_name)
if service_name not in service_users:
service_users[service_name] = set()
service_users[service_name].add(node)
# Iterate through the services and add them accordingly
for service_msg in node_msg.provides:
if service_msg.name in ROSGraphMonitor.SERVICES_OF_INTEREST:
node.provides_service(service_msg.name)
# Finally add this node to the set of nodes
nodes[node.name] = node
# Create the new graph from the graph message
graph = nx.DiGraph()
for node_name, node in nodes.iteritems():
graph.add_node(node)
# Add the action connections
for action_name in node.actions_provided:
if action_name not in action_users:
continue
for action_user in action_users[action_name]:
graph.add_edge(node, action_user)
if len(graph[node][action_user]) == 0:
graph[node][action_user]['services'] = set()
graph[node][action_user]['actions'] = set()
graph[node][action_user]['topics'] = set()
graph[node][action_user]['actions'].add(action_name)
# Add the service connections
for service_name in node.services_provided:
if service_name not in service_users:
continue
for service_user in service_users[service_name]:
graph.add_edge(node, service_user)
if len(graph[node][service_user]) == 0:
graph[node][service_user]['services'] = set()
graph[node][service_user]['actions'] = set()
graph[node][service_user]['topics'] = set()
graph[node][service_user]['services'].add(service_name)
# Add the topic connections
for topic_name in node.topics_provided:
if topic_name not in topic_users:
continue
for topic_user in topic_users[topic_name]:
graph.add_edge(node, topic_user)
if len(graph[node][topic_user]) == 0:
graph[node][topic_user]['services'] = set()
graph[node][topic_user]['actions'] = set()
graph[node][topic_user]['topics'] = set()
graph[node][topic_user]['topics'].add(topic_name)
# Finally update the old graph with this new graph and let the trace
# know
self.graph = graph
self._trace.publish(
ExecutionEvent(stamp=rospy.Time.now(),
name=ROSGraphMonitor.ROSGRAPH_MONITOR_EVENT_NAME,
type=ExecutionEvent.ROSGRAPH_EVENT))