def move(self):
        """
        calculates movement vector
        :return: movement vector
        """
        pose = self._buffer.get_own_pose()
        g = None

        # strongest gradient
        grad = self._buffer.max_reach_attractive_gradient(self.frames,
                                                          self.static,
                                                          self.moving)

        if pose and grad:
            g = gradient.calc_attractive_gradient(grad, pose)

        if not g:
            return None

        # adjust length
        d = calc.vector_length(g)
        if d > self.maxvel:
            g = calc.adjust_length(g, self.maxvel)
        elif 0 < d < self.minvel:
            g = calc.adjust_length(g, self.minvel)

        return g
    def goal_gradient(self):
        """
        :return: vector to goal gradient
        """
        grad = None

        attractive = self._buffer.max_attractive_gradient(self.frames,
                                                          self.static,
                                                          self.moving)
        pose = self._buffer.get_own_pose()

        if attractive and pose:
            grad = gradient.calc_attractive_gradient(attractive, pose)

        return grad
    def strongest_gradient(self,
                           frameids=None,
                           static=True,
                           moving=True,
                           time=None):
        """
        return gradient with strongest potential on robot
        :param frameids: frames to consider
        :param static: bool, consider static gradients
        :param moving: bool, consider moving gradients
        :param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
        :return: soMessage gradient
        """
        tmp_grad = None

        if self.get_last_position() is None:
            return tmp_grad

        # all gradients within view distance
        gradients = self.gradients(frameids, static, moving, time=time)
        tmp_att = -1

        if gradients:
            for grad in gradients:
                if grad.attraction == 1:
                    g = gradient.calc_attractive_gradient(
                        grad, self.get_last_position())
                else:
                    g = gradient.calc_repulsive_gradient(
                        grad, self.get_last_position())

                if abs(g.x) == np.inf or abs(g.y) == np.inf or \
                                abs(g.z) == np.inf:
                    att = np.inf
                else:
                    att = calc.vector_length(g)
                    # attractive potential is defined inverse to repulsive
                    # potential --> adjust attractive value for comparison
                    if grad.attraction == 1:
                        att = 1 - att

                if att > tmp_att:
                    tmp_grad = grad
                    tmp_att = att

        return tmp_grad
    def move(self):
        """
        calculate movement vector
        :return: movement vector
        """
        pose = self._buffer.get_own_pose()
        g = None

        # strongest gradient
        grad = self._buffer.strongest_gradient(self.frames, self.static,
                                               self.moving)

        if pose:
            if grad:
                if grad.attraction == 1:
                    g = gradient.calc_attractive_gradient(grad, pose)
                elif grad.attraction == -1:
                    g = gradient.calc_repulsive_gradient(grad, pose)

                    # robot position is within obstacle goal radius
                    # handle infinite repulsion
                    if g.x == np.inf or g.x == -1 * np.inf:
                        # create random vector with length (goal_radius +
                        # gradient.diffusion)
                        dv = calc.delta_vector(pose.p, grad.p)

                        if calc.vector_length(dv) == 0:
                            g = calc.random_vector(grad.goal_radius +
                                                   grad.diffusion)
                        else:
                            g = calc.adjust_length(dv, grad.goal_radius
                                                   + grad.diffusion)

        if not g:
            return None

        # adjust length
        d = calc.vector_length(g)
        if d > self.maxvel:
            g = calc.adjust_length(g, self.maxvel)
        elif 0 < d < self.minvel:
            g = calc.adjust_length(g, self.minvel)

        return g
    def max_attractive_gradient(self,
                                frameids=None,
                                static=True,
                                moving=True,
                                time=None):
        """
        Method to return relatively nearest attractive gradient
        (= min movement vector length)
        based on attraction values of Balch & Hybinette
        :param frameids: frames to consider
        :param static: consider static gradients
        :param moving: consider moving gradients
        :param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
        :return: relatively nearest attractive gradient (Vector3)
        """

        pose = self.get_last_position()

        if pose is None:
            return []

        gradients = self.attractive_gradients(frameids,
                                              static,
                                              moving,
                                              time=time)
        tmp_grad = None
        tmp_att = np.inf

        if gradients:
            for grad in gradients:
                g = gradient.calc_attractive_gradient(grad, pose)
                att = calc.vector_length(g)
                # attraction decreases with being closer to gradient source
                # / goal area
                if att < tmp_att:
                    tmp_grad = grad
                    tmp_att = att

        return tmp_grad
    def min_attractive_gradient(self,
                                frameids=None,
                                static=True,
                                moving=True,
                                time=None):
        """
        Method to return relatively furthest gradient
        (= maximum attraction value length)
        based on attraction values of Balch & Hybinette
        :param frameids: frames to consider
        :param static: consider static gradients
        :param moving: consider moving gradients
        :param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
        :return: gradient (Vector3)
        """

        pose = self.get_last_position()

        if pose is None:
            return []

        gradients = self.attractive_gradients(frameids,
                                              static,
                                              moving,
                                              time=time)
        tmp_grad = None
        tmp_att = 0

        if gradients:
            for grad in gradients:
                # gradient reach
                g = gradient.calc_attractive_gradient(grad, pose)
                att = calc.vector_length(g)
                if att > tmp_att:
                    tmp_grad = grad
                    tmp_att = att

        return tmp_grad