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 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 = 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, 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. """