visualizer_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 
8 #include <iostream>
9 #include <chrono>
10 #include <thread>
11 #include <gtest/gtest.h>
12 #include "fields2cover/utils/visualizer.h"
13 #include "fields2cover.h"
14 #include "../test_helpers/robot_data.hpp"
15 
16 bool checkIfFileExists(const std::string& file, double max_time_waiting = 10.0) {
17  struct stat buffer;
18  const clock_t begin_time = std::clock();
19  double t {0.0};
20  while (t < max_time_waiting) {
21  std::this_thread::sleep_for(std::chrono::milliseconds(50));
22  t = double(std::clock() - begin_time) / CLOCKS_PER_SEC;
23  if (stat(file.c_str(), &buffer) == 0) {
24  return true;
25  }
26  }
27  return false;
28 }
29 
30 F2CCell create_polygon(void) {
31  F2CCell poly;
32  F2CLinearRing line;
33  line.addPoint(-10, 0);
34  line.addPoint(-10, 20);
35  line.addPoint(10, 20);
36  line.addPoint(10, 0);
37  line.addPoint(-10, 0);
38  poly.addRing(line);
39  return poly;
40 }
41 
42 TEST(fields2cover_utils_visualizer, save_Route_and_Path) {
43  F2CCells cells {
44  F2CCell(F2CLinearRing({
45  F2CPoint(0,0), F2CPoint(2,0),F2CPoint(2,2),F2CPoint(0,2), F2CPoint(0,0)
46  }))
47  };
48  cells.addRing(0, F2CLinearRing({
49  F2CPoint(.4,.4), F2CPoint(.4,.6),F2CPoint(.6,.6),F2CPoint(.6,.4), F2CPoint(.4,.4)
50  }));
51  cells.addRing(0, F2CLinearRing({
52  F2CPoint(1.2,1.2), F2CPoint(1.2,1.6),F2CPoint(1.6,1.6),F2CPoint(1.6,1.2), F2CPoint(1.2,1.2)
53  }));
54  cells *= 3e1;
55 
56  f2c::hg::ConstHL const_hl;
57  F2CCells no_hl = const_hl.generateHeadlandArea(cells, 3, 3);
58  auto hl_swaths = const_hl.generateHeadlandSwaths(cells, 3, 3, false);
59 
60  f2c::sg::BruteForce bf;
61  F2CSwathsByCells swaths = bf.generateSwaths(M_PI/2.0, 3, no_hl);
62 
63  f2c::rp::RoutePlannerBase route_planner;
64  F2CRoute route = route_planner.genRoute(hl_swaths[1], swaths);
65 
66  f2c::Visualizer::figure();
67  f2c::Visualizer::plot(cells);
68  f2c::Visualizer::plot(no_hl);
69  f2c::Visualizer::plot(hl_swaths[1]);
70  f2c::Visualizer::plot(route.asLineString());
71  for (auto&& vs : route.getVectorSwaths()) {
72  f2c::Visualizer::plot(vs);
73  }
74  for (auto&& c : route.getConnections()) {
75  f2c::Visualizer::plot(c);
76  }
77  f2c::Visualizer::axis_equal();
78  f2c::Visualizer::save("test_fig/route_by_pieces.png");
79  EXPECT_TRUE(checkIfFileExists("test_fig/route_by_pieces.png"));
80 
81 
82  f2c::Visualizer::figure();
83  f2c::Visualizer::plot(cells);
84  f2c::Visualizer::plot(no_hl);
85  f2c::Visualizer::plot(hl_swaths[1]);
86  f2c::Visualizer::plot(route);
87  f2c::Visualizer::axis_equal();
88  f2c::Visualizer::save("test_fig/route.png");
89  EXPECT_TRUE(checkIfFileExists("test_fig/route.png"));
90 
91  F2CRobot robot;
92  f2c::pp::PathPlanning path_planner;
93  f2c::pp::DubinsCurves dubins;
94  F2CPath path = path_planner.planPath(robot, route, dubins);
95 
96  f2c::Visualizer::figure();
97  f2c::Visualizer::plot(F2CPath());
98  f2c::Visualizer::plot(cells);
99  f2c::Visualizer::plot(no_hl);
100  f2c::Visualizer::plot(hl_swaths[1]);
101  f2c::Visualizer::plot(path);
102  f2c::Visualizer::axis_equal();
103  f2c::Visualizer::save("test_fig/path.png");
104  EXPECT_TRUE(checkIfFileExists("test_fig/path.png"));
105 }
106 
107 TEST(fields2cover_utils_visualizer, save_Cell) {
108  F2CLinearRing ring1(std::vector<F2CPoint>(
109  {F2CPoint(-3,-3), F2CPoint(4,-2), F2CPoint(4,8), F2CPoint(-3,-3)}));
110  F2CLinearRing ring2(std::vector<F2CPoint>(
111  {F2CPoint(1,1), F2CPoint(3,2), F2CPoint(3,6), F2CPoint(1,1)}));
112  F2CCell cell1;
113  cell1.addRing(ring1);
114  cell1.addRing(ring2);
115  cell1 = cell1 + F2CPoint(15,0);
116  F2CCell cell2 = create_polygon();
117  F2CCells cells;
118  cells.addGeometry(cell1);
119  cells.addGeometry(cell2);
120  f2c::Visualizer::figure();
121  f2c::Visualizer::plot(
122  F2CLineString(F2CPoint(12,4), F2CPoint(20, 4)));
123  f2c::Visualizer::plot(cells);
124  f2c::Visualizer::plot(cell1);
125  f2c::Visualizer::save("test_fig/cells_def.png");
126  EXPECT_TRUE(checkIfFileExists("test_fig/cells_def.png"));
127 
128  f2c::Visualizer::figure();
129  f2c::Visualizer::plot(cells, {0.0, 0.9, 0.0});
130  f2c::Visualizer::plot(cell1, {0.9, 0.0, 0.0});
131  f2c::Visualizer::axis_equal();
132  f2c::Visualizer::save("test_fig/cells.png");
133  EXPECT_TRUE(checkIfFileExists("test_fig/cells.png"));
134 
135  f2c::Visualizer::figure();
136  f2c::Visualizer::plot(
137  F2CLineString(F2CPoint(12,4), F2CPoint(20, 4)), {1.0, 0, 0});
138  f2c::Visualizer::plotFilled(cells, {0.0, 0.9, 0.0});
139  f2c::Visualizer::plotFilled(cell1, {0.9, 0.0, 0.0});
140  f2c::Visualizer::axis_equal();
141  f2c::Visualizer::save("test_fig/cells_filled.png");
142  EXPECT_TRUE(checkIfFileExists("test_fig/cells_filled.png"));
143 
144  f2c::Visualizer::figure();
145  f2c::Visualizer::plotFilled(cells, {0.0, 0.9, 0.0}, {1.0, 0.0, 1.0});
146  f2c::Visualizer::plotFilled(F2CCell(), {0.5,0,0}, {0.5,0.5,0.5});
147  f2c::Visualizer::axis_equal();
148  f2c::Visualizer::save("test_fig/cells_holes.png");
149  EXPECT_TRUE(checkIfFileExists("test_fig/cells_holes.png"));
150 }
151 
152 TEST(fields2cover_utils_visualizer, save_LinearRing) {
153  F2CLinearRing ring(std::vector<F2CPoint>(
154  {F2CPoint(1,1), F2CPoint(3,2), F2CPoint(3,6), F2CPoint(1,1)}));
155  f2c::Visualizer::figure();
156  f2c::Visualizer::plot(ring + F2CPoint(-5,0));
157  f2c::Visualizer::plot(ring, {0.9, 0.0, 0.0});
158  f2c::Visualizer::axis_equal();
159  f2c::Visualizer::save("test_fig/linearring.png");
160  EXPECT_TRUE(checkIfFileExists("test_fig/linearring.png"));
161 }
162 
163 TEST(fields2cover_utils_visualizer, save_Point) {
164  f2c::Visualizer::figure();
165  f2c::Visualizer::plot(F2CPoint(1,5));
166  f2c::Visualizer::axis_equal();
167  f2c::Visualizer::save("test_fig/point.png");
168  EXPECT_TRUE(checkIfFileExists("test_fig/point.png"));
169 }
170 
171 TEST(fields2cover_utils_visualizer, save_field) {
172  f2c::Random rand(1);
173  F2CField field = rand.generateRandField(1e4, 5);
174  F2CRobot robot (3.0);
175 
176  f2c::hg::ConstHL hl_gen;
177  auto no_headlands = hl_gen.generateHeadlands(field.getField(), robot.getCovWidth());
178 
179  f2c::sg::BruteForce swath_gen;
180  auto swaths = swath_gen.generateSwaths(0.1, robot.getCovWidth(), no_headlands);
181 
182  f2c::Visualizer::figure();
183  f2c::Visualizer::plot(F2CPoint(3,4));
184  f2c::Visualizer::plot(field);
185  f2c::Visualizer::plot(no_headlands);
186  f2c::Visualizer::plot(swaths[0], {});
187  f2c::Visualizer::plot(swaths);
188  f2c::Visualizer::axis_equal();
189  f2c::Visualizer::save("test_fig/test_cell.png");
190  EXPECT_TRUE(checkIfFileExists("test_fig/test_cell.png"));
191 }
192 
193 
194 
195 TEST(fields2cover_utils_visualizer, plot_vectors) {
196  F2CPoint p (2.1, 2.2);
197  F2CMultiPoint ps {F2CPoint(0.1, 0.2), F2CPoint(0.3, 0.2)};
198  F2CLineString line1;
199  line1.addPoint( 0.0, 1.0);
200  line1.addPoint( 1.0, 1.0);
201  line1.addPoint( 1.0, 4.0);
202  F2CLineString line2 {F2CPoint(1.7, 1.8), F2CPoint(1.8, 1.8)};
203  F2CLineString line3 {F2CPoint(1.8, 1.9), F2CPoint(1.9, 1.9)};
204  F2CLineString line4 {F2CPoint(2.0, 2.0), F2CPoint(2.9, 2.9)};
205  F2CLinearRing ring {F2CPoint(3.0, 3.0), F2CPoint(3.0, 2.0), F2CPoint(2.0, 2.0), F2CPoint(3.0, 3.0)};
206  F2CMultiLineString lines;
207  lines.addGeometry(line3);
208  lines.addGeometry(line4);
209  F2CSwath swath1(line1);
210  F2CPath path1;
211  path1.appendSwath(swath1, 2.0);
212 
213  f2c::Visualizer::figure();
214  f2c::Visualizer::title("TEST");
215  f2c::Visualizer::plot(lines);
216  f2c::Visualizer::plot(line1, {0,0,0.95});
217  f2c::Visualizer::plot(line2);
218  f2c::Visualizer::xlim(-1, 5);
219  f2c::Visualizer::ylim(-1, 5);
220  f2c::Visualizer::save("test_fig/test_everything2.png");
221  EXPECT_TRUE(checkIfFileExists("test_fig/test_everything2.png"));
222 }
223 
224 TEST(fields2cover_utils_visualizer, save_Point_size) {
225  f2c::Visualizer::figure();
226  f2c::Visualizer::figure_size(100, 100);
227  f2c::Visualizer::plot(F2CPoint(1,5));
228  f2c::Visualizer::axis_equal();
229  f2c::Visualizer::save("test_fig/point_100x100.png");
230  EXPECT_TRUE(checkIfFileExists("test_fig/point_100x100.png"));
231 }
232 
f2c::pp::DubinsCurves
Dubins' curves planner.
Definition: dubins_curves.h:17
f2c::hg::ConstHL
Class to generate headlands with equal width in each border.
Definition: constant_headland.h:18
checkIfFileExists
bool checkIfFileExists(const std::string &file, double max_time_waiting=10.0)
Definition: visualizer_test.cpp:16
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.cells
cells
Definition: 1_basic_types.py:93
1_basic_types.line2
line2
Definition: 1_basic_types.py:61
f2c::Visualizer::plot
static void plot(double x, double y, const std::vector< double > &color={})
Definition: visualizer.cpp:23
f2c::types::Cell::addRing
void addRing(const LinearRing &ring)
Definition: Cell.cpp:123
2_objective_functions.path
path
Definition: 2_objective_functions.py:88
f2c::types::Field
Definition: Field.h:18
6_path_planning.path_planner
path_planner
Definition: 6_path_planning.py:27
F2CCell
f2c::types::Cell F2CCell
Definition: types.h:43
3_headland_generator.rand
rand
Definition: 3_headland_generator.py:11
f2c::Visualizer::title
static void title(const std::string &text)
Definition: visualizer.cpp:276
f2c::types::Swath
Definition: Swath.h:23
TEST
TEST(fields2cover_utils_visualizer, save_Route_and_Path)
Definition: visualizer_test.cpp:42
f2c::pp::PathPlanning
Path planning class to connect a path using a TurningBase class method.
Definition: path_planning.h:19
f2c::types::LinearRing::addPoint
void addPoint(double x, double y, double z=0)
Definition: LinearRing.cpp:105
f2c::Visualizer::xlim
static void xlim(double min, double max)
Definition: visualizer.cpp:280
f2c::Visualizer::axis_equal
static void axis_equal()
Definition: visualizer.cpp:250
f2c::types::MultiLineString
Definition: MultiLineString.h:18
f2c::types::LinearRing
Definition: LinearRing.h:18
2_objective_functions.robot
robot
Definition: 2_objective_functions.py:76
fields2cover.h
6_path_planning.dubins
dubins
Definition: 6_path_planning.py:30
5_route_planning.route
route
Definition: 5_route_planning.py:29
2_objective_functions.swath1
swath1
Definition: 2_objective_functions.py:30
3_headland_generator.const_hl
const_hl
Definition: 3_headland_generator.py:17
5_route_planning.route_planner
route_planner
Definition: 5_route_planning.py:28
f2c::types::Cell
Definition: Cell.h:32
create_polygon
F2CCell create_polygon(void)
Definition: visualizer_test.cpp:30
f2c::hg::ConstHL::generateHeadlands
F2CCells generateHeadlands(const F2CCells &field, double dist_headland) override
Definition: constant_headland.cpp:12
f2c::Visualizer::figure_size
static void figure_size(unsigned int width, unsigned int height)
Change size of current figure.
Definition: visualizer.cpp:260
1_basic_types.ring
ring
Definition: 1_basic_types.py:68
f2c::types::Path
Definition: Path.h:23
visualizer.h
f2c::types::MultiPoint
Definition: MultiPoint.h:18
f2c::types::LineString
Definition: LineString.h:19
F2CLinearRing
f2c::types::LinearRing F2CLinearRing
Definition: types.h:41
8_complete_flow.ps
list ps
Definition: 8_complete_flow.py:43
f2c::types::Cells
Definition: Cells.h:21
F2CLineString
f2c::types::LineString F2CLineString
Definition: types.h:40
f2c::types::Route
Definition: Route.h:23
f2c::types::Point
Definition: Point.h:21
f2c::types::Robot
Definition: Robot.h:25
f2c::Random
Definition: random.h:23
f2c::sg::SwathGeneratorBase::generateSwaths
virtual F2CSwaths generateSwaths(double angle, double op_width, const F2CCell &poly)
Definition: swath_generator_base.cpp:43
3_headland_generator.no_hl
no_hl
Definition: 3_headland_generator.py:18
2_objective_functions.field
field
Definition: 2_objective_functions.py:16
f2c::types::SwathsByCells
Definition: SwathsByCells.h:17
F2CPath
f2c::types::Path F2CPath
Definition: types.h:47
f2c::Visualizer::plotFilled
static void plotFilled(const F2CField &field, const std::vector< double > &color={})
Definition: visualizer.cpp:200
f2c::rp::RoutePlannerBase
Definition: route_planner_base.h:23
1_basic_types.lines
lines
Definition: 1_basic_types.py:73
f2c::Visualizer::figure
static void figure()
Create figure to plot on.
Definition: visualizer.cpp:254
f2c::Visualizer::save
static void save(const std::string &file)
Definition: visualizer.cpp:272
f2c::Visualizer::ylim
static void ylim(double min, double max)
Definition: visualizer.cpp:284
1_basic_types.line1
line1
Definition: 1_basic_types.py:56
F2CPoint
f2c::types::Point F2CPoint
Definition: types.h:38
5_route_planning.bf
bf
Definition: 5_route_planning.py:25
f2c::sg::BruteForce
Definition: brute_force.h:20

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