smach.py

Go to the documentation of this file.

    def _update_server_list(self):
        while self._keep_running:
            # Update the server list
            server_names = self._client.get_servers()
            new_server_names = [sn for sn in server_names if sn not in self._status_subs]

            # Create subscribers for new servers
            for server_name in new_server_names:
                self._structure_subs[server_name] = rospy.Subscriber(
                        server_name+smach_ros.introspection.STRUCTURE_TOPIC,
                        SmachContainerStructure,
                        callback = self._structure_msg_update,
                        callback_args = server_name,
                        queue_size=50)

                self._status_subs[server_name] = rospy.Subscriber(
                        server_name+smach_ros.introspection.STATUS_TOPIC,
                        SmachContainerStatus,
                        callback = self._status_msg_update,
                        queue_size=50)

            # This doesn't need to happen very often
            rospy.sleep(1.0)


            #self.server_combo.AppendItems([s for s in self._servers if s not in current_servers])

            # Grab the first server
            #current_value = self.server_combo.GetValue()
            #if current_value == '' and len(self._servers) > 0:
            #    self.server_combo.SetStringSelection(self._servers[0])
            #    self.set_server(self._servers[0])
   def _status_msg_update(self, msg):
        if not self._keep_running:
            return
        path = msg.path
        #print "STATUS MSG: "+path
        needs_redraw = False
        if path in self._containers:
            container = self._containers[path]
            if container.update_status(msg):
                #print path+": "+str(msg.active_states)
                """
                for child_label in container._children:
                    child_path = '/'.join([path,child_label])
                    if child_path in self.widget.items:
                        if child_label in container._active_states:
                            #print 'SET "'+child_path+'" ACTIVE'
                            pass
                        for shape in self.widget.items[child_path].shapes:
                            if child_label in container._active_states:
                                shape.pen.fillcolor = (0,1,0,1)
                            elif child_label in container._last_active_states:
                                shape.pen.fillcolor = (1,0.85,0.85,1)
                            else:
                                shape.pen.fillcolor = (1,1,1,1)
                """
                self._update_cond.acquire()
                self._update_cond.notify_all()
                self._update_cond.release()

            path_input_str = self.path_input.GetValue()
            if path_input_str == path or get_parent_path(path_input_str) == path:
                wx.PostEvent(
                        self.path_input.GetEventHandler(),
                        wx.CommandEvent(wx.wxEVT_COMMAND_COMBOBOX_SELECTED,self.path_input.GetId()))

    def _structure_msg_update(self, msg, server_name):
        if not self._keep_running:
            return

        path = msg.path
        pathsplit = path.split('/')
        dir = '/'.join(pathsplit[0:-1])

        # Only add if this container has not been added, and its parents have been
        #print "RECEIVED: "+path
        #print "CONTAINERS: "+str(self._containers.keys())
        needs_redraw = False
        if path in self._containers:
            needs_redraw = self._containers[path].update_structure(msg)
        else: #if (dir    == '' or dir in self._containers):
            #print "CONSTRUCTING: "+path
            needs_redraw= True
            self._needs_zoom = True
            container = ContainerNode(server_name, msg)
            self._containers[path] = container
            # Store this as a top container if it is at the root
            if dir=='':
                self._top_containers[path] = container
            # Append paths to selector
            self.path_combo.Append(path)
            self.path_input.Append(path)

        self._update_cond.acquire()
        if needs_redraw:
            self._structure_changed = True
            self._needs_zoom = True
            self._update_cond.notify_all()
        self._update_cond.release()

    def update_structure(self, msg):
        needs_update = False

        if self._children != msg.children\
                or self._internal_outcomes != msg.internal_outcomes\
                or self._outcomes_from != msg.outcomes_from\
                or self._outcomes_to != msg.outcomes_to\
                or self._container_outcomes != msg.container_outcomes:
            needs_update = True

        if needs_update:
            self._children = msg.children
            self._internal_outcomes = msg.internal_outcomes
            self._outcomes_from = msg.outcomes_from
            self._outcomes_to = msg.outcomes_to

            self._container_outcomes = msg.container_outcomes

        return needs_update


    def update_status(self, msg):
        needs_update = False
        if set(msg.initial_states) != set(self._initial_states):
            self._structure_changed = True
            needs_update = True
        if set(msg.active_states) != set(self._active_states):
            needs_update = True

        self._initial_states = msg.initial_states
        self._last_active_states = self._active_states
        self._active_states = msg.active_states
        while True:
            try:
                self._local_data._data = pickle.loads(msg.local_data)
                break
            except ImportError as ie:
                # This will only happen once for each package
                modulename = ie.args[0][16:]
                packagename = modulename[0:modulename.find('.')]
                roslib.load_manifest(packagename)
                self._local_data._data = pickle.loads(msg.local_data)

        self._info = msg.info

        return needs_update