#include <shape_detect.h>
#include <iostream>
+#include <stdio.h>
using namespace std;
ostream& operator << (ostream& os, Shape_detect::Point& point)
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, "set palette model RGB functions .8-gray*2, .8-gray*2, .8-gray*2\n");
fprintf(gnuplot, "plot");
+ for (unsigned i=0; i < arcs.size(); i++) {
+ if (arcs[i].debug)
+ fprintf(gnuplot, "'-' matrix using ($1*%g+%g):($2*%g+%g):3 with image, ",
+ arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.x,
+ arcs[i].debug->acc_scale, arcs[i].debug->acc_origin.y);
+ }
if (cartes.size() > 0)
fprintf(gnuplot, "'-' with points ls 1");
if (lines.size() > 0)
fprintf(gnuplot, ", '-' with circles ls 2");
fprintf(gnuplot, "\n");
+ // Draw accumulators
+ for (int i = 0; i < (int) arcs.size(); i++) {
+ Shape_detect::Arc &a = arcs[i];
+ if (!a.debug)
+ continue;
+ for (int y=0; y < a.debug->acc_size; y++) {
+ for (int x=0; x < a.debug->acc_size; x++) {
+ fprintf(gnuplot, "%d ", a.debug->bitmap[y*a.debug->acc_size+x]);
+ }
+ fprintf(gnuplot, "\n");
+ }
+ fprintf(gnuplot, "e\ne\n");
+ }
if (cartes.size() > 0) {
for (int i = 0; i < (int) cartes.size(); i++) {
fprintf(gnuplot, "e\n");
}
if (arcs.size() > 0) {
+ // Draw circles
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");
}
-
+ fflush(gnuplot);
+ getchar();
+
pclose(gnuplot);
}
-/**
- * There are detected line segments in input file (laser scan data)
- * @param argv name input file with measured data
- * @ingroup shapedet
- */
-int main(int argc, char** argv)
+void get_laser_scan(unsigned short laser_scan[], const char *fname)
{
- Shape_detect sd;
- if (argc < 2) {
- std::cout << "Error: Invalid number of input parameters." << std::endl;
- 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(fname, std::ios_base::in);
// line input file
std::string line;
// number from input file
- int number;
+ unsigned short number;
int idx = 0;
laser_scan[idx] = number;
idx++;
}
-
+}
+
+/**
+ * There are detected line segments in input file (laser scan data)
+ * @param argv name input file with measured data
+ * @ingroup shapedet
+ */
+int main(int argc, char** argv)
+{
+ if (argc < 2 || strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0) {
+ std::cout << "Help:" << std::endl;
+ std::cout << "Example: ./shape_detect_offline -g -f file_name -n seq_length" << std::endl;
+ std::cout << "-g: Gnuplot output." << std::endl;
+ std::cout << "-f file_name example: seq-scan-<NUMBER>." << std::endl;
+ std::cout << "-n seq_length: Maximal length sequence file <NUMBER>." << std::endl;
+ std::cout << std::endl;
+
+ return 0;
+ }
+
+ Shape_detect sd;
+
+ bool plot = false;
+ char fname[50];
+ int number_file = 0;
+ unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
+
+ for (int i = 1; i < argc; i++) {
+ if (strcmp(argv[i], "-g") == 0)
+ plot = true;
+ else if (strcmp(argv[i], "-f") == 0) {
+ i++;
+ strcpy(fname, argv[i]);
+ } else if (strcmp(argv[i], "-n") == 0) {
+ i++;
+ number_file = atoi(argv[i]);
+ } else {
+ std::cout << "Invalid input parameter: " << argv[i] << "." << std::endl;
+ return 1;
+ }
+ }
+
std::vector<Shape_detect::Line> lines;
std::vector<Shape_detect::Arc> arcs;
+ std::vector<Shape_detect::Arc> result;
+
+ if (number_file == 0) {
+ std::cout << "Open file: " << fname << std::endl;
+ get_laser_scan(laser_scan, fname);
+
+ sd.prepare(laser_scan);
+ sd.line_detect(lines);
+ sd.arc_detect(result);
+ } else {
+ for (int i = 1; i <= number_file; i++) {
+ char name[50];
+ if (i < 10)
+ sprintf(name, "%s0%d", fname, i);
+ else
+ sprintf(name, "%s%d", fname, i);
+
+ std::cout << "Open file: " << name << std::endl;
+
+ get_laser_scan(laser_scan, name);
+
+ sd.prepare(laser_scan);
+
+ if (i == 1) {
+ sd.line_detect(lines);
+ sd.arc_detect(result);
+ continue;
+ }
+
+ sd.arc_detect(arcs);
+ result = sd.arcs_compare(result, arcs, 8);
+
+ if (result.size() == 0)
+ break;
+ }
+ }
- // shape detect
- sd.prepare(laser_scan);
- sd.line_detect(lines);
- sd.arc_detect(arcs);
+ std::cout << std::endl << "Result:" << std::endl;
- for (unsigned i = 0; i < lines.size(); i++) {
+ 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;
- }
+ for (unsigned i = 0; i < result.size(); i++)
+ cout << "Arc: " << result[i] << endl;
if (plot)
- gnuplot(sd.getCartes(), lines, arcs);
+ gnuplot(sd.getCartes(), lines, result);
return 0;
}
#include "shape_detect.h"
-
Shape_detect::Shape_detect()
{
Shape_detect::Line_min_points = 7;
Shape_detect::Line_error_threshold = 20;
Shape_detect::Max_distance_point = 300;
- Shape_detect::Arc_std_max = 0.2;//0.15
- Shape_detect::Arc_min_aperture = 1.57; //90 degrees
- Shape_detect::Arc_max_aperture = 2.365; //2.365 #135 degrees
+ Shape_detect::Radius = 100;
+ Shape_detect::Scale = 10;
+ Shape_detect::Arc_min_points = 10;
+ Shape_detect::Arc_max_distance = 2000;
+
+ bresenham_circle(circle);
}
-Shape_detect::Shape_detect(int line_min_points, int line_error_threshold, int max_distance_point)
+Shape_detect::Shape_detect(int line_min_points, int line_error_threshold, int max_distance_point,
+ float radius, float scale, int arc_min_points, int arc_max_distance)
{
Shape_detect::Line_min_points = line_min_points;
Shape_detect::Line_error_threshold = line_error_threshold;
Shape_detect::Max_distance_point = max_distance_point;
+
+ Shape_detect::Radius = radius;
+ Shape_detect::Scale = scale;
+ Shape_detect::Arc_min_points = arc_min_points;
+ Shape_detect::Arc_max_distance = arc_max_distance;
+
+ bresenham_circle(circle);
}
inline Shape_detect::Point Shape_detect::intersection_line(const Shape_detect::Point point, const General_form gen)
return tmp;
}
-inline Shape_detect::Point Shape_detect::rotate(Shape_detect::Point input_point, float rad)
-{
- Shape_detect::Point tmp;
- tmp.x = input_point.x * cos(rad) - input_point.y * sin(rad);
- tmp.y = input_point.x * sin(rad) + input_point.y * cos(rad);
-
- return tmp;
-}
-
inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::Point b)
{
return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}
-bool Shape_detect::fit_arc(int begin, int end, std::vector<Shape_detect::Arc> &arcs)
+std::vector<Shape_detect::Arc> Shape_detect::arcs_compare(std::vector<Shape_detect::Arc> &first, std::vector<Shape_detect::Arc> &second, int eps)
{
- //std::cout << "rozsah: " << begin << " - " << end << " -> ";
+ std::vector<Shape_detect::Arc> result;
- if (end-begin < 4) {
- //std::cout << "mensi nez 4" << std::endl;
- return false;
+ if (first.size() < 1 && second.size() < 1) {
+ return result;
+ }
+ if (first.size() < 1 && second.size() > 0) {
+ return second;
+ }
+ if (first.size() > 0 && second.size() < 1) {
+ return first;
+ }
+
+ for (unsigned int i = 0; i < first.size(); i++) {
+ for (unsigned int j = 0; j < second.size(); j++) {
+ if (point_distance(first[i].center, second[j].center) < eps) {
+ result.push_back(first[i]);
+ break;
+ }
+ }
}
- Point rotated_point = cartes[(end + begin) / 2];
- Point right = cartes[begin];
- Point left = cartes[end];
- rotated_point.x = rotated_point.x - left.x;
- rotated_point.y = rotated_point.y - left.y;
- float angle_to_rotate = atan2((left.x - right.x) / (left.y - right.y),1);
- rotated_point = rotate(rotated_point, angle_to_rotate);
+ return result;
+}
+
+void Shape_detect::bresenham_circle(std::vector<Shape_detect::Point> &circle)
+{
+ int d;
+
+ int scale_radius = abs(Shape_detect::Radius / Shape_detect::Scale);
+
+ int x = 0;
+ int y = scale_radius;
+
+ Shape_detect::Point tmp;
- //std::cout << "rotacni bod: " << rotated_point.x << ", " << rotated_point.y << "; vzdalenost: " << Shape_detect::point_distance(left, right) << "; ";
+ tmp.x = x;
+ tmp.y = y;
- if (-rotated_point.x < .1 * Shape_detect::point_distance(left, right) &&
- -rotated_point.x > Shape_detect::point_distance(left, right)) {
- //std::cout << "rotacni bod" << "; ";
- return false;
+ circle.push_back(tmp);
+
+ d = 3 - (2 * scale_radius);
+
+ for (x = 0; x <= y; x++) {
+ if (d < 0) {
+ y = y;
+ d = (d + (4 * x) + 6);
+ } else {
+ y--;
+ d = d + (4 * (x - y) + 10);
+ }
+
+ tmp.x = x;
+ tmp.y = y;
+
+ circle.push_back(tmp);
}
+
+ return;
+}
- int segment_size = end - begin + 1;
- int slopes_size = segment_size - 3;
+void Shape_detect::hough_transform_arc(int begin, int end, std::vector<Arc> &arcs)
+{
+ float max_x, max_y, min_x, min_y;
- float ma, mb, slopes[slopes_size];
+ max_x = cartes[begin].x;
+ min_x = max_x;
- for (int i = 0; i < slopes_size; i++) {
- Shape_detect::Point tmp = cartes[begin + i + 1];
- ma = atan2(left.y - tmp.y, left.x - tmp.x);
- mb = atan2(right.y - tmp.y, right.x - tmp.x);
+ max_y = cartes[begin].y;
+ min_y = max_y;
- slopes[i] = ma - mb;
+ for (int i = begin; i <= end; i++) {
+ //std::cout << cartes[i].x << " -- " << cartes[i].y << " || ";
+ if (cartes[i].x > max_x)
+ max_x = cartes[i].x;
+ else if (cartes[i].x < min_x)
+ min_x = cartes[i].x;
+
+ if (cartes[i].y > max_y)
+ max_y = cartes[i].y;
+ else if (cartes[i].y < min_y)
+ min_y = cartes[i].y;
}
- float sum = 0;
- for (int i = 0; i < slopes_size; i++)
- sum = sum + slopes[i];
+ const float center_x = (max_x - min_x) / 2 + min_x;
+ const float center_y = (max_y - min_y) / 2 + min_y;
- float average = sum / slopes_size;
+ const int asize = 600 / Shape_detect::Scale;
+ const int amid = asize / 2;
- float totalvar = 0;
- int count = slopes_size;
-
- while (count > 0) {
- count--;
- totalvar = totalvar + ((slopes[count] - average) * (slopes[count] - average));
+ Shape_detect::Arc arc;
+ //arc.debug = 0;
+ arc.debug = new (struct arc_debug);
+
+ if (arc.debug) {
+ arc.debug->bitmap = new int[asize * asize];
+ arc.debug->acc_size = asize;
+ arc.debug->acc_origin.x = center_x - amid * Shape_detect::Scale;
+ arc.debug->acc_origin.y = center_y - amid * Shape_detect::Scale;
+ arc.debug->acc_scale = Shape_detect::Scale;
}
+
+ int accumulator[asize][asize];
- float standard_deviation = sqrt(totalvar / --slopes_size);
+ memset(accumulator, 0, sizeof(accumulator));
- //std::cout << "std: " << standard_deviation << ", Average: " << average << ": ";
+ for (int i = begin; i <= end; i++) {
+ int xc = floor((cartes[i].x - center_x) / Shape_detect::Scale);
+ int yc = floor((cartes[i].y - center_y) / Shape_detect::Scale);
- if (standard_deviation < Arc_std_max && Arc_max_aperture > abs(average) && abs(average) > Arc_min_aperture) {
- Shape_detect::Point tmp;
- tmp.x = right.x - left.x;
- tmp.y = right.y - left.y;
+ for (unsigned int i = 0; i < circle.size(); i++) {
+ int par[8];
- angle_to_rotate = atan2(tmp.y, tmp.x);
+ par[0] = amid + yc + (int) circle[i].x;
+ par[1] = amid + yc - (int) circle[i].x;
+ par[2] = amid + xc + (int) circle[i].x;
+ par[3] = amid + xc - (int) circle[i].x;
+ par[4] = amid + yc + (int) circle[i].y;
+ par[5] = amid + yc - (int) circle[i].y;
+ par[6] = amid + xc + (int) circle[i].y;
+ par[7] = amid + xc - (int) circle[i].y;
- tmp = Shape_detect::rotate(tmp, -angle_to_rotate);
+ if (par[0] > 0 && par[0] < asize && par[6] > 0 && par[6] < asize)
+ accumulator[par[0]][par[6]] += 1;
- float middle = tmp.x / 2.0;
- float height = middle * tan(average - M_PI / 2.0);
+ if (par[1] > 0 && par[1] < asize && par[7] > 0 && par[7] < asize)
+ accumulator[par[1]][par[7]] += 1;
- Shape_detect::Point center;
- center.x = middle;
- center.y = height;
+ if (par[1] > 0 && par[1] < asize && par[6] > 0 && par[6] < asize)
+ accumulator[par[1]][par[6]] += 1;
- center = Shape_detect::rotate(center, angle_to_rotate);
- center.x = center.x + left.x;
- center.y = center.y + left.y;
+ if (par[0] > 0 && par[0] < asize && par[7] > 0 && par[7] < asize)
+ accumulator[par[0]][par[7]] += 1;
- Shape_detect::Arc arc;
- arc.center = center;
- arc.begin = cartes[begin];
- arc.end = cartes[end];
-
- arc.radius = 0;
- for (int i = 0; i < segment_size; i++) {
- tmp.x = cartes[begin + i].x;
- tmp.y = cartes[begin + i].y;
- arc.radius = arc.radius + float(sqrt((tmp.x - center.x)*(tmp.x - center.x)+(tmp.y - center.y)*(tmp.y - center.y)));
+ if (par[4] > 0 && par[4] < asize && par[2] > 0 && par[2] < asize)
+ accumulator[par[4]][par[2]] += 1;
+
+ if (par[5] > 0 && par[5] < asize && par[3] > 0 && par[3] < asize)
+ accumulator[par[5]][par[3]] += 1;
+
+ if (par[5] > 0 && par[5] < asize && par[2] > 0 && par[2] < asize)
+ accumulator[par[5]][par[2]] += 1;
+
+ if (par[4] > 0 && par[4] < asize && par[3] > 0 && par[3] < asize)
+ accumulator[par[4]][par[3]] += 1;
}
+ }
- arc.radius = arc.radius / segment_size;
+ Shape_detect::Point center;
- arcs.push_back(arc);
+ center.x = 0;
+ center.y = 0;
+
+ arc.radius = Shape_detect::Radius; //radius [mm]
+
+ int max = 0;
- //std::cout << "center: " << center.x << "; " << center.y << " >> ";
+ for (int i = 0; i < asize; i++) {
+ for (int j = 0; j < asize; j++) {
+ if (accumulator[i][j] > max) {
- //std::cout << "Detekovan" << std::endl;
- return 1;
+ max = accumulator[i][j];
+
+ center.x = (j - amid) * Shape_detect::Scale + center_x;
+ center.y = (i - amid) * Shape_detect::Scale + center_y;
+ }
+ }
}
- //fit_arc(begin + 1, end - 1, arcs);
+ Shape_detect::Point origin;
- //std::cout << "Nic" << std::endl;
+ origin.x = 0.0;
+ origin.y = 0.0;
- return 0;
+ if (max > (end - begin) / 3 && point_distance(origin, center) > point_distance(origin, cartes[end])) {
+ arc.center = center;
+ memcpy(arc.debug->bitmap, accumulator, sizeof(accumulator));
+ arcs.push_back(arc);
+ }
+
+ return;
}
int Shape_detect::perpendicular_regression(float &r, const int begin, const int end, General_form &gen)
for (int i = 0; i < (int) HOKUYO_ARRAY_SIZE; i++) {
r = (laser_scan[i] <= 19) ? 0 : laser_scan[i];
+
+ fi = HOKUYO_INDEX_TO_RAD(i);
- if (r > 0) {
+ if (r > 0 && fi > (-1 * HOKUYO_RANGE_ANGLE_LEFT / 180.0 * M_PI) && fi < (HOKUYO_RANGE_ANGLE_RIGHT / 180.0 * M_PI)) {
fi = HOKUYO_INDEX_TO_RAD(i);
point.x = r * cos(fi);
point.y = r * sin(fi);
void Shape_detect::prepare(const unsigned short laser_scan[])
{
+ cartes.clear();
polar_to_cartes(laser_scan);
}
int cartes_size = cartes.size();
int end, start = 0;
+
while (start < cartes_size) {
+ Shape_detect::Point tmp_start = cartes[start];
+
end = start + 1;
- while (point_distance(cartes[end-1], cartes[end]) < 50 && end < cartes_size)
+ while (point_distance(cartes[end-1], cartes[end]) < 100
+ && end < cartes_size
+ && cartes[end].x < Shape_detect::Arc_max_distance
+ && cartes[end].y < Shape_detect::Arc_max_distance) {
end++;
+ }
end --;
- fit_arc(start, end, arcs);
+ if (point_distance(tmp_start, cartes[end]) < (2 * Shape_detect::Radius) && (end - start > Shape_detect::Arc_min_points))
+ hough_transform_arc(start, end, arcs);
+
start = end + 1;
}
}
#include <hokuyo.h>
#include <robot.h>
#include <robomath.h>
+#include <robodim.h>
#include <robottype.h>
#include <roboorte_robottype.h>
{
public:
/**
- * The constructor with default setting of detection properties (pro Hokuyo).
+ * The constructor with default setting of detection properties (for Hokuyo).
* Line_min_points = 7
* Line_error_threshold = 20
* Max_distance_point = 300
+ * Radius = 100
+ * Scale = 10
+ * Arc_min_points = 10
+ * Arc_max_distance = 1000
* @ingroup shapedet
*/
Shape_detect (void);
* @param line_min_points the minimal number of points which can create segment line.
* @param line_error_threshold the maximal pointerror from segment line of regression.
* @param max_distance_point the maximal Euclidean distance of point.
+ * @param radius the radius detected arc.
+ * @param scale is precision detected center of arc.
+ * @param arc_min_points the minimal number of points which can create arc.
+ * @param arc_max_distance the maximal distance detected arc from origin coordinates [0; 0]
* @ingroup shapedet
*/
- Shape_detect (int line_min_points, int line_error_threshold, int max_distance_point);
+ Shape_detect (int line_min_points, int line_error_threshold, int max_distance_point,
+ float radius, float scale, int arc_min_points, int arc_max_distance);
/**
* General equation of line -> Ax + By + C = 0.
Point b; /**< end point from a line. */
} Line;
+ struct arc_debug {
+ Point acc_origin;
+ float acc_scale;
+ int acc_size;
+ int *bitmap;
+ };
+
/**
* Arc defined by TODO.
* @ingroup shapedet
Point begin;
Point end;
float radius;
+ struct arc_debug *debug;
} Arc;
/**
*/
void prepare(const unsigned short laser_scan[]);
- /** Returns laser_scan data set by prepare converted to cartesian coordinates */
+ /**
+ * Returns laser_scan data set by prepare converted to cartesian coordinates
+ * @ingroup shapedet
+ */
std::vector<Point> &getCartes();
*/
void arc_detect(std::vector<Arc> &arcs);
+ /**
+ * Is uset for comparing of two detected arcs vectors.
+ * @param &first is vector for comparing.
+ * @param &second is vector for comparing.
+ * @param eps is pertubation measured center of arc.
+ * @return vector equality center of arc.
+ * @ingroup shapedet
+ */
+ std::vector<Arc> arcs_compare(std::vector<Arc> &first, std::vector<Arc> &second, int eps);
+
private:
/**
* The minimal number of points which can create segment line.
*/
int Max_distance_point;
+ /**
+ * The radius detected arc.
+ * @ingroup shapedet
+ */
+ float Radius; // [mm]
+
+ /**
+ * The precision detected center of arc.
+ * @ingroup shapedet
+ */
+ float Scale; // [mm]
+
+ /**
+ * The minimal number of points which can create arc.
+ * @ingroup shapedet
+ */
+ int Arc_max_distance; //[mm]
+
+ /**
+ * The maximal distance detected arc from origin coordinates [0; 0]
+ * @ingroup shapedet
+ */
+ int Arc_min_points;
+
std::vector<Point> cartes;//(HOKUYO_ARRAY_SIZE);
- float Arc_max_aperture;
- float Arc_min_aperture;
- float Arc_std_max;
+ std::vector<Shape_detect::Point> circle;
/**
* Calculation of lines intersection.
*/
inline float point_distance(Point a, Point b);
- inline Point rotate(Point input_point, float rad);
+ void bresenham_circle(std::vector<Point> &circle);
- bool fit_arc(int begin, int end, std::vector<Arc> &arcs);
+ // radius [mm], scale [mm]
+ void hough_transform_arc(int begin, int end, std::vector<Arc> &arcs);
};
#endif // SHAPE_DETECT
# Library with general support functions for the robot
lib_LIBRARIES += robot
robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc \
- motion-control.cc map_handling.c \
+ motion-control.cc map_handling.cc \
match-timing.c
robot_GEN_SOURCES = roboevent.c
include_GEN_HEADERS += roboevent.h
# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robomath roboorte robottype pthread rt m \
orte pathplan sharp map fsm rbtree motion robodim \
- actlib ulut
+ actlib ulut shape_detect
# Automatic generation of event definition files
include-pass_HOOKS = roboevent.c roboevent.h
#include <robomath.h>
#include <hokuyo.h>
+#include <shape_detect.h>
+
+#include "map_handling.h"
+
/*******************************************************************************
* Parameters of Obstacle detection
*******************************************************************************/
-#define OBS_SIZE_M 0.1 /**< Expected size of detected obstacle */
-#define IGNORE_CLOSER_THAN_M 0.1 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
+#define OBS_SIZE_M 0.2 /**< Expected size of detected obstacle */
+#define IGNORE_CLOSER_THAN_M 0.2 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
#define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
#define OBS_FORGET_PERIOD 100 /**< The period of thread_obstacle_forgeting [ms] */
#define OBS_FORGET_SEC 1 /**< Time to completely forget detected obstacle. */
if (!ShmapIsCellInMap(i, j)) continue;
ShmapCell2Point(i, j, &xx, &yy);
if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
- (distance(xx, yy, x, y) < OBS_SIZE_M)) { /* We expect cirtular area around obstacle */
+ (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
}
}
}
+
}
+
+void figure_detected_at(double x, double y, const bool state)
+{
+ int i,j, xcell, ycell;
+ struct robot_pos_type est_pos;
+ struct map *map = robot.map;
+ double xx, yy;
+ bool valid;
+
+ if (!map)
+ return;
+
+ ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
+ /* Ignore obstacles outside playground */
+ if (!valid)
+ return;
+
+ /* Ignore obstacles at marked places */
+ if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
+ return;
+
+ if (state) {
+ map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+
+ /* Mark "protected" area around the obstacle */
+ robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
+
+ int obst_size_cell = (int)ceil(0.2/MAP_CELL_SIZE_M);
+ for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
+ for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
+ if (!ShmapIsCellInMap(i, j)) continue;
+ ShmapCell2Point(i, j, &xx, &yy);
+ if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
+ (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
+ map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
+ }
+ }
+ }
+
+ }
+}
+
/**
* A thread running the trajectory recalc
*
*
* @return
*/
-
void obst_coord(struct robot_pos_type *e, const struct sharp_pos *s, double v, double *x, double *y)
{
double sx, sy, sa;
*y = sy+v*sin(sa);
}
+void get_checkerboard(std::vector<Shape_detect::Point> &team)
+{
+ Shape_detect::Point tmp, newPoint, start;
+ unsigned int i;
+
+ if (robot.team_color) {
+ start.x = 0.625;
+ start.y = 0.525;
+ } else {
+ start.x = 0.975;
+ start.y = 0.525;
+
+ }
+
+ tmp = start;
+
+ for (i = 1; i < 4; i++) {
+ for (int j = 0; j < 3; j++) {
+ newPoint.y = tmp.y + 0.7 * j;
+ newPoint.x = tmp.x;
+ team.push_back(newPoint);
+ }
+ tmp.x = tmp.x + 0.7;
+ }
+
+ if (robot.team_color) {
+ tmp.x = start.x + 0.35;
+ tmp.y = start.y + 0.35;
+ } else {
+ tmp.x = start.x - 0.35;
+ tmp.y = start.y + 0.35;
+
+ }
+
+ for (i = 1; i < 4; i++) {
+ for (int j = 0; j < 2; j++) {
+ newPoint.y = tmp.y + 0.7 * j;
+ newPoint.x = tmp.x;
+ team.push_back(newPoint);
+ }
+ tmp.x = tmp.x + 0.7;
+ }
+
+ if (robot.team_color) {
+ tmp.x = 1.675;
+ tmp.y = 0.175;
+ } else {
+ tmp.x = 1.325;
+ tmp.y = 0.175;
+
+ }
+
+ team.push_back(tmp);
+}
+
+inline float point_distance(Shape_detect::Point a, Shape_detect::Point b)
+{
+ return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
+}
+
void update_map_hokuyo(struct hokuyo_scan_type *s)
{
double x, y;
- //Pos p;
struct robot_pos_type e;
- int i;
+ unsigned int i;
struct sharp_pos beam;
u_int16_t *data;
+ static std::vector<Shape_detect::Point> reds;
+ static std::vector<Shape_detect::Point> blues;
+
+ if (reds.size() < 16) {
+ get_checkerboard(blues);
+ get_checkerboard(reds);
+ }
+
robot_get_est_pos(&e.x, &e.y, &e.phi);
beam.x = HOKUYO_CENTER_OFFSET_M;
data = s->data;
+ Shape_detect shapeDet;
+ std::vector<Shape_detect::Arc> arcs;
+ std::vector<Shape_detect::Point> center_arcs;
+
+ shapeDet.prepare(data);
+ shapeDet.arc_detect(arcs);
+
+ Shape_detect::Point tmpPoint;
+
+ double distance;
+
+ if (arcs.size() > 0) {
+ for (i = 0; i < arcs.size(); i++) {
+ x = arcs[i].center.x / 1000;
+ y = arcs[i].center.y / 1000;
+
+ tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
+ tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
+
+ center_arcs.push_back(tmpPoint);
+ }
+
+ for (i = 0; i < center_arcs.size(); i++) {
+ if (robot.team_color) {
+ for (unsigned int j = 0; j < blues.size(); j++) {
+ distance = point_distance(blues[j], center_arcs[i]);
+ if (distance < 0.05) {
+ figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
+ break;
+ }
+ }
+ } else {
+ for (unsigned int j = 0; j < reds.size(); j++) {
+ distance = point_distance(reds[j], center_arcs[i]);
+ if (distance < 0.05) {
+ figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ bool obstacle = true;
+
for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
beam.ang = HOKUYO_INDEX_TO_RAD(i);
if((beam.ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI))))
if(data[i] > 19 && data[i] < 4000) {
obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
- obstacle_detected_at(x, y, true);
- //obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
- //obstacle_detected_at(x, y, false);
- }
+ tmpPoint.x = x;
+ tmpPoint.y = y;
+
+ if (center_arcs.size() > 0) {
+ for (unsigned int j = 0; j < center_arcs.size(); j++) {
+ if (point_distance(tmpPoint, center_arcs[j]) < 0.12) {
+ obstacle = false;
+ break;
+ }
+ }
+ }
+
+ if (obstacle) {
+ obstacle_detected_at(x, y, true);
+ //obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
+ //obstacle_detected_at(x, y, false);
+ }
+ obstacle = true;
+ }
}
}
#include <robodim.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
void * thread_obstacle_forgeting(void * arg);
/*void update_map(struct sharps_type *s);*/
void update_map_hokuyo(struct hokuyo_scan_type *s);
+#ifdef __cplusplus
+}
+#endif
+
#endif
# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robodim laser-nav robomath roboorte \
robottype pthread rt m orte pathplan sharp map fsm \
- rbtree motion actlib ulut
+ rbtree motion actlib ulut shape_detect
for (unsigned i = 0; i < arcs.size(); i++) {
Shape_detect::Arc *a = &arcs[i];
+ painter->drawPoint(QPoint(a->center.x, a->center.y));
painter->drawEllipse(QRectF(a->center.x - a->radius, a->center.y - a->radius,
2*a->radius, 2*a->radius));
}
projects / eurobot / public.git / commitdiff