Example #1
0
    def getCurrentPosition(self):
        position = Coordinates()
        totalPosition = Coordinates()

        measurement = 0
        numberOfMeasurements = 20

        while (measurement < numberOfMeasurements):
            status = self.pozyx.doPositioning(position,
                                              self.dimension,
                                              self.height,
                                              self.algorithm,
                                              remote_id=self.remote_id)
            if status == POZYX_SUCCESS:
                totalPosition.x = totalPosition.x + position.x
                totalPosition.y = totalPosition.y + position.y
                totalPosition.z = totalPosition.z + position.z
                measurement = measurement + 1
            else:
                """self.printPublishErrorCode("positioning")"""

        totalPosition.x = totalPosition.x / numberOfMeasurements
        totalPosition.y = totalPosition.y / numberOfMeasurements
        totalPosition.z = totalPosition.z / numberOfMeasurements

        return totalPosition
    def getTargetData(self, currentPosition, targetPosition):
        positionError = Coordinates()

        positionError.x = targetPosition.x - currentPosition.x
        positionError.y = targetPosition.y - currentPosition.y

        targetDistance = math.sqrt(math.pow(positionError.x, 2) + math.pow(positionError.y, 2))
        
        targetAngle = 0
        if positionError.x > 0:
            if positionError.y > 0:
                targetAngle = 90 + math.degrees(math.atan(positionError.x / positionError.y))
            else:
                if positionError.y == 0:
                    targetAngle = 0
                else:
                    targetAngle = 270 + math.degrees(math.atan(positionError.x / positionError.y))
        else:
            if positionError.y > 0:
                targetAngle = 90 + math.degrees(math.atan(positionError.x / positionError.y))
            else:
                if positionError.y == 0:
                    targetAngle = 180
                else:
                    targetAngle = 270 + math.degrees(math.atan(positionError.x / positionError.y))

        return [targetDistance, targetAngle]
    def get_tag_velocity(self, position_now, time_now):
        delt_pos = Coordinates()
        #unit: mm
        delt_pos.x = position_now.x - self.pos_last.x
        delt_pos.y = position_now.y - self.pos_last.y
        delt_pos.z = position_now.z - self.pos_last.z
        delt_time = (time_now - self.pos_last_time) * 1000.0  #s-->ms

        self.pos_last = position_now
        self.pos_last_time = time_now

        self.velocity.x = delt_pos.x / delt_time
        #m/s
        self.velocity.y = delt_pos.y / delt_time
        self.velocity.z = delt_pos.z / delt_time

        if self.debug == 2:
            print("Tag velocity is X=" + str(self.velocity.x) + "m/s; Y=" +
                  str(self.velocity.y) + "m/s Z=" + str(self.velocity.z) +
                  "m/s")
    def getCurrentPosition(self):
        position = Coordinates()
        totalPosition = Coordinates()

        status = 0
        while not status:
            status = self.pozyx.doPositioning(
                position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id)

        self.totalPositionX = self.totalPositionX - self.measurementsX.popleft() + position.x
        self.totalPositionY = self.totalPositionY - self.measurementsY.popleft() + position.y
        self.totalPositionZ = self.totalPositionZ - self.measurementsZ.popleft() + position.z
        self.measurementsX.append(position.x)
        self.measurementsY.append(position.y)
        self.measurementsZ.append(position.z)

        totalPosition.x = self.totalPositionX / self.numberOfMeasurements
        totalPosition.y = self.totalPositionY / self.numberOfMeasurements
        totalPosition.z = self.totalPositionZ / self.numberOfMeasurements

        return totalPosition
Example #5
0
    def loop(self):
        """Performs positioning and prints the results."""
        for i, tag in enumerate(self.tags):
            position = Coordinates()
            angles = EulerAngles()
            status = self.pozyx.doPositioning(
                position, self.dimension, self.height, self.algorithm, remote_id=tag)

            if status == POZYX_SUCCESS:
                #BUFFERx.pop(0)
                #BUFFERx.append(int(position.x))
                #BUFFERy.pop(0)
                #BUFFERy.append(int(position.y))
                #tempy = AverageMinMax(BUFFERy)
                # tempx = AverageMinMax(BUFFERx)

                # BUFFERx[1] = BUFFERx[0]
                # BUFFERx[0] = position.x
                # BUFFERy[1] = BUFFERy[0]
                # BUFFERy[0] = position.y
                # tempx = Exponentialfilter(BUFFERx)
                # tempy = Exponentialfilter(BUFFERy)

                theta= -2*pi/3
                position.x = cos(theta)*position.x - sin(theta)*position.y
                position.y = sin(theta)*position.x + cos(theta)*position.y


                uwb_pos = Twist()
                uwb_pos.linear.x = int(position.x) / 1000.
                uwb_pos.linear.y = int(position.y) / 1000.
                uwb_pos.linear.z = int(position.z) / 1000.
                self.printPublishPosition(position, tag)
                self.uwb_target_pub.publish(uwb_pos)
                return False
            else:
                self.printPublishErrorCode("positioning", tag)
                return True 
 def convert_to_ned(self, vector):
     ned_vector = Coordinates()
     ned_vector.x = vector.x * self.cos_yaw - vector.y * self.sin_yaw
     ned_vector.y = vector.x * self.sin_yaw + vector.y * self.cos_yaw
     ned_vector.z = vector.z
     return ned_vector