コード例 #1
0
    def __init__(self, standalone=False, detectorsource="kinect"):
        self.vectorField = VectorField()

        # if the obstacle avoider is run a standalone module,
        # it will get the (scanned) obstacle matrix directly from the
        # obstacle detector instead of from the memory
        self.standalone = standalone

        # initally don't use visualization.
        # enable it with 'startVisualization();
        self.visualize = 0

        self.total_frames = 0
        self.total_delay = 0
        self.total_arch = 0
        self.total_network = 0
        self.total_local = 0

        if standalone == True:
            import vision.obstacledetector
            if detectorsource != "kinect":
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(
                    standalone=True, source='file', filepath=detectorsource)
            else:
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(
                    standalone=True, source='kinect')
        else:
            import memory
            self.m = memory.Memory()

        if self.visualize == 1:
            self.startVisualization()
コード例 #2
0
    def __init__(self, standalone=False, detectorsource="kinect"):
        self.vectorField = VectorField()

        # if the obstacle avoider is run a standalone module,
        # it will get the (scanned) obstacle matrix directly from the
        # obstacle detector instead of from the memory
        self.standalone = standalone

        # initally don't use visualization.
        # enable it with 'startVisualization();
        self.visualize = 0

        self.total_frames = 0
        self.total_delay = 0
        self.total_arch = 0
        self.total_network = 0
        self.total_local = 0

        if standalone == True:
            import vision.obstacledetector
            if detectorsource != "kinect":
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(standalone=True, source='file', filepath=detectorsource)
            else:
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(standalone=True, source='kinect')
        else:
            import memory
            self.m = memory.Memory()

        self.startVisualization()
コード例 #3
0
class ObstacleAvoider:
    """ avoid obstacles using potential field navigation
        NOTE: The magic (calculating the final direction using the obstacle matrix)
                happens in the vectorfield.py module.
    """

    def __init__(self, standalone=False, detectorsource="kinect"):
        self.vectorField = VectorField()

        # if the obstacle avoider is run a standalone module,
        # it will get the (scanned) obstacle matrix directly from the
        # obstacle detector instead of from the memory
        self.standalone = standalone

        # initally don't use visualization.
        # enable it with 'startVisualization();
        self.visualize = 0

        self.total_frames = 0
        self.total_delay = 0
        self.total_arch = 0
        self.total_network = 0
        self.total_local = 0

        if standalone == True:
            import vision.obstacledetector
            if detectorsource != "kinect":
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(standalone=True, source='file', filepath=detectorsource)
            else:
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(standalone=True, source='kinect')
        else:
            import memory
            self.m = memory.Memory()

        self.startVisualization()


    def startVisualization(self):
        """
            Start the visualization module to show the desicions of the robot in real time
        """
        self.visualize = 1
        import visualisation.avoidancevisualizer
        self.visualizer = visualisation.avoidancevisualizer.AvoidanceVisualizer()

    def avoid(self, target):
        """
        Call this function if you want to go to some direction
        The avoider will take care of not bumping into obstacles
        Target (speed, angle) specifies the angle to the target with respect to the robot
        and the speed the robot should have when driving to the target
        """

        completeObstacleMatrix = None # normal obstacle matrix (with every obstacle in it)
        scannedObstacleMatrix = None # obstacle matrix with just the obstacles closest to the robot in it)

        framenumber = 0
        delay = 0
        if self.standalone == True:
            # get obstacle matrix directly from the obstacle detector
            completeObstacleMatrix = self.obstacleDetector.getObstacleMatrix()
        else:
            # get obstacle matrix from memory
            last_observation = self.m.get_last_observation('obstacle_matrix')
            if (last_observation != None):
                completeObstacleMatrix = last_observation[1]['matrix']

                if False: # set to True to enable profiling output
                    scantime = last_observation[1]['mtime']
                    local_delay = last_observation[1]['local_delay']
                    diff = time.time() - scantime
                    network_delay = last_observation[1]['network_delay']
                    architecture_delay = diff - local_delay - network_delay

                    print "Local delay: %10.5f Network delay: %10.5f Architecture delay: %10.5f Total delay: %10.5f" % (local_delay, network_delay, architecture_delay, diff)
                    self.total_frames += 1
                    self.total_network += network_delay
                    self.total_arch += architecture_delay
                    self.total_delay += diff
                    self.total_local += local_delay

                    net_avg = self.total_network / self.total_frames
                    arch_avg = self.total_arch / self.total_frames
                    local_avg = self.total_local / self.total_frames
                    total_avg = self.total_delay / self.total_frames

                    net_per = (net_avg / total_avg) * 100
                    arch_per = (arch_avg / total_avg) * 100
                    local_per = (local_avg / total_avg) * 100
                    print "Avg consumed: local: %10.5f (%4.1f %%) network: %10.5f (%4.1f %%) architecture: %10.5f (%4.1f %%) Total: %10.5f (100 %%)" % (local_avg, local_per, net_avg, net_per, arch_avg, arch_per, total_avg)
            else:
                print "No obstacle matrix retreived!"
                completeObstacleMatrix = None

        if completeObstacleMatrix != None:
            #convert obstacle matrix to scanned obstacle matrix
            scannedObstacleMatrix = self.getScannedObstacleMatrix(completeObstacleMatrix)
            # set the vector field obstacles
            self.vectorField.setObstacleMatrix(scannedObstacleMatrix)
            self.vectorField.setTarget(target)
        else:
            # stand still
            self.vectorField.setTarget((0,0))


        # get the endvector from the vector field
        # this endVector is the final destination of the robot
        endVector = self.vectorField.update_field()

        # if no endVector is retreived (probably no obstaclematrix defined)
        # set the target (specified by a behavior) as endvector
        if endVector == None:
            endVector = target


        # update visualizer (if enabled)
        if self.visualize == 1:
            if completeObstacleMatrix != None:
                self.visualizer.setObstacleMatrix(completeObstacleMatrix)
            if scannedObstacleMatrix != None:
                self.visualizer.setObstacleEdgesMatrix(scannedObstacleMatrix)
                self.visualizer.setObstacleVectors(self.vectorField.getAllObstacleAffectVectors(scannedObstacleMatrix))
            self.visualizer.setTargetVector(target)
            self.visualizer.setEndVector(endVector)

            # draw everything (and exit if needed)
            if self.visualizer.draw() == 'exit':
                sys.exit()

        # if speed is 0, then just return target because the robot shouldn't drive anyway
        if target[0] == 0:
            return target

        # return the endVector. This is the final direction and speed for the robot
        return endVector


    def getScannedObstacleMatrix(self, obstacleMatrix):
        """
            Get the scanned obstacle matrix, generated from the normal obstacle matrix
        """
        return getScannedObstacleMatrix(obstacleMatrix)
