29#include "CoverageControl/extern/gnuplot/gnuplot-iostream.h"
34[[nodiscard]]
bool Plotter::GnuplotCommands(Gnuplot &gp) {
35 std::filesystem::path map_filename{
36 std::filesystem::weakly_canonical(dir +
"/" + plot_name)};
37 std::filesystem::path dir_path{map_filename.parent_path()};
38 if (!std::filesystem::exists(dir_path)) {
39 std::cerr <<
"Directory does not exist: " << dir_path << std::endl;
42 gp <<
"set o '" << map_filename.string() <<
"'\n";
43 gp <<
"set terminal pngcairo enhanced font 'Times," << font_sz <<
"' size "
44 << image_sz <<
"," << image_sz <<
"\n";
45 gp <<
"set palette defined (-5 'black', -1 '" << color_unknown
46 <<
"', 0 'white', 1 '" << color_idf <<
"')\n";
47 gp <<
"set cbrange [-5:1]\n";
48 gp <<
"set size ratio -1\n";
49 gp <<
"set xrange [0:" << range_max <<
"]\n";
50 gp <<
"set yrange [0:" << range_max <<
"]\n";
51 gp <<
"set border linewidth 1.5\n";
52 if (unset_colorbox) gp <<
"unset colorbox\n";
56void Plotter::StreamMap(Gnuplot &gp,
MapType const &map) {
57 for (
int i = 0; i < map.rows(); ++i) {
58 for (
int j = 0; j < map.cols(); ++j) {
59 gp << map(i, j) <<
" ";
63 gp <<
"e" << std::endl;
66void Plotter::PlotMap(Gnuplot &gp,
bool begin) {
71 gp <<
"'-' matrix using ($2*" << resolution <<
"):($1*" << resolution
72 <<
"):3 with image notitle ";
75void Plotter::PlotLine(Gnuplot &gp,
int marker_size, std::string color,
81 gp <<
"'-' with line lw " << marker_size <<
" lc rgb '" << color
85void Plotter::PlotPoints(Gnuplot &gp,
int point_type,
int marker_size,
86 std::string color,
bool begin) {
91 gp <<
"'-' with points pt " << point_type <<
" ps " << marker_size
92 <<
" lc rgb '" << color <<
"' notitle";
95void Plotter::PlotMap(
MapType const &map) {
97 if (GnuplotCommands(gp)) {
98 std::cerr <<
"Error in GnuplotCommands" << std::endl;
106void Plotter::PlotMap(
MapType const &map, PointVector
const &positions) {
108 if (GnuplotCommands(gp)) {
109 std::cerr <<
"Error in GnuplotCommands" << std::endl;
113 PlotPoints(gp, 7, marker_sz, color_robot);
118 for (
auto const &pos : positions) {
119 gp << pos[0] <<
" " << pos[1] << std::endl;
121 gp <<
"e" << std::endl;
124void Plotter::PlotMap(
MapType const &map, PointVector
const &positions,
125 std::vector<std::list<Point2>>
const &trajectories,
126 std::vector<int>
const &robot_status) {
128 if (GnuplotCommands(gp)) {
129 std::cerr <<
"Error in GnuplotCommands" << std::endl;
134 for (
size_t i = 0; i < positions.size(); ++i) {
135 if (robot_status[i] == 0) {
136 PlotLine(gp, marker_sz, color_robot,
false);
138 PlotLine(gp, marker_sz, color_robot_alt,
false);
141 for (
size_t i = 0; i < positions.size(); ++i) {
142 if (robot_status[i] == 0) {
143 PlotPoints(gp, 7, marker_sz, color_robot,
false);
145 PlotPoints(gp, 7, marker_sz, color_robot_alt,
false);
151 for (
auto const &trajectory : trajectories) {
152 for (
auto const &pos : trajectory) {
153 gp << pos[0] <<
" " << pos[1] << std::endl;
155 gp <<
"e" << std::endl;
158 for (
auto const &pos : positions) {
159 gp << pos[0] <<
" " << pos[1] << std::endl;
160 gp <<
"e" << std::endl;
164void Plotter::PlotMap(
MapType const &map, PointVector
const &positions,
165 std::vector<std::list<Point2>>
const &trajectories,
166 std::vector<int>
const &robot_status,
167 double const &communication_range) {
169 if (GnuplotCommands(gp)) {
170 std::cerr <<
"Error in GnuplotCommands" << std::endl;
175 for (
size_t i = 0; i < positions.size(); ++i) {
176 if (robot_status[i] == 0) {
177 PlotLine(gp, marker_sz, color_robot,
false);
179 PlotLine(gp, marker_sz, color_robot_alt,
false);
182 PlotLine(gp, half_marker_sz, color_communication_links,
false);
183 for (
size_t i = 0; i < positions.size(); ++i) {
184 if (robot_status[i] == 0) {
185 PlotPoints(gp, 7, marker_sz, color_robot,
false);
187 PlotPoints(gp, 7, marker_sz, color_robot_alt,
false);
193 for (
auto const &trajectory : trajectories) {
194 for (
auto const &pos : trajectory) {
195 gp << pos[0] <<
" " << pos[1] << std::endl;
197 gp <<
"e" << std::endl;
200 for (
size_t i = 0; i < positions.size(); ++i) {
201 for (
size_t j = i + 1; j < positions.size(); ++j) {
202 if ((positions[i] - positions[j]).norm() < communication_range) {
203 gp << positions[i][0] <<
" " << positions[i][1] <<
"\n";
204 gp << positions[j][0] <<
" " << positions[j][1] <<
"\n";
209 gp <<
"e" << std::endl;
210 for (
auto const &pos : positions) {
211 gp << pos[0] <<
" " << pos[1] << std::endl;
212 gp <<
"e" << std::endl;
216void Plotter::PlotMap(
MapType const &map, PointVector
const &positions,
217 std::vector<std::list<Point2>>
const &voronoi,
218 std::vector<std::list<Point2>>
const &trajectories) {
220 if (GnuplotCommands(gp)) {
221 std::cerr <<
"Error in GnuplotCommands" << std::endl;
226 PlotLine(gp, marker_sz, color_robot,
false);
227 PlotLine(gp, half_marker_sz, color_voronoi,
false);
228 PlotPoints(gp, 7, marker_sz, color_robot,
false);
233 for (
auto const &trajectory : trajectories) {
234 for (
auto const &pos : trajectory) {
235 gp << pos[0] <<
" " << pos[1] <<
"\n";
239 gp <<
"e" << std::endl;
241 for (
auto const &vcell : voronoi) {
242 for (
auto const &pos : vcell) {
243 gp << pos[0] <<
" " << pos[1] <<
"\n";
247 gp <<
"e" << std::endl;
249 for (
auto const &pos : positions) {
250 gp << pos[0] <<
" " << pos[1] <<
"\n";
252 gp <<
"e" << std::endl;
255void Plotter::PlotMap(
MapType const &map, PointVector
const &positions,
256 Voronoi
const &voronoi,
257 std::vector<std::list<Point2>>
const &trajectories) {
259 if (GnuplotCommands(gp)) {
260 std::cerr <<
"Error in GnuplotCommands" << std::endl;
265 PlotLine(gp, marker_sz, color_robot,
false);
266 PlotLine(gp, half_marker_sz, color_voronoi,
false);
267 PlotPoints(gp, 7, marker_sz, color_robot,
false);
272 for (
auto const &trajectory : trajectories) {
273 for (
auto const &pos : trajectory) {
274 gp << pos[0] <<
" " << pos[1] <<
"\n";
278 gp <<
"e" << std::endl;
280 auto voronoi_cells = voronoi.GetVoronoiCells();
281 for (
auto const &vcell : voronoi_cells) {
282 for (
auto const &pos : vcell.cell) {
283 gp << pos[0] <<
" " << pos[1] <<
"\n";
285 auto const &pos = vcell.cell.front();
286 gp << pos[0] <<
" " << pos[1] <<
"\n";
289 gp <<
"e" << std::endl;
291 for (
auto const &pos : positions) {
292 gp << pos[0] <<
" " << pos[1] <<
"\n";
294 gp <<
"e" << std::endl;
297void Plotter::PlotMap(
MapType const &map, PointVector
const &positions,
298 PointVector
const &goals, Voronoi
const &voronoi) {
300 if (GnuplotCommands(gp)) {
301 std::cerr <<
"Error in GnuplotCommands" << std::endl;
306 PlotLine(gp, half_marker_sz, color_voronoi,
false);
307 PlotLine(gp, half_marker_sz, color_robot,
false);
308 PlotPoints(gp, 28, marker_sz, color_robot,
false);
309 PlotPoints(gp, 7, marker_sz, color_robot,
false);
314 auto voronoi_cells = voronoi.GetVoronoiCells();
315 for (
auto const &vcell : voronoi_cells) {
316 for (
auto const &pos : vcell.cell) {
317 gp << pos[0] <<
" " << pos[1] << std::endl;
319 auto const &pos = vcell.cell.front();
320 gp << pos[0] <<
" " << pos[1] << std::endl;
323 gp <<
"e" << std::endl;
325 for (
size_t i = 0; i < positions.size(); ++i) {
326 auto const &pos = positions[i];
327 auto const &goal = goals[i];
328 gp << pos[0] <<
" " << pos[1] << std::endl;
329 gp << goal[0] <<
" " << goal[1] << std::endl;
332 gp <<
"e" << std::endl;
334 for (
auto const &pos : goals) {
335 gp << pos[0] <<
" " << pos[1] << std::endl;
337 gp <<
"e" << std::endl;
339 for (
auto const &pos : positions) {
340 gp << pos[0] <<
" " << pos[1] << std::endl;
342 gp <<
"e" << std::endl;
345void Plotter::PlotMap(
MapType const &map, PointVector
const &positions,
346 std::vector<std::list<Point2>>
const &trajectories,
347 PointVector
const &frontiers) {
349 if (GnuplotCommands(gp)) {
350 std::cerr <<
"Error in GnuplotCommands" << std::endl;
355 PlotLine(gp, marker_sz, color_robot);
356 PlotPoints(gp, 7, marker_sz, color_robot);
357 PlotPoints(gp, 1, half_marker_sz, color_robot);
362 for (
auto const &trajectory : trajectories) {
363 for (
auto const &pos : trajectory) {
364 gp << pos[0] <<
" " << pos[1] << std::endl;
368 gp <<
"e" << std::endl;
370 for (
auto const &pos : positions) {
371 gp << pos[0] <<
" " << pos[1] << std::endl;
373 gp <<
"e" << std::endl;
375 for (
auto const &pos : frontiers) {
376 gp << pos[0] <<
" " << pos[1] << std::endl;
378 gp <<
"e" << std::endl;
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MapType
Namespace for the CoverageControl library.