#include <shape_detect.h>
+#include <iostream>
+using namespace std;
+
+ostream& operator << (ostream& os, Shape_detect::Point& point)
+{
+ os << "(" << point.x << ", " << point.y << ")";
+ return os;
+}
+
+
+ostream& operator << (ostream& os, Shape_detect::Line& line)
+{
+ os << line.a << "--" << line.b;
+ return os;
+}
+
+ostream& operator << (ostream& os, Shape_detect::Arc& arc)
+{
+ os << arc.center << " r=" << arc.radius;
+ return os;
+}
+
+
+
+void gnuplot(std::vector<Shape_detect::Point> &cartes,
+ std::vector<Shape_detect::Line> &lines,
+ std::vector<Shape_detect::Arc> &arcs)
+{
+ FILE *gnuplot;
+ gnuplot = popen("gnuplot", "w");
+ fprintf(gnuplot, "set grid\n");
+ fprintf(gnuplot, "set nokey\n");
+ fprintf(gnuplot, "set size ratio -1\n"); //1.3333
+ fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n");
+ fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n");
+ fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n");
+ fprintf(gnuplot, "set style fill transparent solid 0.2 noborder\n");
+
+ fprintf(gnuplot, "plot");
+ if (cartes.size() > 0)
+ fprintf(gnuplot, "'-' with points ls 1");
+ if (lines.size() > 0)
+ fprintf(gnuplot, ", '-' with linespoints ls 2");
+ if (arcs.size() > 0)
+ fprintf(gnuplot, ", '-' with circles ls 2");
+ fprintf(gnuplot, "\n");
+
+
+ if (cartes.size() > 0) {
+ for (int i = 0; i < (int) cartes.size(); i++) {
+ fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y);
+ }
+ fprintf(gnuplot, "e\n");
+ }
+
+ if (lines.size() > 0) {
+ for (int i = 0; i < (int) lines.size(); i++) {
+ fprintf(gnuplot, "%f %f\n%f %f\n\n",
+ lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
+ }
+ fprintf(gnuplot, "e\n");
+ }
+ if (arcs.size() > 0) {
+ for (int i = 0; i < (int) arcs.size(); i++) {
+ fprintf(gnuplot, "%f %f %f\n",
+ arcs[i].center.x, arcs[i].center.y, arcs[i].radius);
+ }
+ fprintf(gnuplot, "e\n");
+ }
+
+
+ pclose(gnuplot);
+}
/**
* There are detected line segments in input file (laser scan data)
return 1;
}
+ char *fname = NULL;
+ bool plot = false;
+
+ for (int i=1; i<argc; i++) {
+ if (strcmp(argv[i], "-g") == 0)
+ plot = true;
+ else fname = argv[i];
+ }
+
unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
- std::ifstream infile(argv[1], std::ios_base::in);
+ std::ifstream infile(fname, std::ios_base::in);
// line input file
std::string line;
while (std::getline(infile, line, '\n')) {
std::stringstream strStream(line);
strStream >> number;
+ // Ignore out of range data (generated by simulator in robomon)
+ if (number >= 4000)
+ number = 0;
laser_scan[idx] = number;
idx++;
}
sd.line_detect(lines);
sd.arc_detect(arcs);
+ for (unsigned i = 0; i < lines.size(); i++) {
+ cout << "Line: " << lines[i] << endl;
+ }
+
+ for (unsigned i = 0; i < arcs.size(); i++) {
+ cout << "Arc: " << arcs[i] << endl;
+ }
+
+ if (plot)
+ gnuplot(sd.getCartes(), lines, arcs);
+
return 0;
}
#include "shape_detect.h"
-#ifdef GNUPLOT
-void Shape_detect::plot_line(std::vector<Shape_detect::Line> &lines)
-{
- fprintf(gnuplot, "set style line 1 pt 1 lc rgb \"green\"\n");
- fprintf(gnuplot, "set style line 2 lt 2 lc rgb \"red\" lw 2\n");
- fprintf(gnuplot, "set style line 3 pt 2 lc rgb \"blue\"\n");
-
-#ifdef OFFLINE
- fprintf(gnuplot, "plot");
-#else
- fprintf(gnuplot, "plot [-1000:+3000] [-3000:+3000]");
-#endif
- fprintf(gnuplot, "'-' with points ls 1, "); // points
- fprintf(gnuplot, "'-' with linespoints ls 2,"); // lines
- fprintf(gnuplot, "'-' with points ls 3"); //arc point
- fprintf(gnuplot, "\n");
-
- // points data
- for (int i = 0; i < (int) cartes.size(); i++) {
- fprintf(gnuplot, "%g %g\n", cartes[i].x, cartes[i].y);
- }
- fprintf(gnuplot, "e\n");
-
- // lines data
- for (int i = 0; i < (int) lines.size(); i++) {
- fprintf(gnuplot, "%f %f\n%f %f\n\n",
- lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
- }
- fprintf(gnuplot, "e\n");
-}
-
-void Shape_detect::plot_arc(std::vector<Shape_detect::Arc> &arcs)
-{
- // arcs data
- for (int i = 0; i < (int) arcs.size(); i++) {
- fprintf(gnuplot, "%g %g\n", arcs[i].begin.x, arcs[i].begin.y);
- fprintf(gnuplot, "%g %g\n", arcs[i].end.x, arcs[i].end.y);
- }
- fprintf(gnuplot, "e\n");
-
- plot_close();
-}
-
-void Shape_detect::plot_close()
-{
- fflush(gnuplot);
- getchar();
- pclose(gnuplot);
-}
-
-void Shape_detect::plot_init()
-{
- gnuplot = popen("gnuplot", "w");
- fprintf(gnuplot, "set grid\n");
- fprintf(gnuplot, "set nokey\n");
-#ifdef OFFLINE
- fprintf(gnuplot, "set size ratio 1\n"); //1.3333
-#endif
-}
-#endif // GNUPLOT
Shape_detect::Shape_detect()
{
}
+std::vector<Shape_detect::Point> &Shape_detect::getCartes()
+{
+ return cartes;
+}
+
void Shape_detect::prepare(const unsigned short laser_scan[])
{
-#ifdef GNUPLOT
- plot_init();
-#endif
polar_to_cartes(laser_scan);
}
fit_arc(start, end, arcs);
start = end + 1;
}
-#ifdef GNUPLOT
- plot_arc(arcs);
-#endif
}
void Shape_detect::line_detect(std::vector<Shape_detect::Line> &lines)
line_fitting(start, end, lines);
start = end + 1;
}
-#ifdef GNUPLOT
- plot_line(lines);
-#endif
}
The founded line is written into output vector lines. The method line_detect
is finished after research of all vectorparts.
-\section shapedet_debug_mode Debug mode
+\section shapedet_debug_mode Debugging
-The debug mode permits detection of segment lines with connected laser scanner
-or from scanned data, which are saved in a file. GNUPLOT can be used for graphical
-output in debug mode or output vector can be saved in a file for next processing
-in any application.
-
-For setting above properties is used directives define GNUPLOT.
-
-For debugging can be used methods introduced in file main.cc.
+The debug mode permits detection of segment lines with connected laser
+scanner or from scanned data, which are saved in a file. There is a
+program @a shape_detect_offline, which takes the measured data
+(generated for example by @a hokuyo-dump command) and writes out the
+detected lines. It can also plot the results using gnuplot (-g
+option).
\section shapedet_example Example
#include <robottype.h>
#include <roboorte_robottype.h>
-/**
- * Debug mode with GNUPLOT graph.
- * True -> Detected lines are painted by GNUPLOT.
- * False -> Lines isn't painted.
- * @ingroup shapedet
- */
-//#define GNUPLOT
-
/**
* There are detected line segments in input array of measured data (laser_scan)
* by using perpendicular line regression.
* @ingroup shapedet
*/
void prepare(const unsigned short laser_scan[]);
+
+ /** Returns laser_scan data set by prepare converted to cartesian coordinates */
+ std::vector<Point> &getCartes();
+
/**
* There are detected line segments in input array of measured
float Arc_min_aperture;
float Arc_std_max;
-#ifdef GNUPLOT
- FILE *gnuplot;
-
- /**
- * Method paints detection lines in GNUPLOT.
- * For debug mode. It must be defined global flag GNUPLOT.
- * @param &lines is vector which contains detected lines.
- * @param &cartes is vector whose points are expressed in cartesian coordinates.
- * @ingroup shapedet
- */
- void plot_line(std::vector<Line> &lines);
- void plot_arc(std::vector<Arc> &arcs);
-
- /**
- * Method for initialization GNUPLOT - opens pipe and performs basic settings.
- * For debug mode. It must be defined global flag GNUPLOT.
- * @ingroup shapedet
- */
- void plot_init();
- void plot_close();
-#endif
/**
* Calculation of lines intersection.
* @param point which pertains to line.
projects / eurobot / public.git / commitdiff