sailbot_workspace/src/local_pathfinding/test/test_ompl_objectives.py at 0a5463b349f38461c08480d10a64865d475b460b · UBCSailbot/sailbot_workspace · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
import math

import pytest

import local_pathfinding.coord_systems as cs
import local_pathfinding.ompl_objectives as ob
from local_pathfinding.ompl_objectives import (NO_GO_ZONE, WIND_COST_SIN_EXPONENT)


@pytest.mark.parametrize(
    "s1, s2, tw_direction_rad, expected",
    [
        (
            # Wind direction and boat heading north
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            0.0,
            1.0
        ),
        (
            # Wind direction south and boat heading north
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            math.pi,
            1.0
        ),
        (
            # Boat heading north, wind at no-go zone at 45 degrees
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            NO_GO_ZONE,
            1.0
        ),
        (
            # Boat heading north, wind at no-go zone at -45 degrees
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            -NO_GO_ZONE,
            1.0
        ),
        (
            # Boat heading south, wind at no-go zone at 135 degrees
            cs.XY(0.0, 0.0),
            cs.XY(0.0, -1.0),
            math.pi - NO_GO_ZONE,
            1.0
        ),
        (
            # Boat heading south, wind at no-go zone at -135 degrees
            cs.XY(0.0, 0.0),
            cs.XY(0.0, -1.0),
            - math.pi + NO_GO_ZONE,
            1.0
        ),
        (
            # Boat heading east, wind just better than no-go zone
            cs.XY(0.0, 0.0),
            cs.XY(1.0, 0.0),
            NO_GO_ZONE - 0.0001,
            math.sin(2*(NO_GO_ZONE - 0.0001)) ** WIND_COST_SIN_EXPONENT
        ),
        (
            # Boat heading west, wind heading north
            cs.XY(0.0, 0.0),
            cs.XY(-1.0, 0.0),
            0.0,
            0.0
        ),
        (
            # Boat heading west, wind heading south
            cs.XY(0.0, 0.0),
            cs.XY(-1.0, 0.0),
            math.pi,
            0.0
        ),
        (
            # Boat heading north, wind heading at 60 degrees
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            math.pi / 3,
            math.sin(2*math.pi / 3) ** WIND_COST_SIN_EXPONENT
        ),
        (
            # Boat heading north, wind heading at -55 degrees (-0.959931 radians)
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            -0.959931,
            math.sin(2*(-0.959931)) ** WIND_COST_SIN_EXPONENT
        ),
        (
            # Boat heading north, wind heading at 50 degrees (0.872665 radians)
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            0.872665,
            math.sin(2*0.872665) ** WIND_COST_SIN_EXPONENT
        )
    ],
)
def test_wind_direction_cost(
    s1: cs.XY, s2: cs.XY, tw_direction_rad: float, expected: float
):
    result = ob.WindObjective.wind_direction_cost(s1, s2, tw_direction_rad)
    assert result == pytest.approx(expected, abs=1e-12)


@pytest.mark.parametrize(
    "cs1,cs2,wind_direction_rad,wind_speed_kmph,expected",
    [
        (
            cs.XY(0.0, 0.0),
            cs.XY(0.0, 1.0),
            0.0,
            0.0,
            1.0,
        ),  # ZERO SPEED MAX COST
        (
            cs.XY(0, 0),
            cs.XY(1.0, 1.0),
            0.0,
            9.0,
            0.2,
        ),  # SLOW & HIGH COST
        (cs.XY(0, 0), cs.XY(1, -1), 0.0, 37.0, 0.0),  # FAST & LOW COST
    ],
)
def test_speed_cost(
    cs1: tuple, cs2: tuple, wind_direction_rad: float, wind_speed_kmph: float, expected: float
):
    s1 = cs.XY(*cs1)
    s2 = cs.XY(*cs2)
    assert ob.TimeObjective.time_cost(
        s1, s2, wind_direction_rad, wind_speed_kmph
    ) == pytest.approx(expected, abs=0.1)


@pytest.mark.parametrize(
    "heading,wind_direction,wind_speed,expected",
    [
        # 0 deg true wind angle (irons)
        (math.radians(0), math.radians(0), 0, 0),
        (math.radians(0), math.radians(0), 75.0, 0),
        # 180 deg true wind angle (dead run)
        (math.radians(0), math.radians(180), 0, 0),
        (math.radians(0), math.radians(180), 75.0, 10),
    ],
)
def test_get_sailbot_speed(
    heading: float, wind_direction: float, wind_speed: float, expected: float
):
    speed = ob.TimeObjective.get_sailbot_speed(heading, wind_direction, wind_speed)
    assert speed == pytest.approx(expected, abs=0.1)