def generate(self, data, timestamp):
"""
:param py_trees_msgs.Behaviour[] data:
:param ??? timestamp:
"""
graph = self.dotcode_factory.get_graph(rank=self.rank,
rankdir=self.rankdir,
ranksep=self.ranksep)
if len(data) == 0:
self.dotcode_factory.add_node_to_graph(
graph, 'No behaviour data received')
return graph
# DJS - not actually used? In python3 this is a propblem since
# unique_id.UniqueID is not hashable any longer
# behaviour_dict_by_id = {}
# for behaviour in data:
# behaviour_dict_by_id[behaviour.own_id] = behaviour
# first, add nodes to the graph, along with some cached information to
# make it easy to create the coloured edges on the second pass
states = {}
for behaviour in data:
self.dotcode_factory.add_node_to_graph(
graph,
str(behaviour.own_id),
nodelabel=behaviour.name,
shape=self.type_to_shape(behaviour.type),
color=self.active_and_status_colour_hex_map[(
behaviour.is_active, behaviour.status)],
tooltip=self.behaviour_to_tooltip_string(behaviour))
states[unique_id.fromMsg(behaviour.own_id)] = (behaviour.is_active,
behaviour.status)
for behaviour in data:
for child_id in behaviour.child_ids:
# edge colour is set using integer tuples, not hexes
try:
(is_active, status) = states[unique_id.fromMsg(child_id)]
except KeyError:
# the child isn't part of the 'visible' tree
continue
edge_colour = self.active_and_status_colour_tuple_map[(
is_active, status)]
self.dotcode_factory.add_edge_to_graph(graph=graph,
nodename1=str(
behaviour.own_id),
nodename2=str(child_id),
color=edge_colour)
return graph
def filter_behaviours_by_visibility_level(behaviours, visibility_level):
"""
Drops any behaviours whose blackbox level does not match the required visibility
level. See the py_trees.common module for more information.
:param py_trees_msgs.msg.Behaviour[] behaviours
:returns: py_trees_msgs.msg.Behaviour[]
"""
behaviours_by_id = {unique_id.fromMsg(b.own_id): b for b in behaviours}
visible_behaviours = [b for b in behaviours if is_visible(behaviours_by_id,
unique_id.fromMsg(b.own_id),
visibility_level)
]
return visible_behaviours
def _publish_tree_snapshots(self, tree):
"""
Callback that runs on a :class:`BehaviourTree <py_trees.trees.BehaviourTree>` after
it has ticked.
:param tree: the behaviour tree
:type tree: :py:class:`BehaviourTree <py_trees.trees.BehaviourTree>`
"""
if self.publishers is None:
rospy.logerr("BehaviourTree: call setup() on this tree to initialise the ros components")
return
snapshot = "\n\n%s" % py_trees.display.ascii_tree(self.root, snapshot_information=self.snapshot_visitor)
self.publishers.ascii_snapshot.publish(std_msgs.String(snapshot))
for behaviour in self.logging_visitor.tree.behaviours:
behaviour.is_active = True if unique_id.fromMsg(behaviour.own_id) in self.snapshot_visitor.nodes else False
# We're not interested in sending every single tree - only send a
# message when the tree changes.
if self.logging_visitor.tree.behaviours != self.last_tree.behaviours:
if self.root.tip() is None:
rospy.logerr("Behaviours: your tree is returning in an INVALID state (should always be FAILURE, RUNNING or SUCCESS)")
return
self.publishers.tip.publish(conversions.behaviour_to_msg(self.root.tip()))
self.publishers.log_tree.publish(self.logging_visitor.tree)
with self.lock:
if not self._bag_closed:
self.bag.write(self.publishers.log_tree.name, self.logging_visitor.tree)
self.last_tree = self.logging_visitor.tree
def is_root(behaviour_id):
"""
Check the unique id to determine if it is the root (all zeros).
:param uuid.UUID behaviour_id:
"""
return behaviour_id == unique_id.fromMsg(uuid_msgs.UniqueID())
def _allocate_resources(self, msg):
""" Scheduler resource allocation message handler. """
rqr_id = unique_id.fromMsg(msg.requester)
rqr = self.requesters.get(rqr_id)
if rqr: # known requester?
rqr.update(msg)
else: # new requester
self.requesters[rqr_id] = _RequesterStatus(self, msg)
def _allocate_resources(self, msg):
""" Scheduler resource allocation message handler. """
rqr_id = unique_id.fromMsg(msg.requester)
rqr = self.requesters.get(rqr_id)
if rqr: # known requester?
rqr.update(msg)
else: # new requester
self.requesters[rqr_id] = _RequesterStatus(self, msg)
def load(self, unique_identifier, configuration):
"""
Common loading actions required by the various configuration servers.
:param uuid_msgs/UniqueID unique_identifier:
:param configuration: arbitrary value decided upon by children (e.g. ros param namespace for param configuration)
"""
# @todo check if this has already been loaded - use policies to decide how to handle?
self.loaded[unique_id.fromMsg(unique_identifier)] = configuration
def is_visible(behaviours, behaviour_id, visibility_level):
"""
:param {id: py_trees_msgs.Behaviour} behaviours:
:param uuid.UUID behaviour_id:
:param py_trees.common.VisibilityLevel visibility_level
"""
branch_blackbox_level = get_branch_blackbox_level(
behaviours,
unique_id.fromMsg(behaviours[behaviour_id].parent_id),
py_trees.common.BlackBoxLevel.NOT_A_BLACKBOX
)
# see also py_trees.display.generate_pydot_graph
return visibility_level < branch_blackbox_level
def load(self, request):
"""
:param feed_the_troll_msgs.LoadFromParamServerRequest request:
"""
response = feed_the_troll_srvs.LoadFromRosParamResponse(
result=0,
message="no handler, so configuration not processed"
)
super(ROSParameters, self).load(request.unique_identifier, request.namespace)
if self._loading_handler is not None:
(response.result, response.message) = self._loading_handler(unique_id.fromMsg(request.unique_identifier), request.namespace)
return response
def get_branch_blackbox_level(behaviours, behaviour_id, current_level):
"""
Computes the critial (minimum) blackbox level present in the branch above
this behaviour.
:param {id: py_trees_msgs.Behaviour} behaviours: (sub)tree of all behaviours, including this one
:param uuid.UUID behaviour_id: id of this behavour
:param py_trees.common.BlackBoxLevel current_level
"""
if is_root(behaviour_id):
return current_level
parent_id = unique_id.fromMsg(behaviours[behaviour_id].parent_id)
new_level = min(behaviours[behaviour_id].blackbox_level, current_level)
return get_branch_blackbox_level(behaviours, parent_id, new_level)
def unload(self, request):
"""
Handle a configuration unload request from a feeder.
:param feed_the_troll_msgs.UnloadRequest request:
"""
response = feed_the_troll_srvs.UnloadResponse(
result=0, message="no handler, so configuration not unloaded")
uuid_identifier = unique_id.fromMsg(request.unique_identifier)
if self._unloading_handler is not None:
(response.result, response.message) = self._unloading_handler(
uuid_identifier, self.loaded[uuid_identifier])
try:
self.loaded.pop(uuid_identifier)
# let the user decide what/what not to log via the unloading handler
# rospy.loginfo("Troll: unloaded configuration [{0}][{1}]".format(unique_id.fromMsg(request.unique_identifier), self._service_namespace))
except KeyError:
rospy.logwarn(
"Troll: trying to unload an unknown configuration [{0}][{1}]".
format(unique_id.fromMsg(request.unique_identifier)),
self._service_namespace)
return response
def __init__(self, sched, msg):
""" Constructor. """
self.last_msg_time = msg.header.stamp
self.sched = sched
""" Scheduler serving this requester. """
self.requester_id = unique_id.fromMsg(msg.requester)
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet(msg, contents=ActiveRequest)
""" All active requests for this requester. """
feedback_topic = common.feedback_topic(self.requester_id,
self.sched.topic)
rospy.loginfo('requester feedback topic: ' + feedback_topic)
self.pub = rospy.Publisher(feedback_topic, SchedulerRequests,
latch=True)
def __init__(self, sched, msg):
""" Constructor. """
self.sched = sched
""" Scheduler for this requester. """
self.requester_id = unique_id.fromMsg(msg.requester)
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet([], self.requester_id, contents=ResourceReply)
""" All current scheduler responses for this requester. """
self.feedback_topic = common.feedback_topic(self.requester_id, self.sched.topic)
rospy.loginfo("requester feedback topic: " + self.feedback_topic)
self.pub = rospy.Publisher(self.feedback_topic, SchedulerRequests, latch=True)
self.update(msg) # set initial status
def __init__(self, sched, msg):
""" Constructor. """
self.last_msg_time = msg.header.stamp
self.sched = sched
""" Scheduler serving this requester. """
self.requester_id = unique_id.fromMsg(msg.requester)
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet(msg, contents=ActiveRequest)
""" All active requests for this requester. """
feedback_topic = common.feedback_topic(self.requester_id,
self.sched.topic)
rospy.loginfo('requester feedback topic: ' + feedback_topic)
self.pub = rospy.Publisher(feedback_topic,
SchedulerRequests,
latch=True)
def __init__(self, sched, msg):
""" Constructor. """
self.last_msg_time = rospy.Time.now()
""" Local time of last message. Avoid using message header
time stamps because other Concert components may not be using
synchronized clocks."""
self.sched = sched
""" Scheduler serving this requester. """
self.requester_id = unique_id.fromMsg(msg.requester)
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet(msg, contents=ActiveRequest)
""" All active requests for this requester. """
feedback_topic = common.feedback_topic(self.requester_id,
self.sched.topic)
rospy.loginfo('requester feedback topic: ' + feedback_topic)
self.pub = rospy.Publisher(feedback_topic, SchedulerRequests,
latch=True, queue_size=1)
def __init__(self, sched, msg):
""" Constructor. """
self.sched = sched
""" Scheduler for this requester. """
self.requester_id = unique_id.fromMsg(msg.requester)
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet([], self.requester_id, contents=ResourceReply)
""" All current scheduler responses for this requester. """
self.feedback_topic = common.feedback_topic(self.requester_id,
self.sched.topic)
rospy.loginfo('requester feedback topic: ' + self.feedback_topic)
self.pub = rospy.Publisher(self.feedback_topic,
SchedulerRequests,
latch=True)
self.update(msg) # set initial status
def __init__(self, sched, msg):
""" Constructor. """
self.last_msg_time = rospy.Time.now()
""" Local time of last message. Avoid using message header
time stamps because other Concert components may not be using
synchronized clocks."""
self.sched = sched
""" Scheduler serving this requester. """
self.requester_id = unique_id.fromMsg(msg.requester)
""" :class:`uuid.UUID` of this requester. """
self.rset = RequestSet(msg, contents=ActiveRequest)
""" All active requests for this requester. """
feedback_topic = common.feedback_topic(self.requester_id,
self.sched.topic)
rospy.loginfo('requester feedback topic: ' + feedback_topic)
self.pub = rospy.Publisher(feedback_topic,
SchedulerRequests,
latch=True,
queue_size=1)
def __init__(self, reqs, requester_id=None, contents=ResourceRequest):
""" Constructor. """
self.requester_id = requester_id
""" :class:`uuid.UUID` of this requester. """
self.contents = contents
""" Type of objects this request set contains. """
self.stamp = rospy.Time() # zero time stamp
""" ROS time (:class:`rospy.Time`) of last update, or time zero. """
# reqs is either a SchedulerRequests message, or a list of
# Request messages.
if isinstance(reqs, SchedulerRequests):
self.stamp = reqs.header.stamp
self.requester_id = unique_id.fromMsg(reqs.requester)
# Reset *reqs* to list of requests from the message.
reqs = reqs.requests
if self.requester_id is None:
raise TypeError('Requester ID missing.')
self.requests = {}
""" Dictionary of active requests. """
for msg in reqs:
rq = self.contents(msg)
self.requests[rq.uuid] = rq
def __init__(self, reqs, requester_id=None, contents=ResourceRequest):
""" Constructor. """
self.requester_id = requester_id
""" :class:`uuid.UUID` of this requester. """
self.contents = contents
""" Type of objects this request set contains. """
self.stamp = rospy.Time() # zero time stamp
""" ROS time (:class:`rospy.Time`) of last update, or time zero. """
# reqs is either a SchedulerRequests message, or a list of
# Request messages.
if isinstance(reqs, SchedulerRequests):
self.stamp = reqs.header.stamp
self.requester_id = unique_id.fromMsg(reqs.requester)
# Reset *reqs* to list of requests from the message.
reqs = reqs.requests
if self.requester_id is None:
raise TypeError('Requester ID missing.')
self.requests = {}
""" Dictionary of active requests. """
for msg in reqs:
rq = self.contents(msg)
self.requests[rq.uuid] = rq
def get_uuid(self):
""" :returns: UUID of this request.
:rtype: :class:`uuid.UUID`
"""
return unique_id.fromMsg(self.msg.id)
def __str__(self):
""" Generate string representation. """
return 'id: ' + str(unique_id.fromMsg(self.msg.id)) \
+ '\n priority: ' + str(self.msg.priority) \
+ '\n resources: ' + self._str_resources() \
+ '\n status: ' + str(self.msg.status)
def __init__(self, msg):
""" Constructor. """
self.msg = msg
""" Corresponding *scheduler_msgs/Request* message. """
self.uuid = unique_id.fromMsg(msg.id)
""" The :class:`uuid.UUID` of this request. """
def get_uuid(self):
""" :returns: UUID of this request.
:rtype: :class:`uuid.UUID`
"""
return unique_id.fromMsg(self.msg.id)
def __str__(self):
""" Generate string representation. """
return 'id: ' + str(unique_id.fromMsg(self.msg.id)) \
+ '\n priority: ' + str(self.msg.priority) \
+ '\n resources: ' + self._str_resources() \
+ '\n status: ' + str(self.msg.status)
def __init__(self, msg):
""" Constructor. """
self.msg = msg
""" Corresponding *scheduler_msgs/Request* message. """
self.uuid = unique_id.fromMsg(msg.id)
""" The :class:`uuid.UUID` of this request. """
