Path_test.py
Go to the documentation of this file.
1 #==============================================================================
2 # Copyright (C) 2021-2024 Wageningen University - All Rights Reserved
3 # Author: Gonzalo Mier
4 # BSD-3 License
5 #==============================================================================
6 
7 import pytest
8 import math
9 import fields2cover as f2c
10 
11 def near(a, b, error = 1e-7):
12  assert abs(a - b) < error
13 
14 def test_fields2cover_types_path_appendSwath():
15  line1 = f2c.LineString(f2c.VectorPoint(
16  [f2c.Point(0.0, 1.0), f2c.Point(1.0, 1.0), f2c.Point(1.0, 4.0)]));
17  swath1 = f2c.Swath(line1);
18  path1 = f2c.Path();
19  path1.appendSwath(swath1, 2.0);
20  near(path1.getState(0).point.getX(), 0.0);
21  near(path1.getState(0).point.getY(), 1.0);
22  near(path1.getState(0).velocity, 2.0);
23  near(path1.getState(0).len, 1.0);
24  near(path1.getState(0).type, f2c.PathSectionType_SWATH);
25  near(path1.getState(1).point.getX(), 1.0);
26  near(path1.getState(1).point.getY(), 1.0);
27  near(path1.getState(1).velocity, 2.0);
28  near(path1.getState(1).len, 3.0);
29  near(path1.getState(1).type, f2c.PathSectionType_SWATH);
30  near(path1.getTaskTime(), 2.0);
31 
32 def test_fields2cover_types_path_opPlusEqual():
33  swath1 = f2c.Swath(f2c.LineString(f2c.VectorPoint([f2c.Point(0.0, 1.0), f2c.Point(1.0, 1.0), f2c.Point(1.0, 4.0)])));
34  swath2 = f2c.Swath(f2c.LineString(f2c.VectorPoint([f2c.Point(1.0, 4.0), f2c.Point(1.0, 0.0), f2c.Point(0.0, 0.0)])));
35 
36  path1 = f2c.Path();
37  path2 = f2c.Path();
38  path1.appendSwath(swath1, 2.0);
39  path2.appendSwath(swath2, 1.0);
40  path1 += path2;
41  near(path1.getState(0).point.getX(), 0.0);
42  near(path1.getState(0).point.getY(), 1.0);
43  near(path1.getState(0).velocity, 2.0);
44  near(path1.getState(0).len, 1.0);
45  near(path1.getState(0).type, f2c.PathSectionType_SWATH);
46  near(path1.getState(1).point.getX(), 1.0);
47  near(path1.getState(1).point.getY(), 1.0);
48  near(path1.getState(1).velocity, 2.0);
49  near(path1.getState(1).len, 3.0);
50  near(path1.getState(1).type, f2c.PathSectionType_SWATH);
51  near(path1.getState(2).point.getX(), 1.0);
52  near(path1.getState(2).point.getY(), 4.0);
53  near(path1.getState(2).velocity, 1.0);
54  near(path1.getState(2).len, 4.0);
55  near(path1.getState(2).type, f2c.PathSectionType_SWATH);
56  near(path1.getState(3).point.getX(), 1.0);
57  near(path1.getState(3).point.getY(), 0.0);
58  near(path1.getState(3).velocity, 1.0);
59  near(path1.getState(3).len, 1.0);
60  near(path1.getState(3).type, f2c.PathSectionType_SWATH);
61  near(path1.getTaskTime(), 7.0);
62  near(path1.size(), 4);
63 
64 def test_fields2cover_types_path_populateAndReduce():
65  path1 = f2c.Path();
66  path2 = f2c.Path();
67  path1.populate(200);
68  near(path1.size(), 0);
69  near(path1.getTaskTime(), 0.0, 1e-6);
70 
71  swath1 = f2c.Swath(f2c.LineString(f2c.VectorPoint(
72  [f2c.Point(0.0, 0.0), f2c.Point(0.0, 1.0), f2c.Point(2.0, 1.0)])));
73  swath2 = f2c.Swath(f2c.LineString(f2c.VectorPoint(
74  [f2c.Point(3.0, 1.0), f2c.Point(3.0, 4.0), f2c.Point(1.0, 4.0)])));
75 
76  path1.appendSwath(swath1, 2.0);
77  path2.appendSwath(swath2, 1.0);
78  path1 += path2;
79 
80  near(path1.length(), 8);
81  near(path1.getTaskTime(), 6.5, 1e-6);
82  for s in path1.getStates():
83  near(s.dir, f2c.PathDirection_FORWARD);
84  #path_c = path1.clone();
85  path_c = path1;
86  path1.populate(200);
87  assert (path1.length() > 8.0);
88  assert (path1.length() < 2.0 * 8.0);
89  near(path1.size(), 200);
90  near(path1.getTaskTime(), 6.5, 1e-6);
91  for s in path1.getStates():
92  near(s.dir, f2c.PathDirection_FORWARD);
93  near(s.type, f2c.PathSectionType_SWATH);
94  path1.populate(10);
95  assert (path1.length() > 8.0);
96  assert (path1.length() < 2.0 * 8.0);
97  near(path1.size(), 10);
98  near(path1.getTaskTime(), 6.5, 0.1);
99  for s in path1.getStates():
100  near(s.dir, f2c.PathDirection_FORWARD);
101  near(s.type, f2c.PathSectionType_SWATH);
102  path1.populate(500);
103  assert (path1.length() > 8.0);
104  assert (path1.length() < 2.0 * 8.0);
105  path1.reduce(0.1);
106  assert (path1.length() > 8.0);
107  assert (path1.length() < 2.0 * 8.0);
108  path1.reduce(0.1);
109  assert (path1.length() > 8.0);
110  assert (path1.length() < 2.0 * 8.0);
111  assert (path1.size() < 100);
112 
113  for s in path1.getStates():
114  near(s.dir, f2c.PathDirection_FORWARD);
115  near(s.type, f2c.PathSectionType_SWATH);
116 
117 def test_fields2cover_types_path_length():
118  line1 = f2c.LineString();
119  line2 = f2c.LineString();
120  line1.addPoint( 0.0, 1.0);
121  line1.addPoint( 1.0, 1.0);
122  line1.addPoint( 1.0, 4.0);
123  line2.addPoint( 1.0, 4.0);
124  line2.addPoint( 1.0, 0.0);
125  line2.addPoint( 0.0, 0.0);
126  swath1 = f2c.Swath(line1);
127  swath2 = f2c.Swath(line2);
128 
129  path1 = f2c.Path();
130  path2 = f2c.Path();
131  path1.appendSwath(swath1, 2.0);
132  near(path1.length(), 4.0);
133  path2.appendSwath(swath2, 1.0);
134  path1 += path2;
135  near(path1.length(), 9.0);
136  path1 = path2;
137  near(path1.length(), 5.0);
138 
139 def test_fields2cover_types_path_saveLoad():
140  path = f2c.Path();
141 
142  state = f2c.PathState();
143  state.point = f2c.Point(3.124, -4.5, 3);
144  state.angle = -0.5;
145  state.velocity = -2.5;
146  state.len = 2;
147  state.dir = f2c.PathDirection_FORWARD;
148  state.type = f2c.PathSectionType_SWATH;
149  path.addState(state);
150 
151  state = f2c.PathState();
152  state.point = f2c.Point(-2.3, 5);
153  state.angle = 0.1;
154  state.velocity = 4.5;
155  state.len = 6;
156  state.dir = f2c.PathDirection_BACKWARD;
157  state.type = f2c.PathSectionType_SWATH;
158  path.addState(state);
159 
160  assert (path.serializePath() == "3.124 -4.5 3 -0.5 -2.5 2 1 1\n-2.3 5 0 0.1 4.5 6 -1 1\n");
161  path.saveToFile("/tmp/test_path");
162  path_read = f2c.Path();
163  path_read.loadFile("/tmp/test_path");
164  assert (path.serializePath() == path_read.serializePath());
165 
166 def test_fields2cover_types_path_list_points():
167  rand = f2c.Random(42)
168  robot = f2c.Robot(2.0, 6.0)
169  const_hl = f2c.HG_Const_gen()
170  field = rand.generateRandField(1e5, 5)
171  cells = field.getField()
172  no_hl = const_hl.generateHeadlands(cells, 3.0 * robot.getCovWidth())
173  bf = f2c.SG_BruteForce()
174  swaths = bf.generateSwaths(math.pi, robot.getCovWidth(), no_hl.getGeometry(0))
175  snake_sorter = f2c.RP_Snake()
176  swaths = snake_sorter.genSortedSwaths(swaths)
177 
178  robot.setMinTurningRadius(2) # m
179  robot.setMaxDiffCurv(0.1) # 1/m^2
180  path_planner = f2c.PP_PathPlanning()
181  dubins = f2c.PP_DubinsCurvesCC()
182  path = path_planner.planPath(robot, swaths, dubins);
183 
184  n = path.size()
185  points = [path[i].point for i in range(n)]
186 
187  near(n, path.size())
188 
189 
190 
Path_test.test_fields2cover_types_path_saveLoad
def test_fields2cover_types_path_saveLoad()
Definition: Path_test.py:139
Path_test.test_fields2cover_types_path_populateAndReduce
def test_fields2cover_types_path_populateAndReduce()
Definition: Path_test.py:64
Path_test.test_fields2cover_types_path_length
def test_fields2cover_types_path_length()
Definition: Path_test.py:117
Path_test.test_fields2cover_types_path_list_points
def test_fields2cover_types_path_list_points()
Definition: Path_test.py:166
Path_test.test_fields2cover_types_path_appendSwath
def test_fields2cover_types_path_appendSwath()
Definition: Path_test.py:14
Path_test.near
def near(a, b, error=1e-7)
Definition: Path_test.py:11
f2c::Random
Definition: random.h:23
Path_test.test_fields2cover_types_path_opPlusEqual
def test_fields2cover_types_path_opPlusEqual()
Definition: Path_test.py:32

fields2cover
Author(s):
autogenerated on Fri Apr 25 2025 02:18:31