Path_test.cpp
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 #include <gtest/gtest.h>
8 #include "fields2cover/types.h"
9 #include "fields2cover/path_planning/dubins_curves.h"
10 #include "fields2cover/path_planning/path_planning.h"
11 #include "fields2cover/path_planning/dubins_curves_cc.h"
12 #include "fields2cover/route_planning/boustrophedon_order.h"
13 #include "fields2cover/swath_generator/brute_force.h"
14 #include "../test_helpers/path_planning_checker.hpp"
15 
16 TEST(fields2cover_types_path, addState) {
17  F2CPath path;
18  path.addState(F2CPoint(1,2), 0.5*M_PI, 3, f2c::types::PathDirection::FORWARD, f2c::types::PathSectionType::TURN, 5);
19  path.addState(F2CPoint(3,4), 5*M_PI, 5, f2c::types::PathDirection::BACKWARD, f2c::types::PathSectionType::SWATH, 3);
20 
21  const F2CPath const_path = path;
22 
23  auto state0 = path.getState(0);
24  auto c_state0 = const_path.getState(0);
25  EXPECT_EQ(state0.point, c_state0.point);
26  EXPECT_EQ(state0.angle, c_state0.angle);
27 
28  EXPECT_EQ(state0.point, F2CPoint(1,2));
29  EXPECT_EQ(state0.angle, 0.5*M_PI);
30  EXPECT_EQ(state0.len, 3);
31  EXPECT_EQ(int(state0.dir), int(f2c::types::PathDirection::FORWARD));
32  EXPECT_EQ(int(state0.type), int(f2c::types::PathSectionType::TURN));
33  EXPECT_EQ(state0.velocity, 5);
34 
35  auto states = path.getStates();
36  auto c_states = const_path.getStates();
37  EXPECT_EQ(states[0].point, F2CPoint(1,2));
38  EXPECT_EQ(c_states[0].point, F2CPoint(1,2));
39  EXPECT_EQ(states[0].point, path.atStart());
40  EXPECT_EQ(c_states[0].point, const_path.atStart());
41 }
42 
43 
44 
45 TEST(fields2cover_types_path, getDimMin_getDimMax) {
46  F2CPath path;
47  path.setStates(std::vector<F2CPathState>(3));
48  path[0].point = F2CPoint(1, 4);
49  path[1].point = F2CPoint(2, -2);
50  path[2].point = F2CPoint(-3, 0);
51 
52  EXPECT_NEAR(path.getDimMinX(), -3, 1e-7);
53  EXPECT_NEAR(path.getDimMinY(), -2, 1e-7);
54  EXPECT_NEAR(path.getDimMaxX(), 2, 1e-7);
55  EXPECT_NEAR(path.getDimMaxY(), 4, 1e-7);
56 }
57 
58 TEST(fields2cover_types_path, mirrorX_mirrorY) {
59  F2CPath path;
60  path.setStates(std::vector<F2CPathState>(4));
61  path[0].point = F2CPoint(1, 1);
62  path[0].angle = 0;
63  path[1].point = F2CPoint(2, 1);
64  path[1].angle = 1.5*M_PI;
65  path[2].point = F2CPoint(2, -2);
66  path[2].angle = M_PI;
67  path[3].point = F2CPoint(-2, -2);
68  path[3].angle = 0;
69 
70  path.mirrorX();
71  EXPECT_NEAR(path[0].point.getX(), -1, 1e-7);
72  EXPECT_NEAR(path[0].point.getY(), 1, 1e-7);
73  EXPECT_NEAR(path[0].angle, M_PI, 1e-7);
74  EXPECT_NEAR(path[1].point.getX(), -2, 1e-7);
75  EXPECT_NEAR(path[1].point.getY(), 1, 1e-7);
76  EXPECT_NEAR(path[1].angle, 1.5*M_PI, 1e-7);
77  EXPECT_NEAR(path[2].point.getX(), -2, 1e-7);
78  EXPECT_NEAR(path[2].point.getY(), -2, 1e-7);
79  EXPECT_NEAR(path[2].angle, 0, 1e-7);
80  EXPECT_NEAR(path[3].point.getX(), 2, 1e-7);
81  EXPECT_NEAR(path[3].point.getY(), -2, 1e-7);
82  EXPECT_NEAR(path[3].angle, M_PI, 1e-7);
83 
84  path.mirrorY();
85  EXPECT_NEAR(path[0].point.getX(), -1, 1e-7);
86  EXPECT_NEAR(path[0].point.getY(), -1, 1e-7);
87  EXPECT_NEAR(path[0].angle, M_PI, 1e-7);
88  EXPECT_NEAR(path[1].point.getX(), -2, 1e-7);
89  EXPECT_NEAR(path[1].point.getY(), -1, 1e-7);
90  EXPECT_NEAR(path[1].angle, .5*M_PI, 1e-7);
91  EXPECT_NEAR(path[2].point.getX(), -2, 1e-7);
92  EXPECT_NEAR(path[2].point.getY(), 2, 1e-7);
93  EXPECT_NEAR(path[2].angle, 0, 1e-7);
94  EXPECT_NEAR(path[3].point.getX(), 2, 1e-7);
95  EXPECT_NEAR(path[3].point.getY(), 2, 1e-7);
96  EXPECT_NEAR(path[3].angle, M_PI, 1e-7);
97 }
98 
99 TEST(fields2cover_types_path, setters) {
100  F2CPath path;
101  path.setStates(std::vector<F2CPathState>(4));
102  path[0].point = F2CPoint(1, 1);
103  path[0].angle = 0;
104  path[1].point = F2CPoint(2, 1);
105  path[1].angle = 1.5*M_PI;
106  path[2].point = F2CPoint(2, -2);
107  path[2].angle = M_PI;
108  path[3].point = F2CPoint(-2, -2);
109  path[3].angle = 0;
110 
111  path.setBackwardDir();
112  for (auto&& s : path) {
113  EXPECT_EQ(static_cast<int>(s.dir),
114  static_cast<int>(f2c::types::PathDirection::BACKWARD));
115  }
116  path.setForwardDir();
117  for (auto&& s : path) {
118  EXPECT_EQ(static_cast<int>(s.dir),
119  static_cast<int>(f2c::types::PathDirection::FORWARD));
120  }
121  path.setSwathType();
122  for (auto&& s : path) {
123  EXPECT_EQ(static_cast<int>(s.type),
124  static_cast<int>(f2c::types::PathSectionType::SWATH));
125  }
126  path.setTurnType();
127  for (auto&& s : path) {
128  EXPECT_EQ(static_cast<int>(s.type),
129  static_cast<int>(f2c::types::PathSectionType::TURN));
130  }
131 }
132 
133 TEST(fields2cover_types_path, at) {
134  F2CPath path;
135  path.setStates(std::vector<F2CPathState>(3));
136  path[0].point = F2CPoint(-3, 1);
137  path[0].angle = 0;
138  path[0].len = 5.0;
139  path[1].point = F2CPoint(2, 1);
140  path[1].angle = 0.5*M_PI;
141  path[1].len = 2.0;
142  path[2].point = F2CPoint(2, 3);
143  path[2].angle = 0;
144  path[2].len = 3.0;
145  path[2].dir = f2c::types::PathDirection::BACKWARD;
146 
147  EXPECT_NEAR(path.length(), 10.0, 1e-7);
148 
149  EXPECT_NEAR(path.at(0).point.getX(), -3, 1e-7);
150  EXPECT_NEAR(path.at(0).point.getY(), 1, 1e-7);
151  EXPECT_NEAR(path.at(0).angle, 0, 1e-7);
152  EXPECT_NEAR(path.at(-1).point.getX(), -3, 1e-7);
153  EXPECT_NEAR(path.at(-1).point.getY(), 1, 1e-7);
154  EXPECT_NEAR(path.at(-1).angle, 0, 1e-7);
155 
156  EXPECT_NEAR(path.at(2.5).point.getX(), -0.5, 1e-7);
157  EXPECT_NEAR(path.at(2.5).point.getY(), 1, 1e-7);
158  EXPECT_NEAR(path.at(2.5).angle, 0, 1e-7);
159 
160  EXPECT_NEAR(path.at(6).point.getX(), 2, 1e-7);
161  EXPECT_NEAR(path.at(6).point.getY(), 2, 1e-7);
162  EXPECT_NEAR(path.at(6).angle, 0.5*M_PI, 1e-7);
163 
164  EXPECT_NEAR(path.at(7).point.getX(), 2, 1e-7);
165  EXPECT_NEAR(path.at(7).point.getY(), 3, 1e-7);
166  EXPECT_NEAR(path.at(7).angle, 0.5*M_PI, 1e-7);
167 
168  EXPECT_NEAR(path.at(8).point.getX(), 1, 1e-7);
169  EXPECT_NEAR(path.at(8).point.getY(), 3, 1e-7);
170  EXPECT_NEAR(path.at(8).angle, 0, 1e-7);
171 
172  EXPECT_NEAR(path.at(10).point.getX(), -1, 1e-7);
173  EXPECT_NEAR(path.at(10).point.getY(), 3, 1e-7);
174  EXPECT_NEAR(path.at(10).angle, 0, 1e-7);
175  EXPECT_NEAR(path.at(17).point.getX(), -1, 1e-7);
176  EXPECT_NEAR(path.at(17).point.getY(), 3, 1e-7);
177  EXPECT_NEAR(path.at(17).angle, 0, 1e-7);
178 
179 }
180 
181 
182 TEST(fields2cover_types_path, appendSwath) {
183  F2CLineString line1({F2CPoint(0.0, 1.0), F2CPoint(1.0, 1.0), F2CPoint(1.0, 4.0)});
184  F2CSwath swath1(line1);
185  F2CPath path1;
186  path1.appendSwath(swath1, 2.0);
187  EXPECT_EQ(path1.size(), 2);
188  EXPECT_EQ(path1[0].point.getX(), 0.0);
189  EXPECT_EQ(path1[0].point.getY(), 1.0);
190  EXPECT_EQ(path1[0].velocity, 2.0);
191  EXPECT_EQ(path1[0].len, 1.0);
192  EXPECT_EQ(static_cast<int>(path1[0].type),
193  static_cast<int>(f2c::types::PathSectionType::SWATH));
194  EXPECT_EQ(path1[1].point.getX(), 1.0);
195  EXPECT_EQ(path1[1].point.getY(), 1.0);
196  EXPECT_EQ(path1[1].velocity, 2.0);
197  EXPECT_EQ(path1[1].len, 3.0);
198  EXPECT_EQ(static_cast<int>(path1[1].type),
199  static_cast<int>(f2c::types::PathSectionType::SWATH));
200  EXPECT_EQ(path1.getTaskTime(), 2.0);
201 }
202 
203 TEST(fields2cover_types_path, op_plus_equal) {
204  F2CSwath swath1(F2CLineString({F2CPoint(0.0, 1.0), F2CPoint(1.0, 1.0), F2CPoint(1.0, 4.0)}));
205  F2CSwath swath2(F2CLineString({F2CPoint(1.0, 4.0), F2CPoint(1.0, 0.0), F2CPoint(0.0, 0.0)}));
206 
207  F2CPath path1, path2;
208  path1.appendSwath(swath1, 2.0);
209  path2.appendSwath(swath2, 1.0);
210  path1 += path2;
211  EXPECT_EQ(path1[0].point.getX(), 0.0);
212  EXPECT_EQ(path1[0].point.getY(), 1.0);
213  EXPECT_EQ(path1[0].len, 1.0);
214  EXPECT_EQ(path1[0].velocity, 2.0);
215  EXPECT_EQ(static_cast<int>(path1[0].type),
216  static_cast<int>(f2c::types::PathSectionType::SWATH));
217  EXPECT_EQ(path1[1].point.getX(), 1.0);
218  EXPECT_EQ(path1[1].point.getY(), 1.0);
219  EXPECT_EQ(path1[1].velocity, 2.0);
220  EXPECT_EQ(path1[1].len, 3.0);
221  EXPECT_EQ(static_cast<int>(path1[1].type),
222  static_cast<int>(f2c::types::PathSectionType::SWATH));
223  EXPECT_EQ(path1[2].point.getX(), 1.0);
224  EXPECT_EQ(path1[2].point.getY(), 4.0);
225  EXPECT_EQ(path1[2].velocity, 1.0);
226  EXPECT_EQ(path1[2].len, 4.0);
227  EXPECT_EQ(static_cast<int>(path1[2].type),
228  static_cast<int>(f2c::types::PathSectionType::SWATH));
229  EXPECT_EQ(path1[3].point.getX(), 1.0);
230  EXPECT_EQ(path1[3].point.getY(), 0.0);
231  EXPECT_EQ(path1[3].velocity, 1.0);
232  EXPECT_EQ(path1[3].len, 1.0);
233  EXPECT_EQ(static_cast<int>(path1[3].type),
234  static_cast<int>(f2c::types::PathSectionType::SWATH));
235  EXPECT_EQ(path1.getTaskTime(), 7.0);
236  EXPECT_EQ(path1.size(), 4);
237 }
238 
239 TEST(fields2cover_types_path, populate_and_reduce) {
240  F2CPath path1, path2;
241  EXPECT_EQ(path1.size(), 0);
242  EXPECT_EQ(path1.length(), 0.0);
243 
244  path1.populate(200);
245  EXPECT_EQ(path1.size(), 0);
246  EXPECT_EQ(path1.length(), 0.0);
247  EXPECT_NEAR(path1.getTaskTime(), 0.0, 1e-6);
248 
249  F2CSwath swath1(F2CLineString({F2CPoint(0.0, 0.0), F2CPoint(0.0, 1.0), F2CPoint(2.0, 1.0)}));
250  F2CSwath swath2(F2CLineString({F2CPoint(2.0, 1.0), F2CPoint(2.0, 4.0), F2CPoint(1.0, 4.0)}));
251 
252  path1.appendSwath(swath1, 2.0);
253  path2.appendSwath(swath2, 1.0);
254  path1 += path2;
255 
256  EXPECT_EQ(path1.size(), 4);
257  EXPECT_EQ(path1.length(), 7.0);
258  EXPECT_NEAR(path1.getTaskTime(), 5.5, 1e-6);
259  for (auto s : path1) {
260  EXPECT_EQ(static_cast<int>(s.dir),
261  static_cast<int>(f2c::types::PathDirection::FORWARD));
262  }
263  auto path_c = path1;
264  path1.populate(200);
265  EXPECT_EQ(path1.size(), 200);
266  EXPECT_NEAR(path1.length(), 7.0, 1e-2);
267  EXPECT_NEAR(path1.getTaskTime(), 5.5, 1e-2);
268  for (auto s : path1) {
269  EXPECT_EQ(static_cast<int>(s.dir),
270  static_cast<int>(f2c::types::PathDirection::FORWARD));
271  EXPECT_EQ(static_cast<int>(s.type),
272  static_cast<int>(f2c::types::PathSectionType::SWATH));
273  }
274  path1.populate(10);
275  EXPECT_EQ(path1.size(), 10);
276  EXPECT_NEAR(path1.length(), 7.0, 1e-2);
277  EXPECT_NEAR(path1.getTaskTime(), 5.5, 0.5);
278  for (auto&& s : path1) {
279  EXPECT_EQ(static_cast<int>(s.dir),
280  static_cast<int>(f2c::types::PathDirection::FORWARD));
281  EXPECT_EQ(static_cast<int>(s.type),
282  static_cast<int>(f2c::types::PathSectionType::SWATH));
283  }
284  path1.populate(500);
285  EXPECT_EQ(path1.size(), 500);
286  EXPECT_NEAR(path1.length(), 7.0, 1e-2);
287  EXPECT_NEAR(path1.getTaskTime(), 5.5, 0.5);
288  path1.reduce(0.1);
289  EXPECT_NEAR(path1.length(), 7.0, 1e-2);
290  EXPECT_NEAR(path1.getTaskTime(), 5.5, 0.5);
291  path1.reduce(0.1);
292  EXPECT_NEAR(path1.length(), 7.0, 1e-2);
293  EXPECT_NEAR(path1.getTaskTime(), 5.5, 0.5);
294  EXPECT_LT(path1.size(), 100);
295 
296  for (auto s : path1) {
297  EXPECT_EQ(static_cast<int>(s.dir),
298  static_cast<int>(f2c::types::PathDirection::FORWARD));
299  EXPECT_EQ(static_cast<int>(s.type),
300  static_cast<int>(f2c::types::PathSectionType::SWATH));
301  }
302 }
303 
304 TEST(fields2cover_types_path, length) {
305  F2CLineString line1, line2;
306  line1.addPoint( 0.0, 1.0);
307  line1.addPoint( 1.0, 1.0);
308  line1.addPoint( 1.0, 4.0);
309  line2.addPoint( 1.0, 4.0);
310  line2.addPoint( 1.0, 0.0);
311  line2.addPoint( 0.0, 0.0);
312  F2CSwath swath1(line1);
313  F2CSwath swath2(line2);
314 
315  F2CPath path1, path2;
316  path1.appendSwath(swath1, 2.0);
317  EXPECT_EQ(path1.length(), 4.0);
318  path2.appendSwath(swath2, 1.0);
319  path1 += path2;
320  EXPECT_EQ(path1.length(), 9.0);
321  path1 = path2;
322  EXPECT_EQ(path1.length(), 5.0);
323 }
324 
325 TEST(fields2cover_types_path, serialize) {
326  F2CPath path, path_read;
327  f2c::types::PathState state;
328 
329  EXPECT_EQ(0, path.serializePath().compare(""));
330  state.point = F2CPoint(2.3, -5);
331  state.angle = 0.1;
332  state.velocity = 3.0;
333  state.len = 6.0;
334  state.dir = f2c::types::PathDirection::FORWARD;
335  state.type = f2c::types::PathSectionType::TURN;
336  path.addState(state);
337 
338  EXPECT_EQ(path.serializePath(),
339  "2.3 -5 0 0.1 3 6 1 2\n");
340 
341  state.point = F2CPoint(0.1234567890123, -9.87654);
342  state.angle = 0.2;
343  state.velocity = 3.2;
344  state.len = 6.2;
345  state.dir = f2c::types::PathDirection::BACKWARD;
346  state.type = f2c::types::PathSectionType::TURN;
347  path.addState(state);
348 
349  EXPECT_EQ(path.serializePath(10),
350  "2.3 -5 0 0.1 3 6 1 2\n0.123456789 -9.87654 0 0.2 3.2 6.2 -1 2\n");
351 
352  path.saveToFile("/tmp/test_path");
353  path_read.loadFile("/tmp/test_path");
354  EXPECT_EQ(path.serializePath(), path_read.serializePath());
355 }
356 
357 
358 TEST(fields2cover_types_path, moveTo) {
359  F2CLineString line1({F2CPoint(0.0, 1.0), F2CPoint(1.0, 1.0), F2CPoint(1.0, 4.0)});
360  F2CSwath swath1(line1);
361  F2CPath path1;
362  path1.appendSwath(swath1, 2.0);
363  path1.moveTo(F2CPoint(100, 200));
364  EXPECT_EQ(path1[0].point.getX(), 100.0);
365  EXPECT_EQ(path1[0].point.getY(), 201.0);
366  EXPECT_EQ(path1[0].velocity, 2.0);
367  EXPECT_EQ(path1[0].len, 1.0);
368  EXPECT_EQ(static_cast<int>(path1[0].type),
369  static_cast<int>(f2c::types::PathSectionType::SWATH));
370  EXPECT_EQ(path1[1].point.getX(), 101.0);
371  EXPECT_EQ(path1[1].point.getY(), 201.0);
372  EXPECT_EQ(path1[1].velocity, 2.0);
373  EXPECT_EQ(path1[1].len, 3.0);
374  EXPECT_EQ(static_cast<int>(path1[1].type),
375  static_cast<int>(f2c::types::PathSectionType::SWATH));
376  EXPECT_EQ(path1.size(), 2);
377 }
378 
379 
380 TEST(fields2cover_types_path, toLineString) {
381  F2CPath path;
382  path.setStates(std::vector<F2CPathState>(3));
383  path[0].point = F2CPoint(-3, 1);
384  path[0].angle = 0.0;
385  path[0].len = 5.0;
386  path[1].point = F2CPoint(2, 1);
387  path[1].angle = 0.5 * M_PI;
388  path[1].len = 2.0;
389  path[2].point = F2CPoint(2, 3);
390  path[2].angle = 0.0;
391  path[2].len = -3.0;
392 
393  F2CLineString line = path.toLineString();
394 
395  EXPECT_EQ(line.size(), path.size() + 1);
396  EXPECT_NEAR(line.getGeometry(0).getX(), -3, 1e-7);
397  EXPECT_NEAR(line.getGeometry(0).getY(), 1, 1e-7);
398  EXPECT_NEAR(line.getGeometry(1).getX(), 2, 1e-7);
399  EXPECT_NEAR(line.getGeometry(1).getY(), 1, 1e-7);
400  EXPECT_NEAR(line.getGeometry(2).getX(), 2, 1e-7);
401  EXPECT_NEAR(line.getGeometry(2).getY(), 3, 1e-7);
402  EXPECT_NEAR(line.getGeometry(3).getX(), -1, 1e-7);
403  EXPECT_NEAR(line.getGeometry(3).getY(), 3, 1e-7);
404 }
405 
406 TEST(fields2cover_types_path, discretize_swath) {
407  // Define field and robot
408  F2CRobot robot (2.0, 5.0);
409  double field_Y = 100;
410  double field_X = 40;
411  F2CCells field(F2CCell(F2CLinearRing({F2CPoint(0,0), F2CPoint(0,field_Y),
412  F2CPoint(field_X,field_Y), F2CPoint(field_X,0),
413  F2CPoint(0,0)})));
414  // Swath generation
415  f2c::sg::BruteForce bf;
416  F2CSwaths swaths = bf.generateSwaths(0, robot.getCovWidth(), field.getGeometry(0));
417  f2c::rp::BoustrophedonOrder boustrophedon_sorter;
418  auto boustrophedon_swaths = boustrophedon_sorter.genSortedSwaths(swaths, 1);
419  // Path planner
420  f2c::pp::PathPlanning path_planner;
421  robot.setMinTurningRadius(2.5); // m
422  // Create swath connections using Dubins curves with Continuous curvature
423  f2c::pp::DubinsCurvesCC dubins_cc;
424  F2CPath path_dubins_cc = path_planner.planPath(robot, boustrophedon_swaths, dubins_cc);
425  EXPECT_TRUE(IsPathCorrect(path_dubins_cc,
426  path_dubins_cc[0].point, path_dubins_cc[0].angle,
427  path_dubins_cc.atEnd(), path_dubins_cc.back().angle, false));
428 
433  // Discretize swath lines in path object
434  // Specify the step size and the robot velocity for the swath section
435  double discretize_step_size = 2; // Step size for discretization in [m]
436  F2CPath new_path = path_dubins_cc.discretizeSwath(discretize_step_size);
437  for (size_t i = 0; i < new_path.size()-1; i++) {
438  if (new_path[i].type == f2c::types::PathSectionType::SWATH &&
439  new_path[i+1].type == f2c::types::PathSectionType::SWATH) {
440  EXPECT_NEAR(new_path[i].point.distance(new_path[i+1].point),
441  discretize_step_size, 1e-6);
442  }
443  }
444  EXPECT_TRUE(IsPathCorrect(new_path,
445  path_dubins_cc[0].point, path_dubins_cc[0].angle,
446  path_dubins_cc.atEnd(), path_dubins_cc.back().angle, false));
447 
451  double discretize_step_size_2 = 150; // Step size for discretization in [m]
452  F2CPath new_path_2 = path_dubins_cc.discretizeSwath(discretize_step_size_2);
453  for (size_t i = 0; i < new_path_2.size()-1; i++) {
454  if (new_path_2[i].type == f2c::types::PathSectionType::SWATH &&
455  new_path_2[i+1].type == f2c::types::PathSectionType::SWATH) {
456  EXPECT_NEAR(new_path_2[i].point.distance(new_path_2[i+1].point),
457  field_Y, 1e-6);
458  }
459  }
460  EXPECT_TRUE(IsPathCorrect(new_path_2,
461  path_dubins_cc[0].point, path_dubins_cc[0].angle,
462  path_dubins_cc.atEnd(), path_dubins_cc.back().angle, false));
463 
467  F2CPath path;
468  F2CPathState state1;
469  state1.point = F2CPoint(0, 0);
470  state1.angle = 0.5 * boost::math::constants::half_pi<double>();
471  state1.len = F2CPoint(10, 10).distance(state1.point);
472  state1.type = f2c::types::PathSectionType::SWATH;
473  F2CPathState state2;
474  state2.point = F2CPoint(10, 10);
475  state2.angle = 0.5 * boost::math::constants::half_pi<double>();
476  state2.type = f2c::types::PathSectionType::SWATH;
477  path.addState(state1);
478  path.addState(state2);
479  // Discretize swath with a step size
480  double step_size_4 = 5;
481  double approximated_step_size_4 = 4.71405;
482  F2CPath discretized_path = path.discretizeSwath(step_size_4);
483  // Check if the discretized path has the expected number of states
484  EXPECT_EQ(discretized_path.size(), 4); // Original start, one midpoint, and original end
485  // Check if the first and last point is equal to original start and end
486  EXPECT_EQ(discretized_path[0].point, state1.point);
487  EXPECT_EQ(discretized_path[2].atEnd(), state2.point);
488  // Check the midpoint's step_size
489  EXPECT_NEAR(discretized_path[0].point.distance(discretized_path[1].point),
490  approximated_step_size_4 , 1e-4);
491  EXPECT_NEAR(discretized_path[1].point.distance(discretized_path[2].point),
492  approximated_step_size_4 , 1e-4);
493 
494  EXPECT_TRUE(IsPathCorrect(discretized_path, state1.point,
495  0.5*boost::math::constants::half_pi<double>(),
496  state2.point, 0.5 * boost::math::constants::half_pi<double>(), false));
497 }
498 
5_route_planning.swaths
swaths
Definition: 5_route_planning.py:58
f2c::types::Path::appendSwath
void appendSwath(const Swath &swath, double cruise_speed)
Definition: Path.cpp:210
1_basic_types.line2
line2
Definition: 1_basic_types.py:61
dubins_curves_cc.h
f2c::types::Path::getStates
std::vector< PathState > & getStates()
Definition: Path.cpp:42
types.h
2_objective_functions.path
path
Definition: 2_objective_functions.py:88
f2c::types::LineString::getGeometry
void getGeometry(size_t i, Point &point)
Definition: LineString.cpp:54
f2c::types::PathState::point
Point point
Definition: PathState.h:28
6_path_planning.path_planner
path_planner
Definition: 6_path_planning.py:27
f2c::types::Path::getTaskTime
double getTaskTime() const
Definition: Path.cpp:54
brute_force.h
F2CCell
f2c::types::Cell F2CCell
Definition: types.h:43
f2c::pp::DubinsCurvesCC
Dubins' curves planner with continuous curves.
Definition: dubins_curves_cc.h:17
6_path_planning.path_dubins_cc
path_dubins_cc
Definition: 6_path_planning.py:43
f2c::types::PathState::type
PathSectionType type
Definition: PathState.h:32
f2c::types::Swath
Definition: Swath.h:23
f2c::types::Path::size
size_t size() const
Definition: Path.cpp:106
f2c::types::Path::atStart
Point atStart() const
Definition: Path.cpp:252
f2c::pp::PathPlanning
Path planning class to connect a path using a TurningBase class method.
Definition: path_planning.h:19
f2c::types::PathState::dir
PathDirection dir
Definition: PathState.h:31
2_objective_functions.robot
robot
Definition: 2_objective_functions.py:76
f2c::types::PathState
Definition: PathState.h:27
2_objective_functions.swath1
swath1
Definition: 2_objective_functions.py:30
f2c::types::Path::getState
PathState & getState(size_t i)
Definition: Path.cpp:14
f2c::types::PathDirection::FORWARD
@ FORWARD
f2c::types::Path::populate
Path & populate(int number_points=100)
Definition: Path.cpp:346
6_path_planning.dubins_cc
dubins_cc
Definition: 6_path_planning.py:42
5_route_planning.boustrophedon_sorter
boustrophedon_sorter
Definition: 5_route_planning.py:61
f2c::types::Path
Definition: Path.h:23
f2c::types::Path::serializePath
std::string serializePath(size_t digit_precision=6) const
Definition: Path.cpp:283
f2c::rp::BoustrophedonOrder
Definition: boustrophedon_order.h:16
f2c::types::PathDirection::BACKWARD
@ BACKWARD
f2c::types::PathSectionType::SWATH
@ SWATH
f2c::types::LineString
Definition: LineString.h:19
F2CLinearRing
f2c::types::LinearRing F2CLinearRing
Definition: types.h:41
f2c::types::Cells
Definition: Cells.h:21
F2CLineString
f2c::types::LineString F2CLineString
Definition: types.h:40
boustrophedon_order.h
f2c::types::Point
Definition: Point.h:21
f2c::types::Robot
Definition: Robot.h:25
path_planning.h
f2c::types::Path::length
double length(void) const
Definition: Path.cpp:205
f2c::types::PathSectionType::TURN
@ TURN
f2c::types::Geometry::distance
double distance(const Geometry< T2, R2 > &p) const
Compute shortest distance between this and another geometry.
Definition: Geometry_impl.hpp:139
f2c::types::LineString::size
size_t size() const
Definition: LineString.cpp:49
f2c::types::Path::loadFile
void loadFile(const std::string &file)
Definition: Path.cpp:330
2_objective_functions.field
field
Definition: 2_objective_functions.py:16
f2c::types::Path::moveTo
void moveTo(const Point &)
Definition: Path.cpp:138
f2c::types::PathState::len
double len
Definition: PathState.h:30
f2c::types::PathState::angle
double angle
Definition: PathState.h:29
dubins_curves.h
TEST
TEST(fields2cover_types_path, addState)
Definition: Path_test.cpp:16
f2c::types::PathState::velocity
double velocity
Definition: PathState.h:33
f2c::types::Swaths
Definition: Swaths.h:20
1_basic_types.line1
line1
Definition: 1_basic_types.py:56
F2CPoint
f2c::types::Point F2CPoint
Definition: types.h:38
2_objective_functions.swath2
swath2
Definition: 2_objective_functions.py:32
5_route_planning.bf
bf
Definition: 5_route_planning.py:25
f2c::sg::BruteForce
Definition: brute_force.h:20
IsPathCorrect
testing::AssertionResult IsPathCorrect(const F2CPath &path, F2CPoint start, double start_ang, F2CPoint end, double end_ang, bool check_y_lower_limit=true)
Definition: path_planning_checker.hpp:57

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