def get_dispatch_msg(self, action): dispatch_msg = plan_dispatch_msgs.ActionDispatch() arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) if action.intention == 'go': dispatch_msg.name = 'move_base' for slot in action.slots: arg_msg = diag_msgs.KeyValue() if slot.type == 'destination': arg_msg.key = 'to' arg_msg.value = slot.data dispatch_msg.parameters.append(arg_msg) elif action.intention == 'take': for slot in action.slots: arg_msg = diag_msgs.KeyValue() if slot.type == 'object': arg_msg.key = 'obj' elif slot.type == 'source': dispatch_msg.name = 'pick' arg_msg.key = 'surface' elif slot.type == 'destination': dispatch_msg.name = 'place' arg_msg.key = 'surface' arg_msg.value = slot.data dispatch_msg.parameters.append(arg_msg) return dispatch_msg
def get_dispatch_msg(self, obj_name, surface_name): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'obj' arg_msg.value = obj_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'plane' arg_msg.value = surface_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'context' arg_msg.value = self.grasping_context dispatch_msg.parameters.append(arg_msg) return dispatch_msg
def get_dispatch_msg(self, plane_name): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'plane' arg_msg.value = plane_name dispatch_msg.parameters.append(arg_msg) return dispatch_msg
def get_dispatch_msg(self): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'perform_recognition' arg_msg.value = str(self.perform_recognition) dispatch_msg.parameters.append(arg_msg) return dispatch_msg
def __update_kb(self, update_type, fact_list): '''Updates the knowledge base with the facts in the given list. Keyword arguments: @param update_type -- a KnowledgeUpdateTypes constant describing the update type (add or remove facts) @param fact_list -- a list in which each entry is a tuple of the form (predicate, [(variable, value), ...]), where "predicate" is the predicate name and the (variable, value) tuples are the variable values ''' try: for predicate, var_data in fact_list: # we set up the knowledge update request with the given attribute data request = rosplan_srvs.KnowledgeUpdateServiceRequest() request.update_type = update_type request.knowledge.knowledge_type = 1 request.knowledge.attribute_name = predicate for var_name, var_value in var_data: arg_msg = diag_msgs.KeyValue() arg_msg.key = var_name arg_msg.value = var_value request.knowledge.values.append(arg_msg) # we modify the fact into the knowledge base self.knowledge_update_client(request) except Exception as exc: rospy.logerr('[kb_interface] %s', str(exc)) raise KBException('The knowledge base could not be updated')
def _publish(self, evt): msg = DM.DiagnosticArray() msg.header.stamp = rospy.get_rostime() with self.lock: self.entry.values = [] for k, v in self.fields.iteritems(): self.entry.values.append(DM.KeyValue(str(k), str(v))) msg.status.append(self.entry) self.pub.publish(msg)
def publish(self, root: py_trees.behaviour.Behaviour, changed: bool, statistics: py_trees_msgs.Statistics, visited_behaviour_ids: typing.Set[uuid.UUID], visited_blackboard_client_ids: typing.Set[uuid.UUID]): """" Publish a snapshot, including only what has been parameterised. Args: root: the tree changed: whether the tree status / graph changed or not statistics: add tree statistics to the snapshot data visited_behaviour_ids: behaviours on the visited path visited_blackboard_client_ids: blackboard clients belonging to behaviours on the visited path """ if self.last_snapshot_timestamp is None: changed = True self.last_snapshot_timestamp = time.monotonic() current_timestamp = time.monotonic() elapsed_time = current_timestamp - self.last_snapshot_timestamp if_its_close_enough = 0.98 # https://github.com/splintered-reality/py_trees_ros/issues/144 if (not changed and elapsed_time < if_its_close_enough * self.parameters.snapshot_period): return tree_message = py_trees_msgs.BehaviourTree() tree_message.changed = changed # tree for behaviour in root.iterate(): msg = conversions.behaviour_to_msg(behaviour) msg.is_active = True if behaviour.id in visited_behaviour_ids else False tree_message.behaviours.append(msg) # blackboard if self.parameters.blackboard_data: visited_keys = py_trees.blackboard.Blackboard.keys_filtered_by_clients( client_ids=visited_blackboard_client_ids) for key in visited_keys: try: value = str(py_trees.blackboard.Blackboard.get(key)) except KeyError: value = "-" tree_message.blackboard_on_visited_path.append( diagnostic_msgs.KeyValue(key=key, value=value)) # activity stream # TODO: checking the stream is not None is redundant, perhaps use it as an exception check? if self.parameters.blackboard_activity and py_trees.blackboard.Blackboard.activity_stream is not None: tree_message.blackboard_activity = conversions.activity_stream_to_msgs( ) # other if statistics is not None: tree_message.statistics = statistics self.publisher.publish(tree_message) self.last_snapshot_timestamp = current_timestamp
def get_dispatch_msg(self, original_location, destination_location): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'from' arg_msg.value = original_location dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'to' arg_msg.value = destination_location dispatch_msg.parameters.append(arg_msg) return dispatch_msg
def get_dispatch_msg(self, obj_name): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'obj' arg_msg.value = obj_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'throwing_target' arg_msg.value = self.throwing_target_name dispatch_msg.parameters.append(arg_msg) return dispatch_msg
def send_action_feedback(self, success): msg = plan_dispatch_msgs.ActionFeedback() msg.action_id = self.action_id if success: msg.status = self.action_success_msg else: msg.status = self.action_failure_msg action_name_kvp = diag_msgs.KeyValue() action_name_kvp.key = 'action_name' action_name_kvp.value = self.action_name msg.information.append(action_name_kvp) self.feedback_pub.publish(msg)
def send_action_feedback(self, success): '''Publishes a rosplan_dispatch_msgs.msg.ActionFeedback message based on the result of the action execution. Keyword arguments: success -- a Boolean indicating whether the action was successfully executed ''' msg = plan_dispatch_msgs.ActionFeedback() msg.action_id = self.action_id if success: msg.status = self.action_success_msg else: msg.status = self.action_failure_msg action_name_kvp = diag_msgs.KeyValue() action_name_kvp.key = 'action_name' action_name_kvp.value = self.action_name msg.information.append(action_name_kvp) self.feedback_pub.publish(msg)