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))