Exemplo n.º 1
0
    def test_relative_degrees(
        self,
        mock_latitude_to_m_per_d_longitude,
        mock_m_per_latitude
    ):
        mock_latitude_to_m_per_d_longitude.return_value = 1.0
        mock_m_per_latitude.return_value = 1.0

        self.assertAlmostEqual(Telemetry.distance_m(2, 0, 0, 0), 2)
        self.assertAlmostEqual(Telemetry.distance_m(0, 2, 0, 0), 2)
        self.assertAlmostEqual(Telemetry.distance_m(0, 0, 2, 0), 2)
        self.assertAlmostEqual(Telemetry.distance_m(0, 0, 0, 2), 2)

        self.assertAlmostEqual(Telemetry.distance_m(0, 0, 1, 1), math.sqrt(2))

        for mult_1 in (-1, 1):
            for mult_2 in (-1, 1):
                self.assertAlmostEqual(
                    Telemetry.distance_m(mult_1 * 3, mult_2 * 4, 0, 0),
                    5
                )
                self.assertAlmostEqual(
                    Telemetry.distance_m(mult_1 * 4, mult_2 * 3, 0, 0),
                    5
                )
                self.assertAlmostEqual(
                    Telemetry.distance_m(0, 0, mult_1 * 3, mult_2 * 4),
                    5
                )
                self.assertAlmostEqual(
                    Telemetry.distance_m(0, 0, mult_1 * 4, mult_2 * 3),
                    5
                )
    def get_current_waypoint(self, x_m, y_m):
        """Returns the current waypoint."""
        if self._current_waypoint == 0:
            return self._waypoints[self._current_waypoint]
        elif len(self._waypoints) == 1:
            return self._waypoints[self._current_waypoint]
        else:
            current = self._waypoints[self._current_waypoint]
            previous = self._waypoints[self._current_waypoint - 1]
            distance_m = math.sqrt((current[0] - x_m)**2 +
                                   (current[1] - y_m)**2)
            if distance_m < self._distance_m:
                return self._waypoints[self._current_waypoint]

            # Find the point on the line segment from the previous waypoint to
            # the current waypoint that is self._distance_m away and that's in
            # the direction of the next waypoint
            intersections = self._circle_intersection(previous, current,
                                                      (x_m, y_m),
                                                      self._distance_m)
            if len(intersections) == 0:
                # Well, this is bad. I guess we could go for a tangent?
                self._logger.warn(
                    'No chase waypoint in range: {distance}'
                    ' from {point_1}-{point_2}, using tangent'.format(
                        distance=round(self._distance_m, 3),
                        point_1=[round(i, 3) for i in previous],
                        point_2=[round(i, 3) for i in current],
                    ))

                tangent_distance_m = self._tangent_distance_m(
                    (x_m, y_m), previous, current)
                if tangent_distance_m == None:
                    self._logger.warn(
                        'Unable to compute tangent, falling back to waypoint')
                    return current

                intersections = self._circle_intersection(
                    previous,
                    current,
                    (x_m, y_m),
                    tangent_distance_m + 0.1  # Avoid floating point issues
                )

            waypoint = min(intersections,
                           key=lambda point: Telemetry.distance_m(
                               current[0], current[1], point[0], point[1]))
            return waypoint

        raise ValueError('No waypoints left')
    def test_relative_degrees(self, mock_latitude_to_m_per_d_longitude,
                              mock_m_per_latitude):
        mock_latitude_to_m_per_d_longitude.return_value = 1.0
        mock_m_per_latitude.return_value = 1.0

        self.assertAlmostEqual(Telemetry.distance_m(2, 0, 0, 0), 2)
        self.assertAlmostEqual(Telemetry.distance_m(0, 2, 0, 0), 2)
        self.assertAlmostEqual(Telemetry.distance_m(0, 0, 2, 0), 2)
        self.assertAlmostEqual(Telemetry.distance_m(0, 0, 0, 2), 2)

        self.assertAlmostEqual(Telemetry.distance_m(0, 0, 1, 1), math.sqrt(2))

        for mult_1 in (-1, 1):
            for mult_2 in (-1, 1):
                self.assertAlmostEqual(
                    Telemetry.distance_m(mult_1 * 3, mult_2 * 4, 0, 0), 5)
                self.assertAlmostEqual(
                    Telemetry.distance_m(mult_1 * 4, mult_2 * 3, 0, 0), 5)
                self.assertAlmostEqual(
                    Telemetry.distance_m(0, 0, mult_1 * 3, mult_2 * 4), 5)
                self.assertAlmostEqual(
                    Telemetry.distance_m(0, 0, mult_1 * 4, mult_2 * 3), 5)
    def test_central_offset(self, mock_consume):
        """The offset should default to Sparkfun, or be loaded from the KML
        course.
        """
        telemetry = Telemetry()
        self.assertLess(
            telemetry.distance_m(
                # From Google Earth
                40.090764,
                -105.184879,
                CENTRAL_LATITUDE,
                CENTRAL_LONGITUDE),
            100)

        # Smoke test
        course = {'course': ((1, 2), (3, 4)), 'inner': ()}
        with mock.patch.object(Telemetry,
                               'load_kml_from_file_name',
                               return_value=course):
            with mock.patch('builtins.open'):
                Telemetry('file.kml')
