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)
