tablebot: onsite debugging

* deal with laser noise by filtering noisy points
* deal with laser noise by looking only away from robot for goal
* add version number to make it easier to know if upload worked

projects/tablebot/behaviors.hpp

@@ -302,7 +302,7 @@ bool verify_neck(uint32_t * last_stable_stamp, uint32_t stop_time = 250)
 int is_table_present()
 {
   // Inspect only points right in front of us
-  int points_ct = project_points(line_points, MAX_POINTS, true, 0.1f, -0.1f, 0.2f);
+  int points_ct = project_points(line_points, MAX_POINTS, true, 0.0f, 3.0f, 0.1f, -0.1f, 0.2f);
   if (points_ct < 5)
   {
     // Not enough - call table missing
@@ -522,7 +522,7 @@ void run_behavior(uint16_t id, uint32_t stamp)
         }
 
         // Find all points on top of the table, less than 0.5m to either side of the robot
-        int points_ct = project_points(line_points, MAX_POINTS, true, 0.5f, 0.0f, 0.2f);
+        int points_ct = project_points(line_points, MAX_POINTS, true, 0.0f, 3.0f, 0.5f, 0.0f, 0.2f);
 
         // Send those points for debugging
         udp_send_packet((unsigned char *) &line_points, points_ct * 12, return_port, PACKET_PROJECTED_POINTS);
@@ -710,7 +710,7 @@ void run_behavior(uint16_t id, uint32_t stamp)
         }
 
         // Find all points on top of the table, less than 0.5m to either side of the robot
-        int points_ct = project_points(line_points, MAX_POINTS, true, 0.5f, 0.0f, 0.2f);
+        int points_ct = project_points(line_points, MAX_POINTS, true, 0.15f, 3.0f, 0.5f, 0.0f, 0.2f);
 
         // Send those points for debugging
         udp_send_packet((unsigned char *) &line_points, points_ct * 12, return_port, PACKET_PROJECTED_POINTS);

projects/tablebot/main.cpp

@@ -177,6 +177,7 @@ int main(void)
   system_state.pose_y = 0.0f;
   system_state.pose_th = 0.0f;
   system_state.last_motor_command = 0;
+  system_state.version = 100;
 
   // Setup drive motors
   m1_pid.set_max_step(10);

projects/tablebot/segmentation.hpp

@@ -50,7 +50,7 @@ typedef struct
 // Projects the points from the assembler
 // Returns the number of points projected
 int project_points(point_t * points, int max_points, bool project_neck,
-                   float limit_y, float min_z, float max_z)
+                   float min_x, float max_x, float limit_y, float min_z, float max_z)
 {
   float cos_neck = 1.0f, sin_neck = 0.0f;
   if (project_neck)
@@ -86,8 +86,9 @@ int project_points(point_t * points, int max_points, bool project_neck,
     float xx = cos_neck * x;
     float zz = -sin_neck * x + 0.127f;  // Neck is 5" off ground
 
-    if (zz < max_z && zz > min_z &&
-        y > -limit_y && y < limit_y && x > 0.0f)
+    if (xx < max_x && xx > min_x &&
+        zz < max_z && zz > min_z &&
+        y > -limit_y && y < limit_y)
     {
       points[points_idx].x = xx;
       points[points_idx].y = y;
@@ -133,6 +134,23 @@ int extract_segments(point_t * points, int num_points,
     }
     else
     {
+      // If we skip this point (as noise) does the segment continue?
+      int next = i + 1;
+      if (next < num_points)
+      {
+        dx = segments[segment_idx].end.x - points[next].x;
+        dy = segments[segment_idx].end.y - points[next].y;
+        d = dx * dx + dy * dy;
+        if (d < jump_distance_squared)
+        {
+          // Add to current segment
+          segments[segment_idx].end = points[i];
+          ++segments[segment_idx].points;
+          continue;
+        }
+      }
+
+      // Can't make the segment continue
       if (++segment_idx >= max_segments)
       {
         // Out of segments to fill

projects/tablebot/tablebot.hpp

@@ -202,6 +202,7 @@ typedef struct
   float target_dist;
   float target_yaw;
 
+  uint32_t version;
   uint32_t last_motor_command;
 } system_state_t;
 

projects/tablebot/tablebot.py

@@ -57,6 +57,8 @@ class TableBotGUI:
     goal_pose_y = 0
     goal_pose_z = 0
 
+    version = None
+
     # Table size in meters
     # TODO: read this from firmware
     TABLE_LENGTH = 1.2192
@@ -228,6 +230,11 @@ def handle_packets(self):
                     self.target_pose = struct.unpack_from("<f", packet, 96)[0]
                     self.target_yaw = struct.unpack_from("<f", packet, 100)[0]
 
+                    version = struct.unpack_from("<L", packet, 104)[0]
+                    if self.version != version:
+                        self.version = version
+                        print("Version: %i" % version)
+
                     if len(self.pose_x) == 0 or \
                             abs(pose_x - self.pose_x[-1]) > 0.01 or \
                             abs(pose_y - self.pose_y[-1]) > 0.01 or \