robofsm: Tune function for target detection.

Run shape detection and store centers of all detected targets in a global variable (vector).

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 19aeaa94fcc523d6f7d5d6e930853c4897e54e22..96c8925a17577986ca1f30bdcb1c2b77b4be93b7 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -38,6 +38,10 @@ UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file
 
 struct TrajectoryConstraints tcSlow, tcVerySlow;
 
+/**
+ * Vector where all absolute positions of all detected targets are stored.
+ */
+std::vector<robot_pos_type> detected_target;
 
 /**
  * Safe distance for target recognition
@@ -77,11 +81,15 @@ bool close_goal(double goalx, double goaly)
                 return false;
 }
 
+/**
+ * Take data from hokuyo and run shape detection on it.
+ *
+ * Absolute positions of all detected targets centers are stored in alobal variable (vector).
+ *
+ * @return True if at least one target detected, else false.
+ */
 static bool detect_target()
 {
-        // run shape detection
-        // return TRUE when any circle detected
-
         struct hokuyo_scan_type hokuyo = robot.hokuyo;
 
         Shape_detect sd;
@@ -89,23 +97,33 @@ static bool detect_target()
         sd.prepare(hokuyo.data);
         sd.arc_detect(arcs);
 
-        if (arcs.size() > 0) {
-                robot_pos_type e;
-                sharp_pos hok, target;
+        // clear old targets
+        detected_target.clear();
 
-                hok.x = HOKUYO_CENTER_OFFSET_M;
-                hok.y = 0;
+        if (arcs.size() > 0) {
+                robot_pos_type e, target, hok;
 
                 robot_get_est_pos(&e.x, &e.y, &e.phi);
 
-                //TODO save all detected targets and
-                Shape_detect::Arc * a = &arcs[0];
-                hok.x += (double)a->center.x / 1000.0;
-                hok.y = (double)a->center.y / 1000.0;
+                double sinus = sin(e.phi);
+                double cosinus = cos(e.phi);
+
+                // save absolute positions of all detected targets
+                for (int i = 0; i < arcs.size(); i++) {
+                        Shape_detect::Arc *a = &arcs[i];
+
+                        hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
+                        hok.y = (double)a->center.y / 1000.0;
+
+                        /* transform target position which is relative to Hokuyo
+                        center to absolute position in space */
+                        target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
+                        target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
 
-                /* transform target position which is relative to Hokuyo center to absolute position in space */
-                robot.target_pos.x = (hok.x * cos(e.phi)) - (hok.y * sin(e.phi)) + e.x;
-                robot.target_pos.y = (hok.x * sin(e.phi)) + (hok.y * cos(e.phi)) + e.y;
+                        // filter those targets not in playground range
+                        if (goal_is_in_playground(target.x, target.y))
+                                detected_target.push_back(target);
+                }
                 return true;
         } else {
                 return false;