コード例 #4
0
class ObstacleAvoider:
    """ avoid obstacles using potential field navigation
        NOTE: The magic (calculating the final direction using the obstacle matrix)
                happens in the vectorfield.py module.
    """
    def __init__(self, standalone=False, detectorsource="kinect"):
        self.vectorField = VectorField()

        # if the obstacle avoider is run a standalone module,
        # it will get the (scanned) obstacle matrix directly from the
        # obstacle detector instead of from the memory
        self.standalone = standalone

        # initally don't use visualization.
        # enable it with 'startVisualization();
        self.visualize = 0

        self.total_frames = 0
        self.total_delay = 0
        self.total_arch = 0
        self.total_network = 0
        self.total_local = 0

        if standalone == True:
            import vision.obstacledetector
            if detectorsource != "kinect":
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(
                    standalone=True, source='file', filepath=detectorsource)
            else:
                self.obstacleDetector = vision.obstacledetector.ObstacleDetector(
                    standalone=True, source='kinect')
        else:
            import memory
            self.m = memory.Memory()

        if self.visualize == 1:
            self.startVisualization()

    def do_visualize(self):
        # update visualizer (if enabled)
        if self.visualize == 1:
            last_observation = self.m.get_last_observation('obstacle_matrix')
            if (last_observation != None):
                completeObstacleMatrix = last_observation[1]['matrix']
                scannedObstacleMatrix = self.getScannedObstacleMatrix(
                    completeObstacleMatrix)
                self.visualizer.setObstacleMatrix(completeObstacleMatrix)
                self.visualizer.setObstacleEdgesMatrix(scannedObstacleMatrix)
                self.visualizer.draw()

    def startVisualization(self):
        """
            Start the visualization module to show the desicions of the robot in real time
        """
        self.visualize = 1
        import visualisation.avoidancevisualizer
        self.visualizer = visualisation.avoidancevisualizer.AvoidanceVisualizer(
        )

    def avoid(self, target):
        """
        Call this function if you want to go to some direction
        The avoider will take care of not bumping into obstacles
        Target (speed, angle) specifies the angle to the target with respect to the robot
        and the speed the robot should have when driving to the target
        """

        completeObstacleMatrix = None  # normal obstacle matrix (with every obstacle in it)
        scannedObstacleMatrix = None  # obstacle matrix with just the obstacles closest to the robot in it)

        framenumber = 0
        delay = 0
        if self.standalone == True:
            # get obstacle matrix directly from the obstacle detector
            completeObstacleMatrix = self.obstacleDetector.getObstacleMatrix()
        else:
            # get obstacle matrix from memory
            last_observation = self.m.get_last_observation('obstacle_matrix')
            if (last_observation != None):
                completeObstacleMatrix = last_observation[1]['matrix']

                if False:  # set to True to enable profiling output
                    scantime = last_observation[1]['mtime']
                    local_delay = last_observation[1]['local_delay']
                    diff = time.time() - scantime
                    network_delay = last_observation[1]['network_delay']
                    architecture_delay = diff - local_delay - network_delay

                    print "Local delay: %10.5f Network delay: %10.5f Architecture delay: %10.5f Total delay: %10.5f" % (
                        local_delay, network_delay, architecture_delay, diff)
                    self.total_frames += 1
                    self.total_network += network_delay
                    self.total_arch += architecture_delay
                    self.total_delay += diff
                    self.total_local += local_delay

                    net_avg = self.total_network / self.total_frames
                    arch_avg = self.total_arch / self.total_frames
                    local_avg = self.total_local / self.total_frames
                    total_avg = self.total_delay / self.total_frames

                    net_per = (net_avg / total_avg) * 100
                    arch_per = (arch_avg / total_avg) * 100
                    local_per = (local_avg / total_avg) * 100
                    print "Avg consumed: local: %10.5f (%4.1f %%) network: %10.5f (%4.1f %%) architecture: %10.5f (%4.1f %%) Total: %10.5f (100 %%)" % (
                        local_avg, local_per, net_avg, net_per, arch_avg,
                        arch_per, total_avg)
            else:
                print "No obstacle matrix retreived!"
                completeObstacleMatrix = None

        if completeObstacleMatrix != None:
            #convert obstacle matrix to scanned obstacle matrix
            scannedObstacleMatrix = self.getScannedObstacleMatrix(
                completeObstacleMatrix)
            # set the vector field obstacles
            self.vectorField.setObstacleMatrix(scannedObstacleMatrix)
            self.vectorField.setTarget(target)
        else:
            # stand still
            self.vectorField.setTarget((0, 0))

        # get the endvector from the vector field
        # this endVector is the final destination of the robot
        endVector = self.vectorField.update_field()

        # if no endVector is retreived (probably no obstaclematrix defined)
        # set the target (specified by a behavior) as endvector
        if endVector == None:
            endVector = target

        # update visualizer (if enabled)
        if self.visualize == 1:
            if completeObstacleMatrix != None:
                self.visualizer.setObstacleMatrix(completeObstacleMatrix)
            if scannedObstacleMatrix != None:
                self.visualizer.setObstacleEdgesMatrix(scannedObstacleMatrix)
                self.visualizer.setObstacleVectors(
                    self.vectorField.getAllObstacleAffectVectors(
                        scannedObstacleMatrix))
            self.visualizer.setTargetVector(target)
            self.visualizer.setEndVector(endVector)

            # draw everything (and exit if needed)
            if self.visualizer.draw() == 'exit':
                sys.exit()

        # if speed is 0, then just return target because the robot shouldn't drive anyway
        if target[0] == 0:
            return target

        # return the endVector. This is the final direction and speed for the robot
        return endVector

    def getScannedObstacleMatrix(self, obstacleMatrix):
        """
            Get the scanned obstacle matrix, generated from the normal obstacle matrix
        """
        return getScannedObstacleMatrix(obstacleMatrix)