Exemplo n.º 5
0
    def test_central_offset(self, mock_consume):
        """The offset should default to Sparkfun, or be loaded from the KML
        course.
        """
        telemetry = Telemetry()
        self.assertLess(
            telemetry.distance_m(
                # From Google Earth
                40.090764,
                -105.184879,
                CENTRAL_LATITUDE,
                CENTRAL_LONGITUDE
            ),
            100
        )

        # Smoke test
        course = {
            'course': ((1, 2), (3, 4)),
            'inner': ()
        }
        with mock.patch.object(Telemetry, 'load_kml_from_file_name', return_value=course):
            with mock.patch('builtins.open'):
                Telemetry('file.kml')
    def get_current_waypoint(self, x_m, y_m):
        """Returns the current waypoint."""
        if self._current_waypoint == 0:
            return self._waypoints[self._current_waypoint]
        elif len(self._waypoints) == 1:
            return self._waypoints[self._current_waypoint]
        else:
            current = self._waypoints[self._current_waypoint]
            previous = self._waypoints[self._current_waypoint - 1]
            distance_m = math.sqrt(
                (current[0] - x_m) ** 2
                + (current[1] - y_m) ** 2
            )
            if distance_m < self._distance_m:
                return self._waypoints[self._current_waypoint]

            # Find the point on the line segment from the previous waypoint to
            # the current waypoint that is self._distance_m away and that's in
            # the direction of the next waypoint
            intersections = self._circle_intersection(
                previous,
                current,
                (x_m, y_m),
                self._distance_m
            )
            if len(intersections) == 0:
                # Well, this is bad. I guess we could go for a tangent?
                self._logger.warn(
                    'No chase waypoint in range: {distance}'
                    ' from {point_1}-{point_2}, using tangent'.format(
                        distance=round(self._distance_m, 3),
                        point_1=[round(i, 3) for i in previous],
                        point_2=[round(i, 3) for i in current],
                    )
                )

                tangent_distance_m = self._tangent_distance_m(
                    (x_m, y_m),
                    previous,
                    current
                )
                if tangent_distance_m == None:
                    self._logger.warn(
                        'Unable to compute tangent, falling back to waypoint'
                    )
                    return current

                intersections = self._circle_intersection(
                    previous,
                    current,
                    (x_m, y_m),
                    tangent_distance_m + 0.1  # Avoid floating point issues
                )

            waypoint = min(
                intersections,
                key=lambda point: Telemetry.distance_m(
                    current[0],
                    current[1],
                    point[0],
                    point[1]
                )
            )
            return waypoint

        raise ValueError('No waypoints left')